polynom.h 6.87 KB
 Steve Tonneau committed Nov 25, 2016 1 ``````/** `````` Steve Tonneau committed Apr 12, 2017 2 ``````* \file polynom.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 * `````` Steve Tonneau committed Apr 12, 2017 8 ``````* This file contains definitions for the polynom 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 ``````*/ `````` Steve Tonneau committed Apr 12, 2017 14 15 ``````#ifndef _STRUCT_POLYNOM #define _STRUCT_POLYNOM `````` Steve Tonneau committed Nov 25, 2016 16 17 18 19 20 21 22 23 24 25 26 27 `````` #include "MathDefs.h" #include "curve_abc.h" #include #include #include #include namespace spline { `````` Steve Tonneau committed Apr 12, 2017 28 29 ``````/// \class polynom /// \brief Represents a polynomf arbitrary order defined on the interval `````` Steve Tonneau committed Nov 25, 2016 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 /// `````` Steve Tonneau committed Nov 25, 2016 33 ``````template, typename T_Point =std::vector > > `````` Steve Tonneau committed Apr 12, 2017 35 ``````struct polynom : public curve_abc `````` Steve Tonneau committed Nov 25, 2016 36 37 ``````{ typedef Point point_t; `````` Steve Tonneau committed Apr 12, 2017 38 `````` typedef T_Point t_point_t; `````` Steve Tonneau committed Nov 25, 2016 39 40 `````` typedef Time time_t; typedef Numeric num_t; `````` Steve Tonneau committed Nov 25, 2016 41 `````` typedef curve_abc curve_abc_t; `````` Steve Tonneau committed Nov 25, 2016 42 43 44 `````` typedef Eigen::Matrix coeff_t; typedef Eigen::Ref coeff_t_ref; `````` Steve Tonneau committed Nov 25, 2016 45 46 ``````/* Constructors - destructors */ public: `````` Steve Tonneau committed Nov 25, 2016 47 `````` ///\brief Constructor `````` Steve Tonneau committed Nov 25, 2016 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. `````` Steve Tonneau committed Nov 25, 2016 51 52 `````` ///\param min: LOWER bound on interval definition of the spline ///\param max: UPPER bound on interval definition of the spline `````` Steve Tonneau committed Apr 12, 2017 53 `````` polynom(const coeff_t& coefficients, const time_t min, const time_t max) `````` Steve Tonneau committed Nov 25, 2016 54 55 56 57 58 59 `````` : curve_abc_t(), coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1) { safe_check(); } `````` Steve Tonneau committed Nov 25, 2016 60 61 `````` ///\brief Constructor ///\param coefficients : a container containing all coefficients of the spline, starting `````` Steve Tonneau committed Nov 25, 2016 62 63 `````` /// with the zero order coefficient, up to the highest order. Spline order is given /// by the size of the coefficients `````` Steve Tonneau committed Nov 25, 2016 64 65 `````` ///\param min: LOWER bound on interval definition of the spline ///\param max: UPPER bound on interval definition of the spline `````` Steve Tonneau committed Apr 12, 2017 66 `````` polynom(const T_Point& coefficients, const time_t min, const time_t max) `````` Steve Tonneau committed Nov 25, 2016 67 `````` : curve_abc_t(), `````` Steve Tonneau committed Nov 25, 2016 68 69 `````` coefficients_(init_coeffs(coefficients.begin(), coefficients.end())), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1) `````` Steve Tonneau committed Nov 25, 2016 70 `````` { `````` Steve Tonneau committed Nov 25, 2016 71 `````` safe_check(); `````` Steve Tonneau committed Nov 25, 2016 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 `````` Steve Tonneau committed Apr 12, 2017 81 `````` polynom(In zeroOrderCoefficient, In out, const time_t min, const time_t max) `````` Steve Tonneau committed Nov 25, 2016 82 `````` :coefficients_(init_coeffs(zeroOrderCoefficient, out)), dim_(Dim), order_(coefficients_.cols()-1), `````` Steve Tonneau committed Nov 25, 2016 83 `````` t_min_(min), t_max_(max) `````` Steve Tonneau committed Nov 25, 2016 84 `````` { `````` Steve Tonneau committed Nov 25, 2016 85 `````` safe_check(); `````` Steve Tonneau committed Nov 25, 2016 86 87 88 `````` } ///\brief Destructor `````` Steve Tonneau committed Apr 12, 2017 89 `````` ~polynom() `````` Steve Tonneau committed Nov 25, 2016 90 91 92 93 `````` { // NOTHING } `````` Steve Tonneau committed Nov 25, 2016 94 `````` `````` Steve Tonneau committed Apr 12, 2017 95 `````` polynom(const polynom& other) `````` Steve Tonneau committed Nov 25, 2016 96 97 `````` : coefficients_(other.coefficients_), dim_(other.dim_), order_(other.order_), t_min_(other.t_min_), t_max_(other.t_max_){} `````` Steve Tonneau committed Nov 25, 2016 98 `````` `````` 99 `````` `````` Steve Tonneau committed Apr 12, 2017 100 `````` //polynom& operator=(const polynom& other); `````` 101 102 `````` private: `````` Steve Tonneau committed Nov 25, 2016 103 104 105 106 107 108 109 110 111 112 `````` 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 committed Nov 25, 2016 113 `````` `````` Steve Tonneau committed Nov 25, 2016 114 115 116 117 ``````/* Constructors - destructors */ /*Operations*/ public: `````` Steve Tonneau committed Apr 12, 2017 118 `````` /*/// \brief Evaluation of the cubic spline at time t. `````` Steve Tonneau committed Nov 25, 2016 119 120 `````` /// \param t : the time when to evaluate the spine /// \param return : the value x(t) `````` Steve Tonneau committed Jan 26, 2017 121 `````` virtual point_t operator()(const time_t t) const `````` Steve Tonneau committed Nov 25, 2016 122 123 124 `````` { if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");} time_t const dt (t-t_min_); `````` Steve Tonneau committed Nov 25, 2016 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); `````` Steve Tonneau committed Nov 25, 2016 129 `````` return currentPoint_; `````` Steve Tonneau committed Apr 12, 2017 130 131 132 133 134 135 136 137 138 139 140 141 142 143 `````` }*/ /// \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; `````` Steve Tonneau committed Nov 25, 2016 144 `````` } `````` Steve Tonneau committed Nov 28, 2016 145 146 147 `````` /// \brief Evaluation of the derivative spline at time t. `````` Steve Tonneau committed Mar 14, 2017 148 149 `````` /// \param t : the time when to evaluate the spline /// \param order : order of the derivative `````` Steve Tonneau committed Nov 28, 2016 150 `````` /// \param return : the value x(t) `````` Steve Tonneau committed Mar 14, 2017 151 `````` virtual point_t derivate(const time_t t, const std::size_t order) const `````` Steve Tonneau committed Nov 28, 2016 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 `````` { 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; } `````` Steve Tonneau committed Nov 25, 2016 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 `````` Steve Tonneau committed Nov 25, 2016 186 187 188 `````` private: time_t t_min_, t_max_; `````` Steve Tonneau committed Nov 25, 2016 189 190 191 192 ``````/*Attributes*/ private: template `````` Steve Tonneau committed Nov 25, 2016 193 `````` coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) `````` Steve Tonneau committed Nov 25, 2016 194 `````` { `````` Steve Tonneau committed Nov 25, 2016 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; `````` Steve Tonneau committed Nov 25, 2016 199 200 `````` return res; } `````` Steve Tonneau committed Apr 12, 2017 201 ``````}; //class polynom `````` Steve Tonneau committed Nov 25, 2016 202 ``````} `````` Steve Tonneau committed Apr 12, 2017 203 ``````#endif //_STRUCT_POLYNOM `````` Steve Tonneau committed Nov 25, 2016 204