source: rrlib_geometry/curves/tBezierCurve.hpp @ 59:de0986db717a

Last change on this file since 59:de0986db717a was 59:de0986db717a, checked in by Tobias Föhst <foehst@…>, 6 years ago

Reformatted with astyle 2.03

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