polynom.h 6.9 KB
Newer Older
1
/**
2
* \file polynom.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 polynom 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_POLYNOM
#define _STRUCT_POLYNOM
16
17
18
19
20
21
22
23
24
25
26
27

#include "MathDefs.h"

#include "curve_abc.h"

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

namespace spline
{
28
29
/// \class polynom
/// \brief Represents a polynomf arbitrary order defined on the interval
30
31
32
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - t_min_) + ... + d(t - t_min_)^N, where N is the order
///
33
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
34
         typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
35
struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
36
37
{
    typedef Point 	point_t;
38
    typedef T_Point t_point_t;
39
40
    typedef Time 	time_t;
    typedef Numeric	num_t;
41
    typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
42
43
44
    typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
    typedef Eigen::Ref<coeff_t> coeff_t_ref;

45
46
/* Constructors - destructors */
    public:
47
    ///\brief Constructor
Steve Tonneau's avatar
Steve Tonneau committed
48
49
50
    ///\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.
51
52
    ///\param min: LOWER bound on interval definition of the spline
    ///\param max: UPPER bound on interval definition of the spline
53
    polynom(const coeff_t& coefficients, const time_t min, const time_t max)
54
        : curve_abc_t(),
stevet's avatar
stevet committed
55
          coefficients_(coefficients), dim_(Dim), order_(coefficients_.cols()-1), t_min_(min), t_max_(max)
56
57
58
59
    {
        safe_check();
    }

60
61
    ///\brief Constructor
    ///\param coefficients : a container containing all coefficients of the spline, starting
62
63
    /// with the zero order coefficient, up to the highest order. Spline order is given
    /// by the size of the coefficients
64
65
    ///\param min: LOWER bound on interval definition of the spline
    ///\param max: UPPER bound on interval definition of the spline
66
    polynom(const T_Point& coefficients, const time_t min, const time_t max)
67
        : curve_abc_t(),
68
          coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
stevet's avatar
stevet committed
69
          dim_(Dim), order_(coefficients_.cols()-1), t_min_(min), t_max_(max)
70
    {
71
        safe_check();
72
73
74
75
76
77
78
79
80
    }

    ///\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>
81
    polynom(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
stevet's avatar
stevet committed
82
83
        :coefficients_(init_coeffs(zeroOrderCoefficient, out)),
          dim_(Dim), order_(coefficients_.cols()-1), t_min_(min), t_max_(max)
84
    {
85
        safe_check();
86
87
88
    }

    ///\brief Destructor
89
    ~polynom()
90
91
92
93
    {
        // NOTHING
    }

Steve Tonneau's avatar
Steve Tonneau committed
94

95
    polynom(const polynom& other)
stevet's avatar
stevet committed
96
97
        : coefficients_(other.coefficients_),
          dim_(other.dim_), order_(other.order_), t_min_(other.t_min_), t_max_(other.t_max_){}
Steve Tonneau's avatar
Steve Tonneau committed
98

99

100
    //polynom& operator=(const polynom& other);
101
102

    private:
103
104
105
106
107
108
    void safe_check()
    {
        if(Safe)
        {
            if(t_min_ > t_max_)
                std::out_of_range("TODO");
stevet's avatar
stevet committed
109
            if(coefficients_.size() != int(order_+1))
110
111
112
                std::runtime_error("Spline order and coefficients do not match");
        }
    }
Steve Tonneau's avatar
Steve Tonneau committed
113

114
115
116
117
/* Constructors - destructors */

/*Operations*/
    public:
118
    /*///  \brief Evaluation of the cubic spline at time t.
119
120
    ///  \param t : the time when to evaluate the spine
    ///  \param return : the value x(t)
121
    virtual point_t operator()(const time_t t) const
122
123
124
    {
        if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
        time_t const dt (t-t_min_);
125
126
127
128
        time_t cdt(1);
        point_t currentPoint_ = point_t::Zero();
        for(int i = 0; i < order_+1; ++i, cdt*=dt)
            currentPoint_ += cdt *coefficients_.col(i);
129
        return currentPoint_;
130
131
132
133
134
135
136
137
138
139
140
    }*/


    ///  \brief Evaluation of the cubic spline at time t using horner's scheme.
    ///  \param t : the time when to evaluate the spine
    ///  \param return : the value x(t)
    virtual point_t operator()(const time_t t) const
    {
        if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
        time_t const dt (t-t_min_);
        point_t h = coefficients_.col(order_);
stevet's avatar
stevet committed
141
        for(int i=(int)(order_-1); i>=0; i--)
142
143
            h = dt*h + coefficients_.col(i);
        return h;
144
    }
145
146
147


    ///  \brief Evaluation of the derivative spline at time t.
148
149
    ///  \param t : the time when to evaluate the spline
    ///  \param order : order of the derivative
150
    ///  \param return : the value x(t)
151
    virtual point_t derivate(const time_t t, const std::size_t order) const
152
153
154
155
156
    {
        if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
        time_t const dt (t-t_min_);
        time_t cdt(1);
        point_t currentPoint_ = point_t::Zero();
stevet's avatar
stevet committed
157
        for(int i = (int)(order); i < (int)(order_+1); ++i, cdt*=dt)
158
159
160
161
162
163
164
            currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);
        return currentPoint_;
    }

    private:
    num_t fact(const std::size_t n, const std::size_t order) const
    {
stevet's avatar
stevet committed
165
        num_t res(1);
166
        for(std::size_t i = 0; i < order; ++i)
stevet's avatar
stevet committed
167
            res *= (num_t)(n-i);
168
169
170
        return res;
    }

171
172
173
174
175
176
177
178
179
180
181
182
/*Operations*/

/*Helpers*/
    public:
    ///  \brief Returns the minimum time for wich curve is defined
    num_t virtual min() const {return t_min_;}
    ///  \brief Returns the maximum time for wich curve is defined
    num_t virtual max() const {return t_max_;}
/*Helpers*/

/*Attributes*/
    public:
183
184
185
    coeff_t coefficients_; //const
    std::size_t dim_; //const
    std::size_t order_; //const
186
187
188

    private:
    time_t t_min_, t_max_;
189
190
191
192
/*Attributes*/

    private:
    template<typename In>
193
    coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
194
    {
195
196
197
198
        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)
            res.col(i) = *cit;
199
200
        return res;
    }
201
}; //class polynom
202
}
203
#endif //_STRUCT_POLYNOM
204