GCC Code Coverage Report

Directory: ./
File: include/ndcurves/polynomial.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 321 336 95.5%
Branches: 377 704 53.6%
Line Branch Exec Source
1 /**
2 * \file polynomial.h
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 _STRUCT_POLYNOMIAL
14 #define _STRUCT_POLYNOMIAL
15
16 #include <algorithm>
17 #include <functional>
18 #include <iostream>
19 #include <stdexcept>
20
21 #include "MathDefs.h"
22 #include "curve_abc.h"
23
24 namespace ndcurves {
25 /// \class polynomial.
26 /// \brief Represents a polynomial of an arbitrary order defined on the interval
27 /// \f$[t_{min}, t_{max}]\f$. It follows the equation :<br>
28 /// \f$ x(t) = a + b(t - t_{min}) + ... + d(t - t_{min})^N \f$<br>
29 /// where N is the order and \f$ t \in [t_{min}, t_{max}] \f$.
30 ///
31 template <typename Time = double, typename Numeric = Time, bool Safe = false,
32 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
33 typename T_Point =
34 std::vector<Point, Eigen::aligned_allocator<Point> > >
35 struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
36 typedef Point point_t;
37 typedef T_Point t_point_t;
38 typedef Time time_t;
39 typedef Numeric num_t;
40 typedef curve_abc<Time, Numeric, Safe, Point> curve_abc_t;
41 typedef Eigen::MatrixXd coeff_t;
42 typedef Eigen::Ref<coeff_t> coeff_t_ref;
43 typedef polynomial<Time, Numeric, Safe, Point, T_Point> polynomial_t;
44 typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
45
46 /* Constructors - destructors */
47 public:
48 /// \brief Empty constructor. Curve obtained this way can not perform other
49 /// class functions.
50 ///
51
1/2
✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
 135 polynomial() : curve_abc_t(), dim_(0), degree_(0), T_min_(0), T_max_(1.0) {}
52
53 /// \brief Constructor.
54 /// \param coefficients : a reference to an Eigen matrix where each column is
55 /// a coefficient, from the zero order coefficient, up to the highest order.
56 /// Spline order is given by the number of the columns -1. \param min : LOWER
57 /// bound on interval definition of the curve. \param max : UPPER bound on
58 /// interval definition of the curve.
59 112 polynomial(const coeff_t& coefficients, const time_t min, const time_t max)
60 : curve_abc_t(),
61 224 dim_(coefficients.rows()),
62
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 112 coefficients_(coefficients),
63 112 degree_(coefficients.cols() - 1),
64 112 T_min_(min),
65 112 T_max_(max) {
66
2/2
✓ Branch 1 taken 67 times.
✓ Branch 2 taken 2 times.
 112 safe_check();
67 115 }
68
69 /// \brief Constructor
70 /// \param coefficients : a container containing all coefficients of the
71 /// spline, starting
72 /// with the zero order coefficient, up to the highest order. Spline order is
73 /// given by the size of the coefficients.
74 /// \param min : LOWER bound on interval definition of the spline.
75 /// \param max : UPPER bound on interval definition of the spline.
76 42 polynomial(const T_Point& coefficients, const time_t min, const time_t max)
77 : curve_abc_t(),
78 42 dim_(coefficients.begin()->size()),
79
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
80 42 degree_(coefficients_.cols() - 1),
81 42 T_min_(min),
82 42 T_max_(max) {
83
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 safe_check();
84 42 }
85
86 /// \brief Constructor.
87 /// \param zeroOrderCoefficient : an iterator pointing to the first element of
88 /// a structure containing the coefficients
89 /// it corresponds to the zero degree coefficient.
90 /// \param out : an iterator pointing to the last element of a structure
91 /// ofcoefficients. \param min : LOWER bound on interval definition of the
92 /// spline. \param max : UPPER bound on interval definition of the spline.
93 template <typename In>
94 163 polynomial(In zeroOrderCoefficient, In out, const time_t min,
95 const time_t max)
96 : curve_abc_t(),
97 163 dim_(zeroOrderCoefficient->size()),
98
1/2
✓ Branch 1 taken 83 times.
✗ Branch 2 not taken.
 163 coefficients_(init_coeffs(zeroOrderCoefficient, out)),
99 163 degree_(coefficients_.cols() - 1),
100 163 T_min_(min),
101 163 T_max_(max) {
102
1/2
✓ Branch 1 taken 83 times.
✗ Branch 2 not taken.
 163 safe_check();
103 163 }
104
105 ///
106 /// \brief Constructor from boundary condition with C0 : create a polynomial
107 /// that connect exactly init and end (order 1) \param init the initial point
108 /// of the curve \param end the final point of the curve \param min : LOWER
109 /// bound on interval definition of the spline. \param max : UPPER bound on
110 /// interval definition of the spline.
111 ///
112 80 polynomial(const Point& init, const Point& end, const time_t min,
113 const time_t max)
114
1/2
✓ Branch 3 taken 40 times.
✗ Branch 4 not taken.
 80 : dim_(init.size()), degree_(1), T_min_(min), T_max_(max) {
115
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 36 times.
 80 if (T_min_ >= T_max_)
116
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 throw std::invalid_argument("T_min must be strictly lower than T_max");
117
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 35 times.
 72 if (init.size() != end.size())
118
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument(
119 "init and end points must have the same dimensions.");
120
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 70 t_point_t coeffs;
121
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 70 coeffs.push_back(init);
122
4/8
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35 times.
✗ Branch 11 not taken.
 70 coeffs.push_back((end - init) / (max - min));
123
1/2
✓ Branch 3 taken 35 times.
✗ Branch 4 not taken.
 70 coefficients_ = init_coeffs(coeffs.begin(), coeffs.end());
124
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 70 safe_check();
125 90 }
126
127 ///
128 /// \brief Constructor from boundary condition with C1 :
129 /// create a polynomial that connect exactly init and end and thier first
130 /// order derivatives(order 3) \param init the initial point of the curve
131 /// \param d_init the initial value of the derivative of the curve
132 /// \param end the final point of the curve
133 /// \param d_end the final value of the derivative of the curve
134 /// \param min : LOWER bound on interval definition of the spline.
135 /// \param max : UPPER bound on interval definition of the spline.
136 ///
137 38 polynomial(const Point& init, const Point& d_init, const Point& end,
138 const Point& d_end, const time_t min, const time_t max)
139
1/2
✓ Branch 3 taken 19 times.
✗ Branch 4 not taken.
 38 : dim_(init.size()), degree_(3), T_min_(min), T_max_(max) {
140
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 16 times.
 38 if (T_min_ >= T_max_)
141
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 throw std::invalid_argument("T_min must be strictly lower than T_max");
142
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
 32 if (init.size() != end.size())
143 ✗ throw std::invalid_argument(
144 "init and end points must have the same dimensions.");
145
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
 32 if (init.size() != d_init.size())
146 ✗ throw std::invalid_argument(
147 "init and d_init points must have the same dimensions.");
148
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
 32 if (init.size() != d_end.size())
149 ✗ throw std::invalid_argument(
150 "init and d_end points must have the same dimensions.");
151 /* the coefficients [c0 c1 c2 c3] are found by solving the following system
152 of equation (found from the boundary conditions) : [1 0 0 0 ] [c0]
153 [ init ] [1 T T^2 T^3 ] x [c1] = [ end ] [0 1 0 0 ] [c2] [d_init]
154 [0 1 2T 3T^2] [c3] [d_end ]
155 */
156 32 double T = max - min;
157
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Eigen::Matrix<double, 4, 4> m;
158
15/30
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 16 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 16 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 16 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 16 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 16 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 16 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 16 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 16 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 16 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 16 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 16 times.
✗ Branch 44 not taken.
 32 m << 1., 0, 0, 0, 1., T, T * T, T * T * T, 0, 1., 0, 0, 0, 1., 2. * T,
159
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 3. * T * T;
160
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 Eigen::Matrix<double, 4, 4> m_inv = m.inverse();
161
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Eigen::Matrix<double, 4, 1> bc; // boundary condition vector
162
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 coefficients_ = coeff_t::Zero(
163
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 dim_, degree_ + 1); // init coefficient matrix with the right size
164
2/2
✓ Branch 0 taken 48 times.
✓ Branch 1 taken 16 times.
 128 for (size_t i = 0; i < dim_;
165 ++i) { // for each dimension, solve the boundary condition problem :
166
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
 96 bc[0] = init[i];
167
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
 96 bc[1] = end[i];
168
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
 96 bc[2] = d_init[i];
169
2/4
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
 96 bc[3] = d_end[i];
170
4/8
✓ Branch 1 taken 48 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 48 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 48 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 48 times.
✗ Branch 11 not taken.
 96 coefficients_.row(i) = (m_inv * bc).transpose();
171 }
172
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 safe_check();
173 44 }
174
175 ///
176 /// \brief Constructor from boundary condition with C2 :
177 /// create a polynomial that connect exactly init and end and thier first and
178 /// second order derivatives(order 5) \param init the initial point of the
179 /// curve \param d_init the initial value of the derivative of the curve
180 /// \param d_init the initial value of the second derivative of the curve
181 /// \param end the final point of the curve
182 /// \param d_end the final value of the derivative of the curve
183 /// \param d_end the final value of the second derivative of the curve
184 /// \param min : LOWER bound on interval definition of the spline.
185 /// \param max : UPPER bound on interval definition of the spline.
186 ///
187 7882 polynomial(const Point& init, const Point& d_init, const Point& dd_init,
188 const Point& end, const Point& d_end, const Point& dd_end,
189 const time_t min, const time_t max)
190
1/2
✓ Branch 3 taken 7870 times.
✗ Branch 4 not taken.
 7882 : dim_(init.size()), degree_(5), T_min_(min), T_max_(max) {
191
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 7867 times.
 7882 if (T_min_ >= T_max_)
192
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 5 throw std::invalid_argument("T_min must be strictly lower than T_max");
193
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 7867 times.
 7877 if (init.size() != end.size())
194 ✗ throw std::invalid_argument(
195 "init and end points must have the same dimensions.");
196
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 7867 times.
 7877 if (init.size() != d_init.size())
197 ✗ throw std::invalid_argument(
198 "init and d_init points must have the same dimensions.");
199
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 7867 times.
 7877 if (init.size() != d_end.size())
200 ✗ throw std::invalid_argument(
201 "init and d_end points must have the same dimensions.");
202
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 7867 times.
 7877 if (init.size() != dd_init.size())
203 ✗ throw std::invalid_argument(
204 "init and dd_init points must have the same dimensions.");
205
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 7867 times.
 7877 if (init.size() != dd_end.size())
206 ✗ throw std::invalid_argument(
207 "init and dd_end points must have the same dimensions.");
208 /* the coefficients [c0 c1 c2 c3 c4 c5] are found by solving the following
209 system of equation (found from the boundary conditions) : [1 0 0 0 0
210 0 ] [c0] [ init ] [1 T T^2 T^3 T^4 T^5 ] [c1] [ end ] [0
211 1 0 0 0 0 ] [c2] [d_init ] [0 1 2T 3T^2 4T^3 5T^4 ] x
212 [c3] = [d_end ] [0 0 2 0 0 0 ] [c4] [dd_init] [0 0 2 6T
213 12T^2 20T^3] [c5] [dd_end ]
214 */
215 7877 double T = max - min;
216
1/2
✓ Branch 1 taken 7867 times.
✗ Branch 2 not taken.
 7877 Eigen::Matrix<double, 6, 6> m;
217
13/26
✓ Branch 1 taken 7867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7867 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7867 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7867 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7867 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7867 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7867 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7867 times.
✗ Branch 26 not taken.
✓ Branch 29 taken 7867 times.
✗ Branch 30 not taken.
✓ Branch 33 taken 7867 times.
✗ Branch 34 not taken.
✓ Branch 37 taken 7867 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 7867 times.
✗ Branch 41 not taken.
 7877 m << 1., 0, 0, 0, 0, 0, 1., T, T * T, pow(T, 3), pow(T, 4), pow(T, 5), 0,
218
10/20
✓ Branch 1 taken 7867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7867 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7867 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7867 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7867 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7867 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7867 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7867 times.
✗ Branch 26 not taken.
✓ Branch 29 taken 7867 times.
✗ Branch 30 not taken.
 7877 1., 0, 0, 0, 0, 0, 1., 2. * T, 3. * T * T, 4. * pow(T, 3),
219
12/24
✓ Branch 2 taken 7867 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7867 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7867 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7867 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7867 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7867 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7867 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7867 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7867 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 7867 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 7867 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 7867 times.
✗ Branch 36 not taken.
 7877 5. * pow(T, 4), 0, 0, 2, 0, 0, 0, 0, 0, 2, 6. * T, 12. * T * T,
220
1/2
✓ Branch 2 taken 7867 times.
✗ Branch 3 not taken.
 7877 20. * pow(T, 3);
221
2/4
✓ Branch 1 taken 7867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7867 times.
✗ Branch 5 not taken.
 7877 Eigen::Matrix<double, 6, 6> m_inv = m.inverse();
222
1/2
✓ Branch 1 taken 7867 times.
✗ Branch 2 not taken.
 7877 Eigen::Matrix<double, 6, 1> bc; // boundary condition vector
223
1/2
✓ Branch 1 taken 7867 times.
✗ Branch 2 not taken.
 7877 coefficients_ = coeff_t::Zero(
224
1/2
✓ Branch 1 taken 7867 times.
✗ Branch 2 not taken.
 7877 dim_, degree_ + 1); // init coefficient matrix with the right size
225
2/2
✓ Branch 0 taken 23601 times.
✓ Branch 1 taken 7867 times.
 31508 for (size_t i = 0; i < dim_;
226 ++i) { // for each dimension, solve the boundary condition problem :
227
2/4
✓ Branch 1 taken 23601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23601 times.
✗ Branch 5 not taken.
 23631 bc[0] = init[i];
228
2/4
✓ Branch 1 taken 23601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23601 times.
✗ Branch 5 not taken.
 23631 bc[1] = end[i];
229
2/4
✓ Branch 1 taken 23601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23601 times.
✗ Branch 5 not taken.
 23631 bc[2] = d_init[i];
230
2/4
✓ Branch 1 taken 23601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23601 times.
✗ Branch 5 not taken.
 23631 bc[3] = d_end[i];
231
2/4
✓ Branch 1 taken 23601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23601 times.
✗ Branch 5 not taken.
 23631 bc[4] = dd_init[i];
232
2/4
✓ Branch 1 taken 23601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23601 times.
✗ Branch 5 not taken.
 23631 bc[5] = dd_end[i];
233
4/8
✓ Branch 1 taken 23601 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23601 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23601 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23601 times.
✗ Branch 11 not taken.
 23631 coefficients_.row(i) = (m_inv * bc).transpose();
234 }
235
1/2
✓ Branch 1 taken 7867 times.
✗ Branch 2 not taken.
 7877 safe_check();
236 7887 }
237
238 /// \brief Destructor
239 32688 virtual ~polynomial() {}
240
241 16200 polynomial(const polynomial& other)
242 16200 : dim_(other.dim_),
243 16200 coefficients_(other.coefficients_),
244 16200 degree_(other.degree_),
245 16200 T_min_(other.T_min_),
246
1/2
✓ Branch 2 taken 8114 times.
✗ Branch 3 not taken.
 16200 T_max_(other.T_max_) {}
247
248 // polynomial& operator=(const polynomial& other);
249
250 /**
251 * @brief MinimumJerk Build a polynomial curve connecting p_init to p_final
252 * minimizing the time integral of the squared jerk with a zero initial and
253 * final velocity and acceleration
254 * @param p_init the initial point
255 * @param p_final the final point
256 * @param t_min initial time
257 * @param t_max final time
258 * @return the polynomial curve
259 */
260 33 static polynomial_t MinimumJerk(const point_t& p_init, const point_t& p_final,
261 const time_t t_min = 0.,
262 const time_t t_max = 1.) {
263
3/4
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 26 times.
✓ Branch 7 taken 2 times.
 39 polynomial_t out =
264 polynomial(coeff_t::Zero(p_init.size(), 6), t_min, t_max);
265
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 2 times.
 30 MinimumJerk(out, p_init, p_final, t_min, t_max);
266 27 return out;
267 3 }
268
269 /**
270 * @brief MinimumJerk Build a polynomial curve connecting p_init to p_final
271 * minimizing the time integral of the squared jerk with a zero initial and
272 * final velocity and acceleration.
273 * @param out The output polynomial needs to be of the correct size.
274 * @param p_init the initial point
275 * @param p_final the final point
276 * @param t_min initial time
277 * @param t_max final time
278 * @return the polynomial curve
279 */
280 37 static void MinimumJerk(polynomial_t& out, const point_t& p_init,
281 const point_t& p_final, const time_t t_min = 0.,
282 const time_t t_max = 1.) {
283
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 33 times.
 37 if (t_min > t_max)
284 ✗ throw std::invalid_argument(
285 "final time should be superior or equal to initial time.");
286 37 const size_t dim(p_init.size());
287
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 31 times.
 37 if (static_cast<size_t>(p_final.size()) != dim)
288
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 3 throw std::invalid_argument(
289 "Initial and final points must have the same dimension.");
290 34 const double T = t_max - t_min;
291 34 const double T2 = T * T;
292 34 const double T3 = T2 * T;
293 34 const double T4 = T3 * T;
294 34 const double T5 = T4 * T;
295
296
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 31 times.
 34 assert(out.coefficients_.cols() == 6);
297
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 31 times.
 34 assert(out.coefficients_.rows() == static_cast<Eigen::Index>(dim));
298
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 31 times.
 34 assert(out.dim_ == dim);
299
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 34 out.coefficients_.fill(0.0);
300
2/4
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31 times.
✗ Branch 5 not taken.
 34 out.coefficients_.col(0) = p_init;
301
5/10
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 31 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 31 times.
✗ Branch 14 not taken.
 34 out.coefficients_.col(3) = 10 * (p_final - p_init) / T3;
302
5/10
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 31 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 31 times.
✗ Branch 14 not taken.
 34 out.coefficients_.col(4) = -15 * (p_final - p_init) / T4;
303
5/10
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 31 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 31 times.
✗ Branch 14 not taken.
 34 out.coefficients_.col(5) = 6 * (p_final - p_init) / T5;
304 34 out.degree_ = 5;
305 34 out.T_min_ = t_min;
306 34 out.T_max_ = t_max;
307
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 34 out.safe_check();
308 34 }
309
310 private:
311 16214 void safe_check() {
312 if (Safe) {
313
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 8113 times.
 16200 if (T_min_ > T_max_) {
314
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 3 throw std::invalid_argument("Tmin should be inferior to Tmax");
315 }
316
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8113 times.
 16197 if (coefficients_.cols() != int(degree_ + 1)) {
317 ✗ throw std::runtime_error("Spline order and coefficients do not match");
318 }
319 }
320 16211 }
321
322 /* Constructors - destructors */
323
324 /*Operations*/
325 public:
326 /// \brief Evaluation of the cubic spline at time t using horner's scheme.
327 /// \param t : time when to evaluate the spline.
328 /// \return \f$x(t)\f$ point corresponding on spline at time t.
329 217612 virtual point_t operator()(const time_t t) const {
330
1/2
✓ Branch 1 taken 111132 times.
✗ Branch 2 not taken.
 217612 check_if_not_empty();
331
4/4
✓ Branch 0 taken 110123 times.
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 110120 times.
 215604 if ((t < T_min_ || t > T_max_) && Safe) {
332
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 throw std::invalid_argument(
333 "error in polynomial : time t to evaluate should be in range [Tmin, "
334 "Tmax] of the curve");
335 }
336 217596 time_t const dt(t - T_min_);
337
2/4
✓ Branch 1 taken 111124 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111124 times.
✗ Branch 5 not taken.
 217596 point_t h = coefficients_.col(degree_);
338
2/2
✓ Branch 0 taken 503288 times.
✓ Branch 1 taken 111124 times.
 1207679 for (int i = (int)(degree_ - 1); i >= 0; i--) {
339
4/8
✓ Branch 1 taken 503288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 503288 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 503288 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 503288 times.
✗ Branch 11 not taken.
 990083 h = dt * h + coefficients_.col(i);
340 }
341 435192 return h;
342 }
343
344 /**
345 * @brief isApprox check if other and *this are approximately equals.
346 * Only two curves of the same class can be approximately equals, for
347 * comparison between different type of curves see isEquivalent
348 * @param other the other curve to check
349 * @param prec the precision threshold, default
350 * Eigen::NumTraits<Numeric>::dummy_precision()
351 * @return true is the two curves are approximately equals
352 */
353 26 bool isApprox(
354 const polynomial_t& other,
355 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
356
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 52 return ndcurves::isApprox<num_t>(T_min_, other.min()) &&
357 26 ndcurves::isApprox<num_t>(T_max_, other.max()) &&
358
4/6
✓ Branch 0 taken 13 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✓ Branch 7 taken 1 times.
 76 dim_ == other.dim() && degree_ == other.degree() &&
359
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 2 times.
 50 coefficients_.isApprox(other.coefficients_, prec);
360 }
361
362 12 virtual bool isApprox(
363 const curve_abc_t* other,
364 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
365
1/2
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
 12 const polynomial_t* other_cast = dynamic_cast<const polynomial_t*>(other);
366
1/2
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
 12 if (other_cast)
367 12 return isApprox(*other_cast, prec);
368 else
369 ✗ return false;
370 }
371
372 14 virtual bool operator==(const polynomial_t& other) const {
373 14 return isApprox(other);
374 }
375
376 6 virtual bool operator!=(const polynomial_t& other) const {
377 6 return !(*this == other);
378 }
379
380 /// \brief Evaluation of the derivative of order N of spline at time t.
381 /// \param t : the time when to evaluate the spline.
382 /// \param order : order of derivative.
383 /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative
384 /// spline at time t.
385 59565 virtual point_t derivate(const time_t t, const std::size_t order) const {
386
1/2
✓ Branch 1 taken 29787 times.
✗ Branch 2 not taken.
 59565 check_if_not_empty();
387
4/4
✓ Branch 0 taken 29785 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 29783 times.
 59565 if ((t < T_min_ || t > T_max_) && Safe) {
388
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 throw std::invalid_argument(
389 "error in polynomial : time t to evaluate derivative should be in "
390 "range [Tmin, Tmax] of the curve");
391 }
392 59557 time_t const dt(t - T_min_);
393 59557 time_t cdt(1);
394
2/4
✓ Branch 1 taken 29783 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29783 times.
✗ Branch 5 not taken.
 59557 point_t currentPoint_ = point_t::Zero(dim_);
395
2/2
✓ Branch 0 taken 144881 times.
✓ Branch 1 taken 29783 times.
 349283 for (int i = (int)(order); i < (int)(degree_ + 1); ++i, cdt *= dt) {
396
4/8
✓ Branch 2 taken 144881 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 144881 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 144881 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 144881 times.
✗ Branch 12 not taken.
 289726 currentPoint_ += cdt * coefficients_.col(i) * fact(i, order);
397 }
398 119114 return currentPoint_;
399 }
400
401 34 polynomial_t compute_derivate(const std::size_t order) const {
402
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 check_if_not_empty();
403
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 10 times.
 34 if (order == 0) {
404
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 return *this;
405 }
406
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 coeff_t coeff_derivated = deriv_coeff(coefficients_);
407
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 polynomial_t deriv(coeff_derivated, T_min_, T_max_);
408
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 return deriv.compute_derivate(order - 1);
409 }
410
411 /// \brief Compute the derived curve at order N.
412 /// \param order : order of derivative.
413 /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
414 /// curve.
415 12 polynomial_t* compute_derivate_ptr(const std::size_t order) const {
416
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 12 return new polynomial_t(compute_derivate(order));
417 }
418
419 155 Eigen::MatrixXd coeff() const { return coefficients_; }
420
421 2 point_t coeffAtDegree(const std::size_t degree) const {
422 2 point_t res;
423
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (degree <= degree_) {
424
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 res = coefficients_.col(degree);
425 }
426 2 return res;
427 }
428
429 private:
430 289726 num_t fact(const std::size_t n, const std::size_t order) const {
431 289726 num_t res(1);
432
2/2
✓ Branch 0 taken 146834 times.
✓ Branch 1 taken 144881 times.
 583342 for (std::size_t i = 0; i < std::size_t(order); ++i) {
433 293616 res *= (num_t)(n - i);
434 }
435 289726 return res;
436 }
437
438 20 coeff_t deriv_coeff(coeff_t coeff) const {
439
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 8 times.
 20 if (coeff.cols() == 1) // only the constant part is left, fill with 0
440
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 return coeff_t::Zero(coeff.rows(), 1);
441
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 16 coeff_t coeff_derivated(coeff.rows(), coeff.cols() - 1);
442
2/2
✓ Branch 1 taken 13 times.
✓ Branch 2 taken 8 times.
 42 for (std::size_t i = 0; i < std::size_t(coeff_derivated.cols()); i++) {
443
4/8
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
 26 coeff_derivated.col(i) = coeff.col(i + 1) * (num_t)(i + 1);
444 }
445 16 return coeff_derivated;
446 }
447
448 277211 void check_if_not_empty() const {
449
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 140936 times.
 277211 if (coefficients_.size() == 0) {
450 ✗ throw std::runtime_error(
451 "Error in polynomial : there is no coefficients set / did you use "
452 "empty constructor ?");
453 }
454 277211 }
455 /*Operations*/
456
457 public:
458 /*Helpers*/
459 /// \brief Get dimension of curve.
460 /// \return dimension of curve.
461 24414 std::size_t virtual dim() const { return dim_; };
462 /// \brief Get the minimum time for which the curve is defined
463 /// \return \f$t_{min}\f$ lower bound of time range.
464 16471 num_t virtual min() const { return T_min_; }
465 /// \brief Get the maximum time for which the curve is defined.
466 /// \return \f$t_{max}\f$ upper bound of time range.
467 16679 num_t virtual max() const { return T_max_; }
468 /// \brief Get the degree of the curve.
469 /// \return \f$degree\f$, the degree of the curve.
470 135 virtual std::size_t degree() const { return degree_; }
471 /*Helpers*/
472
473 8 polynomial_t& operator+=(const polynomial_t& p1) {
474 8 assert_operator_compatible(p1);
475
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 4 times.
 5 if (p1.degree() > degree()) {
476
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t::coeff_t res = p1.coeff();
477
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 res.block(0, 0, coefficients_.rows(), coefficients_.cols()) +=
478 1 coefficients_;
479
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 coefficients_ = res;
480
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 degree_ = p1.degree();
481 1 } else {
482
4/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
 4 coefficients_.block(0, 0, p1.coeff().rows(), p1.coeff().cols()) +=
483 p1.coeff();
484 }
485 5 return *this;
486 }
487
488 8 polynomial_t& operator-=(const polynomial_t& p1) {
489 8 assert_operator_compatible(p1);
490
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 4 times.
 5 if (p1.degree() > degree()) {
491
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 polynomial_t::coeff_t res = -p1.coeff();
492
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 res.block(0, 0, coefficients_.rows(), coefficients_.cols()) +=
493 1 coefficients_;
494
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 coefficients_ = res;
495
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 degree_ = p1.degree();
496 1 } else {
497
4/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
 4 coefficients_.block(0, 0, p1.coeff().rows(), p1.coeff().cols()) -=
498 p1.coeff();
499 }
500 5 return *this;
501 }
502
503 5 polynomial_t& operator+=(const polynomial_t::point_t& point) {
504
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 coefficients_.col(0) += point;
505 5 return *this;
506 }
507
508 3 polynomial_t& operator-=(const polynomial_t::point_t& point) {
509
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 coefficients_.col(0) -= point;
510 3 return *this;
511 }
512
513 3 polynomial_t& operator/=(const double d) {
514 3 coefficients_ /= d;
515 3 return *this;
516 }
517
518 4 polynomial_t& operator*=(const double d) {
519 4 coefficients_ *= d;
520 4 return *this;
521 }
522
523 /// \brief Compute the cross product of the current polynomial by another
524 /// polynomial.
525 /// The cross product p1Xp2 of 2 polynomials p1 and p2 is defined such that
526 /// forall t, p1Xp2(t) = p1(t) X p2(t), with X designing the cross product.
527 /// This method of course only makes sense for dimension 3 polynomials.
528 /// \param pOther other polynomial to compute the cross product with.
529 /// \return a new polynomial defining the cross product between this and
530 /// pother
531 9 polynomial_t cross(const polynomial_t& pOther) const {
532
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 5 times.
 9 assert_operator_compatible(pOther);
533
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
 4 if (dim() != 3)
534 ✗ throw std::invalid_argument(
535 "Can't perform cross product on polynomials with dimensions != 3 ");
536
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 std::size_t new_degree = degree() + pOther.degree();
537
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 coeff_t nCoeffs = Eigen::MatrixXd::Zero(3, new_degree + 1);
538
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 Eigen::Vector3d currentVec;
539
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 Eigen::Vector3d currentVecCrossed;
540
2/2
✓ Branch 1 taken 17 times.
✓ Branch 2 taken 4 times.
 21 for (long i = 0; i < coefficients_.cols(); ++i) {
541
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 currentVec = coefficients_.col(i);
542
3/4
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 54 times.
✓ Branch 6 taken 17 times.
 71 for (long j = 0; j < pOther.coeff().cols(); ++j) {
543
3/6
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
 54 currentVecCrossed = pOther.coeff().col(j);
544
3/6
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 54 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
 54 nCoeffs.col(i + j) += currentVec.cross(currentVecCrossed);
545 }
546 }
547 // remove last degrees is they are equal to 0
548 4 long final_degree = new_degree;
549
8/10
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 13 times.
✓ Branch 7 taken 2 times.
✓ Branch 8 taken 11 times.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 11 times.
✓ Branch 11 taken 4 times.
 15 while (nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN &&
550 final_degree > 0) {
551 11 --final_degree;
552 }
553
5/10
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
 8 return polynomial_t(nCoeffs.leftCols(final_degree + 1), min(), max());
554 4 }
555
556 /// \brief Compute the cross product of the current polynomial p by a point
557 /// point.
558 /// The cross product pXpoint of is defined such that
559 /// forall t, pXpoint(t) = p(t) X point, with X designing the cross product.
560 /// This method of course only makes sense for dimension 3 polynomials.
561 /// \param point point to compute the cross product with.
562 /// \return a new polynomial defining the cross product between this and
563 /// point
564 2 polynomial_t cross(const polynomial_t::point_t& point) const {
565
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
 2 if (dim() != 3)
566 ✗ throw std::invalid_argument(
567 "Can't perform cross product on polynomials with dimensions != 3 ");
568
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 coeff_t nCoeffs = coefficients_;
569
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Eigen::Vector3d currentVec;
570
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Eigen::Vector3d pointVec = point;
571
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 2 times.
 9 for (long i = 0; i < coefficients_.cols(); ++i) {
572
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 currentVec = coefficients_.col(i);
573
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 nCoeffs.col(i) = currentVec.cross(pointVec);
574 }
575 // remove last degrees is they are equal to 0
576
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 long final_degree = degree();
577
4/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 2 times.
 2 while (nCoeffs.col(final_degree).norm() <= ndcurves::MARGIN &&
578 final_degree > 0) {
579 ✗ --final_degree;
580 }
581
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 4 return polynomial_t(nCoeffs.leftCols(final_degree + 1), min(), max());
582 2 }
583
584 /*Attributes*/
585 std::size_t dim_; // const
586 coeff_t coefficients_; // const
587 std::size_t degree_; // const
588 time_t T_min_, T_max_; // const
589 /*Attributes*/
590
591 private:
592 25 void assert_operator_compatible(const polynomial_t& other) const {
593 25 if ((fabs(min() - other.min()) > ndcurves::MARGIN) ||
594
6/6
✓ Branch 0 taken 19 times.
✓ Branch 1 taken 6 times.
✓ Branch 4 taken 16 times.
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 11 times.
✓ Branch 7 taken 14 times.
 41 (fabs(max() - other.max()) > ndcurves::MARGIN) ||
595
2/2
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 14 times.
 16 dim() != other.dim()) {
596
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 throw std::invalid_argument(
597 "Can't perform base operation (+ - ) on two polynomials with "
598 "different time ranges or different dimensions");
599 }
600 14 }
601
602 template <typename In>
603 278 coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient) {
604 278 std::size_t size =
605
1/2
✓ Branch 1 taken 139 times.
✗ Branch 2 not taken.
 278 std::distance(zeroOrderCoefficient, highestOrderCoefficient);
606
1/2
✓ Branch 1 taken 139 times.
✗ Branch 2 not taken.
 278 coeff_t res = coeff_t(dim_, size);
607 278 int i = 0;
608
2/2
✓ Branch 1 taken 480 times.
✓ Branch 2 taken 139 times.
 1238 for (In cit = zeroOrderCoefficient; cit != highestOrderCoefficient;
609 960 ++cit, ++i) {
610
2/4
✓ Branch 2 taken 480 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 480 times.
✗ Branch 6 not taken.
 960 res.col(i) = *cit;
611 }
612 556 return res;
613 }
614
615 public:
616 // Serialization of the class
617 friend class boost::serialization::access;
618
619 template <class Archive>
620 152 void serialize(Archive& ar, const unsigned int version) {
621 if (version) {
622 // Do something depending on version ?
623 }
624
1/2
✓ Branch 3 taken 76 times.
✗ Branch 4 not taken.
 152 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
625
1/2
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
 152 ar& boost::serialization::make_nvp("dim", dim_);
626
1/2
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
 152 ar& boost::serialization::make_nvp("coefficients", coefficients_);
627
1/2
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
 152 ar& boost::serialization::make_nvp("dim", dim_);
628
1/2
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
 152 ar& boost::serialization::make_nvp("degree", degree_);
629
1/2
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
 152 ar& boost::serialization::make_nvp("T_min", T_min_);
630
1/2
✓ Branch 2 taken 76 times.
✗ Branch 3 not taken.
 152 ar& boost::serialization::make_nvp("T_max", T_max_);
631 }
632
633 }; // class polynomial
634
635 template <typename T, typename N, bool S, typename P, typename TP>
636 6 polynomial<T, N, S, P, TP> operator+(const polynomial<T, N, S, P, TP>& p1,
637 const polynomial<T, N, S, P, TP>& p2) {
638
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 polynomial<T, N, S, P, TP> res(p1);
639
3/4
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 9 return res += p2;
640 6 }
641
642 template <typename T, typename N, bool S, typename P, typename TP>
643 2 polynomial<T, N, S, P, TP> operator+(
644 const polynomial<T, N, S, P, TP>& p1,
645 const typename polynomial<T, N, S, P, TP>::point_t& point) {
646
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 polynomial<T, N, S, P, TP> res(p1);
647
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 return res += point;
648 2 }
649
650 template <typename T, typename N, bool S, typename P, typename TP>
651 1 polynomial<T, N, S, P, TP> operator+(
652 const typename polynomial<T, N, S, P, TP>::point_t& point,
653 const polynomial<T, N, S, P, TP>& p1) {
654
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial<T, N, S, P, TP> res(p1);
655
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return res += point;
656 1 }
657
658 template <typename T, typename N, bool S, typename P, typename TP>
659 2 polynomial<T, N, S, P, TP> operator-(
660 const polynomial<T, N, S, P, TP>& p1,
661 const typename polynomial<T, N, S, P, TP>::point_t& point) {
662
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 polynomial<T, N, S, P, TP> res(p1);
663
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 return res -= point;
664 2 }
665
666 template <typename T, typename N, bool S, typename P, typename TP>
667 1 polynomial<T, N, S, P, TP> operator-(
668 const typename polynomial<T, N, S, P, TP>::point_t& point,
669 const polynomial<T, N, S, P, TP>& p1) {
670
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial<T, N, S, P, TP> res(-p1);
671
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return res += point;
672 1 }
673
674 template <typename T, typename N, bool S, typename P, typename TP>
675 3 polynomial<T, N, S, P, TP> operator-(const polynomial<T, N, S, P, TP>& p1) {
676
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 typename polynomial<T, N, S, P, TP>::coeff_t res = -p1.coeff();
677
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 6 return polynomial<T, N, S, P, TP>(res, p1.min(), p1.max());
678 3 }
679
680 template <typename T, typename N, bool S, typename P, typename TP>
681 6 polynomial<T, N, S, P, TP> operator-(const polynomial<T, N, S, P, TP>& p1,
682 const polynomial<T, N, S, P, TP>& p2) {
683
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 polynomial<T, N, S, P, TP> res(p1);
684
3/4
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 9 return res -= p2;
685 6 }
686
687 template <typename T, typename N, bool S, typename P, typename TP>
688 2 polynomial<T, N, S, P, TP> operator/(const polynomial<T, N, S, P, TP>& p1,
689 const double k) {
690
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 polynomial<T, N, S, P, TP> res(p1);
691
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 return res /= k;
692 2 }
693
694 template <typename T, typename N, bool S, typename P, typename TP>
695 1 polynomial<T, N, S, P, TP> operator*(const polynomial<T, N, S, P, TP>& p1,
696 const double k) {
697
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial<T, N, S, P, TP> res(p1);
698
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return res *= k;
699 1 }
700
701 template <typename T, typename N, bool S, typename P, typename TP>
702 1 polynomial<T, N, S, P, TP> operator*(const double k,
703 const polynomial<T, N, S, P, TP>& p1) {
704
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial<T, N, S, P, TP> res(p1);
705
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 return res *= k;
706 1 }
707
708 } // namespace ndcurves
709
710 DEFINE_CLASS_TEMPLATE_VERSION(
711 SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
712 typename T_Point),
713 SINGLE_ARG(ndcurves::polynomial<Time, Numeric, Safe, Point, T_Point>))
714
715 #endif //_STRUCT_POLYNOMIAL
716