bezier_curve.h
Go to the documentation of this file.
1 
9 #ifndef _CLASS_BEZIERCURVE
10 #define _CLASS_BEZIERCURVE
11 
12 #include <iostream>
13 #include <stdexcept>
14 #include <vector>
15 
16 #include "MathDefs.h"
17 #include "bernstein.h"
18 #include "cross_implementation.h"
19 #include "curve_abc.h"
20 #include "curve_constraint.h"
21 #include "piecewise_curve.h"
22 
23 namespace ndcurves {
29 template <typename Time = double, typename Numeric = Time, bool Safe = false,
30  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
31 struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
32  typedef Point point_t;
33  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
34  typedef Eigen::Ref<const vector_x_t> vector_x_ref_t;
35  typedef Time time_t;
36  typedef Numeric num_t;
38  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
39  typedef typename t_point_t::const_iterator cit_point_t;
41  typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
46 
47  /* Constructors - destructors */
48  public:
52  bezier_curve() : dim_(0), T_min_(0), T_max_(0) {}
53 
63  template <typename In>
64  bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min = 0.,
65  const time_t T_max = 1., const time_t mult_T = 1.)
66  : dim_(PointsBegin->size()),
67  T_min_(T_min),
68  T_max_(T_max),
69  mult_T_(mult_T),
70  size_(std::distance(PointsBegin, PointsEnd)),
71  degree_(size_ - 1),
72  bernstein_(ndcurves::makeBernstein<num_t>((unsigned int)degree_)) {
73  if (bernstein_.size() != size_) {
74  throw std::invalid_argument("Invalid size of polynomial");
75  }
76  In it(PointsBegin);
77  if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
78  throw std::invalid_argument(
79  "can't create bezier min bound is higher than max bound");
80  }
81  for (; it != PointsEnd; ++it) {
82  if (Safe && static_cast<size_t>(it->size()) != dim_)
83  throw std::invalid_argument(
84  "All the control points must have the same dimension.");
85  control_points_.push_back(*it);
86  }
87  }
88 
101  template <typename In>
102  bezier_curve(In PointsBegin, In PointsEnd,
103  const curve_constraints_t& constraints, const time_t T_min = 0.,
104  const time_t T_max = 1., const time_t mult_T = 1.)
105  : dim_(PointsBegin->size()),
106  T_min_(T_min),
107  T_max_(T_max),
108  mult_T_(mult_T),
109  size_(std::distance(PointsBegin, PointsEnd) + 4),
110  degree_(size_ - 1),
111  bernstein_(ndcurves::makeBernstein<num_t>((unsigned int)degree_)) {
112  if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
113  throw std::invalid_argument(
114  "can't create bezier min bound is higher than max bound");
115  }
116  t_point_t updatedList =
117  add_constraints<In>(PointsBegin, PointsEnd, constraints);
118  for (cit_point_t cit = updatedList.begin(); cit != updatedList.end();
119  ++cit) {
120  if (Safe && static_cast<size_t>(cit->size()) != dim_)
121  throw std::invalid_argument(
122  "All the control points must have the same dimension.");
123  control_points_.push_back(*cit);
124  }
125  }
126 
128  : dim_(other.dim_),
129  T_min_(other.T_min_),
130  T_max_(other.T_max_),
131  mult_T_(other.mult_T_),
132  size_(other.size_),
133  degree_(other.degree_),
134  bernstein_(other.bernstein_),
135  control_points_(other.control_points_) {}
136 
138  virtual ~bezier_curve() {}
139 
140  /*Operations*/
144  virtual point_t operator()(const time_t t) const {
145  check_conditions();
146  if (Safe & !(T_min_ <= t && t <= T_max_)) {
147  throw std::invalid_argument(
148  "can't evaluate bezier curve, time t is out of range"); // TODO
149  }
150  if (size_ == 1) {
151  return mult_T_ * control_points_[0];
152  } else {
153  return evalHorner(t);
154  }
155  }
156 
166  bool isApprox(
167  const bezier_curve_t& other,
168  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
169  bool equal = ndcurves::isApprox<num_t>(T_min_, other.min()) &&
170  ndcurves::isApprox<num_t>(T_max_, other.max()) &&
171  dim_ == other.dim() && degree_ == other.degree() &&
172  size_ == other.size_ &&
173  ndcurves::isApprox<Numeric>(mult_T_, other.mult_T_) &&
174  bernstein_ == other.bernstein_;
175  if (!equal) return false;
176  for (size_t i = 0; i < size_; ++i) {
177  if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec))
178  return false;
179  }
180  return true;
181  }
182 
183  virtual bool isApprox(
184  const curve_abc_t* other,
185  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
186  const bezier_curve_t* other_cast =
187  dynamic_cast<const bezier_curve_t*>(other);
188  if (other_cast)
189  return isApprox(*other_cast, prec);
190  else
191  return false;
192  }
193 
194  virtual bool operator==(const bezier_curve_t& other) const {
195  return isApprox(other);
196  }
197 
198  virtual bool operator!=(const bezier_curve_t& other) const {
199  return !(*this == other);
200  }
201 
206  bezier_curve_t compute_derivate(const std::size_t order) const {
207  check_conditions();
208  if (order == 0) {
209  return *this;
210  }
211  t_point_t derived_wp;
212  for (typename t_point_t::const_iterator pit = control_points_.begin();
213  pit != control_points_.end() - 1; ++pit) {
214  derived_wp.push_back((num_t)degree_ * (*(pit + 1) - (*pit)));
215  }
216  if (derived_wp.empty()) {
217  derived_wp.push_back(point_t::Zero(dim_));
218  }
219  bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), T_min_, T_max_,
220  mult_T_ * (1. / (T_max_ - T_min_)));
221  return deriv.compute_derivate(order - 1);
222  }
223 
228  bezier_curve_t* compute_derivate_ptr(const std::size_t order) const {
229  return new bezier_curve_t(compute_derivate(order));
230  }
231 
238  bezier_curve_t compute_primitive(const std::size_t order,
239  const point_t& init) const {
240  check_conditions();
241  if (order == 0) {
242  return *this;
243  }
244  num_t new_degree_inv = 1. / ((num_t)(degree_ + 1));
245  t_point_t n_wp;
246  point_t current_sum(init);
247  n_wp.push_back(current_sum);
248  for (typename t_point_t::const_iterator pit = control_points_.begin();
249  pit != control_points_.end(); ++pit) {
250  current_sum += *pit;
251  n_wp.push_back(current_sum * new_degree_inv);
252  }
253  bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_,
254  mult_T_ * (T_max_ - T_min_));
255  return integ.compute_primitive(order - 1);
256  }
257 
258  bezier_curve_t compute_primitive(const std::size_t order) const {
259  return compute_primitive(order, point_t::Zero(dim_));
260  }
261 
262  bezier_curve_t* compute_primitive_ptr(const std::size_t order,
263  const point_t& init) const {
264  return new bezier_curve_t(compute_primitive(order, init));
265  }
266 
272  bezier_curve_t elevate(const std::size_t order) const {
273  t_point_t new_waypoints = control_points_, temp_waypoints;
274  for (std::size_t i = 1; i <= order; ++i) {
275  num_t new_degree_inv = 1. / ((num_t)(degree_ + i));
276  temp_waypoints.push_back(*new_waypoints.begin());
277  num_t idx_deg_inv = 0.;
278  for (typename t_point_t::const_iterator pit = new_waypoints.begin() + 1;
279  pit != new_waypoints.end(); ++pit) {
280  idx_deg_inv += new_degree_inv;
281  temp_waypoints.push_back(idx_deg_inv * (*(pit - 1)) +
282  (1 - idx_deg_inv) * (*pit));
283  }
284  temp_waypoints.push_back(*(new_waypoints.end() - 1));
285  new_waypoints = temp_waypoints;
286  temp_waypoints.clear();
287  }
288  return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), T_min_,
289  T_max_, mult_T_);
290  }
291 
296  void elevate_self(const std::size_t order) {
297  if (order > 0) (*this) = elevate(order);
298  }
299 
308  virtual point_t derivate(const time_t t, const std::size_t order) const {
309  return compute_derivate(order)(t);
310  }
311 
321  point_t evalBernstein(const Numeric t) const {
322  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
323  point_t res = point_t::Zero(dim_);
324  typename t_point_t::const_iterator control_points_it =
325  control_points_.begin();
326  for (typename std::vector<Bern<Numeric> >::const_iterator cit =
327  bernstein_.begin();
328  cit != bernstein_.end(); ++cit, ++control_points_it) {
329  res += cit->operator()(u) * (*control_points_it);
330  }
331  return res * mult_T_;
332  }
333 
346  point_t evalHorner(const Numeric t) const {
347  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
348  typename t_point_t::const_iterator control_points_it =
349  control_points_.begin();
350  Numeric u_op, bc, tn;
351  u_op = 1.0 - u;
352  bc = 1;
353  tn = 1;
354  point_t tmp = (*control_points_it) * u_op;
355  ++control_points_it;
356  for (unsigned int i = 1; i < degree_; i++, ++control_points_it) {
357  tn = tn * u;
358  bc = bc * ((num_t)(degree_ - i + 1)) / i;
359  tmp = (tmp + tn * bc * (*control_points_it)) * u_op;
360  }
361  return (tmp + tn * u * (*control_points_it)) * mult_T_;
362  }
363 
364  const t_point_t& waypoints() const { return control_points_; }
365 
366  const point_t waypointAtIndex(const std::size_t index) const {
367  point_t waypoint;
368  if (index < control_points_.size()) {
369  waypoint = control_points_[index];
370  }
371  return waypoint;
372  }
373 
382  point_t evalDeCasteljau(const Numeric t) const {
383  // normalize time :
384  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
385  t_point_t pts = deCasteljauReduction(waypoints(), u);
386  while (pts.size() > 1) {
387  pts = deCasteljauReduction(pts, u);
388  }
389  return pts[0] * mult_T_;
390  }
391 
392  t_point_t deCasteljauReduction(const Numeric t) const {
393  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
394  return deCasteljauReduction(waypoints(), u);
395  }
396 
406  t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const {
407  if (u < 0 || u > 1) {
408  throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
409  }
410  if (pts.size() == 1) {
411  return pts;
412  }
413 
414  t_point_t new_pts;
415  for (cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
416  new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
417  }
418  return new_pts;
419  }
420 
425  std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) const {
426  check_conditions();
427  if (fabs(t - T_max_) < MARGIN) {
428  throw std::runtime_error(
429  "can't split curve, interval range is equal to original curve");
430  }
431  t_point_t wps_first(size_), wps_second(size_);
432  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
433  t_point_t casteljau_pts = waypoints();
434  wps_first[0] = casteljau_pts.front();
435  wps_second[degree_] = casteljau_pts.back();
436  size_t id = 1;
437  while (casteljau_pts.size() > 1) {
438  casteljau_pts = deCasteljauReduction(casteljau_pts, u);
439  wps_first[id] = casteljau_pts.front();
440  wps_second[degree_ - id] = casteljau_pts.back();
441  ++id;
442  }
443  bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T_min_, t,
444  mult_T_);
445  bezier_curve_t c_second(wps_second.begin(), wps_second.end(), t, T_max_,
446  mult_T_);
447  return std::make_pair(c_first, c_second);
448  }
449 
455  piecewise_curve_t split(const vector_x_t& times) const {
456  std::vector<bezier_curve_t> curves;
457  bezier_curve_t current = *this;
458  for (int i = 0; i < times.rows(); ++i) {
459  std::pair<bezier_curve_t, bezier_curve_t> pairsplit =
460  current.split(times[i]);
461  curves.push_back(pairsplit.first);
462  current = pairsplit.second;
463  }
464  curves.push_back(current);
465  piecewise_curve_t res;
466  for (typename std::vector<bezier_curve_t>::const_iterator cit =
467  curves.begin();
468  cit != curves.end(); ++cit) {
469  typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
470  res.add_curve_ptr(ptr);
471  }
472  return res;
473  }
474 
483  bezier_curve_t extract(const Numeric t1, const Numeric t2) {
484  if (t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_) {
485  throw std::out_of_range("In Extract curve : times out of bounds");
486  }
487  if (fabs(t1 - T_min_) < MARGIN &&
488  fabs(t2 - T_max_) < MARGIN) // t1=T_min and t2=T_max
489  {
490  return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_,
491  T_max_, mult_T_);
492  }
493  if (fabs(t1 - T_min_) < MARGIN) // t1=T_min
494  {
495  return split(t2).first;
496  }
497  if (fabs(t2 - T_max_) < MARGIN) // t2=T_max
498  {
499  return split(t1).second;
500  }
501  std::pair<bezier_curve_t, bezier_curve_t> c_split = this->split(t1);
502  return c_split.second.split(t2).first;
503  }
504 
515  bezier_curve_t cross(const bezier_curve_t& g) const {
516  // See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form
517  // and http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
518  assert_operator_compatible(g);
519  if (dim() != 3)
520  throw std::invalid_argument(
521  "Can't perform cross product on Bezier curves with dimensions != 3 ");
522  int m = (int)(degree());
523  int n = (int)(g.degree());
524  unsigned int mj, n_ij, mn_i;
525  t_point_t new_waypoints;
526  for (int i = 0; i <= m + n; ++i) {
527  bezier_curve_t::point_t current_point =
528  bezier_curve_t::point_t::Zero(dim());
529  for (int j = std::max(0, i - n); j <= std::min(m, i); ++j) {
530  mj = bin(m, j);
531  n_ij = bin(n, i - j);
532  mn_i = bin(m + n, i);
533  num_t mul = num_t(mj * n_ij) / num_t(mn_i);
534  current_point +=
535  mul * ndcurves::cross(waypointAtIndex(j), g.waypointAtIndex(i - j));
536  }
537  new_waypoints.push_back(current_point);
538  }
539  return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), min(),
540  max(), mult_T_ * g.mult_T_);
541  }
542 
551  bezier_curve_t cross(const bezier_curve_t::point_t& point) const {
552  // See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form
553  // and http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
554  if (dim() != 3)
555  throw std::invalid_argument(
556  "Can't perform cross product on Bezier curves with dimensions != 3 ");
557  t_point_t new_waypoints;
558  for (typename t_point_t::const_iterator cit = waypoints().begin();
559  cit != waypoints().end(); ++cit) {
560  new_waypoints.push_back(ndcurves::cross(*cit, point));
561  }
562  return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), min(),
563  max(), mult_T_);
564  }
565 
566  bezier_curve_t& operator+=(const bezier_curve_t& other) {
567  assert_operator_compatible(other);
568  bezier_curve_t other_elevated =
569  other *
570  (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier
571  if (other.degree() > degree()) {
572  elevate_self(other.degree() - degree());
573  } else if (other_elevated.degree() < degree()) {
574  other_elevated.elevate_self(degree() - other_elevated.degree());
575  }
576  typename t_point_t::const_iterator otherit =
577  other_elevated.control_points_.begin();
578  for (typename t_point_t::iterator it = control_points_.begin();
579  it != control_points_.end(); ++it, ++otherit) {
580  (*it) += (*otherit);
581  }
582  return *this;
583  }
584 
585  bezier_curve_t& operator-=(const bezier_curve_t& other) {
586  assert_operator_compatible(other);
587  bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_);
588  if (other.degree() > degree()) {
589  elevate_self(other.degree() - degree());
590  } else if (other_elevated.degree() < degree()) {
591  other_elevated.elevate_self(degree() - other_elevated.degree());
592  }
593  typename t_point_t::const_iterator otherit =
594  other_elevated.control_points_.begin();
595  for (typename t_point_t::iterator it = control_points_.begin();
596  it != control_points_.end(); ++it, ++otherit) {
597  (*it) -= (*otherit);
598  }
599  return *this;
600  }
601 
602  bezier_curve_t& operator+=(const bezier_curve_t::point_t& point) {
603  for (typename t_point_t::iterator it = control_points_.begin();
604  it != control_points_.end(); ++it) {
605  (*it) += point;
606  }
607  return *this;
608  }
609 
610  bezier_curve_t& operator-=(const bezier_curve_t::point_t& point) {
611  for (typename t_point_t::iterator it = control_points_.begin();
612  it != control_points_.end(); ++it) {
613  (*it) -= point;
614  }
615  return *this;
616  }
617 
618  bezier_curve_t& operator/=(const double d) {
619  for (typename t_point_t::iterator it = control_points_.begin();
620  it != control_points_.end(); ++it) {
621  (*it) /= d;
622  }
623  return *this;
624  }
625 
626  bezier_curve_t& operator*=(const double d) {
627  for (typename t_point_t::iterator it = control_points_.begin();
628  it != control_points_.end(); ++it) {
629  (*it) *= d;
630  }
631  return *this;
632  }
633 
634  private:
639  template <typename In>
640  t_point_t add_constraints(In PointsBegin, In PointsEnd,
641  const curve_constraints_t& constraints) {
642  t_point_t res;
643  num_t T = T_max_ - T_min_;
644  num_t T_square = T * T;
645  point_t P0, P1, P2, P_n_2, P_n_1, PN;
646  P0 = *PointsBegin;
647  PN = *(PointsEnd - 1);
648  P1 = P0 + constraints.init_vel * T / (num_t)degree_;
649  P_n_1 = PN - constraints.end_vel * T / (num_t)degree_;
650  P2 = constraints.init_acc * T_square / (num_t)(degree_ * (degree_ - 1)) +
651  2 * P1 - P0;
652  P_n_2 = constraints.end_acc * T_square / (num_t)(degree_ * (degree_ - 1)) +
653  2 * P_n_1 - PN;
654  res.push_back(P0);
655  res.push_back(P1);
656  res.push_back(P2);
657  for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) {
658  res.push_back(*it);
659  }
660  res.push_back(P_n_2);
661  res.push_back(P_n_1);
662  res.push_back(PN);
663  return res;
664  }
665 
666  void check_conditions() const {
667  if (control_points_.size() == 0) {
668  throw std::runtime_error(
669  "Error in bezier curve : there is no control points set / did you "
670  "use empty constructor ?");
671  } else if (dim_ == 0) {
672  throw std::runtime_error(
673  "Error in bezier curve : Dimension of points is zero / did you use "
674  "empty constructor ?");
675  }
676  }
677 
678  void assert_operator_compatible(const bezier_curve_t& other) const {
679  if ((fabs(min() - other.min()) > MARGIN) ||
680  (fabs(max() - other.max()) > MARGIN)) {
681  throw std::invalid_argument(
682  "Can't perform base operation (+ - ) on two Bezier curves with "
683  "different time ranges");
684  }
685  }
686 
687  /*Operations*/
688 
689  public:
690  /*Helpers*/
693  std::size_t virtual dim() const { return dim_; };
696  virtual time_t min() const { return T_min_; }
699  virtual time_t max() const { return T_max_; }
702  virtual std::size_t degree() const { return degree_; }
703  /*Helpers*/
704 
705  /* Attributes */
707  std::size_t dim_;
710  /*const*/ time_t T_min_;
713  /*const*/ time_t T_max_;
714  /*const*/ time_t mult_T_;
715  /*const*/ std::size_t size_;
716  /*const*/ std::size_t degree_;
717  /*const*/ std::vector<Bern<Numeric> > bernstein_;
718  /*const*/ t_point_t control_points_;
719  /* Attributes */
720 
721  static bezier_curve_t zero(const std::size_t dim, const time_t T = 1.) {
722  std::vector<point_t> ts;
723  ts.push_back(point_t::Zero(dim));
724  return bezier_curve_t(ts.begin(), ts.end(), 0., T);
725  }
726 
727  // Serialization of the class
728  friend class boost::serialization::access;
729 
730  template <class Archive>
731  void serialize(Archive& ar, const unsigned int version) {
732  if (version) {
733  // Do something depending on version ?
734  }
735  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
736  ar& boost::serialization::make_nvp("dim", dim_);
737  ar& boost::serialization::make_nvp("T_min", T_min_);
738  ar& boost::serialization::make_nvp("T_max", T_max_);
739  ar& boost::serialization::make_nvp("mult_T", mult_T_);
740  ar& boost::serialization::make_nvp("size", size_);
741  ar& boost::serialization::make_nvp("degree", degree_);
742  ar& boost::serialization::make_nvp("bernstein", bernstein_);
743  ar& boost::serialization::make_nvp("control_points", control_points_);
744  }
745 }; // End struct bezier_curve
746 
747 template <typename T, typename N, bool S, typename P>
749  const bezier_curve<T, N, S, P>& p2) {
750  bezier_curve<T, N, S, P> res(p1);
751  return res += p2;
752 }
753 
754 template <typename T, typename N, bool S, typename P>
756  std::vector<typename bezier_curve<T, N, S, P>::point_t> ts;
757  for (std::size_t i = 0; i <= p1.degree(); ++i) {
758  ts.push_back(bezier_curve<T, N, S, P>::point_t::Zero(p1.dim()));
759  }
760  bezier_curve<T, N, S, P> res(ts.begin(), ts.end(), p1.min(), p1.max());
761  res -= p1;
762  return res;
763 }
764 
765 template <typename T, typename N, bool S, typename P>
767  const bezier_curve<T, N, S, P>& p2) {
768  bezier_curve<T, N, S, P> res(p1);
769  return res -= p2;
770 }
771 
772 template <typename T, typename N, bool S, typename P>
774  const bezier_curve<T, N, S, P>& p1,
775  const typename bezier_curve<T, N, S, P>::point_t& point) {
776  bezier_curve<T, N, S, P> res(p1);
777  return res -= point;
778 }
779 
780 template <typename T, typename N, bool S, typename P>
782  const typename bezier_curve<T, N, S, P>::point_t& point,
783  const bezier_curve<T, N, S, P>& p1) {
784  bezier_curve<T, N, S, P> res(-p1);
785  return res += point;
786 }
787 
788 template <typename T, typename N, bool S, typename P>
790  const bezier_curve<T, N, S, P>& p1,
791  const typename bezier_curve<T, N, S, P>::point_t& point) {
792  bezier_curve<T, N, S, P> res(p1);
793  return res += point;
794 }
795 
796 template <typename T, typename N, bool S, typename P>
798  const typename bezier_curve<T, N, S, P>::point_t& point,
799  const bezier_curve<T, N, S, P>& p1) {
800  bezier_curve<T, N, S, P> res(p1);
801  return res += point;
802 }
803 
804 template <typename T, typename N, bool S, typename P>
806  const double k) {
807  bezier_curve<T, N, S, P> res(p1);
808  return res /= k;
809 }
810 
811 template <typename T, typename N, bool S, typename P>
813  const double k) {
814  bezier_curve<T, N, S, P> res(p1);
815  return res *= k;
816 }
817 
818 template <typename T, typename N, bool S, typename P>
820  const bezier_curve<T, N, S, P>& p1) {
821  bezier_curve<T, N, S, P> res(p1);
822  return res *= k;
823 }
824 
825 } // namespace ndcurves
826 
828  SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
830 
831 #endif //_CLASS_BEZIERCURVE
Time time_t
Definition: bezier_curve.h:35
#define SINGLE_ARG(...)
Definition: archive.hpp:23
Definition: bernstein.h:20
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
bezier_curve_t elevate(const std::size_t order) const
Computes a Bezier curve of order degrees higher than the current curve, but strictly equivalent...
Definition: bezier_curve.h:272
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
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: bezier_curve.h:45
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:49
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:166
bezier_curve_t compute_primitive(const std::size_t order, const point_t &init) 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:238
Eigen::Vector3d cross(const Eigen::VectorXd &a, const Eigen::VectorXd &b)
Definition: cross_implementation.h:14
virtual bool operator!=(const bezier_curve_t &other) const
Definition: bezier_curve.h:198
bezier_curve()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: bezier_curve.h:52
bezier_curve_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: bezier_curve.h:228
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:37
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:39
virtual time_t max() const =0
Get the maximum time for which the curve is defined.
class allowing to create a cubic hermite spline of any dimension.
interface for a Curve of arbitrary dimension.
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:187
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:102
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: bezier_curve.h:33
virtual point_t operator()(const time_t t) const
Evaluation of the bezier curve at time t.
Definition: bezier_curve.h:144
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:64
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:805
brief Computes all Bernstein polynomes for a certain degree std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Definition: bernstein.h:91
Numeric num_t
Definition: bezier_curve.h:36
bezier_curve< T, N, S, P > operator+(const bezier_curve< T, N, S, P > &p1, const bezier_curve< T, N, S, P > &p2)
Definition: bezier_curve.h:748
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:46
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:321
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:38
boost::shared_ptr< bezier_curve_t > bezier_curve_ptr_t
Definition: bezier_curve.h:41
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:812
void serialize(Archive &ar, const unsigned int version)
Definition: curve_abc.h:158
bezier_curve_t compute_primitive(const std::size_t order) const
Definition: bezier_curve.h:258
virtual std::size_t degree() const =0
Get the degree of the curve.
Definition: fwd.h:57
virtual bool operator==(const bezier_curve_t &other) const
Definition: bezier_curve.h:194
double Time
Definition: effector_spline.h:27
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:230
Eigen::Ref< const vector_x_t > vector_x_ref_t
Definition: bezier_curve.h:34
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: bezier_curve.h:183
bezier_curve< Time, Numeric, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:40
void elevate_self(const std::size_t order)
Elevate the Bezier curve of order degrees higher than the current curve, but strictly equivalent...
Definition: bezier_curve.h:296
#define DEFINE_CLASS_TEMPLATE_VERSION(Template, Type)
Definition: archive.hpp:27
virtual time_t min() const =0
Get the minimum time for which the curve is defined.
point_t init_acc
Definition: curve_constraint.h:59
point_t end_acc
Definition: curve_constraint.h:62
virtual std::size_t dim() const =0
Get dimension of curve.
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
Definition: bezier_curve.h:31
Point point_t
Definition: bezier_curve.h:32
struct to define constraints on start / end velocities and acceleration on a curve ...
piecewise_curve< Time, Numeric, Safe, point_t, point_t, bezier_curve_t > piecewise_curve_t
Definition: bezier_curve.h:43
double Numeric
Definition: effector_spline.h:26
point_t init_vel
Definition: curve_constraint.h:58
class allowing to create a piecewise curve.
bezier_curve(const bezier_curve &other)
Definition: bezier_curve.h:127
Definition: fwd.h:38
point_t end_vel
Definition: curve_constraint.h:61
Definition: curve_constraint.h:20
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:755
virtual ~bezier_curve()
Destructor.
Definition: bezier_curve.h:138
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:37
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: bezier_curve.h:44
bezier_curve_t * compute_primitive_ptr(const std::size_t order, const point_t &init) const
Definition: bezier_curve.h:262