cubic_hermite_spline.h
Go to the documentation of this file.
1 
8 #ifndef _CLASS_CUBICHERMITESPLINE
9 #define _CLASS_CUBICHERMITESPLINE
10 
11 #include "curve_abc.h"
12 #include "bezier_curve.h"
13 #include "piecewise_curve.h"
14 
15 #include "MathDefs.h"
16 
17 #include <vector>
18 #include <stdexcept>
19 
20 #include <iostream>
21 
22 #include <boost/serialization/utility.hpp> // To serialize std::pair
23 
24 namespace curves {
34 template <typename Time = double, typename Numeric = Time, bool Safe = false,
35  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
36 struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
37  typedef Point point_t;
38  typedef std::pair<Point, Point> pair_point_tangent_t;
39  typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<Point> > t_pair_point_tangent_t;
40  typedef std::vector<Time> vector_time_t;
41  typedef Time time_t;
42  typedef Numeric num_t;
46  typedef typename bezier_t::t_point_t t_point_t;
48 
49  public:
53 
59  template <typename In>
60  cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t& time_control_points)
61  : size_(std::distance(PairsBegin, PairsEnd)), degree_(3) {
62  // Check size of pairs container.
63  if (Safe && size_ < 1) {
64  throw std::length_error("can not create cubic_hermite_spline, number of pairs is inferior to 2.");
65  }
66  // Set dimension according to size of points
67  dim_ = PairsBegin->first.size();
68  // Push all pairs in controlPoints
69  In it(PairsBegin);
70  for (; it != PairsEnd; ++it) {
71  if(Safe && (static_cast<size_t>(it->first.size()) != dim_ || static_cast<size_t>(it->second.size()) != dim_))
72  throw std::invalid_argument("All the control points and their derivatives must have the same dimension.");
73  control_points_.push_back(*it);
74  }
75  // Set time
76  setTime(time_control_points);
77  }
78 
80  : dim_(other.dim_),
84  T_min_(other.T_min_),
85  T_max_(other.T_max_),
86  size_(other.size_),
87  degree_(other.degree_) {}
88 
90  virtual ~cubic_hermite_spline() {}
91 
92  /*Operations*/
93  public:
98  virtual Point operator()(const time_t t) const {
99  check_conditions();
100  if (Safe & !(T_min_ <= t && t <= T_max_)) {
101  throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
102  }
103  if (size_ == 1) {
104  return control_points_.front().first;
105  } else {
106  const bezier_t bezier = buildCurrentBezier(t);
107  return bezier(t);
108  }
109  }
110 
119  bool isApprox(const cubic_hermite_spline_t& other,
120  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
121  bool equal = curves::isApprox<num_t>(T_min_, other.min()) && curves::isApprox<num_t>(T_max_, other.max()) &&
122  dim_ == other.dim() && degree_ == other.degree() && size_ == other.size() &&
124  if (!equal) return false;
125  for (std::size_t i = 0; i < size_; ++i) {
126  if ((!control_points_[i].first.isApprox(other.control_points_[i].first, prec)) ||
127  (!control_points_[i].second.isApprox(other.control_points_[i].second, prec)))
128  return false;
129  }
130  return true;
131  }
132 
133  virtual bool isApprox(const curve_abc_t* other,
134  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
135  const cubic_hermite_spline_t* other_cast = dynamic_cast<const cubic_hermite_spline_t*>(other);
136  if (other_cast)
137  return isApprox(*other_cast, prec);
138  else
139  return false;
140  }
141 
142  virtual bool operator==(const cubic_hermite_spline_t& other) const { return isApprox(other); }
143 
144  virtual bool operator!=(const cubic_hermite_spline_t& other) const { return !(*this == other); }
145 
151  virtual Point derivate(const time_t t, const std::size_t order) const {
152  check_conditions();
153  if (Safe & !(T_min_ <= t && t <= T_max_)) {
154  throw std::invalid_argument("can't derivate cubic hermite spline, out of range");
155  }
156  if (size_ == 1) {
157  return control_points_.front().second;
158  } else {
159  const bezier_t bezier = buildCurrentBezier(t);
160  return bezier.derivate(t, order);
161  }
162  }
163 
164  piecewise_bezier_t compute_derivate(const std::size_t order) const {
165  piecewise_bezier_t res;
166  for(size_t i = 0 ; i < size_ - 1 ; ++i){
167  const bezier_t curve = buildCurrentBezier(time_control_points_[i]);
168  res.add_curve(curve.compute_derivate(order));
169  }
170  return res;
171  }
172 
176  piecewise_bezier_t* compute_derivate_ptr(const std::size_t order) const {
177  return new piecewise_bezier_t(compute_derivate(order));
178  }
179 
185  void setTime(const vector_time_t& time_control_points) {
186  time_control_points_ = time_control_points;
187  T_min_ = time_control_points_.front();
188  T_max_ = time_control_points_.back();
189  if (time_control_points.size() != size()) {
190  throw std::length_error("size of time control points should be equal to number of control points");
191  }
192  computeDurationSplines();
193  if (!checkDurationSplines()) {
194  throw std::invalid_argument("time_splines not monotonous, all spline duration should be superior to 0");
195  }
196  }
197 
201  t_pair_point_tangent_t getControlPoints() { return control_points_; }
202 
206  vector_time_t getTime() { return time_control_points_; }
207 
211  std::size_t size() const { return size_; }
212 
216  std::size_t numIntervals() const { return size() - 1; }
217 
218  private:
223  std::size_t findInterval(const time_t t) const {
224  // time before first control point time.
225  if (t <= time_control_points_[0]) {
226  return 0;
227  }
228  // time is after last control point time
229  if (t >= time_control_points_[size_ - 1]) {
230  return size_ - 2;
231  }
232  std::size_t left_id = 0;
233  std::size_t right_id = size_ - 1;
234  while (left_id <= right_id) {
235  const std::size_t middle_id = left_id + (right_id - left_id) / 2;
236  if (time_control_points_.at(middle_id) < t) {
237  left_id = middle_id + 1;
238  } else if (time_control_points_.at(middle_id) > t) {
239  right_id = middle_id - 1;
240  } else {
241  return middle_id;
242  }
243  }
244  return left_id - 1;
245  }
246 
253  bezier_t buildCurrentBezier(const time_t t) const{
254  size_t id_interval = findInterval(t);
255  const pair_point_tangent_t pair0 = control_points_.at(id_interval);
256  const pair_point_tangent_t pair1 = control_points_.at(id_interval + 1);
257  const Time& t0 = time_control_points_[id_interval];
258  const Time& t1 = time_control_points_[id_interval + 1];
259  t_point_t control_points;
260  control_points.reserve(4);
261  control_points.push_back(pair0.first);
262  control_points.push_back(pair0.first + pair0.second / 3. * (t1 - t0));
263  control_points.push_back(pair1.first - pair1.second / 3. * (t1 - t0));
264  control_points.push_back(pair1.first);
265  return bezier_t(control_points.begin(), control_points.end(), t0, t1);
266  }
267 
270  void check_conditions() const {
271  if (control_points_.size() == 0) {
272  throw std::runtime_error(
273  "Error in cubic hermite : there is no control points set / did you use empty constructor ?");
274  } else if (dim_ == 0) {
275  throw std::runtime_error(
276  "Error in cubic hermite : Dimension of points is zero / did you use empty constructor ?");
277  }
278  }
279 
284  void computeDurationSplines() {
285  duration_splines_.clear();
286  Time actual_time;
287  Time prev_time = *(time_control_points_.begin());
288  std::size_t i = 0;
289  for (i = 0; i < size() - 1; i++) {
290  actual_time = time_control_points_.at(i + 1);
291  duration_splines_.push_back(actual_time - prev_time);
292  prev_time = actual_time;
293  }
294  }
295 
299  bool checkDurationSplines() const {
300  std::size_t i = 0;
301  bool is_positive = true;
302  while (is_positive && i < duration_splines_.size()) {
303  is_positive = (duration_splines_.at(i) > 0.);
304  i++;
305  }
306  return is_positive;
307  }
308  /*Operations*/
309 
310  /*Helpers*/
311  public:
314  std::size_t virtual dim() const { return dim_; };
317  Time virtual min() const { return time_control_points_.front(); }
320  Time virtual max() const { return time_control_points_.back(); }
323  virtual std::size_t degree() const { return degree_; }
324  /*Helpers*/
325 
326  /*Attributes*/
328  std::size_t dim_;
330  t_pair_point_tangent_t control_points_;
334  vector_time_t time_control_points_;
339  vector_time_t duration_splines_;
341  /*const*/ Time T_min_;
343  /*const*/ Time T_max_;
345  std::size_t size_;
347  std::size_t degree_;
348  /*Attributes*/
349 
350  // Serialization of the class
352 
353  template <class Archive>
354  void serialize(Archive& ar, const unsigned int version) {
355  if (version) {
356  // Do something depending on version ?
357  }
358  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
359  ar& boost::serialization::make_nvp("dim", dim_);
360  ar& boost::serialization::make_nvp("control_points", control_points_);
361  ar& boost::serialization::make_nvp("time_control_points", time_control_points_);
362  ar& boost::serialization::make_nvp("duration_splines", duration_splines_);
363  ar& boost::serialization::make_nvp("T_min", T_min_);
364  ar& boost::serialization::make_nvp("T_max", T_max_);
365  ar& boost::serialization::make_nvp("size", size_);
366  ar& boost::serialization::make_nvp("degree", degree_);
367  }
368 }; // End struct Cubic hermite spline
369 } // namespace curves
370 #endif //_CLASS_CUBICHERMITESPLINE
cubic_hermite_spline< Time, Numeric, Safe, point_t > cubic_hermite_spline_t
Definition: cubic_hermite_spline.h:44
cubic_hermite_spline()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: cubic_hermite_spline.h:52
virtual std::size_t dim() const
Get dimension of curve.
Definition: cubic_hermite_spline.h:314
vector_time_t duration_splines_
Definition: cubic_hermite_spline.h:339
void setTime(const vector_time_t &time_control_points)
Set time of each control point of cubic hermite spline. Set duration of each spline, Exemple : with values corresponding to times for respectively.
Definition: cubic_hermite_spline.h:185
double Numeric
Definition: effector_spline.h:26
Time time_t
Definition: cubic_hermite_spline.h:41
virtual Time min() const
Get the minimum time for which the curve is defined.
Definition: cubic_hermite_spline.h:317
Time T_min_
Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
Definition: cubic_hermite_spline.h:341
Numeric num_t
Definition: cubic_hermite_spline.h:42
virtual bool operator!=(const cubic_hermite_spline_t &other) const
Definition: cubic_hermite_spline.h:144
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluate the derivative order N of curve at time t. If derivative is to be evaluated several times...
Definition: bezier_curve.h:239
bezier_curve< Time, Numeric, Safe, point_t > bezier_t
Definition: cubic_hermite_spline.h:45
interface for a Curve of arbitrary dimension.
bezier_t::t_point_t t_point_t
Definition: cubic_hermite_spline.h:46
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: cubic_hermite_spline.h:133
vector_time_t time_control_points_
Definition: cubic_hermite_spline.h:334
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:142
Definition: bernstein.h:20
vector_time_t getTime()
Get vector of Time corresponding to Time for each control point.
Definition: cubic_hermite_spline.h:206
virtual bool operator==(const cubic_hermite_spline_t &other) const
Definition: cubic_hermite_spline.h:142
t_pair_point_tangent_t control_points_
Vector of pair < Point, Tangent >.
Definition: cubic_hermite_spline.h:330
std::size_t size() const
Get number of control points contained in the trajectory.
Definition: cubic_hermite_spline.h:211
virtual ~cubic_hermite_spline()
Destructor.
Definition: cubic_hermite_spline.h:90
Point point_t
Definition: cubic_hermite_spline.h:37
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
std::vector< pair_point_tangent_t, Eigen::aligned_allocator< Point > > t_pair_point_tangent_t
Definition: cubic_hermite_spline.h:39
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:39
std::size_t size_
Number of control points (pairs).
Definition: cubic_hermite_spline.h:345
piecewise_curve< Time, Numeric, Safe, point_t, point_t, bezier_t > piecewise_bezier_t
Definition: cubic_hermite_spline.h:47
Definition: cubic_hermite_spline.h:36
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: cubic_hermite_spline.h:320
piecewise_bezier_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: cubic_hermite_spline.h:176
virtual Point operator()(const time_t t) const
Evaluation of the cubic hermite spline at time t.
Definition: cubic_hermite_spline.h:98
t_pair_point_tangent_t getControlPoints()
Get vector of pair (positition, derivative) corresponding to control points.
Definition: cubic_hermite_spline.h:201
std::pair< Point, Point > pair_point_tangent_t
Definition: cubic_hermite_spline.h:38
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: cubic_hermite_spline.h:43
virtual std::size_t degree() const
Get the degree of the curve.
Definition: cubic_hermite_spline.h:323
cubic_hermite_spline(const cubic_hermite_spline &other)
Definition: cubic_hermite_spline.h:79
friend class boost::serialization::access
Definition: cubic_hermite_spline.h:351
Time T_max_
Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
Definition: cubic_hermite_spline.h:343
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t &time_control_points)
Constructor.
Definition: cubic_hermite_spline.h:60
std::size_t dim_
Dim of curve.
Definition: cubic_hermite_spline.h:328
void serialize(Archive &ar, const unsigned int version)
Definition: cubic_hermite_spline.h:354
std::vector< Time > vector_time_t
Definition: cubic_hermite_spline.h:40
class allowing to create a piecewise curve.
bezier_curve_t compute_derivate(const std::size_t order) const
Compute the derived curve at order N. Computes the derivative order N, of bezier curve of parametric...
Definition: bezier_curve.h:185
std::size_t degree_
Degree (Cubic so degree 3)
Definition: cubic_hermite_spline.h:347
bool isApprox(const cubic_hermite_spline_t &other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
isApprox check if other and *this are approximately equals. Only two curves of the same class can be ...
Definition: cubic_hermite_spline.h:119
std::size_t numIntervals() const
Get number of intervals (subsplines) contained in the trajectory.
Definition: cubic_hermite_spline.h:216
Definition: fwd.h:34
double Time
Definition: effector_spline.h:27
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
virtual Point derivate(const time_t t, const std::size_t order) const
Evaluate the derivative of order N of spline at time t.
Definition: cubic_hermite_spline.h:151
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
piecewise_bezier_t compute_derivate(const std::size_t order) const
Definition: cubic_hermite_spline.h:164
Definition: bezier_curve.h:32