polynomial.hpp
Go to the documentation of this file.
1
13#ifndef _parameteric_curves_polynomial_hpp
14#define _parameteric_curves_polynomial_hpp
15
16#include <Eigen/Dense>
17#include <boost/archive/text_iarchive.hpp>
18#include <boost/archive/text_oarchive.hpp>
21#include <stdexcept>
22#include <vector>
23
24namespace parametriccurves {
30template <typename Numeric = double, Eigen::Index Dim = 3,
31 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
32struct Polynomial : public parametriccurves::AbstractCurve<Numeric, Point> {
33 typedef Point point_t;
34 typedef Numeric time_t;
35 typedef Numeric num_t;
36 typedef std::vector<Point, Eigen::aligned_allocator<Point> > t_point_t;
38 typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
39 typedef Eigen::Ref<coeff_t> coeff_t_ref;
40
41 public:
49 Polynomial(const coeff_t& coefficients, const time_t tmin, const time_t tmax)
51 coefficients_(coefficients),
52 dim_(Dim),
53 order_(coefficients_.cols() - 1) {
54 safe_check();
55 }
56
64 Polynomial(const t_point_t& coefficients, const time_t tmin,
65 const time_t tmax)
67 coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
68 dim_(Dim),
69 order_(coefficients_.cols() - 1) {
70 safe_check();
71 }
72
74
82 template <typename In>
83 Polynomial(In zeroOrderCoefficient, In out, const time_t tmin,
84 const time_t tmax)
86 coefficients_(init_coeffs(zeroOrderCoefficient, out)),
87 dim_(Dim),
88 order_(coefficients_.cols() - 1) {
89 safe_check();
90 }
91
94 // NOTHING
95 }
96
97 Polynomial(const Polynomial& other)
98 : curve_abc_t(other.t_min, other.t_max),
100 dim_(other.dim_),
101 order_(other.order_) {}
102
103 // Polynomial& operator=(const Polynomial& other);
104
105 private:
106 void safe_check() {
107 if (this->t_min > this->t_max) std::out_of_range("TODO");
108 if (static_cast<std::size_t>(coefficients_.size()) != order_ + 1)
109 std::runtime_error("Spline order and coefficients do not match");
110 }
111
112 /* Constructors - destructors */
113
114 /*Operations*/
115 public:
116 /*/// \brief Evaluation of the cubic spline at time t.
119 virtual point_t operator()(const time_t t) const
120 {
121 if((t < t_min_ || t > t_max_) && Safe){ throw
122 std::out_of_range("TODO");} time_t const dt (t-t_min_); time_t cdt(1);
123 point_t currentPoint_ = point_t::Zero();
124 for(int i = 0; i < order_+1; ++i, cdt*=dt)
125 currentPoint_ += cdt *coefficients_.col(i);
126 return currentPoint_;
127 }*/
128
132 virtual const point_t operator()(const time_t& t) const {
133 if ((t < this->t_min || t > this->t_max)) {
134 throw std::out_of_range("TODO");
135 }
136 const time_t& dt(t);
138 std::size_t i = order_ - 1;
139 bool ok = true;
140 if (order_ != 0) {
141 while (ok && order_ != 0) {
142 h = dt * h + coefficients_.col(i);
143 if (i == 0)
144 ok = false;
145 else
146 i--;
147 }
148 return h;
149 } else
150 return h;
151 }
152
157 virtual const point_t derivate(const time_t& t,
158 const std::size_t& order) const {
159 if ((t < this->t_min || t > this->t_max)) {
160 throw std::out_of_range("TODO");
161 }
162 const time_t& dt(t);
163 time_t cdt(1);
164 point_t currentPoint_ = point_t::Zero(dim_);
165 for (std::size_t i = order; i < order_ + 1; ++i, cdt *= dt)
166 currentPoint_ += cdt * coefficients_.col(i) * fact(i, order);
167 return currentPoint_;
168 }
169
170 virtual const std::size_t& size() const { return dim_; }
171
172 virtual bool setInitialPoint(const point_t& /*x_init*/) { return false; }
173 virtual bool setInitialPoint(const num_t& /*x_init*/) { return false; }
174
175 protected:
177 std::size_t dim_; // const
178 std::size_t order_; // const
179
180 private:
181 // Serialization of the class
183 template <class Archive>
184 void save(Archive& ar, const unsigned int /*version*/) const {
185 ar& boost::serialization::make_nvp("dim", dim_);
186 ar& boost::serialization::make_nvp("order", order_);
187 ar& boost::serialization::make_nvp("coefficients", coefficients_);
188 ar& boost::serialization::make_nvp("t_min", this->t_min);
189 ar& boost::serialization::make_nvp("t_max", this->t_max);
190 }
191
192 template <class Archive>
193 void load(Archive& ar, const unsigned int /*version*/) {
194 ar& boost::serialization::make_nvp("dim", dim_);
195 ar& boost::serialization::make_nvp("order", order_);
196 ar& boost::serialization::make_nvp("coefficients", coefficients_);
197 ar& boost::serialization::make_nvp("t_min", this->t_min);
198 ar& boost::serialization::make_nvp("t_max", this->t_max);
199 }
200
201 BOOST_SERIALIZATION_SPLIT_MEMBER()
202
203 num_t fact(const std::size_t n, const std::size_t order) const {
204 std::size_t res(1);
205 for (std::size_t i = 0; i < order; ++i) res *= n - i;
206 return static_cast<num_t>(res);
207 }
208
209 /*Attributes*/
210 template <typename In>
211 coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
212 std::size_t size =
213 std::distance(zeroOrderCoefficient, highestOrderCoefficient);
214 coeff_t res = coeff_t(Dim, size);
215 int i = 0;
216 for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient;
217 ++cit, ++i)
218 res.col(i) = *cit;
219 return res;
220 }
221}; // class Polynomial
222} // namespace parametriccurves
223#endif //_STRUCT_POLYNOMIAL
void load(Archive &ar, Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &m, const unsigned int)
Definition: eigen-matrix.hpp:50
void save(Archive &ar, const Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &m, const unsigned int)
Definition: eigen-matrix.hpp:38
Definition: abstract-curve.hpp:16
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
double Numeric
Definition: effector_spline.h:26
unsigned int fact(const unsigned int n)
Computes factorial of a number.
Definition: bernstein.h:24
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: abstract-curve.hpp:21
virtual const time_t tmin() const
Definition: abstract-curve.hpp:47
virtual const time_t tmax() const
Definition: abstract-curve.hpp:48
Represents a Polynomialf arbitrary order defined on the interval [tBegin, tEnd]. It follows the equat...
Definition: polynomial.hpp:32
virtual bool setInitialPoint(const num_t &)
Definition: polynomial.hpp:173
Eigen::Ref< coeff_t > coeff_t_ref
Definition: polynomial.hpp:39
virtual bool setInitialPoint(const point_t &)
Definition: polynomial.hpp:172
Polynomial(const t_point_t &coefficients, const time_t tmin, const time_t tmax)
Constructor.
Definition: polynomial.hpp:64
virtual const point_t derivate(const time_t &t, const std::size_t &order) const
Evaluation of the derivative spline at time t.
Definition: polynomial.hpp:157
Polynomial(const Polynomial &other)
Definition: polynomial.hpp:97
AbstractCurve< Numeric, Point > curve_abc_t
Definition: polynomial.hpp:37
Polynomial()
Definition: polynomial.hpp:73
virtual const std::size_t & size() const
Definition: polynomial.hpp:170
std::size_t dim_
Definition: polynomial.hpp:177
Numeric time_t
Definition: polynomial.hpp:34
virtual const point_t operator()(const time_t &t) const
Evaluation of the cubic spline at time t using horner's scheme.
Definition: polynomial.hpp:132
Numeric num_t
Definition: polynomial.hpp:35
std::size_t order_
Definition: polynomial.hpp:178
Point point_t
Definition: polynomial.hpp:33
~Polynomial()
Destructor.
Definition: polynomial.hpp:93
Eigen::Matrix< double, Dim, Eigen::Dynamic > coeff_t
Definition: polynomial.hpp:38
coeff_t coefficients_
Definition: polynomial.hpp:176
Polynomial(In zeroOrderCoefficient, In out, const time_t tmin, const time_t tmax)
Constructor.
Definition: polynomial.hpp:83
friend class boost::serialization::access
Definition: polynomial.hpp:182
std::vector< Point, Eigen::aligned_allocator< Point > > t_point_t
Definition: polynomial.hpp:36
Polynomial(const coeff_t &coefficients, const time_t tmin, const time_t tmax)
Constructor.
Definition: polynomial.hpp:49