source: rrlib_geometry/curves/tBezierCurve.hpp @ 45:2a407d1589d4

Last change on this file since 45:2a407d1589d4 was 45:2a407d1589d4, checked in by Tobias Föhst <foehst@…>, 8 years ago

Merged with changes from rrlib.org

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