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 | //---------------------------------------------------------------------- |
---|
56 | namespace rrlib |
---|
57 | { |
---|
58 | namespace geometry |
---|
59 | { |
---|
60 | |
---|
61 | |
---|
62 | |
---|
63 | template <typename TElement> |
---|
64 | inline 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 | |
---|
69 | template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type> |
---|
70 | inline 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 | |
---|
82 | template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type> |
---|
83 | inline 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 | |
---|
90 | namespace 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 ¶meter, 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 | |
---|
119 | template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type> |
---|
120 | inline double GetMaxCurvature(const tBezierCurve<Tdimension, TElement, Tdegree> &curve, TElement ¶meter) |
---|
121 | { |
---|
122 | return internal::GetMaxCurvatureHelper(curve, parameter, 0.0, 1.0); |
---|
123 | } |
---|
124 | |
---|
125 | template < size_t Tdimension, typename TElement, unsigned int Tdegree, typename = typename std::enable_if<Tdimension <= 3, int>::type> |
---|
126 | inline double GetMaxCurvature(const tSplineCurve<Tdimension, TElement, Tdegree> &spline, TElement ¶meter) |
---|
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 |
---|