source: rrlib_geometry/curves/tBSplineCurve.hpp @ 50:1dce8c94505d

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

Added some missing const qualifiers

File size: 11.4 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{
68  this->CalculateKnotVector();
69  this->CalculateBezierControlPoints();
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{
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  this->CalculateBezierControlPoints();
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{
88  this->CalculateKnotVector();
89  this->CalculateBezierControlPoints();
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  unsigned int number_of_segments = 0;
99  double current_knot = this->knots[0];
100  for (size_t i = 1; i < this->knots.size(); ++i)
101  {
102    if (current_knot != this->knots[i])
103    {
104      number_of_segments++;
105      current_knot = this->knots[i];
106    }
107  }
108  return number_of_segments;
109};
110
111//template<size_t Tdimension, typename TElement, unsigned int Tdegree>
112//void tBSplineCurve<Tdimension, TElement, Tdegree>::AppendControlPoint(const typename tShape::tPoint &point)
113//{
114//  tSplineCurve::AppendControlPoint(point);
115//  this->CalculateKnotVector();
116//  this->CalculateBezierControlPoints();
117//}
118//
119//----------------------------------------------------------------------
120// tBSplineCurve SetChanged
121//----------------------------------------------------------------------
122template<size_t Tdimension, typename TElement, unsigned int Tdegree>
123void tBSplineCurve<Tdimension, TElement, Tdegree>::SetChanged()
124{
125  tSplineCurve::SetChanged();
126  this->CalculateKnotVector();
127  this->CalculateBezierControlPoints();
128}
129
130template<size_t Tdimension, typename TElement, unsigned int Tdegree>
131void tBSplineCurve<Tdimension, TElement, Tdegree>::CalculateKnotVector()
132{
133  this->knots.clear();
134  assert(this->NumberOfControlPoints() > Tdegree);
135  // calculate knot vector
136  unsigned int length = this->NumberOfControlPoints() + Tdegree + 1;
137  this->knots.reserve(length);
138
139  for (unsigned int i = 0; i < length; ++i)
140  {
141    if (i < Tdegree + 1)
142    {
143      this->knots.push_back(0);
144    }
145    else if ((Tdegree + 1) <= i && i <= this->NumberOfControlPoints())
146    {
147      // inner knot vector (uniform)
148      this->knots.push_back(1.0 / (this->NumberOfControlPoints() - Tdegree) * (i - Tdegree));
149    }
150    else if (i > this->NumberOfControlPoints())
151    {
152      this->knots.push_back(1.0);
153    }
154  }
155}
156
157template<size_t Tdimension, typename TElement, unsigned int Tdegree>
158void tBSplineCurve<Tdimension, TElement, Tdegree>::CalculateBezierControlPoints()
159{
160  this->bezier_control_points.clear();
161  std::vector<double> new_knots;
162  new_knots.reserve(this->knots.size() * Tdegree);
163  std::copy(this->knots.begin(), this->knots.end(), std::back_inserter(new_knots));
164  std::copy(this->ControlPoints().begin(), this->ControlPoints().end(), std::back_inserter(this->bezier_control_points));
165
166  double knot = new_knots[0];
167  unsigned int multiplicity = 1;
168
169  for (std::vector<double>::iterator it = (++new_knots.begin()); it < new_knots.end(); ++it)
170  {
171    if (knot == *it)
172    {
173      multiplicity++;
174    }
175    else
176    {
177      if (multiplicity < Tdegree)
178      {
179        for (unsigned int s = multiplicity; s < Tdegree; s++)
180        {
181          this->bezier_control_points = InsertKnot((it - new_knots.begin()) - multiplicity, new_knots, knot, this->bezier_control_points);
182          new_knots.insert(it, knot);
183        }
184        it += Tdegree - multiplicity;
185      }
186      if (it < new_knots.end() - 1)
187      {
188        knot = *it;
189        multiplicity = 1;
190      }
191    }
192  }
193}
194
195template<size_t Tdimension, typename TElement, unsigned int Tdegree>
196std::vector<typename tBSplineCurve<Tdimension, TElement, Tdegree>::tShape::tPoint> tBSplineCurve<Tdimension, TElement, Tdegree>::InsertKnot(int at, const std::vector<double> &knots_before_insertion, double knot, const std::vector <
197    typename tShape::tPoint > &control_points) const
198{
199  std::vector<typename tShape::tPoint> new_control_points;
200  new_control_points.reserve(control_points.size() + 1);
201
202  // copy unaffected points (index < at-Tdegree+1)
203  std::copy(control_points.begin(), control_points.begin() + at - Tdegree + 1, std::back_inserter(new_control_points));
204
205  // recalculate control points affected by knot insertion
206  for (int i = at - Tdegree + 1; i <= at; i++)
207  {
208    double a = (knot - knots_before_insertion[i]) / (knots_before_insertion[i + Tdegree] - knots_before_insertion[i]);
209    new_control_points.push_back((1 - a) * control_points[i - 1] + a * control_points[i]);
210  }
211
212  // copy unaffected points (index > at)
213  std::copy(control_points.begin() + at, control_points.end(), std::back_inserter(new_control_points));
214
215  return new_control_points;
216}
217
218//----------------------------------------------------------------------
219// tBSplineCurve GetSegmentForParameter
220//----------------------------------------------------------------------
221template <size_t Tdimension, typename TElement, unsigned int Tdegree>
222unsigned int tBSplineCurve<Tdimension, TElement, Tdegree>::GetSegmentForParameter(typename tSplineCurve::tParameter t) const
223{
224  assert((this->knots.front() <= t) && (t <= this->knots.back()));
225  auto it = std::lower_bound(this->knots.begin(), this->knots.end(), t);
226
227  return static_cast<unsigned int>(t == this->knots.front() ? 0 : std::distance(this->knots.begin(), it) - 1);
228}
229
230//----------------------------------------------------------------------
231// tBSplineCurve GetLocalParameter
232//----------------------------------------------------------------------
233template <size_t Tdimension, typename TElement, unsigned int Tdegree>
234typename tSplineCurve<Tdimension, TElement, Tdegree>::tParameter tBSplineCurve<Tdimension, TElement, Tdegree>::GetLocalParameter(typename tSplineCurve::tParameter t) const
235{
236  unsigned int start = this->GetSegmentForParameter(t);
237  unsigned int stop = start + 1;
238  while (this->knots[start] == this->knots[stop])
239  {
240    stop++;
241    assert(stop < this->knots.size());
242  }
243  return (t - this->knots[start]) / (this->knots[stop] - this->knots[start]);
244}
245
246//----------------------------------------------------------------------
247// tBSplineCurve CreateBezierCurveForSegment
248//----------------------------------------------------------------------
249template <size_t Tdimension, typename TElement, unsigned int Tdegree>
250std::shared_ptr<const typename tSplineCurve<Tdimension, TElement, Tdegree>::tBezierCurve> tBSplineCurve<Tdimension, TElement, Tdegree>::CreateBezierCurveForSegment(unsigned int i) const
251{
252  std::vector<typename tShape::tPoint> segment_control_points;
253  std::copy(this->bezier_control_points.begin() + i * Tdegree, this->bezier_control_points.begin() + i * Tdegree + Tdegree + 1, std::back_inserter(segment_control_points));
254  return std::shared_ptr<const typename tSplineCurve::tBezierCurve>(new typename tSplineCurve::tBezierCurve(segment_control_points.begin(), segment_control_points.end()));
255}
256
257//----------------------------------------------------------------------
258// Operators for rrlib_serialization
259//----------------------------------------------------------------------
260#ifdef _LIB_RRLIB_SERIALIZATION_PRESENT_
261
262template <size_t Tdimension, typename TElement, unsigned int Tdegree>
263serialization::tOutputStream &operator << (serialization::tOutputStream &stream, const tBSplineCurve<Tdimension, TElement, Tdegree> &spline)
264{
265  stream << reinterpret_cast<const tSplineCurve<Tdimension, TElement, Tdegree> &>(spline);
266  stream << spline.Knots().size();
267  for (size_t i = 0; i < spline.Knots().size(); ++i)
268  {
269    stream << spline.Knots()[i];
270  }
271  return stream;
272}
273
274template <size_t Tdimension, typename TElement, unsigned int Tdegree>
275serialization::tInputStream &operator >> (serialization::tInputStream &stream, tBSplineCurve<Tdimension, TElement, Tdegree> &spline)
276{
277  stream >> reinterpret_cast<tSplineCurve<Tdimension, TElement, Tdegree> &>(spline);
278  size_t number_of_knots;
279  stream >> number_of_knots;
280  double knots[number_of_knots];
281  for (size_t i = 0; i < number_of_knots; ++i)
282  {
283    stream >> knots[i];
284  }
285  spline.SetKnots(knots, knots + number_of_knots);
286  return stream;
287}
288
289#endif
290
291//----------------------------------------------------------------------
292// End of namespace declaration
293//----------------------------------------------------------------------
294}
295}
Note: See TracBrowser for help on using the repository browser.