source: rrlib_geometry/curves/tSplineCurve.hpp @ 49:d6bed8c3902e

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

Made method to calculate the number of segments virtual in tSplineCurve and fixed the calculation for tBSplineCurve

File size: 18.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    tSplineCurve.hpp
23 *
24 * \author  Tobias Foehst
25 *
26 * \date    2010-09-01
27 *
28 */
29//----------------------------------------------------------------------
30
31//----------------------------------------------------------------------
32// External includes (system with <>, local with "")
33//----------------------------------------------------------------------
34#include <algorithm>
35
36//----------------------------------------------------------------------
37// Internal includes with ""
38//----------------------------------------------------------------------
39
40//----------------------------------------------------------------------
41// Debugging
42//----------------------------------------------------------------------
43#include <cassert>
44
45//----------------------------------------------------------------------
46// Namespace declarations
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// tSplineCurve constructor
67//----------------------------------------------------------------------
68template <size_t Tdimension, typename TElement, unsigned int Tdegree>
69tSplineCurve<Tdimension, TElement, Tdegree>::tSplineCurve() :
70  control_points(Tdegree + 1, tShape::tPoint::Zero())
71{
72  static_assert(Tdegree > 0, "The degree of spline curves must be greater than zero");
73  assert(control_points.size() > Tdegree && "A spline curve needs at least degree + 1 control points");
74  this->bezier_curve_cache.resize(this->NumberOfSegments());
75}
76
77template <size_t Tdimension, typename TElement, unsigned int Tdegree>
78template <typename TIterator>
79tSplineCurve<Tdimension, TElement, Tdegree>::tSplineCurve(TIterator begin, TIterator end)
80{
81  static_assert(Tdegree > 0, "The degree of spline curves must be greater than zero");
82  std::copy(begin, end, std::back_inserter(this->control_points));
83  assert(control_points.size() > Tdegree && "A spline curve needs at least degree + 1 control points");
84  this->bezier_curve_cache.resize(this->NumberOfSegments());
85}
86
87template <size_t Tdimension, typename TElement, unsigned int Tdegree>
88template <typename ... TPoints>
89tSplineCurve<Tdimension, TElement, Tdegree>::tSplineCurve(const typename tShape::tPoint &p1, const typename tShape::tPoint &p2, const TPoints &... rest)
90{
91  static_assert(Tdegree > 0, "The degree of spline curves must be greater than zero");
92  static_assert(sizeof...(rest) + 2 > Tdegree, "A spline curve needs at least degree + 1 control points");
93  util::ProcessVariadicValues<typename tShape::tPoint>([this](const typename tShape::tPoint & x) mutable { this->control_points.push_back(x); }, p1, p2, rest...);
94  this->bezier_curve_cache.resize(this->NumberOfSegments());
95}
96
97//----------------------------------------------------------------------
98// tSplineCurve NumberOfSegments
99//----------------------------------------------------------------------
100template <size_t Tdimension, typename TElement, unsigned int Tdegree>
101const unsigned int tSplineCurve<Tdimension, TElement, Tdegree>::NumberOfSegments() const
102{
103  return this->control_points.size() - Tdegree;
104};
105
106//----------------------------------------------------------------------
107// tSplineCurve SetControlPoint
108//----------------------------------------------------------------------
109template <size_t Tdimension, typename TElement, unsigned int Tdegree>
110void tSplineCurve<Tdimension, TElement, Tdegree>::SetControlPoint(size_t i, const typename tShape::tPoint &point)
111{
112  assert(i < this->control_points.size());
113  this->control_points[i] = point;
114  this->SetChanged();
115};
116
117//----------------------------------------------------------------------
118// tSplineCurve AppendControlPoint
119//----------------------------------------------------------------------
120template <size_t Tdimension, typename TElement, unsigned int Tdegree>
121void tSplineCurve<Tdimension, TElement, Tdegree>::AppendControlPoint(const typename tShape::tPoint &point)
122{
123  this->control_points.push_back(point);
124  this->SetChanged();
125  this->bezier_curve_cache.emplace_back();
126};
127
128//----------------------------------------------------------------------
129// tSplineCurve InsertControlPoint
130//----------------------------------------------------------------------
131template <size_t Tdimension, typename TElement, unsigned int Tdegree>
132void tSplineCurve<Tdimension, TElement, Tdegree>::InsertControlPoint(size_t position, const typename tShape::tPoint &point)
133{
134  assert(position < this->control_points.size());
135  this->control_points.insert(this->control_points.begin() + position, point);
136  this->SetChanged();
137  this->bezier_curve_cache.emplace_back();
138};
139
140//----------------------------------------------------------------------
141// tSplineCurve SetControlPoints
142//----------------------------------------------------------------------
143template <size_t Tdimension, typename TElement, unsigned int Tdegree>
144template <typename TIterator>
145void tSplineCurve<Tdimension, TElement, Tdegree>::SetControlPoints(TIterator begin, TIterator end)
146{
147  this->control_points.assign(begin, end);
148  assert(this->control_points.size() > Tdegree && "A spline curve needs at least degree + 1 control points");
149  this->SetChanged();
150}
151
152//----------------------------------------------------------------------
153// tSplineCurve Evaluation: operator ()
154//----------------------------------------------------------------------
155template <size_t Tdimension, typename TElement, unsigned int Tdegree>
156const typename tShape<Tdimension, TElement>::tPoint tSplineCurve<Tdimension, TElement, Tdegree>::operator()(tParameter t) const
157{
158  assert((0 <= t) && (t <= this->NumberOfSegments()));
159  std::shared_ptr<const tBezierCurve> bezier_curve = this->GetBezierCurveForParameter(t, t);
160  return bezier_curve->operator()(t);
161}
162
163//----------------------------------------------------------------------
164// tSplineCurve GetBezierCurveForParameter
165//----------------------------------------------------------------------
166template <size_t Tdimension, typename TElement, unsigned int Tdegree>
167std::shared_ptr<const typename tSplineCurve<Tdimension, TElement, Tdegree>::tBezierCurve> tSplineCurve<Tdimension, TElement, Tdegree>::GetBezierCurveForParameter(tParameter t) const
168{
169  return this->GetBezierCurveForSegment(this->GetSegmentForParameter(t));
170}
171
172template <size_t Tdimension, typename TElement, unsigned int Tdegree>
173std::shared_ptr<const typename tSplineCurve<Tdimension, TElement, Tdegree>::tBezierCurve> tSplineCurve<Tdimension, TElement, Tdegree>::GetBezierCurveForParameter(tParameter t, tParameter &local_t) const
174{
175  local_t = this->GetLocalParameter(t);
176  return this->GetBezierCurveForParameter(t);
177}
178
179//----------------------------------------------------------------------
180// tSplineCurve GetBezierCurveForSegment
181//----------------------------------------------------------------------
182template <size_t Tdimension, typename TElement, unsigned int Tdegree>
183std::shared_ptr<const typename tSplineCurve<Tdimension, TElement, Tdegree>::tBezierCurve> tSplineCurve<Tdimension, TElement, Tdegree>::GetBezierCurveForSegment(unsigned int i) const
184{
185  assert(i < this->NumberOfSegments());
186  assert(this->bezier_curve_cache.size() == this->NumberOfSegments());
187  if (!this->bezier_curve_cache[i])
188  {
189    this->bezier_curve_cache[i] = this->CreateBezierCurveForSegment(i);
190  }
191  return this->bezier_curve_cache[i];
192}
193
194//----------------------------------------------------------------------
195// tSplineCurve GetIntersections
196//----------------------------------------------------------------------
197template <size_t Tdimension, typename TElement, unsigned int Tdegree>
198template <unsigned int Tother_degree>
199void tSplineCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
200    const tSplineCurve<Tdimension, TElement, Tother_degree> &other_spline) const
201{
202  for (unsigned int i = 0; i < this->NumberOfSegments(); ++i)
203  {
204    size_t last_size = intersection_parameters.size();
205    std::shared_ptr<const tBezierCurve> bezier_curve = this->GetBezierCurveForSegment(i);
206    for (unsigned int k = 0; k < other_spline.NumberOfSegments(); ++k)
207    {
208      bezier_curve->GetIntersections(intersection_points, intersection_parameters, *other_spline.GetBezierCurveForSegment(k));
209    }
210    for (size_t k = last_size; k < intersection_parameters.size(); ++k)
211    {
212      intersection_parameters[k] += i * 1.0;
213    }
214  }
215}
216
217template <size_t Tdimension, typename TElement, unsigned int Tdegree>
218template <unsigned int Tother_degree>
219void tSplineCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
220    const geometry::tBezierCurve<Tdimension, TElement, Tother_degree> &bezier_curve) const
221{
222  for (unsigned int i = 0; i < this->NumberOfSegments(); ++i)
223  {
224    size_t last_size = intersection_parameters.size();
225    this->GetBezierCurveForSegment(i).GetIntersections(intersection_points, intersection_parameters, bezier_curve);
226    for (size_t k = last_size; k < intersection_parameters.size(); ++k)
227    {
228      intersection_parameters[k] += i * 1.0;
229    }
230    if (last_size > 0 && intersection_parameters.size() > last_size)
231    {
232      // at least one intersection existed before this test and at least one was added
233      if (intersection_parameters[last_size] == intersection_parameters[last_size - 1])
234      {
235        intersection_points.erase(intersection_points.begin() + last_size);
236        intersection_parameters.erase(intersection_parameters.begin() + last_size);
237      }
238    }
239  }
240}
241
242template <size_t Tdimension, typename TElement, unsigned int Tdegree>
243void tSplineCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
244    const tLine<Tdimension, TElement> &line) const
245{
246  for (unsigned int i = 0; i < this->NumberOfSegments(); ++i)
247  {
248    size_t last_size = intersection_parameters.size();
249    std::shared_ptr<const tBezierCurve> bezier_curve = this->GetBezierCurveForSegment(i);
250    bezier_curve->GetIntersections(intersection_points, intersection_parameters, line);
251    for (size_t k = last_size; k < intersection_parameters.size(); ++k)
252    {
253      intersection_parameters[k] += i * 1.0;
254    }
255    if (last_size > 0 && intersection_parameters.size() > last_size)
256    {
257      // at least one intersection existed before this test and at least one was added
258      if (intersection_parameters[last_size] == intersection_parameters[last_size - 1])
259      {
260        intersection_points.erase(intersection_points.begin() + last_size);
261        intersection_parameters.erase(intersection_parameters.begin() + last_size);
262      }
263    }
264  }
265}
266
267//----------------------------------------------------------------------
268// tSplineCurve GetClosestPoint
269//----------------------------------------------------------------------
270template <size_t Tdimension, typename TElement, unsigned int Tdegree>
271const typename tShape<Tdimension, TElement>::tPoint tSplineCurve<Tdimension, TElement, Tdegree>::GetClosestPoint(const typename tShape::tPoint &reference_point) const
272{
273  typename tShape::tPoint closest_point = this->GetBezierCurveForSegment(0)->GetClosestPoint(reference_point);
274  double min_distance = (closest_point - reference_point).Length();
275
276  for (size_t i = 1; i < this->NumberOfSegments(); ++i)
277  {
278    typename tShape::tPoint candidate = this->GetBezierCurveForSegment(i)->GetClosestPoint(reference_point);
279    double distance = (candidate - reference_point).Length();
280
281    if (distance < min_distance)
282    {
283      min_distance = distance;
284      closest_point = candidate;
285    }
286  }
287
288  return closest_point;
289}
290
291//----------------------------------------------------------------------
292// tSplineCurve Translate
293//----------------------------------------------------------------------
294template <size_t Tdimension, typename TElement, unsigned int Tdegree>
295tSplineCurve<Tdimension, TElement, Tdegree> &tSplineCurve<Tdimension, TElement, Tdegree>::Translate(const math::tVector<Tdimension, TElement> &translation)
296{
297  for (typename std::vector<typename tShape::tPoint>::iterator it = this->control_points.begin(); it != this->control_points.end(); ++it)
298  {
299    *it += translation;
300  }
301  this->SetChanged();
302  return *this;
303}
304
305//----------------------------------------------------------------------
306// tSplineCurve Rotate
307//----------------------------------------------------------------------
308template <size_t Tdimension, typename TElement, unsigned int Tdegree>
309tSplineCurve<Tdimension, TElement, Tdegree> &tSplineCurve<Tdimension, TElement, Tdegree>::Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation)
310{
311  for (typename std::vector<typename tShape::tPoint>::iterator it = this->control_points.begin(); it != this->control_points.end(); ++it)
312  {
313    *it = rotation * *it;
314  }
315  this->SetChanged();
316  return *this;
317}
318
319//----------------------------------------------------------------------
320// tSplineCurve Transform
321//----------------------------------------------------------------------
322template <size_t Tdimension, typename TElement, unsigned int Tdegree>
323tSplineCurve<Tdimension, TElement, Tdegree> &tSplineCurve<Tdimension, TElement, Tdegree>::Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation)
324{
325#ifndef NDEBUG
326  for (size_t i = 0; i < Tdimension; ++i)
327  {
328    assert(math::IsEqual(transformation[Tdimension][i], 0));
329  }
330  assert(math::IsEqual(transformation[Tdimension][Tdimension], 1));
331  math::tMatrix<Tdimension, Tdimension, TElement> rotation;
332  for (size_t row = 0; row < Tdimension; ++row)
333  {
334    for (size_t column = 0; column < Tdimension; ++column)
335    {
336      rotation[row][column] = transformation[row][column];
337    }
338  }
339  assert(math::IsEqual(rotation.Determinant(), 1));
340#endif
341
342  for (typename std::vector<typename tShape::tPoint>::iterator it = this->control_points.begin(); it != this->control_points.end(); ++it)
343  {
344    *it = transformation.MultiplyHomogeneously(*it);
345  }
346  this->SetChanged();
347  return *this;
348}
349
350//----------------------------------------------------------------------
351// tSplineCurve SetChanged
352//----------------------------------------------------------------------
353template <size_t Tdimension, typename TElement, unsigned int Tdegree>
354void tSplineCurve<Tdimension, TElement, Tdegree>::SetChanged()
355{
356  tShape::SetChanged();
357  for (auto it = this->bezier_curve_cache.begin(); it != this->bezier_curve_cache.end(); ++it)
358  {
359    *it = std::shared_ptr<tBezierCurve>();
360  }
361}
362
363//----------------------------------------------------------------------
364// tSplineCurve UpdateBoundingBox
365//----------------------------------------------------------------------
366template <size_t Tdimension, typename TElement, unsigned int Tdegree>
367void tSplineCurve<Tdimension, TElement, Tdegree>::UpdateBoundingBox(typename tShape::tBoundingBox &bounding_box) const
368{
369  for (size_t i = 0; i < this->NumberOfSegments(); ++i)
370  {
371    bounding_box.Add(this->GetBezierCurveForSegment(i)->BoundingBox());
372  }
373}
374
375//----------------------------------------------------------------------
376// tSplineCurve UpdateCenterOfGravity
377//----------------------------------------------------------------------
378template <size_t Tdimension, typename TElement, unsigned int Tdegree>
379void tSplineCurve<Tdimension, TElement, Tdegree>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const
380{
381  for (size_t i = 0; i < this->NumberOfSegments(); ++i)
382  {
383    center_of_gravity += this->GetBezierCurveForSegment(i)->CenterOfGravity();
384  }
385  center_of_gravity /= this->NumberOfSegments();
386}
387
388//----------------------------------------------------------------------
389// Operators for rrlib_canvas
390//----------------------------------------------------------------------
391#ifdef _LIB_RRLIB_CANVAS_PRESENT_
392
393template <typename TElement, unsigned int Tdegree>
394inline canvas::tCanvas2D &operator << (canvas::tCanvas2D &canvas, const tSplineCurve<2, TElement, Tdegree> &spline_curve)
395{
396  unsigned int number_of_segments = spline_curve.NumberOfSegments();
397  for (size_t i = 0; i < number_of_segments; ++i)
398  {
399    canvas << *spline_curve.GetBezierCurveForSegment(i);
400  }
401
402  return canvas;
403}
404
405#endif
406
407//----------------------------------------------------------------------
408// Operators for rrlib_serialization
409//----------------------------------------------------------------------
410#ifdef _LIB_RRLIB_SERIALIZATION_PRESENT_
411
412template <size_t Tdimension, typename TElement, unsigned int Tdegree>
413serialization::tOutputStream &operator << (serialization::tOutputStream &stream, const tSplineCurve<Tdimension, TElement, Tdegree> &spline)
414{
415  size_t number_of_control_points = spline.NumberOfControlPoints();
416  stream << number_of_control_points;
417  for (size_t i = 0; i < number_of_control_points; ++i)
418  {
419    stream << spline.ControlPoints()[i];
420  }
421  return stream;
422}
423
424template <size_t Tdimension, typename TElement, unsigned int Tdegree>
425serialization::tInputStream &operator >> (serialization::tInputStream &stream, tSplineCurve<Tdimension, TElement, Tdegree> &spline)
426{
427  size_t number_of_control_points;
428  stream >> number_of_control_points;
429  tPoint<Tdimension, TElement> control_points[number_of_control_points];
430  for (size_t i = 0; i < number_of_control_points; ++i)
431  {
432    stream >> control_points[i];
433  }
434  spline.SetControlPoints(control_points, control_points + number_of_control_points);
435  return stream;
436}
437
438#endif
439
440//----------------------------------------------------------------------
441// End of namespace declaration
442//----------------------------------------------------------------------
443}
444}
445
446
447
Note: See TracBrowser for help on using the repository browser.