GCC Code Coverage Report

Directory: ./
File: include/ndcurves/exact_cubic.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 159 162 98.1%
Branches: 198 388 51.0%
Line Branch Exec Source
1 /**
2 * \file exact_cubic.h
3 * \brief class allowing to create an Exact cubic spline.
4 * \author Steve T.
5 * \version 0.1
6 * \date 06/17/2013
7 *
8 * This file contains definitions for the ExactCubic class.
9 * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique
10 * set of cubic splines fulfulling those 4 restrictions :
11 * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
12 * - x_i(t_i+1) = x_i+1* ;
13 * - its derivative is continous at t_i+1
14 * - its acceleration is continous at t_i+1
15 * more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
16 * By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
17 */
18
19 #ifndef _CLASS_EXACTCUBIC
20 #define _CLASS_EXACTCUBIC
21
22 #include <functional>
23 #include <vector>
24
25 #include "MathDefs.h"
26 #include "curve_abc.h"
27 #include "curve_constraint.h"
28 #include "piecewise_curve.h"
29 #include "polynomial.h"
30
31 namespace ndcurves {
32 /// \class ExactCubic.
33 /// \brief Represents a set of cubic splines defining a continuous function
34 /// crossing each of the waypoint given in its initialization.
35 ///
36 template <
37 typename Time = double, typename Numeric = Time, bool Safe = false,
38 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
39 typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
40 typename SplineBase = polynomial<Time, Numeric, Safe, Point, T_Point> >
41 struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
42 typedef Point point_t;
43 typedef const Eigen::Ref<const point_t> point_ref_t;
44 typedef T_Point t_point_t;
45 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
46 typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
47 typedef Time time_t;
48 typedef Numeric num_t;
49 typedef SplineBase spline_t;
50 typedef typename std::vector<spline_t> t_spline_t;
51 typedef typename t_spline_t::iterator it_spline_t;
52 typedef typename t_spline_t::const_iterator cit_spline_t;
53 typedef curve_constraints<Point> spline_constraints;
54
55 typedef exact_cubic<Time, Numeric, Safe, point_t, T_Point, SplineBase>
56 exact_cubic_t;
57 typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
58 typedef piecewise_curve<Time, Numeric, Safe, point_t> piecewise_curve_t;
59 typedef polynomial<Time, Numeric, Safe, point_t> polynomial_t;
60 typedef typename piecewise_curve_t::t_curve_ptr_t t_curve_ptr_t;
61
62 /* Constructors - destructors */
63 public:
64 /// \brief Empty constructor. Add at least one curve to call other class
65 /// functions.
66 ///
67 6 exact_cubic() : piecewise_curve_t() {}
68
69 /// \brief Constructor.
70 /// \param wayPointsBegin : an iterator pointing to the first element of a
71 /// waypoint container. \param wayPointsEns : an iterator pointing to the
72 /// last element of a waypoint container.
73 ///
74 template <typename In>
75 24 exact_cubic(In wayPointsBegin, In wayPointsEnd) : piecewise_curve_t() {
76
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 24 t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd);
77
2/2
✓ Branch 5 taken 23 times.
✓ Branch 6 taken 13 times.
 68 for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
78
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 44 this->add_curve(*it);
79 }
80 24 }
81
82 /// \brief Constructor.
83 /// \param wayPointsBegin : an iterator pointing to the first element of a
84 /// waypoint container. \param wayPointsEns : an iterator pointing to the
85 /// last element of a waypoint container. \param constraints : constraints
86 /// on the init and end velocity / accelerations of the spline.
87 ///
88 template <typename In>
89 9 exact_cubic(In wayPointsBegin, In wayPointsEnd,
90 const spline_constraints& constraints)
91 9 : piecewise_curve_t() {
92
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 t_spline_t subSplines =
93 computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints);
94
2/2
✓ Branch 5 taken 45 times.
✓ Branch 6 taken 9 times.
 54 for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
95
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 this->add_curve(*it);
96 }
97 9 }
98
99 /// \brief Constructor.
100 /// \param subSplines: vector of subSplines.
101 3 exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t() {
102
2/2
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 3 times.
 9 for (cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
103
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 this->add_curve(*it);
104 }
105 3 }
106
107 exact_cubic(const t_curve_ptr_t& subSplines)
108 : piecewise_curve_t(subSplines) {}
109
110 /// \brief Copy Constructor.
111 38 exact_cubic(const exact_cubic& other) : piecewise_curve_t(other) {}
112
113 /// \brief Destructor.
114 112 virtual ~exact_cubic() {}
115
116 3 std::size_t getNumberSplines() { return this->getNumberCurves(); }
117
118 8 spline_t getSplineAt(std::size_t index) {
119 8 std::shared_ptr<spline_t> s_ptr =
120
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 std::dynamic_pointer_cast<spline_t>(this->curves_.at(index));
121
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 if (s_ptr)
122
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 return *s_ptr;
123 else
124 ✗ throw std::runtime_error(
125 "Parent piecewise curve do not contain only curves created from "
126 "exact_cubic class methods");
127 8 }
128
129 private:
130 116 static polynomial_t create_cubic(point_ref_t a, point_ref_t b, point_ref_t c,
131 point_ref_t d, const time_t t_min,
132 const time_t t_max) {
133
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 116 typename polynomial_t::t_point_t coeffs;
134
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 116 coeffs.push_back(a);
135
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 116 coeffs.push_back(b);
136
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 116 coeffs.push_back(c);
137
2/4
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 116 coeffs.push_back(d);
138
1/2
✓ Branch 3 taken 59 times.
✗ Branch 4 not taken.
 232 return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
139 116 }
140 9 static polynomial_t create_quintic(point_ref_t a, point_ref_t b,
141 point_ref_t c, point_ref_t d,
142 point_ref_t e, point_ref_t f,
143 const time_t t_min, const time_t t_max) {
144
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 typename polynomial_t::t_point_t coeffs;
145
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 coeffs.push_back(a);
146
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 coeffs.push_back(b);
147
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 coeffs.push_back(c);
148
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 coeffs.push_back(d);
149
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 coeffs.push_back(e);
150
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 coeffs.push_back(f);
151
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 18 return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
152 9 }
153
154 /// \brief Compute polynom of exact cubic spline from waypoints.
155 /// Compute the coefficients of polynom as in paper : "Task-Space Trajectories
156 /// via Cubic Spline Optimization".<br>
157 /// \f$x_i(t)=a_i+b_i(t-t_i)+c_i(t-t_i)^2\f$<br>
158 /// with \f$a=x\f$, \f$H_1b=H_2x\f$, \f$c=H_3x+H_4b\f$, \f$d=H_5x+H_6b\f$.<br>
159 /// The matrices \f$H\f$ are defined as in the paper in Appendix A.
160 ///
161 template <typename In>
162 24 t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const {
163 24 const std::size_t dim = wayPointsBegin->second.size();
164
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 24 const std::size_t size = std::distance(wayPointsBegin, wayPointsEnd);
165
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 10 if (Safe && size < 1) {
166 ✗ throw std::length_error(
167 "size of waypoints must be superior to 0"); // TODO
168 }
169 24 t_spline_t subSplines;
170
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 24 subSplines.reserve(size);
171 // refer to the paper to understand all this.
172
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX h1 = MatrixX::Zero(size, size);
173
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX h2 = MatrixX::Zero(size, size);
174
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX h3 = MatrixX::Zero(size, size);
175
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX h4 = MatrixX::Zero(size, size);
176
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX h5 = MatrixX::Zero(size, size);
177
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX h6 = MatrixX::Zero(size, size);
178
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX a = MatrixX::Zero(size, dim);
179
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX b = MatrixX::Zero(size, dim);
180
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX c = MatrixX::Zero(size, dim);
181
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX d = MatrixX::Zero(size, dim);
182
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 24 MatrixX x = MatrixX::Zero(size, dim);
183 24 In it(wayPointsBegin), next(wayPointsBegin);
184 24 ++next;
185 // Fill the matrices H as specified in the paper.
186
2/2
✓ Branch 2 taken 23 times.
✓ Branch 3 taken 13 times.
 68 for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
187 44 num_t const dTi((*next).first - (*it).first);
188 44 num_t const dTi_sqr(dTi * dTi);
189 44 num_t const dTi_cube(dTi_sqr * dTi);
190 // filling matrices values
191
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 44 h3(i, i) = -3 / dTi_sqr;
192
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 44 h3(i, i + 1) = 3 / dTi_sqr;
193
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 44 h4(i, i) = -2 / dTi;
194
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 44 h4(i, i + 1) = -1 / dTi;
195
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 44 h5(i, i) = 2 / dTi_cube;
196
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 44 h5(i, i + 1) = -2 / dTi_cube;
197
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 44 h6(i, i) = 1 / dTi_sqr;
198
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 44 h6(i, i + 1) = 1 / dTi_sqr;
199
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 13 times.
 44 if (i + 2 < size) {
200 20 In it2(next);
201 20 ++it2;
202 20 num_t const dTi_1((*it2).first - (*next).first);
203 20 num_t const dTi_1sqr(dTi_1 * dTi_1);
204 // this can be optimized but let's focus on clarity as long as not
205 // needed
206
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 h1(i + 1, i) = 2 / dTi;
207
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
208
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 h1(i + 1, i + 2) = 2 / dTi_1;
209
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 h2(i + 1, i) = -6 / dTi_sqr;
210
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
211
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 h2(i + 1, i + 2) = 6 / dTi_1sqr;
212 }
213
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 44 x.row(i) = (*it).second.transpose();
214 }
215 // adding last x
216
3/6
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 24 x.row(size - 1) = (*it).second.transpose();
217 // Compute coefficients of polynom.
218
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 24 a = x;
219
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 24 PseudoInverse(h1);
220
3/6
✓ 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.
 24 b = h1 * h2 * x; // h1 * b = h2 * x => b = (h1)^-1 * h2 * x
221
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.
 24 c = h3 * x + h4 * b;
222
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.
 24 d = h5 * x + h6 * b;
223 // create splines along waypoints.
224 24 it = wayPointsBegin, next = wayPointsBegin;
225 24 ++next;
226
2/2
✓ Branch 2 taken 23 times.
✓ Branch 3 taken 13 times.
 68 for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
227
10/20
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 23 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 23 times.
✗ Branch 29 not taken.
 44 subSplines.push_back(create_cubic(a.row(i), b.row(i), c.row(i), d.row(i),
228 44 (*it).first, (*next).first));
229 }
230 48 return subSplines;
231 24 }
232
233 template <typename In>
234 9 t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd,
235 const spline_constraints& constraints) const {
236
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
237
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 9 times.
 9 if (Safe && size < 1) {
238 ✗ throw std::length_error(
239 "number of waypoints should be superior to one"); // TODO
240 }
241 9 t_spline_t subSplines;
242
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 subSplines.reserve(size - 1);
243
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 spline_constraints cons = constraints;
244 9 In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
245 9 ++next;
246
2/2
✓ Branch 3 taken 36 times.
✓ Branch 4 taken 9 times.
 45 for (std::size_t i(0); next != end; ++next, ++it, ++i) {
247
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 compute_one_spline<In>(it, next, cons, subSplines);
248 }
249
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 compute_end_spline<In>(it, next, cons, subSplines);
250 18 return subSplines;
251 9 }
252
253 template <typename In>
254 36 void compute_one_spline(In wayPointsBegin, In wayPointsNext,
255 spline_constraints& constraints,
256 t_spline_t& subSplines) const {
257
1/2
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
 36 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
258
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.;
259 36 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
260 36 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
261
7/14
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 36 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 36 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 36 times.
✗ Branch 20 not taken.
 36 const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
262
6/12
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 36 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 36 times.
✗ Branch 17 not taken.
 36 subSplines.push_back(create_cubic(a0, b0, c0, d0, init_t, end_t));
263
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 36 constraints.init_vel = subSplines.back().derivate(end_t, 1);
264
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 36 constraints.init_acc = subSplines.back().derivate(end_t, 2);
265 36 }
266
267 template <typename In>
268 9 void compute_end_spline(In wayPointsBegin, In wayPointsNext,
269 spline_constraints& constraints,
270 t_spline_t& subSplines) const {
271 9 const std::size_t dim = wayPointsBegin->second.size();
272
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
273
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel,
274
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
275 9 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
276 9 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt,
277 9 dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
278 // solving a system of four linear eq with 4 unknows: d0, e0
279
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
 9 const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
280
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
 9 const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
281
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 const point_t alpha_2 = c1 - 2 * c0;
282 9 const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
283 9 const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
284 9 const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
285
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 point_t d, e, f;
286
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 MatrixX rhs = MatrixX::Zero(3, dim);
287
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 rhs.row(0) = alpha_0;
288
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 rhs.row(1) = alpha_1;
289
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 rhs.row(2) = alpha_2;
290
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 Matrix3 eq = Matrix3::Zero(3, 3);
291
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(0, 0) = x_d_0;
292
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(0, 1) = x_e_0;
293
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(0, 2) = x_f_0;
294
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(1, 0) = x_d_1;
295
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(1, 1) = x_e_1;
296
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(1, 2) = x_f_1;
297
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(2, 0) = x_d_2;
298
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(2, 1) = x_e_2;
299
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 eq(2, 2) = x_f_2;
300
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 9 rhs = eq.inverse().eval() * rhs;
301
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 d = rhs.row(0);
302
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 e = rhs.row(1);
303
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 f = rhs.row(2);
304
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
 9 subSplines.push_back(create_quintic(a0, b0, c0, d, e, f, init_t, end_t));
305 9 }
306
307 public:
308 // Serialization of the class
309 friend class boost::serialization::access;
310 template <class Archive>
311 12 void serialize(Archive& ar, const unsigned int version) {
312 if (version) {
313 // Do something depending on version ?
314 }
315
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 12 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(piecewise_curve_t);
316 }
317 };
318 } // namespace ndcurves
319
320 DEFINE_CLASS_TEMPLATE_VERSION(
321 SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
322 typename T_Point, typename SplineBase),
323 SINGLE_ARG(
324 ndcurves::exact_cubic<Time, Numeric, Safe, Point, T_Point, SplineBase>))
325 #endif //_CLASS_EXACTCUBIC
326