source: rrlib_geometry/functions.h @ 93:a5e38d1d01b4

14.08
Last change on this file since 93:a5e38d1d01b4 was 93:a5e38d1d01b4, checked in by Tobias Föhst <tobias.foehst@…>, 12 months ago

Adds functions to get the curvature from a spline curve and to find the maximum curvature on Bezier curves and splines

File size: 5.6 KB
Line 
1//
2// You received this file as part of RRLib
3// Robotics Research Library
4//
5// Copyright (C) Finroc GbR (finroc.org)
6//
7// This program is free software; you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation; either version 2 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License along
18// with this program; if not, write to the Free Software Foundation, Inc.,
19// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20//
21//----------------------------------------------------------------------
22/*!\file    functions.h
23 *
24 * \author  Tobias Foehst
25 *
26 * \date    2010-12-27
27 *
28 * \brief
29 *
30 * \b
31 *
32 * A few words for functions.h
33 *
34 */
35//----------------------------------------------------------------------
36#ifndef __rrlib__geometry__functions_h__
37#define __rrlib__geometry__functions_h__
38
39//----------------------------------------------------------------------
40// External includes with <>
41//----------------------------------------------------------------------
42#include <cmath>
43
44//----------------------------------------------------------------------
45// Internal includes with ""
46//----------------------------------------------------------------------
47
48//----------------------------------------------------------------------
49// Debugging
50//----------------------------------------------------------------------
51#include <cassert>
52
53//----------------------------------------------------------------------
54// Namespace declaration
55//----------------------------------------------------------------------
56namespace rrlib
57{
58namespace geometry
59{
60
61
62
63template <typename TElement>
64inline math::tAngleRad GetSlope(const tLineSegment<2, TElement> &line_segment)
65{
66  return std::atan2(line_segment.End().Y() - line_segment.Begin().Y(), line_segment.End().X() - line_segment.Begin().X());
67}
68
69template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
70inline double GetCurvature(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement parameter)
71{
72  typename tBezierCurve<Tdimension, TElement, Tdegree>::tDerivative first_derivative(curve.GetDerivative());
73  typename tBezierCurve<Tdimension, TElement, Tdegree>::tDerivative::tDerivative second_derivative(first_derivative.GetDerivative());
74
75  math::tVector<3, TElement, math::vector::Cartesian> first(first_derivative(parameter));
76  math::tVector<3, TElement, math::vector::Cartesian> second(second_derivative(parameter));
77  double first_length = first.Length();
78
79  return CrossProduct(first, second).Length() / (first_length * first_length * first_length);
80}
81
82template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
83inline double GetCurvature(const tSplineCurve<Tdimension, TElement, Tdegree> &spline, TElement parameter)
84{
85  TElement local_t;
86  auto bezier = spline.GetBezierCurveForParameter(parameter, local_t);
87  return GetCurvature(*bezier, local_t);
88}
89
90namespace internal
91{
92  template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
93  double GetMaxCurvatureHelper(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement &parameter, TElement lower, TElement upper)
94{
95  auto middle = (upper + lower) / 2;
96  auto lower_half_parameter = (lower + middle) / 2;
97  auto upper_half_parameter = (middle + upper) / 2;
98  auto lower_half_curvature = GetCurvature(curve, lower_half_parameter);
99  auto upper_half_curvature = GetCurvature(curve, upper_half_parameter);
100
101  if (lower_half_curvature > upper_half_curvature)
102  {
103    if (math::IsEqual(lower, lower_half_parameter))
104    {
105      parameter = lower_half_parameter;
106      return lower_half_curvature;
107    }
108    return GetMaxCurvatureHelper(curve, parameter, lower, middle);
109  }
110  if (math::IsEqual(upper, upper_half_parameter))
111  {
112    parameter = upper_half_parameter;
113    return upper_half_curvature;
114  }
115  return GetMaxCurvatureHelper(curve, parameter, middle, upper);
116}
117}
118
119template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
120inline double GetMaxCurvature(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement &parameter)
121{
122  return internal::GetMaxCurvatureHelper(curve, parameter, 0.0, 1.0);
123}
124
125template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type>
126inline double GetMaxCurvature(const tSplineCurve<Tdimension, TElement, Tdegree> &spline, TElement &parameter)
127{
128  assert(spline.NumberOfSegments() > 0);
129  double max_curvature = GetMaxCurvature(*spline.GetBezierCurveForSegment(0), parameter);
130  for (std::size_t i = 1; i < spline.NumberOfSegments(); ++i)
131  {
132    TElement parameter_candidate;
133    double curvature_candidate = GetMaxCurvature(*spline.GetBezierCurveForSegment(i), parameter_candidate);
134    if (curvature_candidate > max_curvature)
135    {
136      max_curvature = curvature_candidate;
137      parameter = i + parameter_candidate;
138    }
139  }
140  return max_curvature;
141}
142
143
144
145//----------------------------------------------------------------------
146// End of namespace declaration
147//----------------------------------------------------------------------
148}
149}
150
151#endif
Note: See TracBrowser for help on using the repository browser.