source: rrlib_geometry/curves/tBezierCurve.hpp @ 78:fab6cb47578c

14.08
Last change on this file since 78:fab6cb47578c was 78:fab6cb47578c, checked in by Tobias Föhst <tobias.foehst@…>, 3 years ago

Bugfix: limiting calculated intersection parameters to valid range in case of numeric issues

File size: 20.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    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#include "rrlib/util/variadic_templates.h"
35
36#include <iomanip>
37//----------------------------------------------------------------------
38// Internal includes with ""
39//----------------------------------------------------------------------
40
41//----------------------------------------------------------------------
42// Debugging
43//----------------------------------------------------------------------
44#include <cassert>
45
46//----------------------------------------------------------------------
47// Namespace declaration
48//----------------------------------------------------------------------
49namespace rrlib
50{
51namespace geometry
52{
53
54//----------------------------------------------------------------------
55// Forward declarations / typedefs / enums
56//----------------------------------------------------------------------
57
58//----------------------------------------------------------------------
59// Const values
60//----------------------------------------------------------------------
61
62//----------------------------------------------------------------------
63// Implementation
64//----------------------------------------------------------------------
65
66//----------------------------------------------------------------------
67// tBezierCurve constructors
68//----------------------------------------------------------------------
69template <size_t Tdimension, typename TElement, unsigned int Tdegree>
70tBezierCurve<Tdimension, TElement, Tdegree>::tBezierCurve()
71{
72  static_assert(Tdegree > 0, "The degree of Bezier curves must be greater than zero");
73  memset(this->control_points, 0, sizeof(this->control_points));
74}
75
76template <size_t Tdimension, typename TElement, unsigned int Tdegree>
77template <typename TIterator>
78tBezierCurve<Tdimension, TElement, Tdegree>::tBezierCurve(TIterator begin, TIterator end)
79{
80  static_assert(Tdegree > 0, "The degree of Bezier curves must be greater than zero");
81  assert(static_cast<size_t>(std::distance(begin, end)) == this->NumberOfControlPoints() && "A Bezier curve must have degree + 1 control points");
82  size_t index = 0;
83  for (TIterator it = begin; it != end; ++it)
84  {
85    this->control_points[index++] = *it;
86  }
87}
88
89template <size_t Tdimension, typename TElement, unsigned int Tdegree>
90template <typename ... TPoints>
91tBezierCurve<Tdimension, TElement, Tdegree>::tBezierCurve(const typename tShape::tPoint &p1, const typename tShape::tPoint &p2, const TPoints &... rest)
92{
93  static_assert(Tdegree > 0, "The degree of Bezier curves must be greater than zero");
94  static_assert(sizeof...(rest) + 1 == Tdegree, "A Bezier curve must have degree + 1 control points");
95  size_t index = 0;
96  util::ProcessVariadicValues([this, &index](const typename tShape::tPoint & x)
97  {
98    this->control_points[index++] = x;
99  },
100  p1, p2, rest...);
101}
102
103//----------------------------------------------------------------------
104// tBezierCurve SetControlPoint
105//----------------------------------------------------------------------
106template <size_t Tdimension, typename TElement, unsigned int Tdegree>
107void tBezierCurve<Tdimension, TElement, Tdegree>::SetControlPoint(size_t i, const typename tShape::tPoint &point)
108{
109  assert(i < this->NumberOfControlPoints());
110  this->control_points[i] = point;
111  this->SetChanged();
112};
113
114//----------------------------------------------------------------------
115// tBezierCurve GetTwist
116//----------------------------------------------------------------------
117template <size_t Tdimension, typename TElement, unsigned int Tdegree>
118const TElement tBezierCurve<Tdimension, TElement, Tdegree>::GetTwist() const
119{
120  TElement twist = 0.0;
121  for (size_t i = 0; i < Tdegree - 1; ++i)
122  {
123    twist = std::max(twist, ((this->control_points[i + 2] - this->control_points[i + 1]) - (this->control_points[i + 1] - this->control_points[i])).Length());
124  }
125  return Tdegree * (Tdegree - 1) * twist;
126}
127
128//----------------------------------------------------------------------
129// tBezierCurve GetSubdivision
130//----------------------------------------------------------------------
131template <size_t Tdimension, typename TElement, unsigned int Tdegree>
132const typename tBezierCurve<Tdimension, TElement, Tdegree>::tSubdivision tBezierCurve<Tdimension, TElement, Tdegree>::GetSubdivision() const
133{
134  const size_t cNUMBER_OF_CONTROL_POINTS = Tdegree + 1;
135  typename tShape::tPoint left_half[cNUMBER_OF_CONTROL_POINTS];
136  typename tShape::tPoint right_half[cNUMBER_OF_CONTROL_POINTS];
137  typename tShape::tPoint temp_points[cNUMBER_OF_CONTROL_POINTS];
138  std::memcpy(temp_points, this->control_points, cNUMBER_OF_CONTROL_POINTS * sizeof(typename tShape::tPoint));
139
140  left_half[0] = temp_points[0];
141  right_half[Tdegree] = temp_points[Tdegree];
142
143  size_t k = 0;
144  while (k < Tdegree)
145  {
146    for (size_t i = 0; i < Tdegree - k; ++i)
147    {
148      temp_points[i] = (temp_points[i] + temp_points[i + 1]) * 0.5;
149    }
150    ++k;
151    left_half[k] = temp_points[0];
152    right_half[Tdegree - k] = temp_points[Tdegree - k];
153  }
154
155  return std::make_pair(tBezierCurve(left_half, left_half + cNUMBER_OF_CONTROL_POINTS), tBezierCurve(right_half, right_half + cNUMBER_OF_CONTROL_POINTS));
156}
157
158//----------------------------------------------------------------------
159// tBezierCurve Evaluation: operator ()
160//----------------------------------------------------------------------
161template <size_t Tdimension, typename TElement, unsigned int Tdegree>
162const typename tShape<Tdimension, TElement>::tPoint tBezierCurve<Tdimension, TElement, Tdegree>::operator()(tParameter t) const
163{
164  assert((0.0 <= t) && (t <= 1.0));
165
166  const size_t cNUMBER_OF_CONTROL_POINTS = Tdegree + 1;
167  typename tShape::tPoint temp_points[cNUMBER_OF_CONTROL_POINTS];
168  std::memcpy(temp_points, this->control_points, cNUMBER_OF_CONTROL_POINTS * sizeof(typename tShape::tPoint));
169
170  size_t k = 0;
171  while (k < Tdegree)
172  {
173    for (size_t i = 0; i < Tdegree - k; ++i)
174    {
175      temp_points[i] = ((1.0 - t) * temp_points[i]) + (t * temp_points[i + 1]);
176    }
177    ++k;
178  }
179
180  return temp_points[0];
181}
182
183//----------------------------------------------------------------------
184// tBezierCurve GetDerivative
185//----------------------------------------------------------------------
186template <size_t Tdimension, typename TElement, unsigned int Tdegree>
187const typename tBezierCurve<Tdimension, TElement, Tdegree>::tDerivative tBezierCurve<Tdimension, TElement, Tdegree>::GetDerivative() const
188{
189  typename tShape::tPoint temp[Tdegree];
190  for (size_t i = 0; i < Tdegree; ++i)
191  {
192    temp[i] = Tdegree * (this->control_points[i + 1] - this->control_points[i]);
193  }
194  return tDerivative(temp, temp + Tdegree);
195}
196
197//----------------------------------------------------------------------
198// tBezierCurve GetIntersections
199//----------------------------------------------------------------------
200template <size_t Tdimension, typename TElement, unsigned int Tdegree>
201template <unsigned int Tother_degree>
202const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
203    const tBezierCurve<Tdimension, TElement, Tother_degree> &other) const
204{
205  return this->GetIntersections(intersection_points, intersection_parameters, other, 0.0, 1.0);
206}
207
208template <size_t Tdimension, typename TElement, unsigned int Tdegree>
209const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
210    const tLine<Tdimension, TElement> &line) const
211{
212  return this->GetIntersections(intersection_points, intersection_parameters, line, 0.0, 1.0);
213}
214
215template <size_t Tdimension, typename TElement, unsigned int Tdegree>
216template <unsigned int Tother_degree>
217const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
218    const tBezierCurve<Tdimension, TElement, Tother_degree> &other,
219    tParameter min_parameter, tParameter max_parameter) const
220{
221  // check if bounding boxes intersect
222  if (!this->BoundingBox().Intersects(other.BoundingBox()))
223  {
224    return false;
225  }
226
227  // subdivide curve with greater twist if necessary
228  TElement own_twist = this->GetTwist();
229  TElement other_twist = other.GetTwist();
230
231  if (own_twist > other_twist && !math::IsEqual(own_twist, 0))
232  {
233    tSubdivision subdivision(this->GetSubdivision());
234    tParameter middle_parameter(0.5 * (min_parameter + max_parameter));
235    bool result = false;
236    result |= subdivision.first.GetIntersections(intersection_points, intersection_parameters, other, min_parameter, middle_parameter);
237    result |= subdivision.second.GetIntersections(intersection_points, intersection_parameters, other, middle_parameter, max_parameter);
238    return result;
239  }
240  else if (!math::IsEqual(other_twist, 0))
241  {
242    tSubdivision subdivision(other.GetSubdivision());
243    bool result = false;
244    result |= this->GetIntersections(intersection_points, intersection_parameters, subdivision.first, min_parameter, max_parameter);
245    result |= this->GetIntersections(intersection_points, intersection_parameters, subdivision.second, min_parameter, max_parameter);
246    return result;
247  }
248
249  // compute the intersection using the baselines of the two control polygons
250  tLine<Tdimension, TElement> own_line_segment(this->control_points[0], this->control_points[Tdegree]);
251  tLine<Tdimension, TElement> other_line_segment(other.ControlPoints()[0], other.ControlPoints()[Tother_degree]);
252  typename tShape::tPoint intersection_point;
253  if (own_line_segment.GetIntersection(intersection_point, other_line_segment) && !math::IsEqual(intersection_point, intersection_points.back(), 0.001))
254  {
255    intersection_points.push_back(intersection_point);
256    const auto baseline_parameter = math::LimitedValue<tParameter>((intersection_points.back() - this->control_points[0]).Length() / (this->control_points[Tdegree] - this->control_points[0]).Length(), 0, 1);
257    assert(!std::isnan(baseline_parameter));
258    intersection_parameters.push_back(min_parameter + (max_parameter - min_parameter) * baseline_parameter);
259    return true;
260  }
261  return false;
262}
263
264template <size_t Tdimension, typename TElement, unsigned int Tdegree>
265const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
266    const tLine<Tdimension, TElement> &line,
267    tParameter min_parameter, tParameter max_parameter) const
268{
269  // check if intersection with line is possible
270  typename tShape::tPoint center_of_bounding_box(math::tVector<Tdimension, TElement>(0.5 * (this->BoundingBox().Min() + this->BoundingBox().Max())));
271  double radius_of_bounding_sphere = 0.5 * (this->BoundingBox().Max() - this->BoundingBox().Min()).Length();
272  if (line.GetDistanceToPoint(center_of_bounding_box) > radius_of_bounding_sphere)
273  {
274    return false;
275  }
276
277  // subdivide curve if necessary
278  if (!math::IsEqual(this->GetTwist(), 0))
279  {
280    tSubdivision subdivision(this->GetSubdivision());
281    tParameter middle_parameter(0.5 * (min_parameter + max_parameter));
282    bool result = false;
283    result |= subdivision.first.GetIntersections(intersection_points, intersection_parameters, line, min_parameter, middle_parameter);
284    result |= subdivision.second.GetIntersections(intersection_points, intersection_parameters, line, middle_parameter, max_parameter);
285    return result;
286  }
287
288  // compute the intersection using the baseline of the control polygon
289  tLineSegment<Tdimension, TElement> own_line_segment(this->control_points[0], this->control_points[Tdegree]);
290  typename tShape::tPoint intersection_point;
291  if (own_line_segment.GetIntersection(intersection_point, line))
292  {
293    intersection_points.push_back(intersection_point);
294    const auto baseline_parameter = math::LimitedValue<tParameter>((intersection_points.back() - this->control_points[0]).Length() / (this->control_points[Tdegree] - this->control_points[0]).Length(), 0, 1);
295    assert(!std::isnan(baseline_parameter));
296    intersection_parameters.push_back(min_parameter + (max_parameter - min_parameter) * baseline_parameter);
297    return true;
298  }
299  return false;
300}
301
302//----------------------------------------------------------------------
303// tBezierCurve GetClosestPoint
304//----------------------------------------------------------------------
305template <size_t Tdimension, typename TElement, unsigned int Tdegree>
306const typename tShape<Tdimension, TElement>::tPoint tBezierCurve<Tdimension, TElement, Tdegree>::GetClosestPoint(const typename tShape::tPoint &reference_point) const
307{
308  tSubdivision subdivision(this->GetSubdivision());
309
310  double squared_distance_to_first_candidate = std::numeric_limits<double>::max();
311  double squared_distance_to_second_candidate = std::numeric_limits<double>::max();
312
313  for (unsigned int i = 0; i < Tdegree; ++i)
314  {
315    tLineSegment<Tdimension, TElement> first_segment_base_line(subdivision.first.ControlPoints()[i], subdivision.first.ControlPoints()[i + 1]);
316    tLineSegment<Tdimension, TElement> second_segment_base_line(subdivision.second.ControlPoints()[i], subdivision.second.ControlPoints()[i + 1]);
317
318    typename tShape::tPoint first_candidate = first_segment_base_line.GetClosestPoint(reference_point);
319    typename tShape::tPoint second_candidate = second_segment_base_line.GetClosestPoint(reference_point);
320
321    double squared_distance_to_first = (reference_point - first_candidate).SquaredLength();
322    double squared_distance_to_second = (reference_point - second_candidate).SquaredLength();
323
324    if (squared_distance_to_first < squared_distance_to_first_candidate)
325    {
326      squared_distance_to_first_candidate = squared_distance_to_first;
327    }
328
329    if (squared_distance_to_second < squared_distance_to_second_candidate)
330    {
331      squared_distance_to_second_candidate = squared_distance_to_second;
332    }
333  }
334
335  if (squared_distance_to_first_candidate < squared_distance_to_second_candidate)
336  {
337    if (subdivision.first.GetTwist() < 1E-6)
338    {
339      tLineSegment<Tdimension, TElement> first_segment_base_line(subdivision.first.ControlPoints()[0], subdivision.first.ControlPoints()[Tdegree]);
340      return first_segment_base_line.GetClosestPoint(reference_point);
341    }
342    return subdivision.first.GetClosestPoint(reference_point);
343  }
344
345  if (subdivision.second.GetTwist() < 1E-6)
346  {
347    tLineSegment<Tdimension, TElement> second_segment_base_line(subdivision.second.ControlPoints()[0], subdivision.second.ControlPoints()[Tdegree]);
348    return second_segment_base_line.GetClosestPoint(reference_point);
349  }
350  return subdivision.second.GetClosestPoint(reference_point);
351}
352
353//----------------------------------------------------------------------
354// tBezierCurve Translate
355//----------------------------------------------------------------------
356template <size_t Tdimension, typename TElement, unsigned int Tdegree>
357tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Translate(const math::tVector<Tdimension, TElement> &translation)
358{
359  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
360  {
361    this->control_points[i] += translation;
362  }
363  this->SetChanged();
364  return *this;
365}
366
367//----------------------------------------------------------------------
368// tBezierCurve Rotate
369//----------------------------------------------------------------------
370template <size_t Tdimension, typename TElement, unsigned int Tdegree>
371tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation)
372{
373  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
374  {
375    this->control_points[i] = rotation * this->control_points[i];
376  }
377  this->SetChanged();
378  return *this;
379}
380
381//----------------------------------------------------------------------
382// tBezierCurve Transform
383//----------------------------------------------------------------------
384template <size_t Tdimension, typename TElement, unsigned int Tdegree>
385tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation)
386{
387#ifndef NDEBUG
388  for (size_t i = 0; i < Tdimension; ++i)
389  {
390    assert(math::IsEqual(transformation[Tdimension][i], 0));
391  }
392  assert(math::IsEqual(transformation[Tdimension][Tdimension], 1));
393  math::tMatrix<Tdimension, Tdimension, TElement> rotation;
394  for (size_t row = 0; row < Tdimension; ++row)
395  {
396    for (size_t column = 0; column < Tdimension; ++column)
397    {
398      rotation[row][column] = transformation[row][column];
399    }
400  }
401  assert(math::IsEqual(rotation.Determinant(), 1));
402#endif
403
404  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
405  {
406    this->control_points[i] = transformation.MultiplyHomogeneously(this->control_points[i]);
407  }
408  this->SetChanged();
409  return *this;
410}
411
412//----------------------------------------------------------------------
413// tBezierCurve UpdateBoundingBox
414//----------------------------------------------------------------------
415template <size_t Tdimension, typename TElement, unsigned int Tdegree>
416void tBezierCurve<Tdimension, TElement, Tdegree>::UpdateBoundingBox(typename tShape::tBoundingBox &bounding_box) const
417{
418  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
419  {
420    bounding_box.Add(this->control_points[i]);
421  }
422}
423
424//----------------------------------------------------------------------
425// tBezierCurve UpdateCenterOfGravity
426//----------------------------------------------------------------------
427template <size_t Tdimension, typename TElement, unsigned int Tdegree>
428void tBezierCurve<Tdimension, TElement, Tdegree>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const
429{
430  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
431  {
432    center_of_gravity += this->control_points[i];
433  }
434  center_of_gravity /= this->NumberOfControlPoints();
435}
436
437//----------------------------------------------------------------------
438// Operators for rrlib_canvas
439//----------------------------------------------------------------------
440#ifdef _LIB_RRLIB_CANVAS_PRESENT_
441
442template <typename TElement, unsigned int Tdegree>
443canvas::tCanvas2D &operator << (canvas::tCanvas2D &canvas, const tBezierCurve<2, TElement, Tdegree> &bezier_curve)
444{
445  canvas.DrawBezierCurve(bezier_curve.ControlPoints(), bezier_curve.ControlPoints() + bezier_curve.NumberOfControlPoints());
446
447  return canvas;
448}
449
450#endif
451
452//----------------------------------------------------------------------
453// Operators for rrlib_serialization
454//----------------------------------------------------------------------
455#ifdef _LIB_RRLIB_SERIALIZATION_PRESENT_
456
457template <size_t Tdimension, typename TElement, unsigned int Tdegree>
458serialization::tOutputStream &operator << (serialization::tOutputStream &stream, const tBezierCurve<Tdimension, TElement, Tdegree> &curve)
459{
460  for (size_t i = 0; i < curve.NumberOfControlPoints(); ++i)
461  {
462    stream << curve.ControlPoints()[i];
463  }
464  return stream;
465}
466
467template <size_t Tdimension, typename TElement, unsigned int Tdegree>
468serialization::tInputStream &operator >> (serialization::tInputStream &stream, tBezierCurve<Tdimension, TElement, Tdegree> &curve)
469{
470  for (size_t i = 0; i < curve.NumberOfControlPoints(); ++i)
471  {
472    typename tBezierCurve<Tdimension, TElement, Tdegree>::tPoint control_point;
473    stream >> control_point;
474    curve.SetControlPoint(i, control_point);
475  }
476  return stream;
477}
478
479#endif
480
481//----------------------------------------------------------------------
482// End of namespace declaration
483//----------------------------------------------------------------------
484}
485}
Note: See TracBrowser for help on using the repository browser.