cubic_hermite_spline.h
Go to the documentation of this file.
1 
8 #ifndef _CLASS_CUBICHERMITESPLINE
9 #define _CLASS_CUBICHERMITESPLINE
10 
11 #include <boost/serialization/utility.hpp> // To serialize std::pair
12 #include <iostream>
13 #include <stdexcept>
14 #include <vector>
15 
16 #include "MathDefs.h"
17 #include "bezier_curve.h"
18 #include "curve_abc.h"
19 #include "piecewise_curve.h"
20 
21 namespace ndcurves {
32 template <typename Time = double, typename Numeric = Time, bool Safe = false,
33  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
34 struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
35  typedef Point point_t;
36  typedef std::pair<Point, Point> pair_point_tangent_t;
37  typedef std::vector<pair_point_tangent_t,
38  Eigen::aligned_allocator<pair_point_tangent_t> >
40  typedef std::vector<Time> vector_time_t;
41  typedef Time time_t;
42  typedef Numeric num_t;
47  typedef typename bezier_t::t_point_t t_point_t;
50 
51  public:
56 
63  template <typename In>
64  cubic_hermite_spline(In PairsBegin, In PairsEnd,
65  const vector_time_t& time_control_points)
66  : size_(std::distance(PairsBegin, PairsEnd)), degree_(3) {
67  // Check size of pairs container.
68  if (Safe && size_ < 1) {
69  throw std::length_error(
70  "can not create cubic_hermite_spline, number of pairs is inferior to "
71  "2.");
72  }
73  // Set dimension according to size of points
74  dim_ = PairsBegin->first.size();
75  // Push all pairs in controlPoints
76  In it(PairsBegin);
77  for (; it != PairsEnd; ++it) {
78  if (Safe && (static_cast<size_t>(it->first.size()) != dim_ ||
79  static_cast<size_t>(it->second.size()) != dim_))
80  throw std::invalid_argument(
81  "All the control points and their derivatives must have the same "
82  "dimension.");
83  control_points_.push_back(*it);
84  }
85  // Set time
86  setTime(time_control_points);
87  }
88 
90  : dim_(other.dim_),
94  T_min_(other.T_min_),
95  T_max_(other.T_max_),
96  size_(other.size_),
97  degree_(other.degree_) {}
98 
101 
102  /*Operations*/
103  public:
108  virtual Point operator()(const time_t t) const {
109  check_conditions();
110  if (Safe & !(T_min_ <= t && t <= T_max_)) {
111  throw std::invalid_argument(
112  "can't evaluate cubic hermite spline, out of range");
113  }
114  if (size_ == 1) {
115  return control_points_.front().first;
116  } else {
117  const bezier_t bezier = buildCurrentBezier(t);
118  return bezier(t);
119  }
120  }
121 
131  bool isApprox(
132  const cubic_hermite_spline_t& other,
133  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
134  bool equal = ndcurves::isApprox<num_t>(T_min_, other.min()) &&
135  ndcurves::isApprox<num_t>(T_max_, other.max()) &&
136  dim_ == other.dim() && degree_ == other.degree() &&
137  size_ == other.size() &&
140  if (!equal) return false;
141  for (std::size_t i = 0; i < size_; ++i) {
142  if ((!control_points_[i].first.isApprox(other.control_points_[i].first,
143  prec)) ||
144  (!control_points_[i].second.isApprox(other.control_points_[i].second,
145  prec)))
146  return false;
147  }
148  return true;
149  }
150 
151  virtual bool isApprox(
152  const curve_abc_t* other,
153  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
154  const cubic_hermite_spline_t* other_cast =
155  dynamic_cast<const cubic_hermite_spline_t*>(other);
156  if (other_cast)
157  return isApprox(*other_cast, prec);
158  else
159  return false;
160  }
161 
162  virtual bool operator==(const cubic_hermite_spline_t& other) const {
163  return isApprox(other);
164  }
165 
166  virtual bool operator!=(const cubic_hermite_spline_t& other) const {
167  return !(*this == other);
168  }
169 
176  virtual Point derivate(const time_t t, const std::size_t order) const {
177  check_conditions();
178  if (Safe & !(T_min_ <= t && t <= T_max_)) {
179  throw std::invalid_argument(
180  "can't derivate cubic hermite spline, out of range");
181  }
182  if (size_ == 1) {
183  return control_points_.front().second;
184  } else {
185  const bezier_t bezier = buildCurrentBezier(t);
186  return bezier.derivate(t, order);
187  }
188  }
189 
190  piecewise_bezier_t compute_derivate(const std::size_t order) const {
191  piecewise_bezier_t res;
192  for (size_t i = 0; i < size_ - 1; ++i) {
193  const bezier_t curve = buildCurrentBezier(time_control_points_[i]);
194  res.add_curve(curve.compute_derivate(order));
195  }
196  return res;
197  }
198 
203  piecewise_bezier_t* compute_derivate_ptr(const std::size_t order) const {
204  return new piecewise_bezier_t(compute_derivate(order));
205  }
206 
213  void setTime(const vector_time_t& time_control_points) {
214  time_control_points_ = time_control_points;
215  T_min_ = time_control_points_.front();
216  T_max_ = time_control_points_.back();
217  if (time_control_points.size() != size()) {
218  throw std::length_error(
219  "size of time control points should be equal to number of control "
220  "points");
221  }
222  computeDurationSplines();
223  if (!checkDurationSplines()) {
224  throw std::invalid_argument(
225  "time_splines not monotonous, all spline duration should be superior "
226  "to 0");
227  }
228  }
229 
234 
239 
243  std::size_t size() const { return size_; }
244 
248  std::size_t numIntervals() const { return size() - 1; }
249 
250  private:
255  std::size_t findInterval(const time_t t) const {
256  // time before first control point time.
257  if (t <= time_control_points_[0]) {
258  return 0;
259  }
260  // time is after last control point time
261  if (t >= time_control_points_[size_ - 1]) {
262  return size_ - 2;
263  }
264  std::size_t left_id = 0;
265  std::size_t right_id = size_ - 1;
266  while (left_id <= right_id) {
267  const std::size_t middle_id = left_id + (right_id - left_id) / 2;
268  if (time_control_points_.at(middle_id) < t) {
269  left_id = middle_id + 1;
270  } else if (time_control_points_.at(middle_id) > t) {
271  right_id = middle_id - 1;
272  } else {
273  return middle_id;
274  }
275  }
276  return left_id - 1;
277  }
278 
286  bezier_t buildCurrentBezier(const time_t t) const {
287  size_t id_interval = findInterval(t);
288  const pair_point_tangent_t pair0 = control_points_.at(id_interval);
289  const pair_point_tangent_t pair1 = control_points_.at(id_interval + 1);
290  const Time& t0 = time_control_points_[id_interval];
291  const Time& t1 = time_control_points_[id_interval + 1];
292  t_point_t control_points;
293  control_points.reserve(4);
294  control_points.push_back(pair0.first);
295  control_points.push_back(pair0.first + pair0.second / 3. * (t1 - t0));
296  control_points.push_back(pair1.first - pair1.second / 3. * (t1 - t0));
297  control_points.push_back(pair1.first);
298  return bezier_t(control_points.begin(), control_points.end(), t0, t1);
299  }
300 
304  void check_conditions() const {
305  if (control_points_.size() == 0) {
306  throw std::runtime_error(
307  "Error in cubic hermite : there is no control points set / did you "
308  "use empty constructor ?");
309  } else if (dim_ == 0) {
310  throw std::runtime_error(
311  "Error in cubic hermite : Dimension of points is zero / did you use "
312  "empty constructor ?");
313  }
314  }
315 
321  void computeDurationSplines() {
322  duration_splines_.clear();
323  Time actual_time;
324  Time prev_time = *(time_control_points_.begin());
325  std::size_t i = 0;
326  for (i = 0; i < size() - 1; i++) {
327  actual_time = time_control_points_.at(i + 1);
328  duration_splines_.push_back(actual_time - prev_time);
329  prev_time = actual_time;
330  }
331  }
332 
336  bool checkDurationSplines() const {
337  std::size_t i = 0;
338  bool is_positive = true;
339  while (is_positive && i < duration_splines_.size()) {
340  is_positive = (duration_splines_.at(i) > 0.);
341  i++;
342  }
343  return is_positive;
344  }
345  /*Operations*/
346 
347  /*Helpers*/
348  public:
351  std::size_t virtual dim() const { return dim_; }
354  Time virtual min() const { return time_control_points_.front(); }
357  Time virtual max() const { return time_control_points_.back(); }
360  virtual std::size_t degree() const { return degree_; }
361  /*Helpers*/
362 
363  /*Attributes*/
365  std::size_t dim_;
381  /*const*/ Time T_min_;
384  /*const*/ Time T_max_;
386  std::size_t size_;
388  std::size_t degree_;
389  /*Attributes*/
390 
391  // Serialization of the class
393 
394  template <class Archive>
395  void serialize(Archive& ar, const unsigned int version) {
396  if (version) {
397  // Do something depending on version ?
398  }
399  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
400  ar& boost::serialization::make_nvp("dim", dim_);
401  ar& boost::serialization::make_nvp("control_points", control_points_);
402  ar& boost::serialization::make_nvp("time_control_points",
404  ar& boost::serialization::make_nvp("duration_splines", duration_splines_);
405  ar& boost::serialization::make_nvp("T_min", T_min_);
406  ar& boost::serialization::make_nvp("T_max", T_max_);
407  ar& boost::serialization::make_nvp("size", size_);
408  ar& boost::serialization::make_nvp("degree", degree_);
409  }
410 }; // End struct Cubic hermite spline
411 } // namespace ndcurves
412 
414  SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
416 #endif //_CLASS_CUBICHERMITESPLINE
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
#define SINGLE_ARG(...)
Definition: archive.hpp:23
class allowing to create a Bezier curve of dimension 1 <= n <= 3.
interface for a Curve of arbitrary dimension.
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
double Numeric
Definition: effector_spline.h:26
double Time
Definition: effector_spline.h:27
Definition: bernstein.h:20
class allowing to create a piecewise curve.
Definition: bezier_curve.h:31
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:38
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:206
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:308
Definition: cubic_hermite_spline.h:34
piecewise_bezier_t compute_derivate(const std::size_t order) const
Definition: cubic_hermite_spline.h:190
void serialize(Archive &ar, const unsigned int version)
Definition: cubic_hermite_spline.h:395
Time time_t
Definition: cubic_hermite_spline.h:41
piecewise_bezier_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: cubic_hermite_spline.h:203
virtual std::size_t degree() const
Get the degree of the curve.
Definition: cubic_hermite_spline.h:360
virtual Time max() const
Get the maximum time for which the curve is defined.
Definition: cubic_hermite_spline.h:357
Time T_max_
Definition: cubic_hermite_spline.h:384
bezier_t::t_point_t t_point_t
Definition: cubic_hermite_spline.h:47
bezier_curve< Time, Numeric, Safe, point_t > bezier_t
Definition: cubic_hermite_spline.h:46
std::size_t degree_
Degree (Cubic so degree 3)
Definition: cubic_hermite_spline.h:388
std::size_t dim_
Dim of curve.
Definition: cubic_hermite_spline.h:365
std::vector< pair_point_tangent_t, Eigen::aligned_allocator< pair_point_tangent_t > > t_pair_point_tangent_t
Definition: cubic_hermite_spline.h:39
cubic_hermite_spline(const cubic_hermite_spline &other)
Definition: cubic_hermite_spline.h:89
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:131
vector_time_t getTime()
Get vector of Time corresponding to Time for each control point.
Definition: cubic_hermite_spline.h:238
std::size_t size_
Number of control points (pairs).
Definition: cubic_hermite_spline.h:386
virtual Time min() const
Get the minimum time for which the curve is defined.
Definition: cubic_hermite_spline.h:354
std::pair< Point, Point > pair_point_tangent_t
Definition: cubic_hermite_spline.h:36
Numeric num_t
Definition: cubic_hermite_spline.h:42
Time T_min_
Definition: cubic_hermite_spline.h:381
void setTime(const vector_time_t &time_control_points)
Set time of each control point of cubic hermite spline. Set duration of each spline,...
Definition: cubic_hermite_spline.h:213
virtual Point operator()(const time_t t) const
Evaluation of the cubic hermite spline at time t.
Definition: cubic_hermite_spline.h:108
virtual std::size_t dim() const
Get dimension of curve.
Definition: cubic_hermite_spline.h:351
std::size_t size() const
Get number of control points contained in the trajectory.
Definition: cubic_hermite_spline.h:243
std::vector< Time > vector_time_t
Definition: cubic_hermite_spline.h:40
vector_time_t time_control_points_
Definition: cubic_hermite_spline.h:372
virtual bool operator!=(const cubic_hermite_spline_t &other) const
Definition: cubic_hermite_spline.h:166
t_pair_point_tangent_t control_points_
Vector of pair < Point, Tangent >.
Definition: cubic_hermite_spline.h:367
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:176
std::size_t numIntervals() const
Get number of intervals (subsplines) contained in the trajectory.
Definition: cubic_hermite_spline.h:248
vector_time_t duration_splines_
Definition: cubic_hermite_spline.h:378
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t &time_control_points)
Constructor.
Definition: cubic_hermite_spline.h:64
piecewise_curve< Time, Numeric, Safe, point_t, point_t, bezier_t > piecewise_bezier_t
Definition: cubic_hermite_spline.h:49
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: cubic_hermite_spline.h:43
friend class boost::serialization::access
Definition: cubic_hermite_spline.h:392
virtual ~cubic_hermite_spline()
Destructor.
Definition: cubic_hermite_spline.h:100
Point point_t
Definition: cubic_hermite_spline.h:35
cubic_hermite_spline()
Empty constructor. Curve obtained this way can not perform other class functions.
Definition: cubic_hermite_spline.h:55
virtual bool operator==(const cubic_hermite_spline_t &other) const
Definition: cubic_hermite_spline.h:162
cubic_hermite_spline< Time, Numeric, Safe, point_t > cubic_hermite_spline_t
Definition: cubic_hermite_spline.h:45
t_pair_point_tangent_t getControlPoints()
Get vector of pair (positition, derivative) corresponding to control points.
Definition: cubic_hermite_spline.h:233
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
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:37
Definition: piecewise_curve.h:37
void add_curve(const Curve &curve)
Definition: piecewise_curve.h:176