source: rrlib_geometry/curves/tSplineCurve.hpp @ 96:5a75c0a06ee1

14.08
Last change on this file since 96:5a75c0a06ee1 was 96:5a75c0a06ee1, checked in by Tobias Föhst <foehst@…>, 4 months ago

Postpones calculation of knot vector and padds splines of higher degree to get end point interpolation

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