polynomial.h
Go to the documentation of this file.
1 
13 #ifndef _STRUCT_POLYNOMIAL
14 #define _STRUCT_POLYNOMIAL
15 
16 #include "MathDefs.h"
17 
18 #include "curve_abc.h"
19 
20 #include <iostream>
21 #include <algorithm>
22 #include <functional>
23 #include <stdexcept>
24 
25 namespace ndcurves {
32 template <typename Time = double, typename Numeric = Time, bool Safe = false,
33  typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
34  typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> > >
35 struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
36  typedef Point point_t;
37  typedef T_Point t_point_t;
38  typedef Time time_t;
39  typedef Numeric num_t;
41  typedef Eigen::MatrixXd coeff_t;
42  typedef Eigen::Ref<coeff_t> coeff_t_ref;
45 
46  /* Constructors - destructors */
47  public:
50  polynomial() : curve_abc_t(), dim_(0), T_min_(0), T_max_(0) {}
51 
58  polynomial(const coeff_t& coefficients, const time_t min, const time_t max)
59  : curve_abc_t(),
60  dim_(coefficients.rows()),
61  coefficients_(coefficients),
62  degree_(coefficients.cols() - 1),
63  T_min_(min),
64  T_max_(max) {
65  safe_check();
66  }
67 
74  polynomial(const T_Point& coefficients, const time_t min, const time_t max)
75  : curve_abc_t(),
76  dim_(coefficients.begin()->size()),
77  coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
78  degree_(coefficients_.cols() - 1),
79  T_min_(min),
80  T_max_(max) {
81  safe_check();
82  }
83 
91  template <typename In>
92  polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
93  : curve_abc_t(),
94  dim_(zeroOrderCoefficient->size()),
95  coefficients_(init_coeffs(zeroOrderCoefficient, out)),
96  degree_(coefficients_.cols() - 1),
97  T_min_(min),
98  T_max_(max) {
99  safe_check();
100  }
101 
107  polynomial(const Point& init, const Point& end, const time_t min, const time_t max)
108  : dim_(init.size()), degree_(1), T_min_(min), T_max_(max) {
109  if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
110  if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
111  t_point_t coeffs;
112  coeffs.push_back(init);
113  coeffs.push_back((end - init) / (max - min));
114  coefficients_ = init_coeffs(coeffs.begin(), coeffs.end());
115  safe_check();
116  }
117 
128  polynomial(const Point& init, const Point& d_init, const Point& end, const Point& d_end, const time_t min,
129  const time_t max)
130  : dim_(init.size()), degree_(3), T_min_(min), T_max_(max) {
131  if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
132  if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
133  if (init.size() != d_init.size())
134  throw std::invalid_argument("init and d_init points must have the same dimensions.");
135  if (init.size() != d_end.size())
136  throw std::invalid_argument("init and d_end points must have the same dimensions.");
137  /* the coefficients [c0 c1 c2 c3] are found by solving the following system of equation
138  (found from the boundary conditions) :
139  [1 0 0 0 ] [c0] [ init ]
140  [1 T T^2 T^3 ] x [c1] = [ end ]
141  [0 1 0 0 ] [c2] [d_init]
142  [0 1 2T 3T^2] [c3] [d_end ]
143  */
144  double T = max - min;
145  Eigen::Matrix<double, 4, 4> m;
146  m << 1., 0, 0, 0, 1., T, T * T, T * T * T, 0, 1., 0, 0, 0, 1., 2. * T, 3. * T * T;
147  Eigen::Matrix<double, 4, 4> m_inv = m.inverse();
148  Eigen::Matrix<double, 4, 1> bc; // boundary condition vector
149  coefficients_ = coeff_t::Zero(dim_, degree_ + 1); // init coefficient matrix with the right size
150  for (size_t i = 0; i < dim_; ++i) { // for each dimension, solve the boundary condition problem :
151  bc[0] = init[i];
152  bc[1] = end[i];
153  bc[2] = d_init[i];
154  bc[3] = d_end[i];
155  coefficients_.row(i) = (m_inv * bc).transpose();
156  }
157  safe_check();
158  }
159 
172  polynomial(const Point& init, const Point& d_init, const Point& dd_init, const Point& end, const Point& d_end,
173  const Point& dd_end, const time_t min, const time_t max)
174  : dim_(init.size()), degree_(5), T_min_(min), T_max_(max) {
175  if (T_min_ >= T_max_) throw std::invalid_argument("T_min must be strictly lower than T_max");
176  if (init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions.");
177  if (init.size() != d_init.size())
178  throw std::invalid_argument("init and d_init points must have the same dimensions.");
179  if (init.size() != d_end.size())
180  throw std::invalid_argument("init and d_end points must have the same dimensions.");
181  if (init.size() != dd_init.size())
182  throw std::invalid_argument("init and dd_init points must have the same dimensions.");
183  if (init.size() != dd_end.size())
184  throw std::invalid_argument("init and dd_end points must have the same dimensions.");
185  /* the coefficients [c0 c1 c2 c3 c4 c5] are found by solving the following system of equation
186  (found from the boundary conditions) :
187  [1 0 0 0 0 0 ] [c0] [ init ]
188  [1 T T^2 T^3 T^4 T^5 ] [c1] [ end ]
189  [0 1 0 0 0 0 ] [c2] [d_init ]
190  [0 1 2T 3T^2 4T^3 5T^4 ] x [c3] = [d_end ]
191  [0 0 2 0 0 0 ] [c4] [dd_init]
192  [0 0 2 6T 12T^2 20T^3] [c5] [dd_end ]
193  */
194  double T = max - min;
195  Eigen::Matrix<double, 6, 6> m;
196  m << 1., 0, 0, 0, 0, 0, 1., T, T * T, pow(T, 3), pow(T, 4), pow(T, 5), 0, 1., 0, 0, 0, 0, 0, 1., 2. * T,
197  3. * T * T, 4. * pow(T, 3), 5. * pow(T, 4), 0, 0, 2, 0, 0, 0, 0, 0, 2, 6. * T, 12. * T * T, 20. * pow(T, 3);
198  Eigen::Matrix<double, 6, 6> m_inv = m.inverse();
199  Eigen::Matrix<double, 6, 1> bc; // boundary condition vector
200  coefficients_ = coeff_t::Zero(dim_, degree_ + 1); // init coefficient matrix with the right size
201  for (size_t i = 0; i < dim_; ++i) { // for each dimension, solve the boundary condition problem :
202  bc[0] = init[i];
203  bc[1] = end[i];
204  bc[2] = d_init[i];
205  bc[3] = d_end[i];
206  bc[4] = dd_init[i];
207  bc[5] = dd_end[i];
208  coefficients_.row(i) = (m_inv * bc).transpose();
209  }
210  safe_check();
211  }
212 
215  // NOTHING
216  }
217 
218  polynomial(const polynomial& other)
219  : dim_(other.dim_),
221  degree_(other.degree_),
222  T_min_(other.T_min_),
223  T_max_(other.T_max_) {}
224 
225  // polynomial& operator=(const polynomial& other);
226 
236  static polynomial_t MinimumJerk(const point_t& p_init, const point_t& p_final, const time_t t_min = 0.,
237  const time_t t_max = 1.) {
238  if (t_min > t_max) throw std::invalid_argument("final time should be superior or equal to initial time.");
239  const size_t dim(p_init.size());
240  if (static_cast<size_t>(p_final.size()) != dim)
241  throw std::invalid_argument("Initial and final points must have the same dimension.");
242  const double T = t_max - t_min;
243  const double T2 = T * T;
244  const double T3 = T2 * T;
245  const double T4 = T3 * T;
246  const double T5 = T4 * T;
247 
248  coeff_t coeffs = coeff_t::Zero(dim, 6); // init coefficient matrix with the right size
249  coeffs.col(0) = p_init;
250  coeffs.col(3) = 10 * (p_final - p_init) / T3;
251  coeffs.col(4) = -15 * (p_final - p_init) / T4;
252  coeffs.col(5) = 6 * (p_final - p_init) / T5;
253  return polynomial_t(coeffs, t_min, t_max);
254  }
255 
256  private:
257  void safe_check() {
258  if (Safe) {
259  if (T_min_ > T_max_) {
260  throw std::invalid_argument("Tmin should be inferior to Tmax");
261  }
262  if (coefficients_.cols() != int(degree_ + 1)) {
263  throw std::runtime_error("Spline order and coefficients do not match");
264  }
265  }
266  }
267 
268  /* Constructors - destructors */
269 
270  /*Operations*/
271  public:
275  virtual point_t operator()(const time_t t) const {
276  check_if_not_empty();
277  if ((t < T_min_ || t > T_max_) && Safe) {
278  throw std::invalid_argument(
279  "error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");
280  }
281  time_t const dt(t - T_min_);
282  point_t h = coefficients_.col(degree_);
283  for (int i = (int)(degree_ - 1); i >= 0; i--) {
284  h = dt * h + coefficients_.col(i);
285  }
286  return h;
287  }
288 
297  bool isApprox(const polynomial_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
298  return ndcurves::isApprox<num_t>(T_min_, other.min()) && ndcurves::isApprox<num_t>(T_max_, other.max()) &&
299  dim_ == other.dim() && degree_ == other.degree() && coefficients_.isApprox(other.coefficients_, prec);
300  }
301 
302  virtual bool isApprox(const curve_abc_t* other,
303  const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
304  const polynomial_t* other_cast = dynamic_cast<const polynomial_t*>(other);
305  if (other_cast)
306  return isApprox(*other_cast, prec);
307  else
308  return false;
309  }
310 
311  virtual bool operator==(const polynomial_t& other) const { return isApprox(other); }
312 
313  virtual bool operator!=(const polynomial_t& other) const { return !(*this == other); }
314 
319  virtual point_t derivate(const time_t t, const std::size_t order) const {
320  check_if_not_empty();
321  if ((t < T_min_ || t > T_max_) && Safe) {
322  throw std::invalid_argument(
323  "error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
324  }
325  time_t const dt(t - T_min_);
326  time_t cdt(1);
327  point_t currentPoint_ = point_t::Zero(dim_);
328  for (int i = (int)(order); i < (int)(degree_ + 1); ++i, cdt *= dt) {
329  currentPoint_ += cdt * coefficients_.col(i) * fact(i, order);
330  }
331  return currentPoint_;
332  }
333 
334  polynomial_t compute_derivate(const std::size_t order) const {
335  check_if_not_empty();
336  if (order == 0) {
337  return *this;
338  }
339  coeff_t coeff_derivated = deriv_coeff(coefficients_);
340  polynomial_t deriv(coeff_derivated, T_min_, T_max_);
341  return deriv.compute_derivate(order - 1);
342  }
343 
347  polynomial_t* compute_derivate_ptr(const std::size_t order) const {
348  return new polynomial_t(compute_derivate(order));
349  }
350 
351  Eigen::MatrixXd coeff() const { return coefficients_; }
352 
353  point_t coeffAtDegree(const std::size_t degree) const {
354  point_t res;
355  if (degree <= degree_) {
356  res = coefficients_.col(degree);
357  }
358  return res;
359  }
360 
361  private:
362  num_t fact(const std::size_t n, const std::size_t order) const {
363  num_t res(1);
364  for (std::size_t i = 0; i < std::size_t(order); ++i) {
365  res *= (num_t)(n - i);
366  }
367  return res;
368  }
369 
370  coeff_t deriv_coeff(coeff_t coeff) const {
371  if (coeff.cols() == 1) // only the constant part is left, fill with 0
372  return coeff_t::Zero(coeff.rows(), 1);
373  coeff_t coeff_derivated(coeff.rows(), coeff.cols() - 1);
374  for (std::size_t i = 0; i < std::size_t(coeff_derivated.cols()); i++) {
375  coeff_derivated.col(i) = coeff.col(i + 1) * (num_t)(i + 1);
376  }
377  return coeff_derivated;
378  }
379 
380  void check_if_not_empty() const {
381  if (coefficients_.size() == 0) {
382  throw std::runtime_error("Error in polynomial : there is no coefficients set / did you use empty constructor ?");
383  }
384  }
385  /*Operations*/
386 
387  public:
388  /*Helpers*/
391  std::size_t virtual dim() const { return dim_; };
394  num_t virtual min() const { return T_min_; }
397  num_t virtual max() const { return T_max_; }
400  virtual std::size_t degree() const { return degree_; }
401  /*Helpers*/
402 
403  polynomial_t& operator+=(const polynomial_t& p1) {
404  assert_operator_compatible(p1);
405  if (p1.degree() > degree()) {
406  polynomial_t::coeff_t res = p1.coeff();
407  res.block(0,0,coefficients_.rows(),coefficients_.cols()) += coefficients_;
408  coefficients_ = res;
409  degree_ = p1.degree();
410  }
411  else{
412  coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) += p1.coeff();
413  }
414  return *this;
415  }
416 
417  polynomial_t& operator-=(const polynomial_t& p1) {
418  assert_operator_compatible(p1);
419  if (p1.degree() > degree()) {
420  polynomial_t::coeff_t res = -p1.coeff();
421  res.block(0,0,coefficients_.rows(),coefficients_.cols()) += coefficients_;
422  coefficients_ = res;
423  degree_ = p1.degree();
424  }
425  else{
426  coefficients_.block(0,0,p1.coeff().rows(),p1.coeff().cols()) -= p1.coeff();
427  }
428  return *this;
429  }
430 
431  polynomial_t& operator+=(const polynomial_t::point_t& point) {
432  coefficients_.col(0) += point;
433  return *this;
434  }
435 
436  polynomial_t& operator-=(const polynomial_t::point_t& point) {
437  coefficients_.col(0) -= point;
438  return *this;
439  }
440 
441  polynomial_t& operator/=(const double d) {
442  coefficients_ /= d;
443  return *this;
444  }
445 
446  polynomial_t& operator*=(const double d) {
447  coefficients_ *= d;
448  return *this;
449  }
450 
457  polynomial_t cross(const polynomial_t& pOther) const {
458  assert_operator_compatible(pOther);
459  if (dim()!= 3)
460  throw std::invalid_argument("Can't perform cross product on polynomials with dimensions != 3 ");
461  std::size_t new_degree =degree() + pOther.degree();
462  coeff_t nCoeffs = Eigen::MatrixXd::Zero(3,new_degree+1);
463  Eigen::Vector3d currentVec;
464  Eigen::Vector3d currentVecCrossed;
465  for(long i = 0; i< coefficients_.cols(); ++i){
466  currentVec = coefficients_.col(i);
467  for(long j = 0; j< pOther.coeff().cols(); ++j){
468  currentVecCrossed = pOther.coeff().col(j);
469  nCoeffs.col(i+j) += currentVec.cross(currentVecCrossed);
470  }
471  }
472  // remove last degrees is they are equal to 0
473  long final_degree = new_degree;
474  while(nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN && final_degree >0){
475  --final_degree;
476  }
477  return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max());
478  }
479 
486  polynomial_t cross(const polynomial_t::point_t& point) const {
487  if (dim()!= 3)
488  throw std::invalid_argument("Can't perform cross product on polynomials with dimensions != 3 ");
489  coeff_t nCoeffs = coefficients_;
490  Eigen::Vector3d currentVec;
491  Eigen::Vector3d pointVec = point;
492  for(long i = 0; i< coefficients_.cols(); ++i){
493  currentVec = coefficients_.col(i);
494  nCoeffs.col(i) = currentVec.cross(pointVec);
495  }
496  // remove last degrees is they are equal to 0
497  long final_degree = degree();
498  while(nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN && final_degree >0){
499  --final_degree;
500  }
501  return polynomial_t(nCoeffs.leftCols(final_degree+1), min(), max());
502  }
503 
504  /*Attributes*/
505  std::size_t dim_; // const
506  coeff_t coefficients_; // const
507  std::size_t degree_; // const
508  time_t T_min_, T_max_; // const
509  /*Attributes*/
510 
511  private:
512 
513  void assert_operator_compatible(const polynomial_t& other) const{
514  if ((fabs(min() - other.min()) > ndcurves::MARGIN) || (fabs(max() - other.max()) > ndcurves::MARGIN) || dim() != other.dim()){
515  throw std::invalid_argument("Can't perform base operation (+ - ) on two polynomials with different time ranges or different dimensions");
516  }
517  }
518 
519  template <typename In>
520  coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
521  std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
522  coeff_t res = coeff_t(dim_, size);
523  int i = 0;
524  for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i) {
525  res.col(i) = *cit;
526  }
527  return res;
528  }
529 
530  public:
531  // Serialization of the class
533 
534  template <class Archive>
535  void serialize(Archive& ar, const unsigned int version) {
536  if (version) {
537  // Do something depending on version ?
538  }
539  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
540  ar& boost::serialization::make_nvp("dim", dim_);
541  ar& boost::serialization::make_nvp("coefficients", coefficients_);
542  ar& boost::serialization::make_nvp("dim", dim_);
543  ar& boost::serialization::make_nvp("degree", degree_);
544  ar& boost::serialization::make_nvp("T_min", T_min_);
545  ar& boost::serialization::make_nvp("T_max", T_max_);
546  }
547 
548 }; // class polynomial
549 
550 template <typename T, typename N, bool S, typename P, typename TP >
552  polynomial<T,N,S,P,TP> res(p1);
553  return res+=p2;
554 }
555 
556 template <typename T, typename N, bool S, typename P, typename TP >
558  polynomial<T,N,S,P,TP> res(p1);
559  return res+=point;
560 }
561 
562 template <typename T, typename N, bool S, typename P, typename TP >
564  polynomial<T,N,S,P,TP> res(p1);
565  return res+=point;
566 }
567 
568 template <typename T, typename N, bool S, typename P, typename TP >
570  polynomial<T,N,S,P,TP> res(p1);
571  return res-=point;
572 }
573 
574 template <typename T, typename N, bool S, typename P, typename TP >
576  polynomial<T,N,S,P,TP> res(-p1);
577  return res+=point;
578 }
579 
580 
581 template <typename T, typename N, bool S, typename P, typename TP >
583  typename polynomial<T,N,S,P,TP>::coeff_t res = -p1.coeff();
584  return polynomial<T,N,S,P,TP>(res,p1.min(),p1.max());
585 }
586 
587 template <typename T, typename N, bool S, typename P, typename TP >
589  polynomial<T,N,S,P,TP> res(p1);
590  return res-=p2;
591 }
592 
593 template <typename T, typename N, bool S, typename P, typename TP >
595  polynomial<T,N,S,P,TP> res(p1);
596  return res/=k;
597 }
598 
599 template <typename T, typename N, bool S, typename P, typename TP >
601  polynomial<T,N,S,P,TP> res(p1);
602  return res*=k;
603 }
604 
605 template <typename T, typename N, bool S, typename P, typename TP >
607  polynomial<T,N,S,P,TP> res(p1);
608  return res*=k;
609 }
610 
611 } // namespace ndcurves
612 
613 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point, typename T_Point),
615 #endif //_STRUCT_POLYNOMIAL
Definition: bernstein.h:20
polynomial< Time, Numeric, Safe, Point, T_Point > polynomial_t
Definition: polynomial.h:43
polynomial_t & operator*=(const double d)
Definition: polynomial.h:446
virtual num_t min() const
Get the minimum time for which the curve is defined.
Definition: polynomial.h:394
polynomial_t & operator+=(const polynomial_t::point_t &point)
Definition: polynomial.h:431
time_t T_max_
Definition: polynomial.h:508
~polynomial()
Destructor.
Definition: polynomial.h:214
coeff_t coefficients_
Definition: polynomial.h:506
std::size_t degree_
Definition: polynomial.h:507
polynomial(const coeff_t &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:58
interface for a Curve of arbitrary dimension.
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t using horner&#39;s scheme.
Definition: polynomial.h:275
polynomial(const polynomial &other)
Definition: polynomial.h:218
bezier_curve< T, N, S, P > operator-(const bezier_curve< T, N, S, P > &p1)
Definition: bezier_curve.h:672
polynomial_t cross(const polynomial_t &pOther) const
Compute the cross product of the current polynomial by another polynomial. The cross product p1Xp2 of...
Definition: polynomial.h:457
friend class boost::serialization::access
Definition: polynomial.h:532
virtual bool operator==(const polynomial_t &other) const
Definition: polynomial.h:311
polynomial(const Point &init, const Point &end, const time_t min, const time_t max)
Constructor from boundary condition with C0 : create a polynomial that connect exactly init and end (...
Definition: polynomial.h:107
point_t coeffAtDegree(const std::size_t degree) const
Definition: polynomial.h:353
curve_abc_t::curve_ptr_t curve_ptr_t
Definition: polynomial.h:44
Numeric num_t
Definition: polynomial.h:39
T_Point t_point_t
Definition: polynomial.h:37
boost::shared_ptr< curve_t > curve_ptr_t
Definition: curve_abc.h:41
curve_abc< Time, Numeric, Safe, Point > curve_abc_t
Definition: polynomial.h:40
virtual bool operator!=(const polynomial_t &other) const
Definition: polynomial.h:313
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
Represents a polynomial of an arbitrary order defined on the interval . It follows the equation : ...
Definition: fwd.h:37
double Time
Definition: effector_spline.h:27
Eigen::Ref< coeff_t > coeff_t_ref
Definition: polynomial.h:42
polynomial(const T_Point &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:74
polynomial_t & operator/=(const double d)
Definition: polynomial.h:441
virtual std::size_t degree() const
Get the degree of the curve.
Definition: polynomial.h:400
virtual num_t max() const
Get the maximum time for which the curve is defined.
Definition: polynomial.h:397
virtual std::size_t dim() const
Get dimension of curve.
Definition: polynomial.h:391
Time time_t
Definition: polynomial.h:38
polynomial_t cross(const polynomial_t::point_t &point) const
Compute the cross product of the current polynomial p by a point point. The cross product pXpoint of ...
Definition: polynomial.h:486
void serialize(Archive &ar, const unsigned int version)
Definition: polynomial.h:535
bool isApprox(const polynomial_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: polynomial.h:297
Eigen::Matrix< Numeric, Eigen::Dynamic, 1 > Point
Definition: effector_spline.h:28
polynomial()
Empty constructor. Curve obtained this way can not perform other class functions. ...
Definition: polynomial.h:50
Point point_t
Definition: polynomial.h:36
polynomial(const Point &init, const Point &d_init, const Point &end, const Point &d_end, const time_t min, const time_t max)
Constructor from boundary condition with C1 : create a polynomial that connect exactly init and end a...
Definition: polynomial.h:128
double Numeric
Definition: effector_spline.h:26
polynomial_t & operator+=(const polynomial_t &p1)
Definition: polynomial.h:403
Eigen::MatrixXd coeff() const
Definition: polynomial.h:351
polynomial_t * compute_derivate_ptr(const std::size_t order) const
Compute the derived curve at order N.
Definition: polynomial.h:347
polynomial_t & operator-=(const polynomial_t::point_t &point)
Definition: polynomial.h:436
time_t T_min_
Definition: polynomial.h:508
bezier_curve< T, N, S, P > operator/(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:714
bezier_curve< T, N, S, P > operator*(const bezier_curve< T, N, S, P > &p1, const double k)
Definition: bezier_curve.h:720
polynomial(const Point &init, const Point &d_init, const Point &dd_init, const Point &end, const Point &d_end, const Point &dd_end, const time_t min, const time_t max)
Constructor from boundary condition with C2 : create a polynomial that connect exactly init and end a...
Definition: polynomial.h:172
virtual bool isApprox(const curve_abc_t *other, const Numeric prec=Eigen::NumTraits< Numeric >::dummy_precision()) const
Definition: polynomial.h:302
std::size_t dim_
Definition: polynomial.h:505
polynomial_t compute_derivate(const std::size_t order) const
Definition: polynomial.h:334
static polynomial_t MinimumJerk(const point_t &p_init, const point_t &p_final, const time_t t_min=0., const time_t t_max=1.)
MinimumJerk Build a polynomial curve connecting p_init to p_final minimizing the time integral of the...
Definition: polynomial.h:236
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative of order N of spline at time t.
Definition: polynomial.h:319
Eigen::MatrixXd coeff_t
Definition: polynomial.h:41
polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
Constructor.
Definition: polynomial.h:92
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:666
polynomial_t & operator-=(const polynomial_t &p1)
Definition: polynomial.h:417