13 #ifndef _STRUCT_POLYNOM 14 #define _STRUCT_POLYNOM 31 template <
typename Time = double,
typename Numeric =
Time, std::size_t Dim = 3,
bool Safe =
false,
32 typename Point = Eigen::Matrix<Numeric, Dim, 1>,
33 typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> > >
40 typedef Eigen::Matrix<double, Dim, Eigen::Dynamic>
coeff_t;
51 polynom(
const coeff_t& coefficients,
const time_t
min,
const time_t
max)
67 polynom(
const T_Point& coefficients,
const time_t
min,
const time_t
max)
69 coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
83 template <
typename In>
84 polynom(In zeroOrderCoefficient, In out,
const time_t
min,
const time_t
max)
102 t_min_(other.t_min_),
103 t_max_(other.t_max_) {}
110 if (t_min_ > t_max_) std::out_of_range(
"TODO");
111 if (
coefficients_.size() != int(
order_ + 1)) std::runtime_error(
"Spline order and coefficients do not match");
137 if ((t < t_min_ || t > t_max_) && Safe) {
138 throw std::out_of_range(
"TODO");
140 time_t
const dt(t - t_min_);
150 virtual point_t
derivate(
const time_t t,
const std::size_t order)
const {
151 if ((t < t_min_ || t > t_max_) && Safe) {
152 throw std::out_of_range(
"TODO");
154 time_t
const dt(t - t_min_);
156 point_t currentPoint_ = point_t::Zero();
157 for (
int i = (
int)(order); i < (int)(
order_ + 1); ++i, cdt *= dt)
158 currentPoint_ += cdt *
coefficients_.col(i) * fact(i, order);
159 return currentPoint_;
163 num_t fact(
const std::size_t n,
const std::size_t order)
const {
165 for (std::size_t i = 0; i < order; ++i) res *= (num_t)(n - i);
174 num_t
virtual min()
const {
return t_min_; }
176 num_t
virtual max()
const {
return t_max_; }
186 time_t t_min_, t_max_;
190 template <
typename In>
191 coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
192 std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
193 coeff_t res =
coeff_t(Dim, size);
195 for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i) res.col(i) = *cit;
200 #endif //_STRUCT_POLYNOM ~polynom()
Destructor.
Definition: polynom.h:94
Point point_t
Definition: polynom.h:35
Time time_t
Definition: polynom.h:37
curve_abc< Time, Numeric, Dim, Safe, Point > curve_abc_t
Definition: polynom.h:39
Definition: bernstein.h:20
T_Point t_point_t
Definition: polynom.h:36
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
virtual num_t min() const
Returns the minimum time for wich curve is defined.
Definition: polynom.h:174
interface for a Curve of arbitrary dimension.
Eigen::Ref< coeff_t > coeff_t_ref
Definition: polynom.h:41
Numeric num_t
Definition: polynom.h:38
double Time
Definition: effector_spline.h:27
double Numeric
Definition: effector_spline.h:26
std::size_t dim_
Definition: polynom.h:182
polynom(const polynom &other)
Definition: polynom.h:98
coeff_t coefficients_
Definition: polynom.h:181
polynom(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:84
virtual point_t operator()(const time_t t) const
Evaluation of the cubic spline at time t using horner's scheme.
Definition: polynom.h:136
Represents a polynomf arbitrary order defined on the interval [tBegin, tEnd]. It follows the equation...
Definition: polynom.h:34
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: curve_abc.h:24
std::size_t order_
Definition: polynom.h:183
polynom(const T_Point &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:67
virtual num_t max() const
Returns the maximum time for wich curve is defined.
Definition: polynom.h:176
virtual point_t derivate(const time_t t, const std::size_t order) const
Evaluation of the derivative spline at time t.
Definition: polynom.h:150
polynom(const coeff_t &coefficients, const time_t min, const time_t max)
Constructor.
Definition: polynom.h:51
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
Eigen::Matrix< double, Dim, Eigen::Dynamic > coeff_t
Definition: polynom.h:40