polynomial.h 9.38 KB
Newer Older
1
/**
2
* \file polynomial.h
3
4
5
6
7
* \brief Definition of a cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
8
* This file contains definitions for the polynomial struct.
9
* It allows the creation and evaluation of natural
Steve Tonneau's avatar
Steve Tonneau committed
10
* smooth splines of arbitrary dimension and order
11
12
13
*/


14
15
#ifndef _STRUCT_POLYNOMIAL
#define _STRUCT_POLYNOMIAL
16
17
18
19
20
21
22
23
24
25

#include "MathDefs.h"

#include "curve_abc.h"

#include <iostream>
#include <algorithm>
#include <functional>
#include <stdexcept>

26
namespace curves
27
{
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 :<br>
  /// \f$ x(t) = a + b(t - t_{min}) + ... + d(t - t_{min})^N \f$<br> 
  /// where N is the order and \f$ t \in [t_{min}, t_{max}] \f$.
  ///
  template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
           typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
36
  struct polynomial : public curve_abc<Time, Numeric, Safe, Point>
37
  {
38
    typedef Point   point_t;
39
    typedef T_Point t_point_t;
40
41
    typedef Time  time_t;
    typedef Numeric num_t;
42
    typedef curve_abc<Time, Numeric, Safe, Point> curve_abc_t;
43
44
    typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
    typedef Eigen::Ref<coeff_t> coeff_t_ref;
45
    typedef polynomial<Time, Numeric, Dim, Safe, Point, T_Point> polynomial_t;
46

47
    /* Constructors - destructors */
48
    public:
49
50
      /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
      ///
51
52
53
      polynomial()
        : T_min_(0), T_max_(0)
      {}
54

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)
62
        : curve_abc_t(),
63
64
65
          coefficients_(coefficients), 
          dim_(Dim), degree_(coefficients_.cols()-1), 
          T_min_(min), T_max_(max)
66
      {
67
        safe_check();
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)
77
        : curve_abc_t(),
78
          coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
79
80
          dim_(Dim), degree_(coefficients_.cols()-1), 
          T_min_(min), T_max_(max)
81
      {
82
        safe_check();
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<typename In>
      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)
96
      {
97
        safe_check();
98
      }
99

100
101
102
103
104
      /// \brief Destructor
      ~polynomial()
      {
      // NOTHING
      }
105

Steve Tonneau's avatar
Steve Tonneau committed
106

107
      polynomial(const polynomial& other)
stevet's avatar
stevet committed
108
        : coefficients_(other.coefficients_),
109
          dim_(other.dim_), degree_(other.degree_), T_min_(other.T_min_), T_max_(other.T_max_)
110
      {}
Steve Tonneau's avatar
Steve Tonneau committed
111

112

113
    //polynomial& operator=(const polynomial& other);
114
115

    private:
116
117
      void safe_check()
      {
118
119
        if(Safe)
        {
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");
          }
128
        }
129
      }
Steve Tonneau's avatar
Steve Tonneau committed
130

131
    /* Constructors - destructors */
132

133
    /*Operations*/
134
    public:
135

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
      {
141
        check_if_not_empty();
142
        if((t < T_min_ || t > T_max_) && Safe)
143
        { 
144
          throw std::invalid_argument("error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");
145
        }
146
        time_t const dt (t-T_min_);
JasonChmn's avatar
JasonChmn committed
147
148
        point_t h = coefficients_.col(degree_);
        for(int i=(int)(degree_-1); i>=0; i--)
149
        {
150
          h = dt*h + coefficients_.col(i);
151
        }
152
        return h;
153
      }
154
155


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
      {
162
        check_if_not_empty();
163
        if((t < T_min_ || t > T_max_) && Safe)
164
        { 
165
          throw std::invalid_argument("error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
166
        }
167
        time_t const dt (t-T_min_);
168
        time_t cdt(1);
JasonChmn's avatar
JasonChmn committed
169
        point_t currentPoint_ = point_t::Zero(dim_);
JasonChmn's avatar
JasonChmn committed
170
        for(int i = (int)(order); i < (int)(degree_+1); ++i, cdt*=dt) 
171
        {
172
          currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);
173
        }
174
        return currentPoint_;
175
      }
176

177
178
      polynomial_t compute_derivate(const std::size_t order) const
      {
179
        check_if_not_empty();
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);
      }

189
    private:
190
191
      num_t fact(const std::size_t n, const std::size_t order) const
      {
stevet's avatar
stevet committed
192
        num_t res(1);
193
        for(std::size_t i = 0; i < order; ++i)
194
        {
195
          res *= (num_t)(n-i);
196
        }
197
        return res;
198
      }
199
200
201
202

      coeff_t deriv_coeff(coeff_t coeff) const
      {
        coeff_t coeff_derivated(coeff.rows(), coeff.cols()-1);
JasonChmn's avatar
JasonChmn committed
203
204
        for (std::size_t i=0; i<std::size_t(coeff_derivated.cols()); i++) {
          coeff_derivated.col(i) = coeff.col(i+1)*(num_t)(i+1);
205
206
207
        }
        return coeff_derivated;
      }
208
209
210
211
212
213
214
215

      void check_if_not_empty() const
      {
        if (coefficients_.size() == 0)
        {
          throw std::runtime_error("Error in polynomial : there is no coefficients set / did you use empty constructor ?");
        }
      }
216
    /*Operations*/
217

218
    public:
219
      /*Helpers*/
220
221
222
      /// \brief Get dimension of curve.
      /// \return dimension of curve.
      std::size_t virtual dim() const{return dim_;};
223
224
225
226
227
228
229
230
231
232
233
234
      /// \brief Get the minimum time for which the curve is defined
      /// \return \f$t_{min}\f$ lower bound of time range.
      num_t virtual min() const {return T_min_;}
      /// \brief Get the maximum time for which the curve is defined.
      /// \return \f$t_{max}\f$ upper bound of time range.
      num_t virtual max() const {return T_max_;}
      /*Helpers*/

      /*Attributes*/
      coeff_t coefficients_; //const
      std::size_t dim_; //const
      std::size_t degree_; //const
235
      time_t T_min_, T_max_; //const
236
      /*Attributes*/
237
238

    private:
239
240
241
      template<typename In>
      coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
      {
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)
245
        {
246
          res.col(i) = *cit;
247
        }
248
        return res;
249
      }
250

251
    public:
252
253
      // Serialization of the class
      friend class boost::serialization::access;
254

255
256
      template<class Archive>
      void serialize(Archive& ar, const unsigned int version){
257
        if (version) {
258
          // Do something depending on version ?
259
        }
260
        ar & boost::serialization::make_nvp("dim", dim_);
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_);
266
      }
267

268
  }; //class polynomial
269

270
} // namespace curves
271
#endif //_STRUCT_POLYNOMIAL
272