polynom.h 6.84 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 38 39
{
    typedef Point 	point_t;
    typedef Time 	time_t;
    typedef Numeric	num_t;
40
    typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
41 42 43
    typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
    typedef Eigen::Ref<coeff_t> coeff_t_ref;

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

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

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

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

Steve Tonneau's avatar
Steve Tonneau committed
93

94
    polynom(const polynom& other)
95 96
        : 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
97

98

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

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

113 114 115 116
/* Constructors - destructors */

/*Operations*/
    public:
117
    /*///  \brief Evaluation of the cubic spline at time t.
118 119
    ///  \param t : the time when to evaluate the spine
    ///  \param return : the value x(t)
120
    virtual point_t operator()(const time_t t) const
121 122 123
    {
        if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
        time_t const dt (t-t_min_);
124 125 126 127
        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);
128
        return currentPoint_;
129 130 131 132 133 134 135 136 137 138 139 140 141 142
    }*/


    ///  \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_);
        for(int i=order_-1; i>=0; i--)
            h = dt*h + coefficients_.col(i);
        return h;
143
    }
144 145 146


    ///  \brief Evaluation of the derivative spline at time t.
147 148
    ///  \param t : the time when to evaluate the spline
    ///  \param order : order of the derivative
149
    ///  \param return : the value x(t)
150
    virtual point_t derivate(const time_t t, const std::size_t order) const
151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169
    {
        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();
        for(int i = order; i < order_+1; ++i, cdt*=dt)
            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
    {
        std::size_t res(1);
        for(std::size_t i = 0; i < order; ++i)
            res *= n-i;
        return res;
    }

170 171 172 173 174 175 176 177 178 179 180 181
/*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:
182 183 184
    coeff_t coefficients_; //const
    std::size_t dim_; //const
    std::size_t order_; //const
185 186 187

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

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