source: rrlib_geometry/curves/tBSplineCurve.hpp @ 51:0d26d6c7f188

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

Adjusted knot type in tBSplineCurve and changed bezier_curve_cache to not being populated before used

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