source: rrlib_geometry/curves/tBSplineCurve.hpp @ 89:4bf089ff3edf

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

Mapping global curve parameters on knot vector of B spline to get the same behavior as in the other spline curves

File size: 12.0 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    tBSplineCurve.cpp
23 *
24 * \author  Patrick Fleischmann
25 *
26 * \date    2012-04-18
27 *
28 */
29//----------------------------------------------------------------------
30//----------------------------------------------------------------------
31// External includes (system with <>, local with "")
32//----------------------------------------------------------------------
33//----------------------------------------------------------------------
34// Internal includes with ""
35//----------------------------------------------------------------------
36//----------------------------------------------------------------------
37// Debugging
38//----------------------------------------------------------------------
39#include <cassert>
40
41//----------------------------------------------------------------------
42// Namespace declaration
43//----------------------------------------------------------------------
44namespace rrlib
45{
46namespace geometry
47{
48
49//----------------------------------------------------------------------
50// Forward declarations / typedefs / enums
51//----------------------------------------------------------------------
52
53//----------------------------------------------------------------------
54// Const values
55//----------------------------------------------------------------------
56
57//----------------------------------------------------------------------
58// Implementation
59//----------------------------------------------------------------------
60
61//----------------------------------------------------------------------
62// tBSplineCurve constructor
63//----------------------------------------------------------------------
64template <size_t Tdimension, typename TElement, unsigned int Tdegree>
65tBSplineCurve<Tdimension, TElement, Tdegree>::tBSplineCurve() :
66  tSplineCurve(),
67  number_of_segments(-1)
68{
69  this->CalculateKnotVector();
70}
71
72template<size_t Tdimension, typename TElement, unsigned int Tdegree>
73template<typename TIterator, typename TKnotIterator>
74tBSplineCurve<Tdimension, TElement, Tdegree>::tBSplineCurve(TIterator control_points_begin, TIterator control_points_end, TKnotIterator knots_begin, TKnotIterator knots_end) :
75  tSplineCurve(control_points_begin, control_points_end),
76  number_of_segments(-1)
77{
78  // copy knots
79  std::copy(knots_begin, knots_end, std::back_inserter(this->knots));
80  assert(this->knots.size() == this->ControlPoints().size() + Tdegree + 1);
81}
82
83template<size_t Tdimension, typename TElement, unsigned int Tdegree>
84template<typename TIterator>
85tBSplineCurve<Tdimension, TElement, Tdegree>::tBSplineCurve(TIterator begin, TIterator end) :
86  tSplineCurve(begin, end),
87  number_of_segments(-1)
88{
89  this->CalculateKnotVector();
90}
91
92//----------------------------------------------------------------------
93// tBSplineCurve NumberOfSegments
94//----------------------------------------------------------------------
95template <size_t Tdimension, typename TElement, unsigned int Tdegree>
96const unsigned int tBSplineCurve<Tdimension, TElement, Tdegree>::NumberOfSegments() const
97{
98  if (this->number_of_segments == static_cast<unsigned int>(-1))
99  {
100    this->number_of_segments = 0;
101    for (size_t i = 0; i < this->knots.size() - 1; ++i)
102    {
103      if (this->knots[i] != this->knots[i + 1])
104      {
105        this->number_of_segments++;
106      }
107    }
108  }
109  return this->number_of_segments;
110};
111
112//----------------------------------------------------------------------
113// tBSplineCurve SetChanged
114//----------------------------------------------------------------------
115template<size_t Tdimension, typename TElement, unsigned int Tdegree>
116void tBSplineCurve<Tdimension, TElement, Tdegree>::SetChanged()
117{
118  tSplineCurve::SetChanged();
119  this->CalculateKnotVector();
120  this->bezier_control_point_cache.clear();
121  this->number_of_segments = -1;
122}
123
124template<size_t Tdimension, typename TElement, unsigned int Tdegree>
125void tBSplineCurve<Tdimension, TElement, Tdegree>::CalculateKnotVector()
126{
127  this->knots.clear();
128  assert(this->NumberOfControlPoints() > Tdegree);
129  // calculate knot vector
130  unsigned int length = this->NumberOfControlPoints() + Tdegree + 1;
131  this->knots.reserve(length);
132
133  for (unsigned int i = 0; i < length; ++i)
134  {
135    if (i < Tdegree + 1)
136    {
137      this->knots.push_back(0);
138    }
139    else if ((Tdegree + 1) <= i && i <= this->NumberOfControlPoints())
140    {
141      // inner knot vector (uniform)
142      this->knots.push_back(1.0 / (this->NumberOfControlPoints() - Tdegree) * (i - Tdegree));
143    }
144    else if (i > this->NumberOfControlPoints())
145    {
146      this->knots.push_back(1.0);
147    }
148  }
149}
150
151template<size_t Tdimension, typename TElement, unsigned int Tdegree>
152void tBSplineCurve<Tdimension, TElement, Tdegree>::CalculateBezierControlPoints() const
153{
154  for (std::size_t i = 0, k = 1; k < this->knots.size(); ++i, ++k)
155  {
156    assert(this->knots[i] <= this->knots[k]);
157  }
158  this->bezier_control_point_cache.clear();
159  std::vector<typename tSplineCurve::tParameter> new_knots;
160  new_knots.reserve(this->knots.size() * Tdegree);
161  std::copy(this->knots.begin(), this->knots.end(), std::back_inserter(new_knots));
162  std::copy(this->ControlPoints().begin(), this->ControlPoints().end(), std::back_inserter(this->bezier_control_point_cache));
163
164  typename tSplineCurve::tParameter knot = new_knots[0];
165  unsigned int multiplicity = 1;
166
167  for (auto it = (++new_knots.begin()); it < new_knots.end(); ++it)
168  {
169    if (knot == *it)
170    {
171      multiplicity++;
172    }
173    else
174    {
175      if (multiplicity < Tdegree)
176      {
177        for (unsigned int s = multiplicity; s < Tdegree; s++)
178        {
179          this->bezier_control_point_cache = InsertKnot((it - new_knots.begin()) - multiplicity, new_knots, knot, this->bezier_control_point_cache);
180          new_knots.insert(it, knot);
181        }
182        it += Tdegree - multiplicity;
183      }
184      if (it < new_knots.end() - 1)
185      {
186        knot = *it;
187        multiplicity = 1;
188      }
189    }
190  }
191}
192
193template<size_t Tdimension, typename TElement, unsigned int Tdegree>
194std::vector<typename tBSplineCurve<Tdimension, TElement, Tdegree>::tShape::tPoint> tBSplineCurve<Tdimension, TElement, Tdegree>::InsertKnot(int at, const std::vector<typename tSplineCurve::tParameter> &knots_before_insertion, typename tSplineCurve::tParameter knot, const std::vector<typename tShape::tPoint> &control_points)
195{
196  std::vector<typename tShape::tPoint> new_control_points;
197  new_control_points.reserve(control_points.size() + 1);
198
199  // copy unaffected points (index < at-Tdegree+1)
200  std::copy(control_points.begin(), control_points.begin() + at - Tdegree + 1, std::back_inserter(new_control_points));
201
202  // recalculate control points affected by knot insertion
203  for (int i = at - Tdegree + 1; i <= at; i++)
204  {
205    typename tSplineCurve::tParameter a = (knot - knots_before_insertion[i]) / (knots_before_insertion[i + Tdegree] - knots_before_insertion[i]);
206    new_control_points.push_back((1 - a) * control_points[i - 1] + a * control_points[i]);
207  }
208
209  // copy unaffected points (index > at)
210  std::copy(control_points.begin() + at, control_points.end(), std::back_inserter(new_control_points));
211
212  return new_control_points;
213}
214
215//----------------------------------------------------------------------
216// tBSplineCurve GetSegmentForParameter
217//----------------------------------------------------------------------
218template <size_t Tdimension, typename TElement, unsigned int Tdegree>
219unsigned int tBSplineCurve<Tdimension, TElement, Tdegree>::GetSegmentForParameter(typename tSplineCurve::tParameter t) const
220{
221  t /= this->NumberOfSegments() + this->knots.front() * (this->knots.back() - this->knots.front());
222  assert((this->knots.front() <= t) && (t <= this->knots.back()));
223  assert(this->NumberOfSegments() > 0);
224  unsigned int segment = 0;
225  for (size_t i = 0; i < this->knots.size() - 1; ++i)
226  {
227    if (this->knots[i] != this->knots[i + 1])
228    {
229      if (this->knots[i + 1] > t)
230      {
231        return segment;
232      }
233      segment++;
234    }
235  }
236  assert(t == this->knots.back());
237  return this->NumberOfSegments() - 1;
238}
239
240//----------------------------------------------------------------------
241// tBSplineCurve GetLocalParameter
242//----------------------------------------------------------------------
243template <size_t Tdimension, typename TElement, unsigned int Tdegree>
244typename tSplineCurve<Tdimension, TElement, Tdegree>::tParameter tBSplineCurve<Tdimension, TElement, Tdegree>::GetLocalParameter(typename tSplineCurve::tParameter t) const
245{
246  t /= this->NumberOfSegments() + this->knots.front() * (this->knots.back() - this->knots.front());
247  assert((this->knots.front() <= t) && (t <= this->knots.back()));
248  assert(this->NumberOfSegments() > 0);
249  unsigned int segment = 0;
250  for (size_t i = 0; i < this->knots.size() - 1; ++i)
251  {
252    if (this->knots[i] != this->knots[i + 1])
253    {
254      if (this->knots[i + 1] > t)
255      {
256        return (t - this->knots[i]) / (this->knots[i + 1] - this->knots[i]);
257      }
258      segment++;
259    }
260  }
261  assert(t == this->knots.back());
262  return 1;
263}
264
265//----------------------------------------------------------------------
266// tBSplineCurve CreateBezierCurveForSegment
267//----------------------------------------------------------------------
268template <size_t Tdimension, typename TElement, unsigned int Tdegree>
269std::shared_ptr<const typename tSplineCurve<Tdimension, TElement, Tdegree>::tBezierCurve> tBSplineCurve<Tdimension, TElement, Tdegree>::CreateBezierCurveForSegment(unsigned int i) const
270{
271  if (this->bezier_control_point_cache.empty())
272  {
273    this->CalculateBezierControlPoints();
274  }
275  std::vector<typename tShape::tPoint> segment_control_points;
276  std::copy(this->bezier_control_point_cache.begin() + i * Tdegree, this->bezier_control_point_cache.begin() + i * Tdegree + Tdegree + 1, std::back_inserter(segment_control_points));
277  return std::shared_ptr<const typename tSplineCurve::tBezierCurve>(new typename tSplineCurve::tBezierCurve(segment_control_points.begin(), segment_control_points.end()));
278}
279
280//----------------------------------------------------------------------
281// Operators for rrlib_serialization
282//----------------------------------------------------------------------
283#ifdef _LIB_RRLIB_SERIALIZATION_PRESENT_
284
285template <size_t Tdimension, typename TElement, unsigned int Tdegree>
286serialization::tOutputStream &operator << (serialization::tOutputStream &stream, const tBSplineCurve<Tdimension, TElement, Tdegree> &spline)
287{
288  stream << reinterpret_cast<const tSplineCurve<Tdimension, TElement, Tdegree> &>(spline);
289  stream << spline.Knots().size();
290  for (size_t i = 0; i < spline.Knots().size(); ++i)
291  {
292    stream << spline.Knots()[i];
293  }
294  return stream;
295}
296
297template <size_t Tdimension, typename TElement, unsigned int Tdegree>
298serialization::tInputStream &operator >> (serialization::tInputStream &stream, tBSplineCurve<Tdimension, TElement, Tdegree> &spline)
299{
300  stream >> reinterpret_cast<tSplineCurve<Tdimension, TElement, Tdegree> &>(spline);
301  size_t number_of_knots;
302  stream >> number_of_knots;
303  typename tSplineCurve<Tdimension, TElement, Tdegree>::tParameter knots[number_of_knots];
304  for (size_t i = 0; i < number_of_knots; ++i)
305  {
306    stream >> knots[i];
307  }
308  spline.SetKnots(knots, knots + number_of_knots);
309  return stream;
310}
311
312#endif
313
314//----------------------------------------------------------------------
315// End of namespace declaration
316//----------------------------------------------------------------------
317}
318}
Note: See TracBrowser for help on using the repository browser.