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