source: rrlib_geometry/curves/tBezierCurve.hpp @ 23:6b2994bf47c6

Last change on this file since 23:6b2994bf47c6 was 18:5b1657e5f5c8, checked in by Tobias Föhst <foehst@…>, 9 years ago

Fixed issues with static const values and added GetCurvature for 2- and 3-dimensional Bezier curves

File size: 17.7 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
8// modify it under the terms of the GNU General Public License
9// as published by the Free Software Foundation; either version 2
10// of the License, or (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
18// along with this program; if not, write to the Free Software
19// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
20//
21//----------------------------------------------------------------------
22/*!\file    tBezierCurve.hpp
23 *
24 * \author  Tobias Foehst
25 *
26 * \date    2009-05-25
27 *
28 */
29//----------------------------------------------------------------------
30
31//----------------------------------------------------------------------
32// External includes (system with <>, local with "")
33//----------------------------------------------------------------------
34
35//----------------------------------------------------------------------
36// Internal includes with ""
37//----------------------------------------------------------------------
38
39//----------------------------------------------------------------------
40// Debugging
41//----------------------------------------------------------------------
42#include <cassert>
43
44//----------------------------------------------------------------------
45// Namespace declaration
46//----------------------------------------------------------------------
47namespace rrlib
48{
49namespace geometry
50{
51
52//----------------------------------------------------------------------
53// Forward declarations / typedefs / enums
54//----------------------------------------------------------------------
55
56//----------------------------------------------------------------------
57// Const values
58//----------------------------------------------------------------------
59
60//----------------------------------------------------------------------
61// Implementation
62//----------------------------------------------------------------------
63
64//----------------------------------------------------------------------
65// tBezierCurve constructors
66//----------------------------------------------------------------------
67template <size_t Tdimension, typename TElement, unsigned int Tdegree>
68template <typename TIterator>
69tBezierCurve<Tdimension, TElement, Tdegree>::tBezierCurve(TIterator begin, TIterator end)
70    : tShape()
71{
72  static_assert(Tdegree > 0, "The degree of Bezier curves must be greater than zero");
73  assert(static_cast<size_t>(std::distance(begin, end)) == this->NumberOfControlPoints());
74  size_t index = 0;
75  for (TIterator it = begin; it != end; ++it)
76  {
77    this->control_points[index++] = *it;
78  }
79}
80
81template <size_t Tdimension, typename TElement, unsigned int Tdegree>
82template <typename TSTLContainer>
83tBezierCurve<Tdimension, TElement, Tdegree>::tBezierCurve(const TSTLContainer &control_points)
84{
85  std::cout << "control_points: " << control_points.size() << std::endl;
86  std::cout << "degree: " << Tdegree << std::endl;
87  std::cout << "expected number of points: " << this->NumberOfControlPoints() << std::endl;
88  static_assert(Tdegree > 0, "The degree of Bezier curves must be greater than zero");
89  assert(control_points.size() == this->NumberOfControlPoints());
90  for (size_t i = 0; i < control_points.size(); ++i)
91  {
92    this->control_points[i] = control_points[i];
93  }
94}
95
96//----------------------------------------------------------------------
97// tBezierCurve SetControlPoint
98//----------------------------------------------------------------------
99template <size_t Tdimension, typename TElement, unsigned int Tdegree>
100void tBezierCurve<Tdimension, TElement, Tdegree>::SetControlPoint(size_t i, const typename tShape::tPoint &point)
101{
102  assert(i < this->NumberOfControlPoints());
103  this->control_points[i] = point;
104  this->SetChanged();
105};
106
107//----------------------------------------------------------------------
108// tBezierCurve GetTwist
109//----------------------------------------------------------------------
110template <size_t Tdimension, typename TElement, unsigned int Tdegree>
111const TElement tBezierCurve<Tdimension, TElement, Tdegree>::GetTwist() const
112{
113  TElement twist = 0.0;
114  for (size_t i = 0; i < Tdegree - 1; ++i)
115  {
116    twist = std::max(twist, ((this->control_points[i + 2] - this->control_points[i + 1]) - (this->control_points[i + 1] - this->control_points[i])).Length());
117  }
118  return Tdegree *(Tdegree - 1) * twist;
119}
120
121//----------------------------------------------------------------------
122// tBezierCurve GetSubdivision
123//----------------------------------------------------------------------
124template <size_t Tdimension, typename TElement, unsigned int Tdegree>
125const typename tBezierCurve<Tdimension, TElement, Tdegree>::tSubdivision tBezierCurve<Tdimension, TElement, Tdegree>::GetSubdivision() const
126{
127  typename tShape::tPoint left_half[this->NumberOfControlPoints()];
128  typename tShape::tPoint right_half[this->NumberOfControlPoints()];
129  typename tShape::tPoint temp_points[this->NumberOfControlPoints()];
130  std::memcpy(temp_points, this->control_points, this->NumberOfControlPoints() * sizeof(typename tShape::tPoint));
131
132  left_half[0] = temp_points[0];
133  right_half[Tdegree] = temp_points[Tdegree];
134
135  size_t k = 0;
136  while (k < Tdegree)
137  {
138    for (size_t i = 0; i < Tdegree - k; ++i)
139    {
140      temp_points[i] = (temp_points[i] + temp_points[i + 1]) * 0.5;
141    }
142    ++k;
143    left_half[k] = temp_points[0];
144    right_half[Tdegree - k] = temp_points[Tdegree - k];
145  }
146
147  return std::make_pair(tBezierCurve(left_half, left_half + this->NumberOfControlPoints()), tBezierCurve(right_half, right_half + this->NumberOfControlPoints()));
148}
149
150//----------------------------------------------------------------------
151// tBezierCurve Evaluation: operator ()
152//----------------------------------------------------------------------
153template <size_t Tdimension, typename TElement, unsigned int Tdegree>
154const typename tShape<Tdimension, TElement>::tPoint tBezierCurve<Tdimension, TElement, Tdegree>::operator()(tParameter t) const
155{
156  assert((0.0 <= t) && (t <= 1.0));
157
158  typename tShape::tPoint temp_points[this->NumberOfControlPoints()];
159  std::memcpy(temp_points, this->control_points, (this->NumberOfControlPoints()) * sizeof(typename tShape::tPoint));
160
161  size_t k = 0;
162  while (k < Tdegree)
163  {
164    for (size_t i = 0; i < Tdegree - k; ++i)
165    {
166      temp_points[i] = ((1.0 - t) * temp_points[i]) + (t * temp_points[i + 1]);
167    }
168    ++k;
169  }
170
171  return temp_points[0];
172}
173
174//----------------------------------------------------------------------
175// tBezierCurve GetDerivative
176//----------------------------------------------------------------------
177template <size_t Tdimension, typename TElement, unsigned int Tdegree>
178const typename tBezierCurve<Tdimension, TElement, Tdegree>::tDerivative tBezierCurve<Tdimension, TElement, Tdegree>::GetDerivative() const
179{
180  typename tShape::tPoint temp[Tdegree];
181  for (size_t i = 0; i < Tdegree; ++i)
182  {
183    temp[i] = Tdegree * (this->control_points[i + 1] - this->control_points[i]);
184  }
185  return tDerivative(temp, temp + Tdegree);
186}
187
188//----------------------------------------------------------------------
189// tBezierCurve GetIntersections
190//----------------------------------------------------------------------
191template <size_t Tdimension, typename TElement, unsigned int Tdegree>
192template <unsigned int Tother_degree>
193const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
194    const tBezierCurve<Tdimension, TElement, Tother_degree> &other) const
195{
196  return this->GetIntersections(intersection_points, intersection_parameters, other, 0.0, 1.0);
197}
198
199template <size_t Tdimension, typename TElement, unsigned int Tdegree>
200const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
201    const tLine<Tdimension, TElement> &line) const
202{
203  return this->GetIntersections(intersection_points, intersection_parameters, line, 0.0, 1.0);
204}
205
206template <size_t Tdimension, typename TElement, unsigned int Tdegree>
207template <unsigned int Tother_degree>
208const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
209    const tBezierCurve<Tdimension, TElement, Tother_degree> &other,
210    tParameter min_parameter, tParameter max_parameter) const
211{
212  // check if bounding boxes intersect
213  if (!this->BoundingBox().Intersects(other.BoundingBox()))
214  {
215    return false;
216  }
217
218  // subdivide curve with greater twist if necessary
219  TElement own_twist = this->GetTwist();
220  TElement other_twist = other.GetTwist();
221
222  if (own_twist > other_twist && !math::IsEqual(own_twist, 0))
223  {
224    tSubdivision subdivision(this->GetSubdivision());
225    tParameter middle_parameter(0.5 *(min_parameter + max_parameter));
226    bool result = false;
227    result |= subdivision.first.GetIntersections(intersection_points, intersection_parameters, other, min_parameter, middle_parameter);
228    result |= subdivision.second.GetIntersections(intersection_points, intersection_parameters, other, middle_parameter, max_parameter);
229    return result;
230  }
231  else if (!math::IsEqual(other_twist, 0))
232  {
233    tSubdivision subdivision(other.GetSubdivision());
234    bool result = false;
235    result |= this->GetIntersections(intersection_points, intersection_parameters, subdivision.first, min_parameter, max_parameter);
236    result |= this->GetIntersections(intersection_points, intersection_parameters, subdivision.second, min_parameter, max_parameter);
237    return result;
238  }
239
240  // compute the intersection using the baselines of the two control polygons
241  tLine<Tdimension, TElement> own_line_segment(this->control_points[0], this->control_points[Tdegree]);
242  tLine<Tdimension, TElement> other_line_segment(other.GetControlPoint(0), other.GetControlPoint(Tother_degree));
243  typename tShape::tPoint intersection_point;
244  if (own_line_segment.GetIntersection(intersection_point, other_line_segment) && !math::IsEqual(intersection_point, intersection_points.back(), 0.001))
245  {
246    intersection_points.push_back(intersection_point);
247    const tParameter baseline_parameter((intersection_points.back() - this->control_points[0]).Length() / (this->control_points[Tdegree] - this->control_points[0]).Length());
248    assert(!isnan(baseline_parameter));
249    intersection_parameters.push_back(min_parameter + (max_parameter - min_parameter) * baseline_parameter);
250    return true;
251  }
252  return false;
253}
254
255template <size_t Tdimension, typename TElement, unsigned int Tdegree>
256const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
257    const tLine<Tdimension, TElement> &line,
258    tParameter min_parameter, tParameter max_parameter) const
259{
260  // check if intersection with line is possible
261  typename tShape::tPoint center_of_bounding_box(math::tVector<Tdimension, TElement>(0.5 *(this->BoundingBox().Min() + this->BoundingBox().Max())));
262  double radius_of_bounding_sphere = 0.5 * (this->BoundingBox().Max() - this->BoundingBox().Min()).Length();
263  if (line.GetDistanceToPoint(center_of_bounding_box) > radius_of_bounding_sphere)
264  {
265    return false;
266  }
267
268  // subdivide curve if necessary
269  if (!math::IsEqual(this->GetTwist(), 0))
270  {
271    tSubdivision subdivision(this->GetSubdivision());
272    tParameter middle_parameter(0.5 *(min_parameter + max_parameter));
273    bool result = false;
274    result |= subdivision.first.GetIntersections(intersection_points, intersection_parameters, line, min_parameter, middle_parameter);
275    result |= subdivision.second.GetIntersections(intersection_points, intersection_parameters, line, middle_parameter, max_parameter);
276    return result;
277  }
278
279  // compute the intersection using the baseline of the control polygon
280  tLineSegment<Tdimension, TElement> own_line_segment(this->control_points[0], this->control_points[Tdegree]);
281  typename tShape::tPoint intersection_point;
282  if (own_line_segment.GetIntersection(intersection_point, line))
283  {
284    intersection_points.push_back(intersection_point);
285    const tParameter baseline_parameter((intersection_points.back() - this->control_points[0]).Length() / (this->control_points[Tdegree] - this->control_points[0]).Length());
286    assert(!isnan(baseline_parameter));
287    intersection_parameters.push_back(min_parameter + (max_parameter - min_parameter) * baseline_parameter);
288    return true;
289  }
290  return false;
291}
292
293//----------------------------------------------------------------------
294// tBezierCurve GetClosestPoint
295//----------------------------------------------------------------------
296template <size_t Tdimension, typename TElement, unsigned int Tdegree>
297const typename tShape<Tdimension, TElement>::tPoint tBezierCurve<Tdimension, TElement, Tdegree>::GetClosestPoint(const typename tShape::tPoint &reference_point) const
298{
299  tSubdivision subdivision(this->GetSubdivision());
300
301  tLineSegment<Tdimension, TElement> first_segment_base_line(subdivision.first.GetControlPoint(0), subdivision.first.GetControlPoint(Tdegree));
302  tLineSegment<Tdimension, TElement> second_segment_base_line(subdivision.second.GetControlPoint(0), subdivision.second.GetControlPoint(Tdegree));
303
304  typename tShape::tPoint first_candidate = first_segment_base_line.GetClosestPoint(reference_point);
305  typename tShape::tPoint second_candidate = second_segment_base_line.GetClosestPoint(reference_point);
306
307  double squared_distance_to_first_candidate = (reference_point - first_candidate).SquaredLength();
308  double squared_distance_to_second_candidate = (reference_point - second_candidate).SquaredLength();
309
310  if (squared_distance_to_first_candidate < squared_distance_to_second_candidate)
311  {
312    if (subdivision.first.GetTwist() < 1E-6)
313    {
314      return first_candidate;
315    }
316    return subdivision.first.GetClosestPoint(reference_point);
317  }
318
319  if (subdivision.second.GetTwist() < 1E-6)
320  {
321    return second_candidate;
322  }
323  return subdivision.second.GetClosestPoint(reference_point);
324}
325
326//----------------------------------------------------------------------
327// tBezierCurve Translate
328//----------------------------------------------------------------------
329template <size_t Tdimension, typename TElement, unsigned int Tdegree>
330tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Translate(const math::tVector<Tdimension, TElement> &translation)
331{
332  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
333  {
334    this->control_points[i] += translation;
335  }
336  this->SetChanged();
337  return *this;
338}
339
340//----------------------------------------------------------------------
341// tBezierCurve Rotate
342//----------------------------------------------------------------------
343template <size_t Tdimension, typename TElement, unsigned int Tdegree>
344tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation)
345{
346  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
347  {
348    this->control_points[i] = rotation * this->control_points[i];
349  }
350  this->SetChanged();
351  return *this;
352}
353
354//----------------------------------------------------------------------
355// tBezierCurve Transform
356//----------------------------------------------------------------------
357template <size_t Tdimension, typename TElement, unsigned int Tdegree>
358tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation)
359{
360#ifndef NDEBUG
361  for (size_t i = 0; i < Tdimension; ++i)
362  {
363    assert(math::IsEqual(transformation[Tdimension][i], 0));
364  }
365  assert(math::IsEqual(transformation[Tdimension][Tdimension], 1));
366  math::tMatrix<Tdimension, Tdimension, TElement> rotation;
367  for (size_t row = 0; row < Tdimension; ++row)
368  {
369    for (size_t column = 0; column < Tdimension; ++column)
370    {
371      rotation[row][column] = transformation[row][column];
372    }
373  }
374  assert(math::IsEqual(rotation.Determinant(), 0));
375#endif
376
377  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
378  {
379    this->control_points[i] = transformation.MultiplyHomogeneously(this->control_points[i]);
380  }
381  this->SetChanged();
382  return *this;
383}
384
385//----------------------------------------------------------------------
386// tBezierCurve UpdateBoundingBox
387//----------------------------------------------------------------------
388template <size_t Tdimension, typename TElement, unsigned int Tdegree>
389void tBezierCurve<Tdimension, TElement, Tdegree>::UpdateBoundingBox(typename tShape::tBoundingBox &bounding_box) const
390{
391  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
392  {
393    bounding_box.Add(this->control_points[i]);
394  }
395}
396
397//----------------------------------------------------------------------
398// tBezierCurve UpdateCenterOfGravity
399//----------------------------------------------------------------------
400template <size_t Tdimension, typename TElement, unsigned int Tdegree>
401void tBezierCurve<Tdimension, TElement, Tdegree>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const
402{
403  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
404  {
405    center_of_gravity += this->control_points[i];
406  }
407  center_of_gravity /= this->NumberOfControlPoints();
408}
409
410
411
412//----------------------------------------------------------------------
413// End of namespace declaration
414//----------------------------------------------------------------------
415}
416}
Note: See TracBrowser for help on using the repository browser.