source: rrlib_geometry/curves/tBSplineCurve.hpp @ 47:73eab926c138

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

Added serialization operators for curves

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