polynomial.h 9.38 KB
 Steve Tonneau committed Nov 25, 2016 1 /**  JasonChmn committed May 02, 2019 2 * \file polynomial.h  Steve Tonneau committed Nov 25, 2016 3 4 5 6 7 * \brief Definition of a cubic spline. * \author Steve T. * \version 0.1 * \date 06/17/2013 *  JasonChmn committed May 02, 2019 8 * This file contains definitions for the polynomial struct.  Steve Tonneau committed Nov 25, 2016 9 * It allows the creation and evaluation of natural  Steve Tonneau committed Nov 25, 2016 10 * smooth splines of arbitrary dimension and order  Steve Tonneau committed Nov 25, 2016 11 12 13 */  JasonChmn committed May 02, 2019 14 15 #ifndef _STRUCT_POLYNOMIAL #define _STRUCT_POLYNOMIAL  Steve Tonneau committed Nov 25, 2016 16 17 18 19 20 21 22 23 24 25  #include "MathDefs.h" #include "curve_abc.h" #include #include #include #include  JasonChmn committed Apr 17, 2019 26 namespace curves  Steve Tonneau committed Nov 25, 2016 27 {  JasonChmn committed Sep 03, 2019 28 29 30 31 32 33 34 35  /// \class polynomial. /// \brief Represents a polynomial of an arbitrary order defined on the interval /// \f$[t_{min}, t_{max}]\f$. It follows the equation :
/// \f$x(t) = a + b(t - t_{min}) + ... + d(t - t_{min})^N \f$
/// where N is the order and \f$t \in [t_{min}, t_{max}] \f$. /// template, typename T_Point =std::vector > >  JasonChmn committed Sep 03, 2019 36  struct polynomial : public curve_abc  JasonChmn committed Sep 03, 2019 37  {  JasonChmn committed Sep 03, 2019 38  typedef Point point_t;  Steve Tonneau committed Apr 12, 2017 39  typedef T_Point t_point_t;  JasonChmn committed Sep 03, 2019 40 41  typedef Time time_t; typedef Numeric num_t;  JasonChmn committed Sep 03, 2019 42  typedef curve_abc curve_abc_t;  Steve Tonneau committed Nov 25, 2016 43 44  typedef Eigen::Matrix coeff_t; typedef Eigen::Ref coeff_t_ref;  JasonChmn committed Sep 03, 2019 45  typedef polynomial polynomial_t;  Steve Tonneau committed Nov 25, 2016 46   JasonChmn committed Sep 03, 2019 47  /* Constructors - destructors */  Steve Tonneau committed Nov 25, 2016 48  public:  JasonChmn committed Sep 03, 2019 49 50  /// \brief Empty constructor. Curve obtained this way can not perform other class functions. ///  JasonChmn committed Sep 03, 2019 51 52 53  polynomial() : T_min_(0), T_max_(0) {}  JasonChmn committed Sep 03, 2019 54   JasonChmn committed Sep 03, 2019 55 56 57 58 59 60 61  /// \brief Constructor. /// \param coefficients : a reference to an Eigen matrix where each column is a coefficient, /// from the zero order coefficient, up to the highest order. Spline order is given /// by the number of the columns -1. /// \param min : LOWER bound on interval definition of the curve. /// \param max : UPPER bound on interval definition of the curve. polynomial(const coeff_t& coefficients, const time_t min, const time_t max)  Steve Tonneau committed Nov 25, 2016 62  : curve_abc_t(),  63 64 65  coefficients_(coefficients), dim_(Dim), degree_(coefficients_.cols()-1), T_min_(min), T_max_(max)  JasonChmn committed Sep 03, 2019 66  {  Steve Tonneau committed Nov 25, 2016 67  safe_check();  JasonChmn committed Sep 03, 2019 68 69 70 71 72 73 74 75 76  } /// \brief Constructor /// \param coefficients : a container containing all coefficients of the spline, starting /// with the zero order coefficient, up to the highest order. Spline order is given /// by the size of the coefficients. /// \param min : LOWER bound on interval definition of the spline. /// \param max : UPPER bound on interval definition of the spline. polynomial(const T_Point& coefficients, const time_t min, const time_t max)  Steve Tonneau committed Nov 25, 2016 77  : curve_abc_t(),  Steve Tonneau committed Nov 25, 2016 78  coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),  79 80  dim_(Dim), degree_(coefficients_.cols()-1), T_min_(min), T_max_(max)  JasonChmn committed Sep 03, 2019 81  {  Steve Tonneau committed Nov 25, 2016 82  safe_check();  JasonChmn committed Sep 03, 2019 83 84 85 86 87 88 89 90 91 92  } /// \brief Constructor. /// \param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients /// it corresponds to the zero degree coefficient. /// \param out : an iterator pointing to the last element of a structure ofcoefficients. /// \param min : LOWER bound on interval definition of the spline. /// \param max : UPPER bound on interval definition of the spline. template polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)  93 94 95  : coefficients_(init_coeffs(zeroOrderCoefficient, out)), dim_(Dim), degree_(coefficients_.cols()-1), T_min_(min), T_max_(max)  JasonChmn committed Sep 03, 2019 96  {  Steve Tonneau committed Nov 25, 2016 97  safe_check();  JasonChmn committed Sep 03, 2019 98  }  Steve Tonneau committed Nov 25, 2016 99   JasonChmn committed Sep 03, 2019 100 101 102 103 104  /// \brief Destructor ~polynomial() { // NOTHING }  Steve Tonneau committed Nov 25, 2016 105   Steve Tonneau committed Nov 25, 2016 106   JasonChmn committed Sep 03, 2019 107  polynomial(const polynomial& other)  stevet committed Mar 19, 2018 108  : coefficients_(other.coefficients_),  JasonChmn committed Sep 03, 2019 109  dim_(other.dim_), degree_(other.degree_), T_min_(other.T_min_), T_max_(other.T_max_)  JasonChmn committed Sep 03, 2019 110  {}  Steve Tonneau committed Nov 25, 2016 111   112   JasonChmn committed May 02, 2019 113  //polynomial& operator=(const polynomial& other);  114 115  private:  JasonChmn committed Sep 03, 2019 116 117  void safe_check() {  Steve Tonneau committed Nov 25, 2016 118 119  if(Safe) {  JasonChmn committed Sep 03, 2019 120 121 122 123 124 125 126 127  if(T_min_ > T_max_) { std::invalid_argument("Tmin should be inferior to Tmax"); } if(coefficients_.size() != int(degree_+1)) { std::runtime_error("Spline order and coefficients do not match"); }  Steve Tonneau committed Nov 25, 2016 128  }  JasonChmn committed Sep 03, 2019 129  }  Steve Tonneau committed Nov 25, 2016 130   JasonChmn committed Sep 03, 2019 131  /* Constructors - destructors */  Steve Tonneau committed Nov 25, 2016 132   JasonChmn committed Sep 03, 2019 133  /*Operations*/  Steve Tonneau committed Nov 25, 2016 134  public:  Steve Tonneau committed Apr 12, 2017 135   JasonChmn committed Sep 03, 2019 136 137 138 139 140  /// \brief Evaluation of the cubic spline at time t using horner's scheme. /// \param t : time when to evaluate the spline. /// \return \f$x(t)\f$ point corresponding on spline at time t. virtual point_t operator()(const time_t t) const {  JasonChmn committed Sep 03, 2019 141  check_if_not_empty();  JasonChmn committed Sep 03, 2019 142  if((t < T_min_ || t > T_max_) && Safe)  JasonChmn committed Apr 30, 2019 143  {  JasonChmn committed Sep 03, 2019 144  throw std::invalid_argument("error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");  JasonChmn committed Apr 30, 2019 145  }  JasonChmn committed Sep 03, 2019 146  time_t const dt (t-T_min_);  JasonChmn committed Sep 03, 2019 147 148  point_t h = coefficients_.col(degree_); for(int i=(int)(degree_-1); i>=0; i--)  JasonChmn committed Apr 30, 2019 149  {  JasonChmn committed Sep 03, 2019 150  h = dt*h + coefficients_.col(i);  JasonChmn committed Apr 30, 2019 151  }  Steve Tonneau committed Apr 12, 2017 152  return h;  JasonChmn committed Sep 03, 2019 153  }  Steve Tonneau committed Nov 28, 2016 154 155   JasonChmn committed Sep 03, 2019 156 157 158 159 160 161  /// \brief Evaluation of the derivative of order N of spline at time t. /// \param t : the time when to evaluate the spline. /// \param order : order of derivative. /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t. virtual point_t derivate(const time_t t, const std::size_t order) const {  JasonChmn committed Sep 03, 2019 162  check_if_not_empty();  JasonChmn committed Sep 03, 2019 163  if((t < T_min_ || t > T_max_) && Safe)  JasonChmn committed Apr 30, 2019 164  {  JasonChmn committed Sep 03, 2019 165  throw std::invalid_argument("error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");  JasonChmn committed Apr 30, 2019 166  }  JasonChmn committed Sep 03, 2019 167  time_t const dt (t-T_min_);  Steve Tonneau committed Nov 28, 2016 168  time_t cdt(1);  JasonChmn committed Sep 03, 2019 169  point_t currentPoint_ = point_t::Zero(dim_);  JasonChmn committed Sep 03, 2019 170  for(int i = (int)(order); i < (int)(degree_+1); ++i, cdt*=dt)  JasonChmn committed Apr 30, 2019 171  {  JasonChmn committed Sep 03, 2019 172  currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);  JasonChmn committed Apr 30, 2019 173  }  Steve Tonneau committed Nov 28, 2016 174  return currentPoint_;  JasonChmn committed Sep 03, 2019 175  }  Steve Tonneau committed Nov 28, 2016 176   JasonChmn committed Sep 03, 2019 177 178  polynomial_t compute_derivate(const std::size_t order) const {  JasonChmn committed Sep 03, 2019 179  check_if_not_empty();  JasonChmn committed Sep 03, 2019 180 181 182 183 184 185 186 187 188  if(order == 0) { return *this; } coeff_t coeff_derivated = deriv_coeff(coefficients_); polynomial_t deriv(coeff_derivated, T_min_, T_max_); return deriv.compute_derivate(order-1); }  Steve Tonneau committed Nov 28, 2016 189  private:  JasonChmn committed Sep 03, 2019 190 191  num_t fact(const std::size_t n, const std::size_t order) const {  stevet committed Mar 19, 2018 192  num_t res(1);  Steve Tonneau committed Nov 28, 2016 193  for(std::size_t i = 0; i < order; ++i)  JasonChmn committed Apr 30, 2019 194  {  JasonChmn committed Sep 03, 2019 195  res *= (num_t)(n-i);  JasonChmn committed Apr 30, 2019 196  }  Steve Tonneau committed Nov 28, 2016 197  return res;  JasonChmn committed Sep 03, 2019 198  }  JasonChmn committed Sep 03, 2019 199 200 201 202  coeff_t deriv_coeff(coeff_t coeff) const { coeff_t coeff_derivated(coeff.rows(), coeff.cols()-1);  JasonChmn committed Sep 03, 2019 203 204  for (std::size_t i=0; i coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {  Steve Tonneau committed Nov 25, 2016 242 243 244  std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient); coeff_t res = coeff_t(Dim, size); int i = 0; for(In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i)  JasonChmn committed Apr 30, 2019 245  {  JasonChmn committed Sep 03, 2019 246  res.col(i) = *cit;  JasonChmn committed Apr 30, 2019 247  }  Steve Tonneau committed Nov 25, 2016 248  return res;  JasonChmn committed Sep 03, 2019 249  }  JasonChmn committed Sep 03, 2019 250   251  public:  JasonChmn committed Sep 03, 2019 252 253  // Serialization of the class friend class boost::serialization::access;  254   JasonChmn committed Sep 03, 2019 255 256  template void serialize(Archive& ar, const unsigned int version){  JasonChmn committed Sep 03, 2019 257  if (version) {  JasonChmn committed Sep 03, 2019 258  // Do something depending on version ?  JasonChmn committed Sep 03, 2019 259  }  JasonChmn committed Sep 03, 2019 260  ar & boost::serialization::make_nvp("dim", dim_);  JasonChmn committed Sep 03, 2019 261 262 263 264 265  ar & boost::serialization::make_nvp("coefficients", coefficients_); ar & boost::serialization::make_nvp("dim", dim_); ar & boost::serialization::make_nvp("degree", degree_); ar & boost::serialization::make_nvp("T_min", T_min_); ar & boost::serialization::make_nvp("T_max", T_max_);  JasonChmn committed Sep 03, 2019 266  }  267   JasonChmn committed Sep 03, 2019 268  }; //class polynomial  JasonChmn committed Sep 03, 2019 269   JasonChmn committed Apr 17, 2019 270 } // namespace curves  JasonChmn committed May 02, 2019 271 #endif //_STRUCT_POLYNOMIAL  Steve Tonneau committed Nov 25, 2016 272