source: rrlib_geometry/curves/tBezierCurve.hpp @ 17:3daa58ced492

Last change on this file since 17:3daa58ced492 was 17:3daa58ced492, checked in by Tobias Föhst <foehst@…>, 9 years ago

Merged with changes from rrlab

File size: 17.8 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
35//----------------------------------------------------------------------
36// Internal includes with ""
37//----------------------------------------------------------------------
38
39//----------------------------------------------------------------------
40// Debugging
41//----------------------------------------------------------------------
42#include <cassert>
43
44//----------------------------------------------------------------------
45// Namespace declaration
46//----------------------------------------------------------------------
47namespace rrlib
48{
49namespace geometry
50{
51
52//----------------------------------------------------------------------
53// Forward declarations / typedefs / enums
54//----------------------------------------------------------------------
55
56//----------------------------------------------------------------------
57// Const values
58//----------------------------------------------------------------------
59template <size_t Tdimension, typename TElement, unsigned int Tdegree>
60const unsigned int tBezierCurve<Tdimension, TElement, Tdegree>::cDEGREE = Tdegree;
61
62template <size_t Tdimension, typename TElement, unsigned int Tdegree>
63const unsigned int tBezierCurve<Tdimension, TElement, Tdegree>::cNUMBER_OF_CONTROL_POINTS = Tdegree + 1;
64
65//----------------------------------------------------------------------
66// Implementation
67//----------------------------------------------------------------------
68
69//----------------------------------------------------------------------
70// tBezierCurve constructors
71//----------------------------------------------------------------------
72template <size_t Tdimension, typename TElement, unsigned int Tdegree>
73template <typename TIterator>
74tBezierCurve<Tdimension, TElement, Tdegree>::tBezierCurve(TIterator begin, TIterator end)
75    : tShape()
76{
77  static_assert(Tdegree > 0, "The degree of Bezier curves must be greater than zero");
78  assert(static_cast<size_t>(std::distance(begin, end)) == this->cNUMBER_OF_CONTROL_POINTS);
79  size_t index = 0;
80  for (TIterator it = begin; it != end; ++it)
81  {
82    this->control_points[index++] = *it;
83  }
84}
85
86template <size_t Tdimension, typename TElement, unsigned int Tdegree>
87template <typename TSTLContainer>
88tBezierCurve<Tdimension, TElement, Tdegree>::tBezierCurve(const TSTLContainer &control_points)
89{
90  static_assert(Tdegree > 0, "The degree of Bezier curves must be greater than zero");
91  assert(control_points.size() == this->cNUMBER_OF_CONTROL_POINTS);
92  for (size_t i = 0; i < control_points.size(); ++i)
93  {
94    this->control_points[i] = control_points[i];
95  }
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->cNUMBER_OF_CONTROL_POINTS);
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->cNUMBER_OF_CONTROL_POINTS];
130  typename tShape::tPoint right_half[this->cNUMBER_OF_CONTROL_POINTS];
131  typename tShape::tPoint temp_points[this->cNUMBER_OF_CONTROL_POINTS];
132  std::memcpy(temp_points, this->control_points, this->cNUMBER_OF_CONTROL_POINTS * 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->cNUMBER_OF_CONTROL_POINTS), tBezierCurve(right_half, right_half + this->cNUMBER_OF_CONTROL_POINTS));
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->cNUMBER_OF_CONTROL_POINTS];
161  std::memcpy(temp_points, this->control_points, (this->cNUMBER_OF_CONTROL_POINTS) * 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.GetControlPoint(0), other.GetControlPoint(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  tLineSegment<Tdimension, TElement> first_segment_base_line(subdivision.first.GetControlPoint(0), subdivision.first.GetControlPoint(Tdegree));
304  tLineSegment<Tdimension, TElement> second_segment_base_line(subdivision.second.GetControlPoint(0), subdivision.second.GetControlPoint(Tdegree));
305
306  typename tShape::tPoint first_candidate = first_segment_base_line.GetClosestPoint(reference_point);
307  typename tShape::tPoint second_candidate = second_segment_base_line.GetClosestPoint(reference_point);
308
309  double squared_distance_to_first_candidate = (reference_point - first_candidate).SquaredLength();
310  double squared_distance_to_second_candidate = (reference_point - second_candidate).SquaredLength();
311
312  if (squared_distance_to_first_candidate < squared_distance_to_second_candidate)
313  {
314    if (subdivision.first.GetTwist() < 1E-6)
315    {
316      return first_candidate;
317    }
318    return subdivision.first.GetClosestPoint(reference_point);
319  }
320
321  if (subdivision.second.GetTwist() < 1E-6)
322  {
323    return second_candidate;
324  }
325  return subdivision.second.GetClosestPoint(reference_point);
326}
327
328//----------------------------------------------------------------------
329// tBezierCurve Translate
330//----------------------------------------------------------------------
331template <size_t Tdimension, typename TElement, unsigned int Tdegree>
332tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Translate(const math::tVector<Tdimension, TElement> &translation)
333{
334  for (size_t i = 0; i < this->cNUMBER_OF_CONTROL_POINTS; ++i)
335  {
336    this->control_points[i] += translation;
337  }
338  this->SetChanged();
339  return *this;
340}
341
342//----------------------------------------------------------------------
343// tBezierCurve Rotate
344//----------------------------------------------------------------------
345template <size_t Tdimension, typename TElement, unsigned int Tdegree>
346tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation)
347{
348  for (size_t i = 0; i < this->cNUMBER_OF_CONTROL_POINTS; ++i)
349  {
350    this->control_points[i] = rotation * this->control_points[i];
351  }
352  this->SetChanged();
353  return *this;
354}
355
356//----------------------------------------------------------------------
357// tBezierCurve Transform
358//----------------------------------------------------------------------
359template <size_t Tdimension, typename TElement, unsigned int Tdegree>
360tBezierCurve<Tdimension, TElement, Tdegree> &tBezierCurve<Tdimension, TElement, Tdegree>::Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation)
361{
362#ifndef NDEBUG
363  for (size_t i = 0; i < Tdimension; ++i)
364  {
365    assert(math::IsEqual(transformation[Tdimension][i], 0));
366  }
367  assert(math::IsEqual(transformation[Tdimension][Tdimension], 1));
368  math::tMatrix<Tdimension, Tdimension, TElement> rotation;
369  for (size_t row = 0; row < Tdimension; ++row)
370  {
371    for (size_t column = 0; column < Tdimension; ++column)
372    {
373      rotation[row][column] = transformation[row][column];
374    }
375  }
376  assert(math::IsEqual(rotation.Determinant(), 0));
377#endif
378
379  for (size_t i = 0; i < this->cNUMBER_OF_CONTROL_POINTS; ++i)
380  {
381    this->control_points[i] = transformation.MultiplyHomogeneously(this->control_points[i]);
382  }
383  this->SetChanged();
384  return *this;
385}
386
387//----------------------------------------------------------------------
388// tBezierCurve UpdateBoundingBox
389//----------------------------------------------------------------------
390template <size_t Tdimension, typename TElement, unsigned int Tdegree>
391void tBezierCurve<Tdimension, TElement, Tdegree>::UpdateBoundingBox(typename tShape::tBoundingBox &bounding_box) const
392{
393  for (size_t i = 0; i < this->cNUMBER_OF_CONTROL_POINTS; ++i)
394  {
395    bounding_box.Add(this->control_points[i]);
396  }
397}
398
399//----------------------------------------------------------------------
400// tBezierCurve UpdateCenterOfGravity
401//----------------------------------------------------------------------
402template <size_t Tdimension, typename TElement, unsigned int Tdegree>
403void tBezierCurve<Tdimension, TElement, Tdegree>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const
404{
405  for (size_t i = 0; i < this->cNUMBER_OF_CONTROL_POINTS; ++i)
406  {
407    center_of_gravity += this->control_points[i];
408  }
409  center_of_gravity /= this->cNUMBER_OF_CONTROL_POINTS;
410}
411
412
413
414//----------------------------------------------------------------------
415// End of namespace declaration
416//----------------------------------------------------------------------
417}
418}
Note: See TracBrowser for help on using the repository browser.