source: rrlib_geometry/curves/tBezierCurve.hpp @ 76:e6f537899176

14.08
Last change on this file since 76:e6f537899176 was 76:e6f537899176, checked in by Max Reichardt <max.reichardt@…>, 3 years ago

Fixes compile error on e.g. Ubuntu 16.04 platforms

File size: 20.5 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 tParameter baseline_parameter((intersection_points.back() - this->control_points[0]).Length() / (this->control_points[Tdegree] - this->control_points[0]).Length());
256    assert(!std::isnan(baseline_parameter));
257    intersection_parameters.push_back(min_parameter + (max_parameter - min_parameter) * baseline_parameter);
258    return true;
259  }
260  return false;
261}
262
263template <size_t Tdimension, typename TElement, unsigned int Tdegree>
264const bool tBezierCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
265    const tLine<Tdimension, TElement> &line,
266    tParameter min_parameter, tParameter max_parameter) const
267{
268  // check if intersection with line is possible
269  typename tShape::tPoint center_of_bounding_box(math::tVector<Tdimension, TElement>(0.5 * (this->BoundingBox().Min() + this->BoundingBox().Max())));
270  double radius_of_bounding_sphere = 0.5 * (this->BoundingBox().Max() - this->BoundingBox().Min()).Length();
271  if (line.GetDistanceToPoint(center_of_bounding_box) > radius_of_bounding_sphere)
272  {
273    return false;
274  }
275
276  // subdivide curve if necessary
277  if (!math::IsEqual(this->GetTwist(), 0))
278  {
279    tSubdivision subdivision(this->GetSubdivision());
280    tParameter middle_parameter(0.5 * (min_parameter + max_parameter));
281    bool result = false;
282    result |= subdivision.first.GetIntersections(intersection_points, intersection_parameters, line, min_parameter, middle_parameter);
283    result |= subdivision.second.GetIntersections(intersection_points, intersection_parameters, line, middle_parameter, max_parameter);
284    return result;
285  }
286
287  // compute the intersection using the baseline of the control polygon
288  tLineSegment<Tdimension, TElement> own_line_segment(this->control_points[0], this->control_points[Tdegree]);
289  typename tShape::tPoint intersection_point;
290  if (own_line_segment.GetIntersection(intersection_point, line))
291  {
292    intersection_points.push_back(intersection_point);
293    const tParameter baseline_parameter((intersection_points.back() - this->control_points[0]).Length() / (this->control_points[Tdegree] - this->control_points[0]).Length());
294    assert(!std::isnan(baseline_parameter));
295    intersection_parameters.push_back(min_parameter + (max_parameter - min_parameter) * baseline_parameter);
296    return true;
297  }
298  return false;
299}
300
301//----------------------------------------------------------------------
302// tBezierCurve GetClosestPoint
303//----------------------------------------------------------------------
304template <size_t Tdimension, typename TElement, unsigned int Tdegree>
305const typename tShape<Tdimension, TElement>::tPoint tBezierCurve<Tdimension, TElement, Tdegree>::GetClosestPoint(const typename tShape::tPoint &reference_point) const
306{
307  tSubdivision subdivision(this->GetSubdivision());
308
309  double squared_distance_to_first_candidate = std::numeric_limits<double>::max();
310  double squared_distance_to_second_candidate = std::numeric_limits<double>::max();
311
312  for (unsigned int i = 0; i < Tdegree; ++i)
313  {
314    tLineSegment<Tdimension, TElement> first_segment_base_line(subdivision.first.ControlPoints()[i], subdivision.first.ControlPoints()[i + 1]);
315    tLineSegment<Tdimension, TElement> second_segment_base_line(subdivision.second.ControlPoints()[i], subdivision.second.ControlPoints()[i + 1]);
316
317    typename tShape::tPoint first_candidate = first_segment_base_line.GetClosestPoint(reference_point);
318    typename tShape::tPoint second_candidate = second_segment_base_line.GetClosestPoint(reference_point);
319
320    double squared_distance_to_first = (reference_point - first_candidate).SquaredLength();
321    double squared_distance_to_second = (reference_point - second_candidate).SquaredLength();
322
323    if (squared_distance_to_first < squared_distance_to_first_candidate)
324    {
325      squared_distance_to_first_candidate = squared_distance_to_first;
326    }
327
328    if (squared_distance_to_second < squared_distance_to_second_candidate)
329    {
330      squared_distance_to_second_candidate = squared_distance_to_second;
331    }
332  }
333
334  if (squared_distance_to_first_candidate < squared_distance_to_second_candidate)
335  {
336    if (subdivision.first.GetTwist() < 1E-6)
337    {
338      tLineSegment<Tdimension, TElement> first_segment_base_line(subdivision.first.ControlPoints()[0], subdivision.first.ControlPoints()[Tdegree]);
339      return first_segment_base_line.GetClosestPoint(reference_point);
340    }
341    return subdivision.first.GetClosestPoint(reference_point);
342  }
343
344  if (subdivision.second.GetTwist() < 1E-6)
345  {
346    tLineSegment<Tdimension, TElement> second_segment_base_line(subdivision.second.ControlPoints()[0], subdivision.second.ControlPoints()[Tdegree]);
347    return second_segment_base_line.GetClosestPoint(reference_point);
348  }
349  return subdivision.second.GetClosestPoint(reference_point);
350}
351
352//----------------------------------------------------------------------
353// tBezierCurve Translate
354//----------------------------------------------------------------------
355template <size_t Tdimension, typename TElement, unsigned int Tdegree>
356tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Translate(const math::tVector<Tdimension, TElement> &translation)
357{
358  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
359  {
360    this->control_points[i] += translation;
361  }
362  this->SetChanged();
363  return *this;
364}
365
366//----------------------------------------------------------------------
367// tBezierCurve Rotate
368//----------------------------------------------------------------------
369template <size_t Tdimension, typename TElement, unsigned int Tdegree>
370tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation)
371{
372  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
373  {
374    this->control_points[i] = rotation * this->control_points[i];
375  }
376  this->SetChanged();
377  return *this;
378}
379
380//----------------------------------------------------------------------
381// tBezierCurve Transform
382//----------------------------------------------------------------------
383template <size_t Tdimension, typename TElement, unsigned int Tdegree>
384tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation)
385{
386#ifndef NDEBUG
387  for (size_t i = 0; i < Tdimension; ++i)
388  {
389    assert(math::IsEqual(transformation[Tdimension][i], 0));
390  }
391  assert(math::IsEqual(transformation[Tdimension][Tdimension], 1));
392  math::tMatrix<Tdimension, Tdimension, TElement> rotation;
393  for (size_t row = 0; row < Tdimension; ++row)
394  {
395    for (size_t column = 0; column < Tdimension; ++column)
396    {
397      rotation[row][column] = transformation[row][column];
398    }
399  }
400  assert(math::IsEqual(rotation.Determinant(), 1));
401#endif
402
403  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
404  {
405    this->control_points[i] = transformation.MultiplyHomogeneously(this->control_points[i]);
406  }
407  this->SetChanged();
408  return *this;
409}
410
411//----------------------------------------------------------------------
412// tBezierCurve UpdateBoundingBox
413//----------------------------------------------------------------------
414template <size_t Tdimension, typename TElement, unsigned int Tdegree>
415void tBezierCurve<Tdimension, TElement, Tdegree>::UpdateBoundingBox(typename tShape::tBoundingBox &bounding_box) const
416{
417  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
418  {
419    bounding_box.Add(this->control_points[i]);
420  }
421}
422
423//----------------------------------------------------------------------
424// tBezierCurve UpdateCenterOfGravity
425//----------------------------------------------------------------------
426template <size_t Tdimension, typename TElement, unsigned int Tdegree>
427void tBezierCurve<Tdimension, TElement, Tdegree>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const
428{
429  for (size_t i = 0; i < this->NumberOfControlPoints(); ++i)
430  {
431    center_of_gravity += this->control_points[i];
432  }
433  center_of_gravity /= this->NumberOfControlPoints();
434}
435
436//----------------------------------------------------------------------
437// Operators for rrlib_canvas
438//----------------------------------------------------------------------
439#ifdef _LIB_RRLIB_CANVAS_PRESENT_
440
441template <typename TElement, unsigned int Tdegree>
442canvas::tCanvas2D &operator << (canvas::tCanvas2D &canvas, const tBezierCurve<2, TElement, Tdegree> &bezier_curve)
443{
444  canvas.DrawBezierCurve(bezier_curve.ControlPoints(), bezier_curve.ControlPoints() + bezier_curve.NumberOfControlPoints());
445
446  return canvas;
447}
448
449#endif
450
451//----------------------------------------------------------------------
452// Operators for rrlib_serialization
453//----------------------------------------------------------------------
454#ifdef _LIB_RRLIB_SERIALIZATION_PRESENT_
455
456template <size_t Tdimension, typename TElement, unsigned int Tdegree>
457serialization::tOutputStream &operator << (serialization::tOutputStream &stream, const tBezierCurve<Tdimension, TElement, Tdegree> &curve)
458{
459  for (size_t i = 0; i < curve.NumberOfControlPoints(); ++i)
460  {
461    stream << curve.ControlPoints()[i];
462  }
463  return stream;
464}
465
466template <size_t Tdimension, typename TElement, unsigned int Tdegree>
467serialization::tInputStream &operator >> (serialization::tInputStream &stream, tBezierCurve<Tdimension, TElement, Tdegree> &curve)
468{
469  for (size_t i = 0; i < curve.NumberOfControlPoints(); ++i)
470  {
471    typename tBezierCurve<Tdimension, TElement, Tdegree>::tPoint control_point;
472    stream >> control_point;
473    curve.SetControlPoint(i, control_point);
474  }
475  return stream;
476}
477
478#endif
479
480//----------------------------------------------------------------------
481// End of namespace declaration
482//----------------------------------------------------------------------
483}
484}
Note: See TracBrowser for help on using the repository browser.