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