source: rrlib_geometry/curves/tSplineCurve.hpp @ 17:3daa58ced492

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

Merged with changes from rrlab

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