Loading...
Searching...
No Matches
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:
52 dim_(Dim),
54 safe_check();
55 }
56
65 const time_t tmax)
68 dim_(Dim),
70 safe_check();
71 }
72
74
82 template <typename In>
84 const time_t tmax)
87 dim_(Dim),
89 safe_check();
90 }
91
94 // NOTHING
95 }
96
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
Definition abstract-curve.hpp:16
Eigen::Matrix< Numeric, 3, 1 > Point
Definition effector_spline.h:28
double Numeric
Definition effector_spline.h:26
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
time_t t_max
Definition abstract-curve.hpp:65
time_t t_min
Definition abstract-curve.hpp:64
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
Definition curve-constraint.hpp:18