source: rrlib_geometry/curves/tBSplineCurve.hpp @ 52:26851bc6daf6

Last change on this file since 52:26851bc6daf6 was 52:26851bc6daf6, checked in by Tobias Föhst <foehst@…>, 7 years ago

Made tBSplineCurve::NumberOfSegments cache its result and fixed the lookup functions for local segments and parameters

File size: 11.7 KB
Line 
1//
2// You received this file as part of RRLib
3// Robotics Research Library
4//
5// Copyright (C) AG Robotersysteme TU Kaiserslautern
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    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  this->bezier_control_point_cache.clear();
155  std::vector<typename tSplineCurve::tParameter> new_knots;
156  new_knots.reserve(this->knots.size() * Tdegree);
157  std::copy(this->knots.begin(), this->knots.end(), std::back_inserter(new_knots));
158  std::copy(this->ControlPoints().begin(), this->ControlPoints().end(), std::back_inserter(this->bezier_control_point_cache));
159
160  typename tSplineCurve::tParameter knot = new_knots[0];
161  unsigned int multiplicity = 1;
162
163  for (auto it = (++new_knots.begin()); it < new_knots.end(); ++it)
164  {
165    if (knot == *it)
166    {
167      multiplicity++;
168    }
169    else
170    {
171      if (multiplicity < Tdegree)
172      {
173        for (unsigned int s = multiplicity; s < Tdegree; s++)
174        {
175          this->bezier_control_point_cache = InsertKnot((it - new_knots.begin()) - multiplicity, new_knots, knot, this->bezier_control_point_cache);
176          new_knots.insert(it, knot);
177        }
178        it += Tdegree - multiplicity;
179      }
180      if (it < new_knots.end() - 1)
181      {
182        knot = *it;
183        multiplicity = 1;
184      }
185    }
186  }
187}
188
189template<size_t Tdimension, typename TElement, unsigned int Tdegree>
190std::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)
191{
192  std::vector<typename tShape::tPoint> new_control_points;
193  new_control_points.reserve(control_points.size() + 1);
194
195  // copy unaffected points (index < at-Tdegree+1)
196  std::copy(control_points.begin(), control_points.begin() + at - Tdegree + 1, std::back_inserter(new_control_points));
197
198  // recalculate control points affected by knot insertion
199  for (int i = at - Tdegree + 1; i <= at; i++)
200  {
201    typename tSplineCurve::tParameter a = (knot - knots_before_insertion[i]) / (knots_before_insertion[i + Tdegree] - knots_before_insertion[i]);
202    new_control_points.push_back((1 - a) * control_points[i - 1] + a * control_points[i]);
203  }
204
205  // copy unaffected points (index > at)
206  std::copy(control_points.begin() + at, control_points.end(), std::back_inserter(new_control_points));
207
208  return new_control_points;
209}
210
211//----------------------------------------------------------------------
212// tBSplineCurve GetSegmentForParameter
213//----------------------------------------------------------------------
214template <size_t Tdimension, typename TElement, unsigned int Tdegree>
215unsigned int tBSplineCurve<Tdimension, TElement, Tdegree>::GetSegmentForParameter(typename tSplineCurve::tParameter t) const
216{
217  assert((this->knots.front() <= t) && (t <= this->knots.back()));
218  assert(this->NumberOfSegments() > 0);
219  unsigned int segment = 0;
220  for (size_t i = 0; i < this->knots.size() - 1; ++i)
221  {
222    if (this->knots[i] != this->knots[i + 1])
223    {
224      if (this->knots[i + 1] > t)
225      {
226        return segment;
227      }
228      segment++;
229    }
230  }
231  assert(t == this->knots.back());
232  return this->NumberOfSegments() - 1;
233}
234
235//----------------------------------------------------------------------
236// tBSplineCurve GetLocalParameter
237//----------------------------------------------------------------------
238template <size_t Tdimension, typename TElement, unsigned int Tdegree>
239typename tSplineCurve<Tdimension, TElement, Tdegree>::tParameter tBSplineCurve<Tdimension, TElement, Tdegree>::GetLocalParameter(typename tSplineCurve::tParameter t) const
240{
241  assert((this->knots.front() <= t) && (t <= this->knots.back()));
242  assert(this->NumberOfSegments() > 0);
243  unsigned int segment = 0;
244  for (size_t i = 0; i < this->knots.size() - 1; ++i)
245  {
246    if (this->knots[i] != this->knots[i + 1])
247    {
248      if (this->knots[i + 1] > t)
249      {
250        return (t - this->knots[i]) / (this->knots[i + 1] - this->knots[i]);
251      }
252      segment++;
253    }
254  }
255  assert(t == this->knots.back());
256  return 1;
257}
258
259//----------------------------------------------------------------------
260// tBSplineCurve CreateBezierCurveForSegment
261//----------------------------------------------------------------------
262template <size_t Tdimension, typename TElement, unsigned int Tdegree>
263std::shared_ptr<const typename tSplineCurve<Tdimension, TElement, Tdegree>::tBezierCurve> tBSplineCurve<Tdimension, TElement, Tdegree>::CreateBezierCurveForSegment(unsigned int i) const
264{
265  if (this->bezier_control_point_cache.empty())
266  {
267    this->CalculateBezierControlPoints();
268  }
269  std::vector<typename tShape::tPoint> segment_control_points;
270  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));
271  return std::shared_ptr<const typename tSplineCurve::tBezierCurve>(new typename tSplineCurve::tBezierCurve(segment_control_points.begin(), segment_control_points.end()));
272}
273
274//----------------------------------------------------------------------
275// Operators for rrlib_serialization
276//----------------------------------------------------------------------
277#ifdef _LIB_RRLIB_SERIALIZATION_PRESENT_
278
279template <size_t Tdimension, typename TElement, unsigned int Tdegree>
280serialization::tOutputStream &operator << (serialization::tOutputStream &stream, const tBSplineCurve<Tdimension, TElement, Tdegree> &spline)
281{
282  stream << reinterpret_cast<const tSplineCurve<Tdimension, TElement, Tdegree> &>(spline);
283  stream << spline.Knots().size();
284  for (size_t i = 0; i < spline.Knots().size(); ++i)
285  {
286    stream << spline.Knots()[i];
287  }
288  return stream;
289}
290
291template <size_t Tdimension, typename TElement, unsigned int Tdegree>
292serialization::tInputStream &operator >> (serialization::tInputStream &stream, tBSplineCurve<Tdimension, TElement, Tdegree> &spline)
293{
294  stream >> reinterpret_cast<tSplineCurve<Tdimension, TElement, Tdegree> &>(spline);
295  size_t number_of_knots;
296  stream >> number_of_knots;
297  typename tSplineCurve<Tdimension, TElement, Tdegree>::tParameter knots[number_of_knots];
298  for (size_t i = 0; i < number_of_knots; ++i)
299  {
300    stream >> knots[i];
301  }
302  spline.SetKnots(knots, knots + number_of_knots);
303  return stream;
304}
305
306#endif
307
308//----------------------------------------------------------------------
309// End of namespace declaration
310//----------------------------------------------------------------------
311}
312}
Note: See TracBrowser for help on using the repository browser.