source: rrlib_geometry/curves/tSplineCurve.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: 14.4 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>
69template <typename TIterator>
70tSplineCurve<Tdimension, TElement, Tdegree>::tSplineCurve(TIterator begin, TIterator end)
71{
72  std::copy(begin, end, std::back_inserter(this->control_points));
73  assert(control_points.size() > Tdegree);
74}
75
76//----------------------------------------------------------------------
77// tSplineCurve SetControlPoint
78//----------------------------------------------------------------------
79template <size_t Tdimension, typename TElement, unsigned int Tdegree>
80void tSplineCurve<Tdimension, TElement, Tdegree>::SetControlPoint(size_t i, const typename tShape::tPoint &point)
81{
82  assert(i < this->control_points.size());
83  this->control_points[i] = point;
84  this->SetChanged();
85};
86
87//----------------------------------------------------------------------
88// tSplineCurve AppendControlPoint
89//----------------------------------------------------------------------
90template <size_t Tdimension, typename TElement, unsigned int Tdegree>
91void tSplineCurve<Tdimension, TElement, Tdegree>::AppendControlPoint(const typename tShape::tPoint &point)
92{
93  this->control_points.push_back(point);
94  this->SetChanged();
95};
96
97//----------------------------------------------------------------------
98// tSplineCurve InsertControlPoint
99//----------------------------------------------------------------------
100template <size_t Tdimension, typename TElement, unsigned int Tdegree>
101void tSplineCurve<Tdimension, TElement, Tdegree>::InsertControlPoint(typename std::vector<typename tShape::tPoint>::iterator position, const typename tShape::tPoint &point)
102{
103  this->control_points.insert(position, point);
104  this->SetChanged();
105};
106
107//----------------------------------------------------------------------
108// tSplineCurve Evaluation: operator ()
109//----------------------------------------------------------------------
110template <size_t Tdimension, typename TElement, unsigned int Tdegree>
111const typename tShape<Tdimension, TElement>::tPoint tSplineCurve<Tdimension, TElement, Tdegree>::operator()(tParameter t) const
112{
113  assert((0 <= t) && (t <= this->GetNumberOfSegments()));
114  const tBezierCurve bezier_curve(this->GetBezierCurveForParameter(t, t));
115  return bezier_curve(t);
116}
117
118//----------------------------------------------------------------------
119// tSplineCurve GetBezierCurveForParameter
120//----------------------------------------------------------------------
121template <size_t Tdimension, typename TElement, unsigned int Tdegree>
122const typename tSplineCurve<Tdimension, TElement, Tdegree>::tBezierCurve tSplineCurve<Tdimension, TElement, Tdegree>::GetBezierCurveForParameter(tParameter t) const
123{
124  assert((0 <= t) && (t <= this->GetNumberOfSegments()));
125  return this->GetBezierCurveForSegment(static_cast<size_t>(t < this->GetNumberOfSegments() ? t : t - 1.0));
126}
127
128template <size_t Tdimension, typename TElement, unsigned int Tdegree>
129const typename tSplineCurve<Tdimension, TElement, Tdegree>::tBezierCurve tSplineCurve<Tdimension, TElement, Tdegree>::GetBezierCurveForParameter(tParameter t, tParameter &local_t) const
130{
131  local_t = t < this->GetNumberOfSegments() ? t - std::trunc(t) : 1.0;
132  return this->GetBezierCurveForParameter(t);
133}
134
135//----------------------------------------------------------------------
136// tSplineCurve GetIntersections
137//----------------------------------------------------------------------
138template <size_t Tdimension, typename TElement, unsigned int Tdegree>
139template <unsigned int Tother_degree>
140void tSplineCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
141    const tSplineCurve<Tdimension, TElement, Tother_degree> &other_spline) const
142{
143  for (unsigned int i = 0; i < this->GetNumberOfSegments(); ++i)
144  {
145    size_t last_size = intersection_parameters.size();
146    const tBezierCurve bezier_curve(this->GetBezierCurveForSegment(i));
147    for (unsigned int k = 0; k < other_spline.GetNumberOfSegments(); ++k)
148    {
149      bezier_curve.GetIntersections(intersection_points, intersection_parameters, other_spline.GetBezierCurveForSegment(k));
150    }
151    for (size_t k = last_size; k < intersection_parameters.size(); ++k)
152    {
153      intersection_parameters[k] += i * 1.0;
154    }
155  }
156}
157
158template <size_t Tdimension, typename TElement, unsigned int Tdegree>
159template <unsigned int Tother_degree>
160void tSplineCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
161    const geometry::tBezierCurve<Tdimension, TElement, Tother_degree> &bezier_curve) const
162{
163  for (unsigned int i = 0; i < this->GetNumberOfSegments(); ++i)
164  {
165    size_t last_size = intersection_parameters.size();
166    this->GetBezierCurveForSegment(i).GetIntersections(intersection_points, intersection_parameters, bezier_curve);
167    for (size_t k = last_size; k < intersection_parameters.size(); ++k)
168    {
169      intersection_parameters[k] += i * 1.0;
170    }
171    if (last_size > 0 && intersection_parameters.size() > last_size)
172    {
173      // at least one intersection existed before this test and at least one was added
174      if (intersection_parameters[last_size] == intersection_parameters[last_size - 1])
175      {
176        intersection_points.erase(intersection_points.begin() + last_size);
177        intersection_parameters.erase(intersection_parameters.begin() + last_size);
178      }
179    }
180  }
181}
182
183template <size_t Tdimension, typename TElement, unsigned int Tdegree>
184void tSplineCurve<Tdimension, TElement, Tdegree>::GetIntersections(std::vector<typename tShape::tPoint> &intersection_points, std::vector<tParameter> &intersection_parameters,
185    const tLine<Tdimension, TElement> &line) const
186{
187  for (unsigned int i = 0; i < this->NumberOfSegments(); ++i)
188  {
189    size_t last_size = intersection_parameters.size();
190    const tBezierCurve bezier_curve(this->GetBezierCurveForSegment(i));
191    bezier_curve.GetIntersections(intersection_points, intersection_parameters, line);
192    for (size_t k = last_size; k < intersection_parameters.size(); ++k)
193    {
194      intersection_parameters[k] += i * 1.0;
195    }
196    if (last_size > 0 && intersection_parameters.size() > last_size)
197    {
198      // at least one intersection existed before this test and at least one was added
199      if (intersection_parameters[last_size] == intersection_parameters[last_size - 1])
200      {
201        intersection_points.erase(intersection_points.begin() + last_size);
202        intersection_parameters.erase(intersection_parameters.begin() + last_size);
203      }
204    }
205  }
206}
207
208//----------------------------------------------------------------------
209// tSplineCurve GetClosestPoint
210//----------------------------------------------------------------------
211template <size_t Tdimension, typename TElement, unsigned int Tdegree>
212const typename tShape<Tdimension, TElement>::tPoint tSplineCurve<Tdimension, TElement, Tdegree>::GetClosestPoint(const typename tShape::tPoint &reference_point) const
213{
214  typename tShape::tPoint closest_point = this->GetBezierCurveForSegment(0).GetClosestPoint(reference_point);
215  double min_distance = (closest_point - reference_point).Length();
216
217  for (size_t i = 1; i < this->NumberOfSegments(); ++i)
218  {
219    typename tShape::tPoint candidate = this->GetBezierCurveForSegment(i).GetClosestPoint(reference_point);
220    double distance = (candidate - reference_point).Length();
221
222    if (distance < min_distance)
223    {
224      min_distance = distance;
225      closest_point = candidate;
226    }
227  }
228
229  return closest_point;
230}
231
232//----------------------------------------------------------------------
233// tSplineCurve Translate
234//----------------------------------------------------------------------
235template <size_t Tdimension, typename TElement, unsigned int Tdegree>
236tSplineCurve<Tdimension, TElement, Tdegree> &tSplineCurve<Tdimension, TElement, Tdegree>::Translate(const math::tVector<Tdimension, TElement> &translation)
237{
238  for (typename std::vector<typename tShape::tPoint>::iterator it = this->control_points.begin(); it != this->control_points.end(); ++it)
239  {
240    *it += translation;
241  }
242  this->SetChanged();
243  return *this;
244}
245
246//----------------------------------------------------------------------
247// tSplineCurve Rotate
248//----------------------------------------------------------------------
249template <size_t Tdimension, typename TElement, unsigned int Tdegree>
250tSplineCurve<Tdimension, TElement, Tdegree> &tSplineCurve<Tdimension, TElement, Tdegree>::Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation)
251{
252  for (typename std::vector<typename tShape::tPoint>::iterator it = this->control_points.begin(); it != this->control_points.end(); ++it)
253  {
254    *it = rotation * *it;
255  }
256  this->SetChanged();
257  return *this;
258}
259
260//----------------------------------------------------------------------
261// tSplineCurve Transform
262//----------------------------------------------------------------------
263template <size_t Tdimension, typename TElement, unsigned int Tdegree>
264tSplineCurve<Tdimension, TElement, Tdegree> &tSplineCurve<Tdimension, TElement, Tdegree>::Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation)
265{
266#ifndef NDEBUG
267  for (size_t i = 0; i < Tdimension; ++i)
268  {
269    assert(math::IsEqual(transformation[Tdimension][i], 0));
270  }
271  assert(math::IsEqual(transformation[Tdimension][Tdimension], 1));
272  math::tMatrix<Tdimension, Tdimension, TElement> rotation;
273  for (size_t row = 0; row < Tdimension; ++row)
274  {
275    for (size_t column = 0; column < Tdimension; ++column)
276    {
277      rotation[row][column] = transformation[row][column];
278    }
279  }
280  assert(math::IsEqual(rotation.Determinant(), 0));
281#endif
282
283  for (typename std::vector<typename tShape::tPoint>::iterator it = this->control_points.begin(); it != this->control_points.end(); ++it)
284  {
285    *it = transformation.MultiplyHomogeneously(*it);
286  }
287  this->SetChanged();
288  return *this;
289}
290
291//----------------------------------------------------------------------
292// tSplineCurve UpdateBoundingBox
293//----------------------------------------------------------------------
294template <size_t Tdimension, typename TElement, unsigned int Tdegree>
295void tSplineCurve<Tdimension, TElement, Tdegree>::UpdateBoundingBox(typename tShape::tBoundingBox &bounding_box) const
296{
297  for (size_t i = 0; i < this->NumberOfSegments(); ++i)
298  {
299    bounding_box.Add(this->GetBezierCurveForSegment(i).BoundingBox());
300  }
301}
302
303//----------------------------------------------------------------------
304// tSplineCurve UpdateCenterOfGravity
305//----------------------------------------------------------------------
306template <size_t Tdimension, typename TElement, unsigned int Tdegree>
307void tSplineCurve<Tdimension, TElement, Tdegree>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const
308{
309  for (size_t i = 0; i < this->NumberOfSegments(); ++i)
310  {
311    center_of_gravity += this->GetBezierCurveForSegment(i).CenterOfGravity();
312  }
313  center_of_gravity /= this->NumberOfSegments();
314}
315
316//----------------------------------------------------------------------
317// Operators for rrlib_canvas
318//----------------------------------------------------------------------
319#ifdef _LIB_RRLIB_CANVAS_PRESENT_
320
321template <typename TElement>
322inline canvas::tCanvas2D &operator << (canvas::tCanvas2D &canvas, const tSplineCurve<2, TElement, 3> &spline)
323{
324  unsigned int number_of_segments = spline.NumberOfSegments();
325  typename tSplineCurve<2, TElement, 3>::tBezierCurve bezier_curve = spline.GetBezierCurveForSegment(0);
326  canvas.StartPath(bezier_curve.ControlPoints()[0]);
327  for (unsigned int i = 1; i < number_of_segments; ++i)
328  {
329    canvas.AppendCubicBezierCurve(bezier_curve.ControlPoints() + 1, bezier_curve.ControlPoints() + 4);
330    bezier_curve = spline.GetBezierCurveForSegment(i);
331  }
332  canvas.AppendCubicBezierCurve(bezier_curve.ControlPoints() + 1, bezier_curve.ControlPoints() + 4);
333
334  return canvas;
335}
336
337#endif
338
339//----------------------------------------------------------------------
340// End of namespace declaration
341//----------------------------------------------------------------------
342}
343}
Note: See TracBrowser for help on using the repository browser.