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 "cross_implementation.h"
14 #include "bernstein.h"
15 #include "curve_constraint.h"
16 #include "piecewise_curve.h"
17 
18 #include "MathDefs.h"
19 
20 #include <vector>
21 #include <stdexcept>
22 
23 #include <iostream>
24 
25 namespace ndcurves {
31 template <typename Time = double, typename Numeric = Time, bool Safe = false,
32  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
33 struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
34  typedef Point point_t;
35  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
36  typedef Eigen::Ref<const vector_x_t> vector_x_ref_t;
37  typedef Time time_t;
38  typedef Numeric num_t;
40  typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
41  typedef typename t_point_t::const_iterator cit_point_t;
43  typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
47 
48  /* Constructors - destructors */
49  public:
52  bezier_curve() : dim_(0), T_min_(0), T_max_(0) {}
53 
62  template <typename In>
63  bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min = 0., const time_t T_max = 1.,
64  const time_t mult_T = 1.)
65  : dim_(PointsBegin->size()),
66  T_min_(T_min),
67  T_max_(T_max),
68  mult_T_(mult_T),
69  size_(std::distance(PointsBegin, PointsEnd)),
70  degree_(size_ - 1),
71  bernstein_(ndcurves::makeBernstein<num_t>((unsigned int)degree_)) {
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_(ndcurves::makeBernstein<num_t>((unsigned int)degree_)) {
106  if (Safe && (size_ < 1 || T_max_ <= T_min_)) {
107  throw std::invalid_argument("can't create bezier min bound is higher than max bound");
108  }
109  t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
110  for (cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) {
111  if (Safe && static_cast<size_t>(cit->size()) != dim_)
112  throw std::invalid_argument("All the control points must have the same dimension.");
113  control_points_.push_back(*cit);
114  }
115  }
116 
118  : dim_(other.dim_),
119  T_min_(other.T_min_),
120  T_max_(other.T_max_),
121  mult_T_(other.mult_T_),
122  size_(other.size_),
123  degree_(other.degree_),
124  bernstein_(other.bernstein_),
125  control_points_(other.control_points_) {}
126 
128  virtual ~bezier_curve() {}
129 
130  /*Operations*/
134  virtual point_t operator()(const time_t t) const {
135  check_conditions();
136  if (Safe & !(T_min_ <= t && t <= T_max_)) {
137  throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
138  }
139  if (size_ == 1) {
140  return mult_T_ * control_points_[0];
141  } else {
142  return evalHorner(t);
143  }
144  }
145 
154  bool isApprox(const bezier_curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
155  bool equal = ndcurves::isApprox<num_t>(T_min_, other.min()) && ndcurves::isApprox<num_t>(T_max_, other.max()) &&
156  dim_ == other.dim() && degree_ == other.degree() && size_ == other.size_ &&
157  ndcurves::isApprox<Numeric>(mult_T_, other.mult_T_) && bernstein_ == other.bernstein_;
158  if (!equal) return false;
159  for (size_t i = 0; i < size_; ++i) {
160  if (!control_points_.at(i).isApprox(other.control_points_.at(i), prec)) return false;
161  }
162  return true;
163  }
164 
165  virtual bool isApprox(const curve_abc_t* other,
166  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
167  const bezier_curve_t* other_cast = dynamic_cast<const bezier_curve_t*>(other);
168  if (other_cast)
169  return isApprox(*other_cast, prec);
170  else
171  return false;
172  }
173 
174  virtual bool operator==(const bezier_curve_t& other) const { return isApprox(other); }
175 
176  virtual bool operator!=(const bezier_curve_t& other) const { return !(*this == other); }
177 
182  bezier_curve_t compute_derivate(const std::size_t order) const {
183  check_conditions();
184  if (order == 0) {
185  return *this;
186  }
187  t_point_t derived_wp;
188  for (typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end() - 1; ++pit) {
189  derived_wp.push_back((num_t)degree_ * (*(pit + 1) - (*pit)));
190  }
191  if (derived_wp.empty()) {
192  derived_wp.push_back(point_t::Zero(dim_));
193  }
194  bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(), T_min_, T_max_, mult_T_ * (1. / (T_max_ - T_min_)));
195  return deriv.compute_derivate(order - 1);
196  }
197 
201  bezier_curve_t* compute_derivate_ptr(const std::size_t order) const {
202  return new bezier_curve_t(compute_derivate(order));
203  }
204 
211  bezier_curve_t compute_primitive(const std::size_t order, const point_t& init) const {
212  check_conditions();
213  if (order == 0) {
214  return *this;
215  }
216  num_t new_degree_inv = 1. / ((num_t)(degree_ + 1));
217  t_point_t n_wp;
218  point_t current_sum(init);
219  n_wp.push_back(current_sum);
220  for (typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit) {
221  current_sum += *pit;
222  n_wp.push_back(current_sum * new_degree_inv);
223  }
224  bezier_curve_t integ(n_wp.begin(), n_wp.end(), T_min_, T_max_, mult_T_ * (T_max_ - T_min_));
225  return integ.compute_primitive(order - 1);
226  }
227 
228  bezier_curve_t compute_primitive(const std::size_t order) const {
229  return compute_primitive(order, point_t::Zero(dim_));
230  }
231 
232  bezier_curve_t* compute_primitive_ptr(const std::size_t order, const point_t& init) const {
233  return new bezier_curve_t(compute_primitive(order, init));
234  }
235 
240  bezier_curve_t elevate(const std::size_t order) const {
241  t_point_t new_waypoints = control_points_, temp_waypoints;
242  for (std::size_t i = 1; i <= order; ++i) {
243  num_t new_degree_inv = 1. / ((num_t)(degree_ + i));
244  temp_waypoints.push_back(*new_waypoints.begin());
245  num_t idx_deg_inv = 0.;
246  for (typename t_point_t::const_iterator pit = new_waypoints.begin() + 1; pit != new_waypoints.end(); ++pit) {
247  idx_deg_inv += new_degree_inv;
248  temp_waypoints.push_back(idx_deg_inv * (*(pit - 1)) + (1 - idx_deg_inv) * (*pit));
249  }
250  temp_waypoints.push_back(*(new_waypoints.end() - 1));
251  new_waypoints = temp_waypoints;
252  temp_waypoints.clear();
253  }
254  return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), T_min_, T_max_, mult_T_);
255  }
256 
260  void elevate_self(const std::size_t order) {
261  if (order > 0) (*this) = elevate(order);
262  }
263 
271  virtual point_t derivate(const time_t t, const std::size_t order) const { return compute_derivate(order)(t); }
272 
281  point_t evalBernstein(const Numeric t) const {
282  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
283  point_t res = point_t::Zero(dim_);
284  typename t_point_t::const_iterator control_points_it = control_points_.begin();
285  for (typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin(); cit != bernstein_.end();
286  ++cit, ++control_points_it) {
287  res += cit->operator()(u) * (*control_points_it);
288  }
289  return res * mult_T_;
290  }
291 
304  point_t evalHorner(const Numeric t) const {
305  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
306  typename t_point_t::const_iterator control_points_it = control_points_.begin();
307  Numeric u_op, bc, tn;
308  u_op = 1.0 - u;
309  bc = 1;
310  tn = 1;
311  point_t tmp = (*control_points_it) * u_op;
312  ++control_points_it;
313  for (unsigned int i = 1; i < degree_; i++, ++control_points_it) {
314  tn = tn * u;
315  bc = bc * ((num_t)(degree_ - i + 1)) / i;
316  tmp = (tmp + tn * bc * (*control_points_it)) * u_op;
317  }
318  return (tmp + tn * u * (*control_points_it)) * mult_T_;
319  }
320 
321  const t_point_t& waypoints() const { return control_points_; }
322 
323  const point_t waypointAtIndex(const std::size_t index) const {
324  point_t waypoint;
325  if (index < control_points_.size()) {
326  waypoint = control_points_[index];
327  }
328  return waypoint;
329  }
330 
337  point_t evalDeCasteljau(const Numeric t) const {
338  // normalize time :
339  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
340  t_point_t pts = deCasteljauReduction(waypoints(), u);
341  while (pts.size() > 1) {
342  pts = deCasteljauReduction(pts, u);
343  }
344  return pts[0] * mult_T_;
345  }
346 
347  t_point_t deCasteljauReduction(const Numeric t) const {
348  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
349  return deCasteljauReduction(waypoints(), u);
350  }
351 
361  t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const {
362  if (u < 0 || u > 1) {
363  throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
364  }
365  if (pts.size() == 1) {
366  return pts;
367  }
368 
369  t_point_t new_pts;
370  for (cit_point_t cit = pts.begin(); cit != (pts.end() - 1); ++cit) {
371  new_pts.push_back((1 - u) * (*cit) + u * (*(cit + 1)));
372  }
373  return new_pts;
374  }
375 
380  std::pair<bezier_curve_t, bezier_curve_t> split(const Numeric t) const {
381  check_conditions();
382  if (fabs(t - T_max_) < MARGIN) {
383  throw std::runtime_error("can't split curve, interval range is equal to original curve");
384  }
385  t_point_t wps_first(size_), wps_second(size_);
386  const Numeric u = (t - T_min_) / (T_max_ - T_min_);
387  t_point_t casteljau_pts = waypoints();
388  wps_first[0] = casteljau_pts.front();
389  wps_second[degree_] = casteljau_pts.back();
390  size_t id = 1;
391  while (casteljau_pts.size() > 1) {
392  casteljau_pts = deCasteljauReduction(casteljau_pts, u);
393  wps_first[id] = casteljau_pts.front();
394  wps_second[degree_ - id] = casteljau_pts.back();
395  ++id;
396  }
397  bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T_min_, t, mult_T_);
398  bezier_curve_t c_second(wps_second.begin(), wps_second.end(), t, T_max_, mult_T_);
399  return std::make_pair(c_first, c_second);
400  }
401 
407  piecewise_curve_t split(const vector_x_t& times) const {
408  std::vector<bezier_curve_t> curves;
409  bezier_curve_t current = *this;
410  for (int i = 0; i < times.rows(); ++i) {
411  std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
412  curves.push_back(pairsplit.first);
413  current = pairsplit.second;
414  }
415  curves.push_back(current);
416  piecewise_curve_t res;
417  for (typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end(); ++cit) {
418  typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
419  res.add_curve_ptr(ptr);
420  }
421  return res;
422  }
423 
430  bezier_curve_t extract(const Numeric t1, const Numeric t2) {
431  if (t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_) {
432  throw std::out_of_range("In Extract curve : times out of bounds");
433  }
434  if (fabs(t1 - T_min_) < MARGIN && fabs(t2 - T_max_) < MARGIN) // t1=T_min and t2=T_max
435  {
436  return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
437  }
438  if (fabs(t1 - T_min_) < MARGIN) // t1=T_min
439  {
440  return split(t2).first;
441  }
442  if (fabs(t2 - T_max_) < MARGIN) // t2=T_max
443  {
444  return split(t1).second;
445  }
446  std::pair<bezier_curve_t, bezier_curve_t> c_split = this->split(t1);
447  return c_split.second.split(t2).first;
448  }
449 
457  bezier_curve_t cross(const bezier_curve_t& g) const {
458  // See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
459  // http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
460  assert_operator_compatible(g);
461  if (dim() != 3) throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 ");
462  int m = (int)(degree());
463  int n = (int)(g.degree());
464  unsigned int mj, n_ij, mn_i;
465  t_point_t new_waypoints;
466  for (int i = 0; i <= m + n; ++i) {
467  bezier_curve_t::point_t current_point = bezier_curve_t::point_t::Zero(dim());
468  for (int j = std::max(0, i - n); j <= std::min(m, i); ++j) {
469  mj = bin(m, j);
470  n_ij = bin(n, i - j);
471  mn_i = bin(m + n, i);
472  num_t mul = num_t(mj * n_ij) / num_t(mn_i);
473  current_point += mul * ndcurves::cross(waypointAtIndex(j), g.waypointAtIndex(i - j));
474  }
475  new_waypoints.push_back(current_point);
476  }
477  return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), min(), max(), mult_T_ * g.mult_T_);
478  }
479 
486  bezier_curve_t cross(const bezier_curve_t::point_t& point) const {
487  // See Farouki and Rajan 1988 Alogirthms for polynomials in Bernstein form and
488  // http://web.mit.edu/hyperbook/Patrikalakis-Maekawa-Cho/node10.html
489  if (dim() != 3) throw std::invalid_argument("Can't perform cross product on Bezier curves with dimensions != 3 ");
490  t_point_t new_waypoints;
491  for (typename t_point_t::const_iterator cit = waypoints().begin(); cit != waypoints().end(); ++cit) {
492  new_waypoints.push_back(ndcurves::cross(*cit, point));
493  }
494  return bezier_curve_t(new_waypoints.begin(), new_waypoints.end(), min(), max(), mult_T_);
495  }
496 
497  bezier_curve_t& operator+=(const bezier_curve_t& other) {
498  assert_operator_compatible(other);
499  bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_); // TODO remove mult_T_ from Bezier
500  if (other.degree() > degree()) {
501  elevate_self(other.degree() - degree());
502  } else if (other_elevated.degree() < degree()) {
503  other_elevated.elevate_self(degree() - other_elevated.degree());
504  }
505  typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin();
506  for (typename t_point_t::iterator it = control_points_.begin(); it != control_points_.end(); ++it, ++otherit) {
507  (*it) += (*otherit);
508  }
509  return *this;
510  }
511 
512  bezier_curve_t& operator-=(const bezier_curve_t& other) {
513  assert_operator_compatible(other);
514  bezier_curve_t other_elevated = other * (other.mult_T_ / this->mult_T_);
515  if (other.degree() > degree()) {
516  elevate_self(other.degree() - degree());
517  } else if (other_elevated.degree() < degree()) {
518  other_elevated.elevate_self(degree() - other_elevated.degree());
519  }
520  typename t_point_t::const_iterator otherit = other_elevated.control_points_.begin();
521  for (typename t_point_t::iterator it = control_points_.begin(); it != control_points_.end(); ++it, ++otherit) {
522  (*it) -= (*otherit);
523  }
524  return *this;
525  }
526 
527  bezier_curve_t& operator+=(const bezier_curve_t::point_t& point) {
528  for (typename t_point_t::iterator it = control_points_.begin(); it != control_points_.end(); ++it) {
529  (*it) += point;
530  }
531  return *this;
532  }
533 
534  bezier_curve_t& operator-=(const bezier_curve_t::point_t& point) {
535  for (typename t_point_t::iterator it = control_points_.begin(); it != control_points_.end(); ++it) {
536  (*it) -= point;
537  }
538  return *this;
539  }
540 
541  bezier_curve_t& operator/=(const double d) {
542  for (typename t_point_t::iterator it = control_points_.begin(); it != control_points_.end(); ++it) {
543  (*it) /= d;
544  }
545  return *this;
546  }
547 
548  bezier_curve_t& operator*=(const double d) {
549  for (typename t_point_t::iterator it = control_points_.begin(); it != control_points_.end(); ++it) {
550  (*it) *= d;
551  }
552  return *this;
553  }
554 
555  private:
560  template <typename In>
561  t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints) {
562  t_point_t res;
563  num_t T = T_max_ - T_min_;
564  num_t T_square = T * T;
565  point_t P0, P1, P2, P_n_2, P_n_1, PN;
566  P0 = *PointsBegin;
567  PN = *(PointsEnd - 1);
568  P1 = P0 + constraints.init_vel * T / (num_t)degree_;
569  P_n_1 = PN - constraints.end_vel * T / (num_t)degree_;
570  P2 = constraints.init_acc * T_square / (num_t)(degree_ * (degree_ - 1)) + 2 * P1 - P0;
571  P_n_2 = constraints.end_acc * T_square / (num_t)(degree_ * (degree_ - 1)) + 2 * P_n_1 - PN;
572  res.push_back(P0);
573  res.push_back(P1);
574  res.push_back(P2);
575  for (In it = PointsBegin + 1; it != PointsEnd - 1; ++it) {
576  res.push_back(*it);
577  }
578  res.push_back(P_n_2);
579  res.push_back(P_n_1);
580  res.push_back(PN);
581  return res;
582  }
583 
584  void check_conditions() const {
585  if (control_points_.size() == 0) {
586  throw std::runtime_error(
587  "Error in bezier curve : there is no control points set / did you use empty constructor ?");
588  } else if (dim_ == 0) {
589  throw std::runtime_error(
590  "Error in bezier curve : Dimension of points is zero / did you use empty constructor ?");
591  }
592  }
593 
594  void assert_operator_compatible(const bezier_curve_t& other) const {
595  if ((fabs(min() - other.min()) > MARGIN) || (fabs(max() - other.max()) > MARGIN)) {
596  throw std::invalid_argument(
597  "Can't perform base operation (+ - ) on two Bezier curves with different time ranges");
598  }
599  }
600 
601  /*Operations*/
602 
603  public:
604  /*Helpers*/
607  std::size_t virtual dim() const { return dim_; };
610  virtual time_t min() const { return T_min_; }
613  virtual time_t max() const { return T_max_; }
616  virtual std::size_t degree() const { return degree_; }
617  /*Helpers*/
618 
619  /* Attributes */
621  std::size_t dim_;
623  /*const*/ time_t T_min_;
625  /*const*/ time_t T_max_;
626  /*const*/ time_t mult_T_;
627  /*const*/ std::size_t size_;
628  /*const*/ std::size_t degree_;
629  /*const*/ std::vector<Bern<Numeric> > bernstein_;
630  /*const*/ t_point_t control_points_;
631  /* Attributes */
632 
633  static bezier_curve_t zero(const std::size_t dim, const time_t T = 1.) {
634  std::vector<point_t> ts;
635  ts.push_back(point_t::Zero(dim));
636  return bezier_curve_t(ts.begin(), ts.end(), 0., T);
637  }
638 
639  // Serialization of the class
640  friend class boost::serialization::access;
641 
642  template <class Archive>
643  void serialize(Archive& ar, const unsigned int version) {
644  if (version) {
645  // Do something depending on version ?
646  }
647  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
648  ar& boost::serialization::make_nvp("dim", dim_);
649  ar& boost::serialization::make_nvp("T_min", T_min_);
650  ar& boost::serialization::make_nvp("T_max", T_max_);
651  ar& boost::serialization::make_nvp("mult_T", mult_T_);
652  ar& boost::serialization::make_nvp("size", size_);
653  ar& boost::serialization::make_nvp("degree", degree_);
654  ar& boost::serialization::make_nvp("bernstein", bernstein_);
655  ar& boost::serialization::make_nvp("control_points", control_points_);
656  }
657 }; // End struct bezier_curve
658 
659 template <typename T, typename N, bool S, typename P>
661  bezier_curve<T, N, S, P> res(p1);
662  return res += p2;
663 }
664 
665 template <typename T, typename N, bool S, typename P>
667  std::vector<typename bezier_curve<T, N, S, P>::point_t> ts;
668  for (std::size_t i = 0; i <= p1.degree(); ++i) {
669  ts.push_back(bezier_curve<T, N, S, P>::point_t::Zero(p1.dim()));
670  }
671  bezier_curve<T, N, S, P> res(ts.begin(), ts.end(), p1.min(), p1.max());
672  res -= p1;
673  return res;
674 }
675 
676 template <typename T, typename N, bool S, typename P>
678  bezier_curve<T, N, S, P> res(p1);
679  return res -= p2;
680 }
681 
682 template <typename T, typename N, bool S, typename P>
684  const typename bezier_curve<T, N, S, P>::point_t& point) {
685  bezier_curve<T, N, S, P> res(p1);
686  return res -= point;
687 }
688 
689 template <typename T, typename N, bool S, typename P>
691  const bezier_curve<T, N, S, P>& p1) {
692  bezier_curve<T, N, S, P> res(-p1);
693  return res += point;
694 }
695 
696 template <typename T, typename N, bool S, typename P>
698  const typename bezier_curve<T, N, S, P>::point_t& point) {
699  bezier_curve<T, N, S, P> res(p1);
700  return res += point;
701 }
702 
703 template <typename T, typename N, bool S, typename P>
705  const bezier_curve<T, N, S, P>& p1) {
706  bezier_curve<T, N, S, P> res(p1);
707  return res += point;
708 }
709 
710 template <typename T, typename N, bool S, typename P>
712  bezier_curve<T, N, S, P> res(p1);
713  return res /= k;
714 }
715 
716 template <typename T, typename N, bool S, typename P>
718  bezier_curve<T, N, S, P> res(p1);
719  return res *= k;
720 }
721 
722 template <typename T, typename N, bool S, typename P>
724  bezier_curve<T, N, S, P> res(p1);
725  return res *= k;
726 }
727 
728 } // namespace ndcurves
729 
730 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
732 
733 #endif //_CLASS_BEZIERCURVE
Time time_t
Definition: bezier_curve.h:37
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:271
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:240
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:182
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: bezier_curve.h:46
boost::shared_ptr< curve_t > curve_ptr_t
Definition: piecewise_curve.h:40
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:154
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:211
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:176
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:201
curve_constraints< point_t > curve_constraints_t
Definition: bezier_curve.h:39
t_point_t::const_iterator cit_point_t
Definition: bezier_curve.h:41
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:153
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
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > vector_x_t
Definition: bezier_curve.h:35
virtual point_t operator()(const time_t t) const
Evaluation of the bezier curve at time t.
Definition: bezier_curve.h:134
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:63
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:711
brief Computes all Bernstein polynomes for a certain degree std::vector< Bern< Numeric > > makeBernstein(const unsigned int n)
Definition: bernstein.h:88
Numeric num_t
Definition: bezier_curve.h:38
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:660
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:41
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:281
std::vector< point_t, Eigen::aligned_allocator< point_t > > t_point_t
Definition: bezier_curve.h:40
boost::shared_ptr< bezier_curve_t > bezier_curve_ptr_t
Definition: bezier_curve.h:43
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:717
void serialize(Archive &ar, const unsigned int version)
Definition: curve_abc.h:139
bezier_curve_t compute_primitive(const std::size_t order) const
Definition: bezier_curve.h:228
virtual std::size_t degree() const =0
Get the degree of the curve.
Definition: fwd.h:49
virtual bool operator==(const bezier_curve_t &other) const
Definition: bezier_curve.h:174
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:208
Eigen::Ref< const vector_x_t > vector_x_ref_t
Definition: bezier_curve.h:36
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: bezier_curve.h:165
bezier_curve< Time, Numeric, Safe, Point > bezier_curve_t
Definition: bezier_curve.h:42
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:260
virtual time_t min() const =0
Get the minimum time for which the curve is defined.
point_t init_acc
Definition: curve_constraint.h:58
point_t end_acc
Definition: curve_constraint.h:61
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:33
Point point_t
Definition: bezier_curve.h:34
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:44
double Numeric
Definition: effector_spline.h:26
point_t init_vel
Definition: curve_constraint.h:57
class allowing to create a piecewise curve.
bezier_curve(const bezier_curve &other)
Definition: bezier_curve.h:117
Definition: fwd.h:34
point_t end_vel
Definition: curve_constraint.h:60
Definition: curve_constraint.h:22
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:666
virtual ~bezier_curve()
Destructor.
Definition: bezier_curve.h:128
Represents a curve of dimension Dim. If value of parameter Safe is false, no verification is made on ...
Definition: curve_abc.h:34
curve_abc< Time, Numeric, Safe, point_t > curve_abc_t
Definition: bezier_curve.h:45
bezier_curve_t * compute_primitive_ptr(const std::size_t order, const point_t &init) const
Definition: bezier_curve.h:232