source: rrlib_geometry/curves/tBezierCurve.hpp

tip
Last change on this file was 84:ab3beb283aa3, checked in by Tobias Föhst <tobias.foehst@…>, 3 years ago

Fixes detection of infinite intersection points for line segments and general intersection of degraded lines

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