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