GCC Code Coverage Report

Directory: ./
File: include/parametric-curves/polynomial.hpp
Date: 2025-05-07 13:05:43
Exec Total Coverage
Lines: 65 84 77.4%
Branches: 43 94 45.7%
Line Branch Exec Source
1 /**
2 * \file polynomial.hpp
3 * \brief Definition of a cubic spline.
4 * \author Steve T.
5 * \version 0.1
6 * \date 06/17/2013
7 *
8 * This file contains definitions for the Polynomial struct.
9 * It allows the creation and evaluation of natural
10 * smooth splines of arbitrary dimension and order
11 */
12
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>
19 #include <parametric-curves/abstract-curve.hpp>
20 #include <parametric-curves/serialization/eigen-matrix.hpp>
21 #include <stdexcept>
22 #include <vector>
23
24 namespace parametriccurves {
25 /// \class Polynomial
26 /// \brief Represents a Polynomialf arbitrary order defined on the interval
27 /// [tBegin, tEnd]. It follows the equation
28 /// x(t) = a + b(t - t_min_) + ... + d(t - t_min_)^N, where N is the order
29 ///
30 template <typename Numeric = double, Eigen::Index Dim = 3,
31 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
32 struct 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;
37 typedef AbstractCurve<Numeric, Point> curve_abc_t;
38 typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
39 typedef Eigen::Ref<coeff_t> coeff_t_ref;
40
41 public:
42 ///\brief Constructor
43 ///\param coefficients : a reference to an Eigen matrix where each column is a
44 /// coefficient,
45 /// from the zero order coefficient, up to the highest order. Spline order is
46 /// given by the number of the columns -1.
47 ///\param min: LOWER bound on interval definition of the spline
48 ///\param max: UPPER bound on interval definition of the spline
49 1 Polynomial(const coeff_t& coefficients, const time_t tmin, const time_t tmax)
50 : curve_abc_t(tmin, tmax),
51 1 coefficients_(coefficients),
52 1 dim_(Dim),
53
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 order_(coefficients_.cols() - 1) {
54
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 safe_check();
55 1 }
56
57 ///\brief Constructor
58 ///\param coefficients : a container containing all coefficients of the
59 /// spline, starting
60 /// with the zero order coefficient, up to the highest order. Spline order is
61 /// given by the size of the coefficients
62 ///\param min: LOWER bound on interval definition of the spline
63 ///\param max: UPPER bound on interval definition of the spline
64 1 Polynomial(const t_point_t& coefficients, const time_t tmin,
65 const time_t tmax)
66 : curve_abc_t(tmin, tmax),
67
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
68 1 dim_(Dim),
69 2 order_(coefficients_.cols() - 1) {
70
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 safe_check();
71 1 }
72
73 ✗ Polynomial() {}
74
75 ///\brief Constructor
76 ///\param zeroOrderCoefficient : an iterator pointing to the first element of
77 /// a structure containing the coefficients
78 /// it corresponds to the zero degree coefficient
79 ///\param out : an iterator pointing to the last element of a structure
80 /// ofcoefficients \param min: LOWER bound on interval definition of the
81 /// spline \param max: UPPER bound on interval definition of the spline
82 template <typename In>
83 56 Polynomial(In zeroOrderCoefficient, In out, const time_t tmin,
84 const time_t tmax)
85 : curve_abc_t(tmin, tmax),
86 56 coefficients_(init_coeffs(zeroOrderCoefficient, out)),
87 56 dim_(Dim),
88
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 order_(coefficients_.cols() - 1) {
89
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 safe_check();
90 56 }
91
92 ///\brief Destructor
93 116 ~Polynomial() {
94 // NOTHING
95 116 }
96
97 54 Polynomial(const Polynomial& other)
98 54 : curve_abc_t(other.t_min, other.t_max),
99 54 coefficients_(other.coefficients_),
100 54 dim_(other.dim_),
101
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 54 order_(other.order_) {}
102
103 // Polynomial& operator=(const Polynomial& other);
104
105 private:
106 59 void safe_check() {
107
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 30 times.
 59 if (this->t_min > this->t_max) std::out_of_range("TODO");
108
2/2
✓ Branch 1 taken 27 times.
✓ Branch 2 taken 3 times.
 59 if (static_cast<std::size_t>(coefficients_.size()) != order_ + 1)
109 53 std::runtime_error("Spline order and coefficients do not match");
110 59 }
111
112 /* Constructors - destructors */
113
114 /*Operations*/
115 public:
116 /*/// \brief Evaluation of the cubic spline at time t.
117 /// \param t : the time when to evaluate the spine
118 /// \param return : the value x(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
129 /// \brief Evaluation of the cubic spline at time t using horner's scheme.
130 /// \param t : the time when to evaluate the spine
131 /// \param return : the value x(t)
132 60 virtual const point_t operator()(const time_t& t) const {
133
4/4
✓ Branch 0 taken 29 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 28 times.
 60 if ((t < this->t_min || t > this->t_max)) {
134
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 throw std::out_of_range("TODO");
135 }
136 56 const time_t& dt(t);
137
1/2
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
 56 point_t h = coefficients_.col(order_);
138 56 std::size_t i = order_ - 1;
139 56 bool ok = true;
140
1/2
✓ Branch 0 taken 28 times.
✗ Branch 1 not taken.
 56 if (order_ != 0) {
141
3/4
✓ Branch 0 taken 88 times.
✓ Branch 1 taken 28 times.
✓ Branch 2 taken 88 times.
✗ Branch 3 not taken.
 232 while (ok && order_ != 0) {
142
3/6
✓ Branch 2 taken 88 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 88 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 88 times.
✗ Branch 9 not taken.
 176 h = dt * h + coefficients_.col(i);
143
2/2
✓ Branch 0 taken 28 times.
✓ Branch 1 taken 60 times.
 176 if (i == 0)
144 56 ok = false;
145 else
146 120 i--;
147 }
148 56 return h;
149 } else
150 ✗ return h;
151 }
152
153 /// \brief Evaluation of the derivative spline at time t.
154 /// \param t : the time when to evaluate the spline
155 /// \param order : order of the derivative
156 /// \param return : the value x(t)
157 48 virtual const point_t derivate(const time_t& t,
158 const std::size_t& order) const {
159
2/4
✓ Branch 0 taken 24 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 24 times.
 48 if ((t < this->t_min || t > this->t_max)) {
160 ✗ throw std::out_of_range("TODO");
161 }
162 48 const time_t& dt(t);
163 48 time_t cdt(1);
164
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 48 point_t currentPoint_ = point_t::Zero(dim_);
165
2/2
✓ Branch 0 taken 68 times.
✓ Branch 1 taken 24 times.
 184 for (std::size_t i = order; i < order_ + 1; ++i, cdt *= dt)
166
4/8
✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 68 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 68 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 68 times.
✗ Branch 12 not taken.
 136 currentPoint_ += cdt * coefficients_.col(i) * fact(i, order);
167 96 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:
176 coeff_t coefficients_; // const
177 std::size_t dim_; // const
178 std::size_t order_; // const
179
180 private:
181 // Serialization of the class
182 friend class boost::serialization::access;
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 136 num_t fact(const std::size_t n, const std::size_t order) const {
204 136 std::size_t res(1);
205
2/2
✓ Branch 0 taken 96 times.
✓ Branch 1 taken 68 times.
 328 for (std::size_t i = 0; i < order; ++i) res *= n - i;
206 136 return static_cast<num_t>(res);
207 }
208
209 /*Attributes*/
210 template <typename In>
211 58 coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
212 58 std::size_t size =
213
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 58 std::distance(zeroOrderCoefficient, highestOrderCoefficient);
214
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
 58 coeff_t res = coeff_t(Dim, size);
215 58 int i = 0;
216
2/2
✓ Branch 1 taken 120 times.
✓ Branch 2 taken 29 times.
 298 for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient;
217 240 ++cit, ++i)
218
2/4
✓ Branch 2 taken 120 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
 240 res.col(i) = *cit;
219 116 return res;
220 }
221 }; // class Polynomial
222 } // namespace parametriccurves
223 #endif //_STRUCT_POLYNOMIAL
224