source: rrlib_geometry/curves/tSplineCurve.hpp @ 60:a22756932ab5

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

Added and updated license information

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