source: rrlib_geometry/curves/tBezierCurve.hpp @ 24:627bf1924b86

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

Added streaming operators for rrlib_canvas

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