bezier_curve.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_BEZIERCURVE
10 #define _CLASS_BEZIERCURVE
11 
12 #include "curve_abc.h"
13 #include "bernstein.h"
14 #include "curve_constraint.h"
15 #include "piecewise_curve.h"
16 
17 #include "MathDefs.h"
18 
19 #include <vector>
20 #include <stdexcept>
21 
22 #include <iostream>
23 
24 namespace curves {
30 template <typename Time = double, typename Numeric = Time, bool Safe = false,
31  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
32 struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
33  typedef Point point_t;
34  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
35  typedef Eigen::Ref<const vector_x_t> vector_x_ref_t;
36  typedef Time time_t;
37  typedef Numeric num_t;
39  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
40  typedef typename t_point_t::const_iterator cit_point_t;
42  typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
46 
47  /* Constructors - destructors */
48  public:
51  bezier_curve() : dim_(0), T_min_(0), T_max_(0) {}
52 
61  template <typename In>
62  bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min = 0., const time_t T_max = 1.,
63  const time_t mult_T = 1.)
64  : dim_(PointsBegin->size()),
65  T_min_(T_min),
66  T_max_(T_max),
67  mult_T_(mult_T),
68  size_(std::distance(PointsBegin, PointsEnd)),
69  degree_(size_ - 1),
70  bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
71  {
72  if (bernstein_.size() != size_) {
73  throw std::invalid_argument("Invalid size of polynomial");
74  }
75  In it(PointsBegin);
76  if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
77  throw std::invalid_argument("can't create bezier min bound is higher than max bound");
78  }
79  for (; it != PointsEnd; ++it) {
80  if(Safe && static_cast<size_t>(it->size()) != dim_)
81  throw std::invalid_argument("All the control points must have the same dimension.");
82  control_points_.push_back(*it);
83  }
84  }
85 
96  template <typename In>
97  bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T_min = 0.,
98  const time_t T_max = 1., const time_t mult_T = 1.)
99  : dim_(PointsBegin->size()),
100  T_min_(T_min),
101  T_max_(T_max),
102  mult_T_(mult_T),
103  size_(std::distance(PointsBegin, PointsEnd) + 4),
104  degree_(size_ - 1),
105  bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
106  {
107  if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
108  throw std::invalid_argument("can't create bezier min bound is higher than max bound");
109  }
110  t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
111  for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) {
112  if(Safe && static_cast<size_t>(cit->size()) != dim_)
113  throw std::invalid_argument("All the control points must have the same dimension.");
114  control_points_.push_back(*cit);
115  }
116  }
117 
119  : dim_(other.dim_),
120  T_min_(other.T_min_),
121  T_max_(other.T_max_),
122  mult_T_(other.mult_T_),
123  size_(other.size_),
124  degree_(other.degree_),
125  bernstein_(other.bernstein_),
126  control_points_(other.control_points_) {}
127 
130  // NOTHING
131  }
132 
133  /*Operations*/
137  virtual point_t operator()(const time_t t) const {
138  check_conditions();
139  if (Safe & !(T_min_ <= t && t <= T_max_)) {
140  throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
141  }
142  if (size_ == 1) {
143  return mult_T_ * control_points_[0];
144  } else {
145  return evalHorner(t);
146  }
147  }
148 
157  bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
158  bool equal = curves::isApprox<num_t>(T_min_, other.min()) && curves::isApprox<num_t>(T_max_, other.max()) &&
159  dim_ == other.dim() && degree_ == other.degree() && size_ == other.size_ &&
160  curves::isApprox<Numeric>(mult_T_, other.mult_T_) && bernstein_ == other.bernstein_;
161  if (!equal) return false;
162  for (size_t i = 0; i < size_; ++i) {
163  if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec)) return false;
164  }
165  return true;
166  }
167 
168  virtual bool isApprox(const curve_abc_t* other,
169  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
170  const bezier_curve_t* other_cast = dynamic_cast<const bezier_curve_t*>(other);
171  if (other_cast)
172  return isApprox(*other_cast, prec);
173  else
174  return false;
175  }
176 
177  virtual bool operator==(const bezier_curve_t& other) const { return isApprox(other); }
178 
179  virtual bool operator!=(const bezier_curve_t& other) const { return !(*this == other); }
180 
185  bezier_curve_t compute_derivate(const std::size_t order) const {
186  check_conditions();
187  if (order == 0) {
188  return *this;
189  }
190  t_point_t derived_wp;
191  for (typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end() - 1; ++pit) {
192  derived_wp.push_back((num_t)degree_ * (*(pit + 1) - (*pit)));
193  }
194  if (derived_wp.empty()) {
195  derived_wp.push_back(point_t::Zero(dim_));
196  }
197  bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), T_min_, T_max_, mult_T_ * (1. / (T_max_ - T_min_)));
198  return deriv.compute_derivate(order - 1);
199  }
200 
204  bezier_curve_t* compute_derivate_ptr(const std::size_t order) const {
205  return new bezier_curve_t(compute_derivate(order));
206  }
207 
213  bezier_curve_t compute_primitive(const std::size_t order) const {
214  check_conditions();
215  if (order == 0) {
216  return *this;
217  }
218  num_t new_degree = (num_t)(degree_ + 1);
219  t_point_t n_wp;
220  point_t current_sum = point_t::Zero(dim_);
221  // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
222  // then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
223  n_wp.push_back(current_sum);
224  for (typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit) {
225  current_sum += *pit;
226  n_wp.push_back(current_sum / new_degree);
227  }
228  bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_, mult_T_ * (T_max_ - T_min_));
229  return integ.compute_primitive(order - 1);
230  }
231 
239  virtual point_t derivate(const time_t t, const std::size_t order) const { return compute_derivate(order)(t); }
240 
249  point_t evalBernstein(const Numeric t) const {
250  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
251  point_t res = point_t::Zero(dim_);
252  typename t_point_t::const_iterator control_points_it = control_points_.begin();
253  for (typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin(); cit != bernstein_.end();
254  ++cit, ++control_points_it) {
255  res += cit->operator()(u) * (*control_points_it);
256  }
257  return res * mult_T_;
258  }
259 
272  point_t evalHorner(const Numeric t) const {
273  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
274  typename t_point_t::const_iterator control_points_it = control_points_.begin();
275  Numeric u_op, bc, tn;
276  u_op = 1.0 - u;
277  bc = 1;
278  tn = 1;
279  point_t tmp = (*control_points_it) * u_op;
280  ++control_points_it;
281  for (unsigned int i = 1; i < degree_; i++, ++control_points_it) {
282  tn = tn * u;
283  bc = bc * ((num_t)(degree_ - i + 1)) / i;
284  tmp = (tmp + tn * bc * (*control_points_it)) * u_op;
285  }
286  return (tmp + tn * u * (*control_points_it)) * mult_T_;
287  }
288 
289  const t_point_t& waypoints() const { return control_points_; }
290 
291  const point_t waypointAtIndex(const std::size_t index) const {
292  point_t waypoint;
293  if (index < control_points_.size()) {
294  waypoint = control_points_[index];
295  }
296  return waypoint;
297  }
298 
305  point_t evalDeCasteljau(const Numeric t) const {
306  // normalize time :
307  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
308  t_point_t pts = deCasteljauReduction(waypoints(), u);
309  while (pts.size() > 1) {
310  pts = deCasteljauReduction(pts, u);
311  }
312  return pts[0] * mult_T_;
313  }
314 
315  t_point_t deCasteljauReduction(const Numeric t) const {
316  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
317  return deCasteljauReduction(waypoints(), u);
318  }
319 
329  t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const {
330  if (u < 0 || u > 1) {
331  throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
332  }
333  if (pts.size() == 1) {
334  return pts;
335  }
336 
337  t_point_t new_pts;
338  for (cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
339  new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
340  }
341  return new_pts;
342  }
343 
348  std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) const {
349  check_conditions();
350  if (fabs(t - T_max_) < MARGIN) {
351  throw std::runtime_error("can't split curve, interval range is equal to original curve");
352  }
353  t_point_t wps_first(size_), wps_second(size_);
354  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
355  t_point_t casteljau_pts = waypoints();
356  wps_first[0] = casteljau_pts.front();
357  wps_second[degree_] = casteljau_pts.back();
358  size_t id = 1;
359  while (casteljau_pts.size() > 1) {
360  casteljau_pts = deCasteljauReduction(casteljau_pts, u);
361  wps_first[id] = casteljau_pts.front();
362  wps_second[degree_ - id] = casteljau_pts.back();
363  ++id;
364  }
365  bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T_min_, t, mult_T_);
366  bezier_curve_t c_second(wps_second.begin(), wps_second.end(), t, T_max_, mult_T_);
367  return std::make_pair(c_first, c_second);
368  }
369 
375  piecewise_curve_t split(const vector_x_t& times) const {
376  std::vector<bezier_curve_t> curves;
377  bezier_curve_t current = *this;
378  for (int i = 0; i < times.rows(); ++i) {
379  std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
380  curves.push_back(pairsplit.first);
381  current = pairsplit.second;
382  }
383  curves.push_back(current);
384  piecewise_curve_t res;
385  for (typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end(); ++cit) {
386  typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
387  res.add_curve_ptr(ptr);
388  }
389  return res;
390  }
391 
398  bezier_curve_t extract(const Numeric t1, const Numeric t2) {
399  if (t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_) {
400  throw std::out_of_range("In Extract curve : times out of bounds");
401  }
402  if (fabs(t1 - T_min_) < MARGIN && fabs(t2 - T_max_) < MARGIN) // t1=T_min and t2=T_max
403  {
404  return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
405  }
406  if (fabs(t1 - T_min_) < MARGIN) // t1=T_min
407  {
408  return split(t2).first;
409  }
410  if (fabs(t2 - T_max_) < MARGIN) // t2=T_max
411  {
412  return split(t1).second;
413  }
414  std::pair<bezier_curve_t, bezier_curve_t> c_split = this->split(t1);
415  return c_split.second.split(t2).first;
416  }
417 
418  private:
423  template <typename In>
424  t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) {
425  t_point_t res;
426  num_t T = T_max_ - T_min_;
427  num_t T_square = T * T;
428  point_t P0, P1, P2, P_n_2, P_n_1, PN;
429  P0 = *PointsBegin;
430  PN = *(PointsEnd - 1);
431  P1 = P0 + constraints.init_vel * T / (num_t)degree_;
432  P_n_1 = PN - constraints.end_vel * T / (num_t)degree_;
433  P2 = constraints.init_acc * T_square / (num_t)(degree_ * (degree_ - 1)) + 2 * P1 - P0;
434  P_n_2 = constraints.end_acc * T_square / (num_t)(degree_ * (degree_ - 1)) + 2 * P_n_1 - PN;
435  res.push_back(P0);
436  res.push_back(P1);
437  res.push_back(P2);
438  for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) {
439  res.push_back(*it);
440  }
441  res.push_back(P_n_2);
442  res.push_back(P_n_1);
443  res.push_back(PN);
444  return res;
445  }
446 
447  void check_conditions() const {
448  if (control_points_.size() == 0) {
449  throw std::runtime_error(
450  "Error in bezier curve : there is no control points set / did you use empty constructor ?");
451  } else if (dim_ == 0) {
452  throw std::runtime_error(
453  "Error in bezier curve : Dimension of points is zero / did you use empty constructor ?");
454  }
455  }
456  /*Operations*/
457 
458  public:
459  /*Helpers*/
462  std::size_t virtual dim() const { return dim_; };
465  virtual time_t min() const { return T_min_; }
468  virtual time_t max() const { return T_max_; }
471  virtual std::size_t degree() const { return degree_; }
472  /*Helpers*/
473 
474  /* Attributes */
476  std::size_t dim_;
478  /*const*/ time_t T_min_;
480  /*const*/ time_t T_max_;
481  /*const*/ time_t mult_T_;
482  /*const*/ std::size_t size_;
483  /*const*/ std::size_t degree_;
484  /*const*/ std::vector<Bern<Numeric> > bernstein_;
485  /*const*/ t_point_t control_points_;
486  static const double MARGIN;
487  /* Attributes */
488 
489  static bezier_curve_t zero(const std::size_t dim, const time_t T = 1.) {
490  std::vector<point_t> ts;
491  ts.push_back(point_t::Zero(dim));
492  return bezier_curve_t(ts.begin(), ts.end(), 0., T);
493  }
494 
495  // Serialization of the class
496  friend class boost::serialization::access;
497 
498  template <class Archive>
499  void serialize(Archive& ar, const unsigned int version) {
500  if (version) {
501  // Do something depending on version ?
502  }
503  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
504  ar& boost::serialization::make_nvp("dim", dim_);
505  ar& boost::serialization::make_nvp("T_min", T_min_);
506  ar& boost::serialization::make_nvp("T_max", T_max_);
507  ar& boost::serialization::make_nvp("mult_T", mult_T_);
508  ar& boost::serialization::make_nvp("size", size_);
509  ar& boost::serialization::make_nvp("degree", degree_);
510  ar& boost::serialization::make_nvp("bernstein", bernstein_);
511  ar& boost::serialization::make_nvp("control_points", control_points_);
512  }
513 }; // End struct bezier_curve
514 
515 template <typename Time, typename Numeric, bool Safe, typename Point>
517 
518 } // namespace curves
519 
520 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
522 
523 #endif //_CLASS_BEZIERCURVE
Point point_t
Definition: bezier_curve.h:33
virtual time_t max() const =0
Get the maximum time for which the curve is defined.
Eigen::Ref< const vector_x_t > vector_x_ref_t
Definition: bezier_curve.h:35
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: bezier_curve.h:168
bezier_curve()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: bezier_curve.h:51
void add_curve_ptr(const curve_ptr_t &cf)
Add a new curve to piecewise curve, which should be defined in where is equal to of the actual pie...
Definition: piecewise_curve.h:153
virtual bool operator!=(const bezier_curve_t &other) const
Definition: bezier_curve.h:179
double Numeric
Definition: effector_spline.h:26
point_t end_vel
Definition: curve_constraint.h:45
bezier_curve_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: bezier_curve.h:204
bool isApprox(const bezier_curve_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: bezier_curve.h:157
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: bezier_curve.h:44
bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
Constructor. Given the first and last point of a control points set, create the bezier curve...
Definition: bezier_curve.h:62
point_t init_acc
Definition: curve_constraint.h:43
virtual time_t min() const =0
Get the minimum time for which the curve is defined.
Definition: curve_constraint.h:21
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
interface for a Curve of arbitrary dimension.
Definition: bernstein.h:20
brief Computes all Bernstein polynomes for a certain degree std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Definition: bernstein.h:88
point_t init_vel
Definition: curve_constraint.h:42
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:40
piecewise_curve< Time, Numeric, Safe, point_t, point_t, bezier_curve_t > piecewise_curve_t
Definition: bezier_curve.h:43
point_t end_acc
Definition: curve_constraint.h:46
~bezier_curve()
Destructor.
Definition: bezier_curve.h:129
std::vector< bezier_curve< Numeric, Numeric, true, linear_variable< Numeric > > > split(const problem_definition< Point, Numeric > &pDef, problem_data< Point, Numeric > &pData)
Definition: details.h:208
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:39
bezier_curve< Time, Numeric, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:41
virtual std::size_t degree() const =0
Get the degree of the curve.
virtual std::size_t dim() const =0
Get dimension of curve.
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: bezier_curve.h:45
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:38
boost::shared_ptr< bezier_curve_t > bezier_curve_ptr_t
Definition: bezier_curve.h:42
void serialize(Archive &ar, const unsigned int version)
Definition: curve_abc.h:139
bezier_curve_t compute_primitive(const std::size_t order) const
Compute the primitive of the curve at order N. Computes the primitive at order N of bezier curve of p...
Definition: bezier_curve.h:213
virtual point_t operator()(const time_t t) const
Evaluation of the bezier curve at time t.
Definition: bezier_curve.h:137
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:40
struct to define constraints on start / end velocities and acceleration on a curve ...
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:41
Numeric num_t
Definition: bezier_curve.h:37
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t &constraints, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
Constructor with constraints. This constructor will add 4 points (2 after the first one...
Definition: bezier_curve.h:97
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
bezier_curve(const bezier_curve &other)
Definition: bezier_curve.h:118
Time time_t
Definition: bezier_curve.h:36
Definition: fwd.h:34
double Time
Definition: effector_spline.h:27
point_t evalBernstein(const Numeric t) const
Evaluate all Bernstein polynomes for a certain degree. A bezier curve with N control points is repres...
Definition: bezier_curve.h:249
Definition: fwd.h:49
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
Definition: bezier_curve.h:32
virtual bool operator==(const bezier_curve_t &other) const
Definition: bezier_curve.h:177
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: bezier_curve.h:34