source: rrlib_geometry/curves/tSplineCurve.hpp @ 69:115bb81d02a2

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

Fix for clang: replaced array of runtime-determined size by std::vector

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