GCC Code Coverage Report

Directory: ./
File: tests/Main.cpp
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 1844 2386 77.3%
Branches: 2575 6018 42.8%
Line Branch Exec Source
1 #include <cmath>
2 #include <ctime>
3 #include <iostream>
4 #include <memory>
5 #include <string>
6
7 #include "load_problem.h"
8 #include "ndcurves/bezier_curve.h"
9 #include "ndcurves/cubic_hermite_spline.h"
10 #include "ndcurves/curve_conversion.h"
11 #include "ndcurves/exact_cubic.h"
12 #include "ndcurves/fwd.h"
13 #include "ndcurves/helpers/effector_spline.h"
14 #include "ndcurves/helpers/effector_spline_rotation.h"
15 #include "ndcurves/optimization/definitions.h"
16 #include "ndcurves/piecewise_curve.h"
17 #include "ndcurves/polynomial.h"
18 #include "ndcurves/se3_curve.h"
19 #include "ndcurves/serialization/curves.hpp"
20 #include "ndcurves/so3_linear.h"
21
22 using namespace std;
23
24 namespace ndcurves {
25 typedef exact_cubic<double, double, true, Eigen::Matrix<double, 1, 1> >
26 exact_cubic_one;
27 typedef exact_cubic_t::spline_constraints spline_constraints_t;
28
29 typedef std::pair<double, pointX_t> Waypoint;
30 typedef std::vector<Waypoint> T_Waypoint;
31 typedef Eigen::Matrix<double, 1, 1> point_one;
32 typedef std::pair<double, point_one> WaypointOne;
33 typedef std::vector<WaypointOne> T_WaypointOne;
34 typedef std::pair<pointX_t, pointX_t> pair_point_tangent_t;
35 typedef std::vector<pair_point_tangent_t,
36 Eigen::aligned_allocator<pair_point_tangent_t> >
37 t_pair_point_tangent_t;
38
39 const double margin = 1e-3;
40 1299 bool QuasiEqual(const double a, const double b) {
41 1299 return std::fabs(a - b) < margin;
42 }
43 405 bool QuasiEqual(const point3_t a, const point3_t b) {
44 405 bool equal = true;
45
2/2
✓ Branch 0 taken 1215 times.
✓ Branch 1 taken 405 times.
 1620 for (size_t i = 0; i < 3; ++i) {
46
2/4
✓ Branch 0 taken 1215 times.
✗ Branch 1 not taken.
✓ Branch 5 taken 1215 times.
✗ Branch 6 not taken.
 1215 equal = equal && QuasiEqual(a[i], b[i]);
47 }
48 405 return equal;
49 }
50 } // End namespace ndcurves
51
52 using namespace ndcurves;
53
54 ✗ ostream& operator<<(ostream& os, const point3_t& pt) {
55 ✗ os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
56 ✗ return os;
57 }
58
59 900 void ComparePoints(const transform_t& pt1, const transform_t& pt2,
60 const std::string& errmsg, bool& error,
61 double prec = Eigen::NumTraits<double>::dummy_precision(),
62 bool notequal = false) {
63
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 900 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 900 times.
 900 if (!pt1.isApprox(pt2, prec) && !notequal) {
64 ✗ error = true;
65 ✗ std::cout << errmsg << " translation :" << pt1.translation() << " ; "
66 ✗ << pt2.translation() << std::endl
67 ✗ << "rotation : " << pt1.rotation() << " ; " << pt2.rotation()
68 ✗ << std::endl;
69 }
70 900 }
71
72 206996 void ComparePoints(const Eigen::MatrixXd& pt1, const Eigen::MatrixXd& pt2,
73 const std::string& errmsg, bool& error,
74 double prec = Eigen::NumTraits<double>::dummy_precision(),
75 bool notequal = false) {
76
5/8
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 206994 times.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 206996 times.
 206996 if (!pt1.isApprox(pt2, prec) && !(pt1.isZero(prec) && pt2.isZero(prec)) &&
77 ✗ !notequal) {
78 ✗ error = true;
79 ✗ std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
80 }
81 206996 }
82
83 template <typename curve1, typename curve2>
84 70 void CompareCurves(const curve1& c1, const curve2& c2,
85 const std::string& errMsg, bool& error,
86 double prec = Eigen::NumTraits<double>::dummy_precision()) {
87 70 double T_min = c1.min();
88 70 double T_max = c1.max();
89
3/6
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 35 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 35 times.
 70 if (!QuasiEqual(T_min, c2.min()) || !QuasiEqual(T_max, c2.max())) {
90 std::cout
91 << errMsg
92 ✗ << "CompareCurves, ERROR, time min and max of curves do not match ["
93 ✗ << T_min << "," << T_max << "] "
94 ✗ << " and [" << c2.min() << "," << c2.max() << "] " << std::endl;
95 ✗ error = true;
96 } else {
97 // derivative in T_min and T_max
98
5/10
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 35 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 35 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 35 times.
✗ Branch 15 not taken.
 70 ComparePoints(c1.derivate(T_min, 1), c2.derivate(T_min, 1),
99 errMsg + " Derivates at tMin do not match.", error, prec,
100 false);
101
5/10
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 35 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 35 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 35 times.
✗ Branch 15 not taken.
 70 ComparePoints(c1.derivate(T_max, 1), c2.derivate(T_max, 1),
102 errMsg + " Derivates at tMax do not match.", error, prec,
103 false);
104 // Test values on curves
105
2/2
✓ Branch 0 taken 6880 times.
✓ Branch 1 taken 35 times.
 13830 for (double i = T_min; i <= T_max; i += 0.01) {
106
8/16
✓ Branch 1 taken 6880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6880 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6880 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6880 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6880 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6880 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 5980 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 5980 times.
✗ Branch 23 not taken.
 13760 ComparePoints(c1(i), c2(i),
107 errMsg + " Curves evaluation do not match at t = " +
108 boost::lexical_cast<std::string>(i),
109 error, prec, false);
110 }
111 }
112 70 }
113
114 /*Cubic Function tests*/
115 1 void PolynomialCubicFunctionTest(bool& error) {
116
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
119
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pointX_t vec;
122
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(a);
123
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(b);
124
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(c);
125
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(d);
126
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 polynomial_t cf(vec.begin(), vec.end(), 0, 1);
127
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t res1;
128
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf(0);
129
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t x0(1, 2, 3);
130
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(x0, res1, errMsg + "(0) ", error);
131
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t x1(9, 15, 19);
132
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf(1);
133
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(x1, res1, errMsg + "(1) ", error);
134
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t x2(3.125, 5.25, 7.125);
135
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf(0.5);
136
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(x2, res1, errMsg + "(0.5) ", error);
137 1 vec.clear();
138
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(a);
139
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(b);
140
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(c);
141
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(d);
142
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t cf2(vec, 0.5, 1);
143
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf2(0.5);
144
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(x0, res1, errMsg + "x3 ", error);
145 1 error = true;
146 try {
147
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cf2(0.4);
148 1 } catch (...) {
149 1 error = false;
150
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
151
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
152 std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range "
153 ✗ "value\n";
154 }
155 1 error = true;
156 try {
157
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cf2(1.1);
158 1 } catch (...) {
159 1 error = false;
160
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
161
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
162 std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range "
163 ✗ "value\n";
164 }
165
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(cf.max(), 1.0)) {
166 ✗ error = true;
167 std::cout
168 ✗ << "Evaluation of cubic cf error, MaxBound should be equal to 1\n";
169 }
170
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(cf.min(), 0.0)) {
171 ✗ error = true;
172 std::cout
173 ✗ << "Evaluation of cubic cf error, MinBound should be equal to 1\n";
174 }
175 // Test derivate and compute_derivative
176 // Order 1
177
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 curve_abc_t* cf_derivated = cf.compute_derivate_ptr(1);
178
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 ComparePoints(cf.derivate(0, 1), (*cf_derivated)(0),
179
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errMsg + " - derivate order 1 : ", error);
180
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 ComparePoints(cf.derivate(0.3, 1), (*cf_derivated)(0.3),
181
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errMsg + " - derivate order 1 : ", error);
182
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 ComparePoints(cf.derivate(0.5, 1), (*cf_derivated)(0.5),
183
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errMsg + " - derivate order 1 : ", error);
184
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 ComparePoints(cf.derivate(1, 1), (*cf_derivated)(1),
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errMsg + " - derivate order 1 : ", error);
186 // Order 2
187
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t cf_derivated_2 = cf.compute_derivate(2);
188
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 ComparePoints(cf.derivate(0, 2), (cf_derivated_2)(0),
189
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errMsg + " - derivate order 1 : ", error);
190
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 ComparePoints(cf.derivate(0.3, 2), (cf_derivated_2)(0.3),
191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errMsg + " - derivate order 1 : ", error);
192
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 ComparePoints(cf.derivate(0.5, 2), (cf_derivated_2)(0.5),
193
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errMsg + " - derivate order 1 : ", error);
194
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 2 ComparePoints(cf.derivate(1, 2), (cf_derivated_2)(1),
195
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errMsg + " - derivate order 1 : ", error);
196 1 }
197
198 /*bezier_curve Function tests*/
199 1 void BezierCurveTest(bool& error) {
200
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
201
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
202
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
203
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
204
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
205 1 std::vector<point3_t> params;
206
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
207 // 1d curve in [0,1]
208
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf1(params.begin(), params.end());
209
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t res1;
210
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf1(0);
211
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t x10 = a;
212
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(x10, res1, errMsg + "1(0) ", error);
213
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf1(1);
214
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(x10, res1, errMsg + "1(1) ", error);
215 // 2d curve in [0,1]
216
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
217
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf(params.begin(), params.end());
218
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf(0);
219
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t x20 = a;
220
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(x20, res1, errMsg + "2(0) ", error);
221
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t x21 = b;
222
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf(1);
223
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(x21, res1, errMsg + "2(1) ", error);
224 // 3d curve in [0,1]
225
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
226
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf3(params.begin(), params.end());
227
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf3(0);
228
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(a, res1, errMsg + "3(0) ", error);
229
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf3(1);
230
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(c, res1, errMsg + "3(1) ", error);
231 // 4d curve in [1,2]
232
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(d);
233
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf4(params.begin(), params.end(), 1., 2.);
234 // testing bernstein polynomials
235
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf5(params.begin(), params.end(), 1., 2.);
236 std::string errMsg2(
237 "In test BezierCurveTest ; Bernstein polynomials do not evaluate as "
238
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "analytical evaluation");
239
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t cf5_derivated = cf5.compute_derivate(1);
240
241
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
 11 for (double d = 1.; d < 2.; d += 0.1) {
242
5/10
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
 10 ComparePoints(cf5.evalBernstein(d), cf5(d), errMsg2, error);
243
5/10
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
 10 ComparePoints(cf5.evalHorner(d), cf5(d), errMsg2, error);
244
5/10
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
 10 ComparePoints(cf5_derivated.evalBernstein(d), cf5_derivated(d), errMsg2,
245 error);
246
5/10
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
 10 ComparePoints(cf5_derivated.evalHorner(d), cf5_derivated(d), errMsg2,
247 error);
248
5/10
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
 10 ComparePoints(cf5.derivate(d, 1), cf5_derivated(d), errMsg2, error);
249 }
250 1 bool error_in(true);
251 try {
252
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cf(-0.4);
253 1 } catch (...) {
254 1 error_in = false;
255
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
256
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error_in) {
257 std::cout << "Evaluation of bezier cf error, -0.4 should be an out of "
258 ✗ "range value\n";
259 ✗ error = true;
260 }
261 1 error_in = true;
262 try {
263
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cf(1.1);
264 1 } catch (...) {
265 1 error_in = false;
266
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
267
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error_in) {
268 std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range "
269 ✗ "value\n";
270 ✗ error = true;
271 }
272
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(cf.max(), 1.0)) {
273 ✗ error = true;
274 std::cout
275 ✗ << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
276 }
277
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(cf.min(), 0.0)) {
278 ✗ error = true;
279 std::cout
280 ✗ << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
281 }
282 1 }
283
284 1 void BezierCurveTestCompareHornerAndBernstein(bool&) // error
285 {
286 using namespace std;
287 1 std::vector<double> values;
288
2/2
✓ Branch 0 taken 100000 times.
✓ Branch 1 taken 1 times.
 100001 for (int i = 0; i < 100000; ++i) {
289
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 values.push_back(rand() / (double)RAND_MAX);
290 }
291 // first compare regular evaluation (low dim pol)
292
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
293
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
294
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
295
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
296
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t e(3, 61, 7);
297
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t f(3, 56, 7);
298
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t g(3, 36, 7);
299
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t h(43, 6, 7);
300
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t i(3, 6, 77);
301 1 std::vector<point3_t> params;
302
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
303
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
304
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
305 // 3d curve
306
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf(params.begin(), params.end()); // defined in [0,1]
307 // Check all evaluation of bezier curve
308 clock_t s0, e0, s1, e1, s2, e2, s3, e3;
309 1 s0 = clock();
310 1 for (std::vector<double>::const_iterator cit = values.begin();
311
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
312
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 cf(*cit);
313 }
314 1 e0 = clock();
315 1 s1 = clock();
316 1 for (std::vector<double>::const_iterator cit = values.begin();
317
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
318
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 cf.evalBernstein(*cit);
319 }
320 1 e1 = clock();
321
322 1 s2 = clock();
323 1 for (std::vector<double>::const_iterator cit = values.begin();
324
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
325
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 cf.evalHorner(*cit);
326 }
327 1 e2 = clock();
328 1 s3 = clock();
329 1 for (std::vector<double>::const_iterator cit = values.begin();
330
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
331
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 cf.evalDeCasteljau(*cit);
332 }
333 1 e3 = clock();
334
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC
335
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
336
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC
337
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
338
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC
339
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
340
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC
341
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
342
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "now with high order polynomial " << std::endl;
343
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(d);
344
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(e);
345
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(f);
346
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(g);
347
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(h);
348
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(i);
349
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf2(params.begin(), params.end());
350 1 s1 = clock();
351 1 for (std::vector<double>::const_iterator cit = values.begin();
352
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
353
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 cf2.evalBernstein(*cit);
354 }
355 1 e1 = clock();
356 1 s2 = clock();
357 1 for (std::vector<double>::const_iterator cit = values.begin();
358
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
359
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 cf2.evalHorner(*cit);
360 }
361 1 e2 = clock();
362 1 s0 = clock();
363 1 for (std::vector<double>::const_iterator cit = values.begin();
364
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
365
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 cf2(*cit);
366 }
367 1 e0 = clock();
368 1 s3 = clock();
369 1 for (std::vector<double>::const_iterator cit = values.begin();
370
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
371
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 cf2.evalDeCasteljau(*cit);
372 }
373 1 e3 = clock();
374
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC
375
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
376
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC
377
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
378
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC
379
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
380
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC
381
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
382 1 }
383
384 1 void BezierDerivativeCurveTest(bool& error) {
385 std::string errMsg(
386 "In test BezierDerivativeCurveTest ;, Error While checking value of "
387
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "point on curve : ");
388
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
389
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
390
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
391 1 std::vector<point3_t> params;
392
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
393
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
394
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
395
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf3(params.begin(), params.end());
396
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 ComparePoints(cf3(0), cf3.derivate(0., 1), errMsg, error, true);
397
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 ComparePoints(point3_t::Zero(), cf3.derivate(0., 100), errMsg, error);
398 1 }
399
400 1 void BezierDerivativeCurveTimeReparametrizationTest(bool& error) {
401 std::string errMsg(
402 "In test BezierDerivativeCurveTimeReparametrizationTest, Error While "
403
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "checking value of point on curve : ");
404
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
405
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
406
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
407
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 4, 5);
408
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t e(3, 4, 5);
409
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t f(3, 4, 5);
410 1 std::vector<point3_t> params;
411
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
412
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
413
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
414
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(d);
415
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(e);
416
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(f);
417 1 double Tmin = 0.;
418 1 double Tmax = 2.;
419 1 double diffT = Tmax - Tmin;
420
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf(params.begin(), params.end());
421
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cfT(params.begin(), params.end(), Tmin, Tmax);
422
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 ComparePoints(cf(0.5), cfT(1), errMsg, error);
423
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 1 ComparePoints(cf.derivate(0.5, 1), cfT.derivate(1, 1) * (diffT), errMsg,
424 error);
425
7/14
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 1 ComparePoints(cf.derivate(0.5, 2), cfT.derivate(1, 2) * diffT * diffT, errMsg,
426 error);
427 1 }
428
429 1 void BezierDerivativeCurveConstraintTest(bool& error) {
430 std::string errMsg0(
431 "In test BezierDerivativeCurveConstraintTest, Error While checking value "
432
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "of point on curve : ");
433
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
434
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
435
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
436
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t::curve_constraints_t constraints(3);
437
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_vel = point3_t(-1, -1, -1);
438
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_acc = point3_t(-2, -2, -2);
439
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_vel = point3_t(-10, -10, -10);
440
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_acc = point3_t(-20, -20, -20);
441 1 std::vector<point3_t> params;
442
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
443
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
444
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
445 1 bezier_t::num_t T_min = 1.0;
446 1 bezier_t::num_t T_max = 3.0;
447
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf(params.begin(), params.end(), constraints, T_min, T_max);
448
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(a, cf(T_min), errMsg0, error);
449
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(c, cf(T_max), errMsg0, error);
450
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(constraints.init_vel, cf.derivate(T_min, 1), errMsg0, error);
451
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(constraints.end_vel, cf.derivate(T_max, 1), errMsg0, error);
452
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(constraints.init_acc, cf.derivate(T_min, 2), errMsg0, error);
453
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(constraints.end_vel, cf.derivate(T_max, 1), errMsg0, error);
454
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(constraints.end_acc, cf.derivate(T_max, 2), errMsg0, error);
455 std::string errMsg1(
456 "In test BezierDerivativeCurveConstraintTest, Error While checking "
457
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "checking degree of bezier curve :");
458 std::string errMsg2(
459 "In test BezierDerivativeCurveConstraintTest, Error While checking "
460
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "checking size of bezier curve :");
461
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (cf.degree_ != params.size() + 3) {
462 ✗ error = true;
463 ✗ std::cout << errMsg1 << cf.degree_ << " ; " << params.size() + 3
464 ✗ << std::endl;
465 }
466
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (cf.size_ != params.size() + 4) {
467 ✗ error = true;
468 ✗ std::cout << errMsg2 << cf.size_ << " ; " << params.size() + 4 << std::endl;
469 }
470 1 }
471
472 1 void toPolynomialConversionTest(bool& error) {
473 // bezier to polynomial
474 std::string errMsg(
475 "In test BezierToPolynomialConversionTest, Error While checking value of "
476
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "point on curve : ");
477
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
478
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
479
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
480
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
481
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t e(3, 61, 7);
482
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t f(3, 56, 7);
483
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t g(3, 36, 7);
484
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t h(43, 6, 7);
485
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t i(3, 6, 77);
486 1 std::vector<point3_t> control_points;
487
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(a);
488
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(b);
489
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(c);
490
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(d);
491
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(e);
492
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(f);
493
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(g);
494
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(h);
495
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(i);
496 1 bezier_t::num_t T_min = 1.0;
497 1 bezier_t::num_t T_max = 3.0;
498
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t bc(control_points.begin(), control_points.end(), T_min, T_max);
499
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t pol = polynomial_from_curve<polynomial_t>(bc);
500
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<polynomial_t, bezier_t>(pol, bc, errMsg, error);
501 1 }
502
503 1 void cubicConversionTest(bool& error) {
504 std::string errMsg0(
505 "In test CubicConversionTest - convert hermite to, Error While checking "
506
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "value of point on curve : ");
507 std::string errMsg1(
508 "In test CubicConversionTest - convert bezier to, Error While checking "
509
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "value of point on curve : ");
510 std::string errMsg2(
511 "In test CubicConversionTest - convert polynomial to, Error While "
512
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "checking value of point on curve : ");
513 // Create cubic hermite spline : Test hermite to bezier/polynomial
514
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p0(1, 2, 3);
515
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t m0(2, 3, 4);
516
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p1(3, 4, 5);
517
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t m1(3, 6, 7);
518
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pair_point_tangent_t pair0(p0, m0);
519
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pair_point_tangent_t pair1(p1, m1);
520
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pair_point_tangent_t control_points;
521
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(pair0);
522
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(pair1);
523 1 std::vector<double> time_control_points;
524 1 polynomial_t::num_t T_min = 1.0;
525 1 polynomial_t::num_t T_max = 3.0;
526
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(T_min);
527
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(T_max);
528 cubic_hermite_spline_t chs0(control_points.begin(), control_points.end(),
529
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 time_control_points);
530 // hermite to bezier
531 // std::cout<<"======================= \n";
532 // std::cout<<"hermite to bezier \n";
533
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t bc0 = bezier_from_curve<bezier_t>(chs0);
534
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<cubic_hermite_spline_t, bezier_t>(chs0, bc0, errMsg0, error);
535 // hermite to pol
536 // std::cout<<"======================= \n";
537 // std::cout<<"hermite to polynomial \n";
538
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t pol0 = polynomial_from_curve<polynomial_t>(chs0);
539
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0,
540 error);
541 // pol to hermite
542 // std::cout<<"======================= \n";
543 // std::cout<<"polynomial to hermite \n";
544 cubic_hermite_spline_t chs1 =
545
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 hermite_from_curve<cubic_hermite_spline_t>(pol0);
546
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol0, chs1, errMsg2,
547 error);
548 // pol to bezier
549 // std::cout<<"======================= \n";
550 // std::cout<<"polynomial to bezier \n";
551
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t bc1 = bezier_from_curve<bezier_t>(pol0);
552
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<bezier_t, polynomial_t>(bc1, pol0, errMsg2, error);
553 // Bezier to pol
554 // std::cout<<"======================= \n";
555 // std::cout<<"bezier to polynomial \n";
556
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t pol1 = polynomial_from_curve<polynomial_t>(bc0);
557
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<bezier_t, polynomial_t>(bc0, pol1, errMsg1, error);
558 // bezier => hermite
559 // std::cout<<"======================= \n";
560 // std::cout<<"bezier to hermite \n";
561
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t>(bc0);
562
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<bezier_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
563
564 // Test : compute derivative of bezier => Convert it to polynomial
565
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 curve_abc_t* bc_der = bc0.compute_derivate_ptr(1);
566
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t pol_test = polynomial_from_curve<polynomial_t>(*bc_der);
567
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<curve_abc_t, polynomial_t>(*bc_der, pol_test, errMsg1, error);
568
569 // check that an error is correctly raised when degree > 3:
570
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
571
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
572
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
573
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
574
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t e(3, 6, 7);
575
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pointX_t vec;
576
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(a);
577
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(b);
578
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(c);
579
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(d);
580
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(e);
581
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 polynomial_t pol_4(vec.begin(), vec.end(), 0, 1);
582
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (pol_4.degree() != 4) {
583 std::cout << "In test CubicConversionTest - Error in the creatin of the "
584 ✗ "polynomial"
585 ✗ << std::endl;
586 ✗ error = true;
587 }
588 try {
589 cubic_hermite_spline_t chs3 =
590
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 hermite_from_curve<cubic_hermite_spline_t>(pol_4);
591 std::cout << "In test CubicConversionTest - Cannot convert to hermite from "
592 ✗ "degree > 3, should raise an error"
593 ✗ << std::endl;
594 ✗ error = true;
595
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
596 1 }
597 try {
598
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 bezier_t b3 = bezier_from_curve<bezier_t>(pol_4);
599 std::cout << "In test CubicConversionTest - Cannot convert to bezier from "
600 ✗ "degree > 3, should raise an error"
601 ✗ << std::endl;
602 ✗ error = true;
603
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
604 1 }
605 1 }
606
607 /*Exact Cubic Function tests*/
608 1 void ExactCubicNoErrorTest(bool& error) {
609 // Create an exact cubic spline with 7 waypoints => 6 polynomials defined in
610 // [0.0,3.0]
611 1 ndcurves::T_Waypoint waypoints;
612
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 1 times.
 8 for (double i = 0.0; i <= 3.0; i = i + 0.5) {
613
4/8
✓ 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.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 7 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
614 }
615
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
616 // Test number of polynomials in exact cubic
617
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::size_t numberSegments = exactCubic.getNumberSplines();
618
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (numberSegments != 6) {
619 ✗ error = true;
620 std::cout
621 ✗ << "In ExactCubicNoErrorTest, Error While checking number of splines"
622 ✗ << numberSegments << " ; " << 6 << std::endl;
623 }
624 // Test getSplineAt function
625
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (std::size_t i = 0; i < numberSegments; i++) {
626
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 exactCubic.getSplineAt(i);
627 }
628 // Other tests
629 try {
630
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 exactCubic(0.0);
631
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 exactCubic(3.0);
632 ✗ } catch (...) {
633 ✗ error = true;
634 std::cout << "Evaluation of ExactCubicNoErrorTest error when testing value "
635 ✗ "on bounds\n";
636 }
637 1 error = true;
638 try {
639
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 exactCubic(3.2);
640 1 } catch (...) {
641 1 error = false;
642
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
643
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
644 std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of "
645 ✗ "range value\n";
646 }
647
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(exactCubic.max(), 3.0)) {
648 ✗ error = true;
649 std::cout << "Evaluation of exactCubic error, MaxBound should be equal to "
650 ✗ "3 but is : "
651 ✗ << exactCubic.max() << "\n";
652 }
653
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(exactCubic.min(), 0.0)) {
654 ✗ error = true;
655 std::cout << "Evaluation of exactCubic error, MinBound should be equal to "
656 ✗ "0 but is : "
657 ✗ << exactCubic.min() << "\n";
658 }
659 1 }
660
661 /*Exact Cubic Function tests*/
662 1 void ExactCubicTwoPointsTest(bool& error) {
663 // Create an exact cubic spline with 2 waypoints => 1 polynomial defined in
664 // [0.0,1.0]
665 1 ndcurves::T_Waypoint waypoints;
666
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 for (double i = 0.0; i < 2.0; ++i) {
667
4/8
✓ 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.
 2 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
668 }
669
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
670
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 point3_t res1 = exactCubic(0);
671 std::string errmsg0(
672 "in ExactCubicTwoPointsTest, Error While checking that given wayPoints "
673
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "are crossed (expected / obtained)");
674
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(point3_t(0, 0, 0), res1, errmsg0, error);
675
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = exactCubic(1);
676
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(point3_t(1, 1, 1), res1, errmsg0, error);
677 // Test number of polynomials in exact cubic
678
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::size_t numberSegments = exactCubic.getNumberSplines();
679
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (numberSegments != 1) {
680 ✗ error = true;
681 std::cout
682 ✗ << "In ExactCubicTwoPointsTest, Error While checking number of splines"
683 ✗ << numberSegments << " ; " << 1 << std::endl;
684 }
685 // Test getSplineAt
686 std::string errmsg1(
687
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "in ExactCubicTwoPointsTest, Error While checking value on curve");
688
6/12
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 1 ComparePoints(exactCubic(0.5), (exactCubic.getSplineAt(0))(0.5), errmsg1,
689 error);
690 1 }
691
692 1 void ExactCubicOneDimTest(bool& error) {
693 1 ndcurves::T_WaypointOne waypoints;
694
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_one zero;
695
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 zero(0, 0) = 9;
696
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_one one;
697
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 one(0, 0) = 14;
698
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_one two;
699
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 two(0, 0) = 25;
700
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 waypoints.push_back(std::make_pair(0., zero));
701
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 waypoints.push_back(std::make_pair(1., one));
702
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 waypoints.push_back(std::make_pair(2., two));
703
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
704
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_one res1 = exactCubic(0);
705 std::string errmsg(
706 "in ExactCubicOneDim Error While checking that given wayPoints are "
707
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "crossed (expected / obtained)");
708
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(zero, res1, errmsg, error);
709
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res1 = exactCubic(1);
710
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(one, res1, errmsg, error);
711 1 }
712
713 3 void CheckWayPointConstraint(
714 const std::string& errmsg, const double step, const ndcurves::T_Waypoint&,
715 const exact_cubic_t* curve, bool& error,
716 double prec = Eigen::NumTraits<double>::dummy_precision()) {
717
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 point3_t res1;
718
2/2
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 3 times.
 21 for (double i = 0; i <= 1; i = i + step) {
719
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 res1 = (*curve)(i);
720
4/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 18 ComparePoints(point3_t(i, i, i), res1, errmsg, error, prec);
721 }
722 3 }
723
724 1 void ExactCubicPointsCrossedTest(bool& error) {
725 1 ndcurves::T_Waypoint waypoints;
726
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 1; i = i + 0.2) {
727
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
728 }
729
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
730 std::string errmsg(
731 "Error While checking that given wayPoints are crossed (expected / "
732
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
733
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
734 1 }
735
736 1 void ExactCubicVelocityConstraintsTest(bool& error) {
737 1 ndcurves::T_Waypoint waypoints;
738
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 1; i = i + 0.2) {
739
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
740 }
741 std::string errmsg(
742 "Error in ExactCubicVelocityConstraintsTest (1); while checking that "
743 "given wayPoints are crossed (expected / "
744
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
745
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 spline_constraints_t constraints(3);
746
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_vel = point3_t(0, 0, 0);
747
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_vel = point3_t(0, 0, 0);
748
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_acc = point3_t(0, 0, 0);
749
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_acc = point3_t(0, 0, 0);
750
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exact_cubic_t exactCubic(waypoints.begin(), waypoints.end(), constraints);
751 // now check that init and end velocity are 0
752
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
753 std::string errmsg3(
754 "Error in ExactCubicVelocityConstraintsTest (2); while checking "
755
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "derivative (expected / obtained)");
756 // now check derivatives
757
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 ComparePoints(constraints.init_vel, exactCubic.derivate(0, 1), errmsg3, error,
758 1e-10);
759
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 ComparePoints(constraints.end_vel, exactCubic.derivate(1, 1), errmsg3, error,
760 1e-10);
761
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 ComparePoints(constraints.init_acc, exactCubic.derivate(0, 2), errmsg3, error,
762 1e-10);
763
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 ComparePoints(constraints.end_acc, exactCubic.derivate(1, 2), errmsg3, error,
764 1e-10);
765
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_vel = point3_t(1, 2, 3);
766
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_vel = point3_t(-1, -2, -3);
767
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_acc = point3_t(4, 5, 6);
768
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_acc = point3_t(-4, -4, -6);
769 std::string errmsg2(
770 "Error in ExactCubicVelocityConstraintsTest (3); while checking that "
771 "given wayPoints are crossed (expected / "
772
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
773
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exact_cubic_t exactCubic2(waypoints.begin(), waypoints.end(), constraints);
774
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error, 1e-10);
775 std::string errmsg4(
776 "Error in ExactCubicVelocityConstraintsTest (4); while checking "
777
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "derivative (expected / obtained)");
778 // now check derivatives
779
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 ComparePoints(constraints.init_vel, exactCubic2.derivate(0, 1), errmsg4,
780 error, 1e-10);
781
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 ComparePoints(constraints.end_vel, exactCubic2.derivate(1, 1), errmsg4, error,
782 1e-10);
783
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 ComparePoints(constraints.init_acc, exactCubic2.derivate(0, 2), errmsg4,
784 error, 1e-10);
785
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 ComparePoints(constraints.end_acc, exactCubic2.derivate(1, 2), errmsg4, error,
786 1e-10);
787 1 }
788
789 template <typename CurveType>
790 20 void CheckPointOnline(const std::string& errmsg, const point3_t& A,
791 const point3_t& B, const double target,
792 const CurveType* curve, bool& error) {
793
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 point3_t res1 = curve->operator()(target);
794
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 point3_t ar = (res1 - A);
795
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 ar.normalize();
796
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 point3_t rb = (B - res1);
797
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 rb.normalize();
798
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
 20 if (ar.dot(rb) < 0.99999) {
799 ✗ error = true;
800 ✗ std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose()
801 ✗ << "\n ; " << target << " ; " << res1.transpose() << std::endl;
802 }
803 20 }
804
805 1 void EffectorTrajectoryTest(bool& error) {
806 // create arbitrary trajectory
807 1 ndcurves::T_Waypoint waypoints;
808
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 10; i = i + 2) {
809
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
810 }
811
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 helpers::exact_cubic_t* eff_traj = helpers::effector_spline(
812
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 waypoints.begin(), waypoints.end(), Eigen::Vector3d::UnitZ(),
813
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::Vector3d(0, 0, 2), 1, 0.02, 1, 0.5);
814
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t zero(0, 0, 0);
815
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t off1(0, 0, 1);
816
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t off2(10, 10, 10.02);
817
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t end(10, 10, 10);
818 std::string errmsg(
819 "Error in EffectorTrajectoryTest; while checking waypoints (expected / "
820
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
821 std::string errmsg2(
822 "Error in EffectorTrajectoryTest; while checking derivative (expected / "
823
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
824 // first check start / goal positions
825
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(zero, (*eff_traj)(0), errmsg, error);
826
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(off1, (*eff_traj)(1), errmsg, error);
827
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
828
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(end, (*eff_traj)(10), errmsg, error);
829 // now check derivatives
830
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(zero, (*eff_traj).derivate(0, 1), errmsg2, error);
831
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(zero, (*eff_traj).derivate(10, 1), errmsg2, error);
832
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(zero, (*eff_traj).derivate(0, 2), errmsg2, error);
833
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(zero, (*eff_traj).derivate(10, 2), errmsg2, error);
834 // check that end and init splines are line
835 std::string errmsg3(
836 "Error in EffectorTrajectoryTest; while checking that init/end splines "
837 "are line (point A/ point B, time value / "
838
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "point obtained) \n");
839
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
 11 for (double i = 0.1; i < 1; i += 0.1) {
840
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 CheckPointOnline<helpers::exact_cubic_t>(
841
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 30 errmsg3, (*eff_traj)(0), (*eff_traj)(1), i, eff_traj, error);
842 }
843
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
 11 for (double i = 9.981; i < 10; i += 0.002) {
844
3/6
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
 20 CheckPointOnline<helpers::exact_cubic_t>(
845
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 30 errmsg3, (*eff_traj)(9.5), (*eff_traj)(10), i, eff_traj, error);
846 }
847
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 delete eff_traj;
848 1 }
849
850 6 helpers::quat_t GetXRotQuat(const double theta) {
851
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 Eigen::AngleAxisd m(theta, Eigen::Vector3d::UnitX());
852
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
 12 return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
853 }
854
855 ✗ double GetXRotFromQuat(helpers::quat_ref_const_t q) {
856 ✗ Eigen::Quaterniond quat(q.data());
857 ✗ Eigen::AngleAxisd m(quat);
858 ✗ return m.angle() / M_PI * 180.;
859 }
860
861 1 void EffectorSplineRotationNoRotationTest(bool& error) {
862 // create arbitrary trajectory
863 1 ndcurves::T_Waypoint waypoints;
864
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 10; i = i + 2) {
865
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
866 }
867 helpers::effector_spline_rotation eff_traj(waypoints.begin(),
868
9/18
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
 1 waypoints.end());
869
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_init;
870
7/14
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 q_init << 0., 0., 0., 0., 0., 0., 1.;
871
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_end;
872
7/14
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 q_end << 10., 10., 10., 0., 0., 0., 1.;
873
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_to;
874
7/14
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 q_to << 0., 0, 0.02, 0., 0., 0., 1.;
875
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_land;
876
7/14
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 q_land << 10, 10, 10.02, 0, 0., 0., 1.;
877
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_mod;
878
7/14
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 q_mod << 6., 6., 6., 0., 0., 0., 1.;
879 std::string errmsg(
880 "Error in EffectorSplineRotationNoRotationTest; while checking waypoints "
881
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "(expected / obtained)");
882
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_init, eff_traj(0), errmsg, error);
883
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_to, eff_traj(0.02), errmsg, error);
884
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_land, eff_traj(9.98), errmsg, error);
885
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_mod, eff_traj(6), errmsg, error);
886
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_end, eff_traj(10), errmsg, error);
887 1 }
888
889 1 void EffectorSplineRotationRotationTest(bool& error) {
890 // create arbitrary trajectory
891 1 ndcurves::T_Waypoint waypoints;
892
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 10; i = i + 2) {
893
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
894 }
895
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::quat_t init_quat = GetXRotQuat(M_PI);
896 helpers::effector_spline_rotation eff_traj(waypoints.begin(), waypoints.end(),
897
8/16
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
 1 init_quat);
898
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 helpers::config_t q_init = helpers::config_t::Zero();
899
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 q_init.tail<4>() = init_quat;
900
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_end;
901
7/14
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 q_end << 10., 10., 10., 0., 0., 0., 1.;
902
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_to = q_init;
903
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q_to(2) += 0.02;
904
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_land = q_end;
905
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q_land(2) += 0.02;
906
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::quat_t q_mod = GetXRotQuat(M_PI_2);
907 ;
908 std::string errmsg(
909 "Error in EffectorSplineRotationRotationTest; while checking waypoints "
910
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "(expected / obtained)");
911
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_init, eff_traj(0), errmsg, error);
912
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_to, eff_traj(0.02), errmsg, error);
913
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_land, eff_traj(9.98), errmsg, error);
914
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 ComparePoints(q_mod, eff_traj(5).tail<4>(), errmsg, error);
915
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_end, eff_traj(10), errmsg, error);
916 1 }
917
918 1 void EffectorSplineRotationWayPointRotationTest(bool& error) {
919 // create arbitrary trajectory
920 1 ndcurves::T_Waypoint waypoints;
921
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 10; i = i + 2) {
922
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
923 }
924
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::quat_t init_quat = GetXRotQuat(0);
925 1 helpers::t_waypoint_quat_t quat_waypoints_;
926
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::quat_t q_pi_0 = GetXRotQuat(0);
927
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
928
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::quat_t q_pi = GetXRotQuat(M_PI);
929
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 quat_waypoints_.push_back(std::make_pair(0.4, q_pi_0));
930
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 quat_waypoints_.push_back(std::make_pair(6, q_pi_2));
931
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 quat_waypoints_.push_back(std::make_pair(8, q_pi));
932 helpers::effector_spline_rotation eff_traj(waypoints.begin(), waypoints.end(),
933 quat_waypoints_.begin(),
934
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
 2 quat_waypoints_.end());
935
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 helpers::config_t q_init = helpers::config_t::Zero();
936
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 q_init.tail<4>() = init_quat;
937
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_end;
938
7/14
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 q_end << 10., 10., 10., 0., 0., 0., 1.;
939
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 q_end.tail<4>() = q_pi;
940
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_mod;
941
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 q_mod.head<3>() = point3_t(6, 6, 6);
942
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 q_mod.tail<4>() = q_pi_2;
943
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_to = q_init;
944
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q_to(2) += 0.02;
945
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 helpers::config_t q_land = q_end;
946
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q_land(2) += 0.02;
947 std::string errmsg(
948 "Error in EffectorSplineRotationWayPointRotationTest; while checking "
949
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "waypoints (expected / obtained)");
950
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_init, eff_traj(0), errmsg, error);
951
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_to, eff_traj(0.02), errmsg, error);
952
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_land, eff_traj(9.98), errmsg, error);
953
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_mod, eff_traj(6), errmsg, error);
954
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(q_end, eff_traj(10), errmsg, error);
955 1 }
956
957 1 void TestReparametrization(bool& error) {
958
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 helpers::rotation_spline s;
959 1 const helpers::exact_cubic_constraint_one_dim& sp = s.time_reparam_;
960
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(sp.min(), 0.0)) {
961 ✗ std::cout << "in TestReparametrization; min value is not 0, got "
962 ✗ << sp.min() << std::endl;
963 ✗ error = true;
964 }
965
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(sp.max(), 1.0)) {
966 ✗ std::cout << "in TestReparametrization; max value is not 1, got "
967 ✗ << sp.max() << std::endl;
968 ✗ error = true;
969 }
970
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!QuasiEqual(sp(1)[0], 1.0)) {
971 ✗ std::cout << "in TestReparametrization; end value is not 1, got "
972 ✗ << sp(1)[0] << std::endl;
973 ✗ error = true;
974 }
975
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!QuasiEqual(sp(0)[0], 0.0)) {
976 ✗ std::cout << "in TestReparametrization; init value is not 0, got "
977 ✗ << sp(0)[0] << std::endl;
978 ✗ error = true;
979 }
980
2/2
✓ Branch 0 taken 500 times.
✓ Branch 1 taken 1 times.
 501 for (double i = 0; i < 1; i += 0.002) {
981
5/10
✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 500 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 500 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 500 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 500 times.
 500 if (sp(i)[0] > sp(i + 0.002)[0]) {
982 ✗ std::cout << "in TestReparametrization; reparametrization not monotonous "
983 ✗ << sp.max() << std::endl;
984 ✗ error = true;
985 }
986 }
987 1 }
988
989 6 point3_t randomPoint(const double min, const double max) {
990 6 point3_t p;
991
2/2
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 6 times.
 24 for (size_t i = 0; i < 3; ++i) {
992 18 p[i] = (rand() / (double)RAND_MAX) * (max - min) + min;
993 }
994 6 return p;
995 }
996
997 1 void BezierEvalDeCasteljau(bool& error) {
998 using namespace std;
999 1 std::vector<double> values;
1000
2/2
✓ Branch 0 taken 100000 times.
✓ Branch 1 taken 1 times.
 100001 for (int i = 0; i < 100000; ++i) {
1001
1/2
✓ Branch 2 taken 100000 times.
✗ Branch 3 not taken.
 100000 values.push_back(rand() / RAND_MAX);
1002 }
1003 // first compare regular evaluation (low dim pol)
1004
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
1005
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
1006
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
1007
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
1008
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t e(3, 61, 7);
1009
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t f(3, 56, 7);
1010
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t g(3, 36, 7);
1011
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t h(43, 6, 7);
1012
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t i(3, 6, 77);
1013 1 std::vector<point3_t> params;
1014
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
1015
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
1016
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
1017 // 3d curve
1018
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf(params.begin(), params.end());
1019 std::string errmsg(
1020 "Error in BezierEvalDeCasteljau; while comparing actual bezier "
1021
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "evaluation and de Casteljau : ");
1022 1 for (std::vector<double>::const_iterator cit = values.begin();
1023
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
1024
5/10
✓ Branch 3 taken 100000 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100000 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 100000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100000 times.
✗ Branch 17 not taken.
 100000 ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
1025 }
1026
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(d);
1027
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(e);
1028
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(f);
1029
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(g);
1030
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(h);
1031
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(i);
1032
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf2(params.begin(), params.end());
1033 1 for (std::vector<double>::const_iterator cit = values.begin();
1034
2/2
✓ Branch 2 taken 100000 times.
✓ Branch 3 taken 1 times.
 100001 cit != values.end(); ++cit) {
1035
5/10
✓ Branch 3 taken 100000 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100000 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 100000 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100000 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100000 times.
✗ Branch 17 not taken.
 100000 ComparePoints(cf2.evalDeCasteljau(*cit), cf2(*cit), errmsg, error);
1036 }
1037 1 }
1038
1039 1 void BezierElevate(bool& error) {
1040 using namespace std;
1041 1 std::vector<double> values;
1042
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
 11 for (int i = 0; i < 10; ++i) {
1043
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 values.push_back(double(rand()) / double(RAND_MAX));
1044 }
1045 // first compare regular evaluation (low dim pol)
1046
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
1047
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
1048
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
1049 1 std::vector<point3_t> params;
1050
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
1051
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
1052
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
1053 // 3d curve
1054
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t cf(params.begin(), params.end());
1055
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t cf2 = cf.elevate(1);
1056
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t cf3 = cf2.elevate(1);
1057
2/6
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (cf2.degree() - cf.degree() != 1 && cf3.degree() - cf.degree() != 2) {
1058 ✗ error = true;
1059 std::string errmsg(
1060 "Error in BezierElevate; Degree mismatched for elevated curves. "
1061 ✗ "Expected 1 / 2, got: ");
1062 ✗ std::cout << errmsg << cf2.degree() - cf.degree() << " ; "
1063 ✗ << cf3.degree() - cf.degree() << std::endl;
1064 }
1065 std::string errmsg(
1066 "Error in BezierElevate; Elevated curves do not have the same evaluation "
1067
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ": ");
1068 1 for (std::vector<double>::const_iterator cit = values.begin();
1069
2/2
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 1 times.
 10 cit != values.end() - 1; ++cit) {
1070
5/10
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 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 ComparePoints(cf2(*cit), cf(*(cit)), errmsg, error);
1071
5/10
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 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 ComparePoints(cf3(*cit), cf(*cit), errmsg, error);
1072 }
1073 1 }
1074
1075 /**
1076 * @brief BezierSplitCurve test the 'split' method of bezier curve
1077 * @param error
1078 */
1079 1 void BezierSplitCurve(bool& error) {
1080 // test for degree 5
1081 1 size_t n = 5;
1082 1 double t_min = 0.2;
1083 1 double t_max = 10;
1084 double aux0, aux1;
1085 std::string errMsg0(
1086 "BezierSplitCurve, ERROR initial point of the splitted curve doesn't "
1087
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "correspond to the original");
1088 std::string errMsg1(
1089 "BezierSplitCurve, ERROR splitting point of the splitted curve doesn't "
1090
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "correspond to the original");
1091 std::string errMsg2(
1092 "BezierSplitCurve, ERROR final point of the splitted curve doesn't "
1093
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "correspond to the original");
1094 std::string errMsg3(
1095 "BezierSplitCurve, ERROR while checking value on curve and curves "
1096
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "splitted");
1097 std::string errMsg4(
1098 "BezierSplitCurve, ERROR Degree of the splitted curve are not the same "
1099
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "as the original curve");
1100 std::string errMsg5(
1101 "BezierSplitCurve, ERROR duration of the splitted curve doesn't "
1102
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "correspond to the original");
1103 std::string errMsg6(
1104
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "BezierSplitCurve, ERROR while checking value on curve extracted");
1105
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 for (size_t i = 0; i < 1; ++i) {
1106 // build a random curve and split it at random time :
1107 // std::cout<<"build a random curve"<<std::endl;
1108
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a;
1109 1 std::vector<point3_t> wps;
1110
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (size_t j = 0; j <= n; ++j) {
1111
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 wps.push_back(randomPoint(-10., 10.));
1112 }
1113 1 double t0 = (rand() / (double)RAND_MAX) * (t_max - t_min) + t_min;
1114 1 double t1 = (rand() / (double)RAND_MAX) * (t_max - t0) + t0;
1115 1 double ts = (rand() / (double)RAND_MAX) * (t1 - t0) + t0;
1116
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t c(wps.begin(), wps.end(), t0, t1);
1117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::pair<bezier_t, bezier_t> cs = c.split(ts);
1118 // test on splitted curves :
1119
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (!((c.degree_ == cs.first.degree_) &&
1120
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 (c.degree_ == cs.second.degree_))) {
1121 ✗ error = true;
1122 ✗ std::cout << errMsg4 << std::endl;
1123 }
1124 1 aux0 = c.max() - c.min();
1125 1 aux1 =
1126 1 (cs.first.max() - cs.first.min() + cs.second.max() - cs.second.min());
1127
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!QuasiEqual(aux0, aux1)) {
1128 ✗ error = true;
1129 ✗ std::cout << errMsg5 << std::endl;
1130 }
1131
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(cs.first.max(), ts)) {
1132 ✗ error = true;
1133 ✗ std::cout << errMsg0 << std::endl;
1134 }
1135
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 ComparePoints(c(t0), cs.first(t0), errMsg0, error);
1136
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 ComparePoints(cs.first(ts), cs.second(ts), errMsg1, error);
1137
5/10
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 1 ComparePoints(c(t1), cs.second(cs.second.max()), errMsg2, error);
1138 // check along curve :
1139 1 double ti = t0;
1140
2/2
✓ Branch 0 taken 55 times.
✓ Branch 1 taken 1 times.
 56 while (ti <= ts) {
1141
5/10
✓ Branch 2 taken 55 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 55 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 55 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 55 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 55 times.
✗ Branch 15 not taken.
 55 ComparePoints(cs.first(ti), c(ti), errMsg3, error);
1142 55 ti += 0.01;
1143 }
1144
2/2
✓ Branch 0 taken 85 times.
✓ Branch 1 taken 1 times.
 86 while (ti <= t1) {
1145
5/10
✓ Branch 2 taken 85 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 85 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 85 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 85 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 85 times.
✗ Branch 15 not taken.
 85 ComparePoints(cs.second(ti), c(ti), errMsg3, error);
1146 85 ti += 0.01;
1147 }
1148 // Test extract function
1149 bezier_t bezier_extracted0 =
1150
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 c.extract(t0 + 0.01, t1 - 0.01); // T_min < t0 < t1 < T_max
1151
2/2
✓ Branch 2 taken 138 times.
✓ Branch 3 taken 1 times.
 139 for (double t = bezier_extracted0.min(); t < bezier_extracted0.max();
1152 138 t += 0.01) {
1153
5/10
✓ Branch 2 taken 138 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 138 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 138 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 138 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 138 times.
✗ Branch 15 not taken.
 138 ComparePoints(bezier_extracted0(t), c(t), errMsg6, error);
1154 }
1155 bezier_t bezier_extracted1 =
1156
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 c.extract(t0, t1 - 0.01); // T_min = t0 < t1 < T_max
1157
2/2
✓ Branch 2 taken 139 times.
✓ Branch 3 taken 1 times.
 140 for (double t = bezier_extracted1.min(); t < bezier_extracted1.max();
1158 139 t += 0.01) {
1159
5/10
✓ Branch 2 taken 139 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 139 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 139 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 139 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 139 times.
✗ Branch 15 not taken.
 139 ComparePoints(bezier_extracted1(t), c(t), errMsg6, error);
1160 }
1161 bezier_t bezier_extracted2 =
1162
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 c.extract(t0 + 0.01, t1); // T_min < t0 < t1 = T_max
1163
2/2
✓ Branch 2 taken 139 times.
✓ Branch 3 taken 1 times.
 140 for (double t = bezier_extracted2.min(); t < bezier_extracted2.max();
1164 139 t += 0.01) {
1165
5/10
✓ Branch 2 taken 139 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 139 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 139 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 139 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 139 times.
✗ Branch 15 not taken.
 139 ComparePoints(bezier_extracted2(t), c(t), errMsg6, error);
1166 }
1167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t bezier_extracted3 = c.extract(t0, t1); // T_min = t0 < t1 = T_max
1168
2/2
✓ Branch 2 taken 140 times.
✓ Branch 3 taken 1 times.
 141 for (double t = bezier_extracted3.min(); t < bezier_extracted3.max();
1169 140 t += 0.01) {
1170
5/10
✓ Branch 2 taken 140 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 140 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 140 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 140 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 140 times.
✗ Branch 15 not taken.
 140 ComparePoints(bezier_extracted3(t), c(t), errMsg6, error);
1171 }
1172 1 }
1173 1 }
1174
1175 /* cubic hermite spline function test */
1176 1 void CubicHermitePairsPositionDerivativeTest(bool& error) {
1177 try {
1178 std::string errmsg1(
1179 "in Cubic Hermite 2 pairs (pos,vel), Error While checking that given "
1180 "wayPoints are crossed (expected / "
1181
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained) : ");
1182 std::string errmsg2(
1183 "in Cubic Hermite 2 points, Error While checking value of point on "
1184
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "curve : ");
1185 std::string errmsg3(
1186 "in Cubic Hermite 2 points, Error While checking value of tangent on "
1187
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "curve : ");
1188 1 std::vector<pair_point_tangent_t> control_points;
1189
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t res1;
1190
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p0(0., 0., 0.);
1191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p1(1., 2., 3.);
1192
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p2(4., 4., 4.);
1193
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t t0(0.5, 0.5, 0.5);
1194
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t t1(0.1, 0.2, -0.5);
1195
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t t2(0.1, 0.2, 0.3);
1196 1 std::vector<double> time_control_points, time_control_points_test;
1197 // Two pairs
1198 1 control_points.clear();
1199
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 control_points.push_back(pair_point_tangent_t(p0, t0));
1200
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 control_points.push_back(pair_point_tangent_t(p1, t1));
1201
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(0.); // Time at P0
1202
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(1.); // Time at P1
1203 // Create cubic hermite spline
1204 cubic_hermite_spline_t cubic_hermite_spline_1Pair(
1205
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 control_points.begin(), control_points.end(), time_control_points);
1206 // Dimension
1207
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (cubic_hermite_spline_1Pair.dim() != 3) {
1208 ✗ error = true;
1209 std::cout
1210 ✗ << "Cubic hermite spline test, Error : Dimension of curve is wrong\n";
1211 }
1212 // Check
1213
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_1Pair(0.); // t=0
1214
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(p0, res1, errmsg1, error);
1215
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_1Pair(1.); // t=1
1216
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(p1, res1, errmsg1, error);
1217 // Test derivative : two pairs
1218
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_1Pair.derivate(0., 1);
1219
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(t0, res1, errmsg3, error);
1220
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_1Pair.derivate(1., 1);
1221
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(t1, res1, errmsg3, error);
1222 // Three pairs
1223
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 control_points.push_back(pair_point_tangent_t(p2, t2));
1224 1 time_control_points.clear();
1225
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(0.); // Time at P0
1226
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(2.); // Time at P1
1227
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(5.); // Time at P2
1228 cubic_hermite_spline_t cubic_hermite_spline_2Pairs(
1229
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 control_points.begin(), control_points.end(), time_control_points);
1230 // Check
1231
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs(0.); // t=0
1232
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(p0, res1, errmsg1, error);
1233
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs(2.); // t=2
1234
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(p1, res1, errmsg2, error);
1235
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs(5.); // t=5
1236
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(p2, res1, errmsg1, error);
1237 // Test derivative : three pairs
1238
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs.derivate(0., 1);
1239
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(t0, res1, errmsg3, error);
1240
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs.derivate(2., 1);
1241
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(t1, res1, errmsg3, error);
1242
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs.derivate(5., 1);
1243
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(t2, res1, errmsg3, error);
1244 // Test time control points by default [0,1] => with N control points :
1245 // Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... |
1246 // Time at P_(N-1)= (N-1)/(N-1)= 1.0
1247 1 time_control_points_test.clear();
1248
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points_test.push_back(0.); // Time at P0
1249
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points_test.push_back(0.5); // Time at P1
1250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points_test.push_back(1.0); // Time at P2
1251
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cubic_hermite_spline_2Pairs.setTime(time_control_points_test);
1252
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs(0.); // t=0
1253
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(p0, res1, errmsg1, error);
1254
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
1255
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(p1, res1, errmsg2, error);
1256
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs(1.); // t=1
1257
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(p2, res1, errmsg1, error);
1258 // Test getTime
1259 try {
1260
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cubic_hermite_spline_2Pairs.getTime();
1261 ✗ } catch (...) {
1262 ✗ error = false;
1263 }
1264
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
1265 ✗ std::cout << "Cubic hermite spline test, Error when calling getTime\n";
1266 }
1267 // Test derivative : three pairs, time default
1268
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs.derivate(0., 1);
1269
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(t0, res1, errmsg3, error);
1270
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs.derivate(0.5, 1);
1271
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(t1, res1, errmsg3, error);
1272
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cubic_hermite_spline_2Pairs.derivate(1., 1);
1273
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(t2, res1, errmsg3, error);
1274
1275
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p_derivate, p_compute_derivate;
1276
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
 5 for (size_t order = 1; order < 5; ++order) {
1277
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 std::stringstream ss;
1278 ss << "in Cubic Hermite 2 points, "
1279 "compute_derivate do not lead to the same results as derivate for "
1280
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 "order = ";
1281
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 ss << order << std::endl;
1282 curve_ptr_t derivate_ptr(
1283
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 cubic_hermite_spline_2Pairs.compute_derivate_ptr(order));
1284 4 double t = 0.;
1285
2/2
✓ Branch 0 taken 44 times.
✓ Branch 1 taken 4 times.
 48 while (t <= 1.) {
1286
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 p_derivate = cubic_hermite_spline_2Pairs.derivate(t, order);
1287
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 44 p_compute_derivate = derivate_ptr->operator()(t);
1288
4/8
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 44 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 44 times.
✗ Branch 12 not taken.
 44 ComparePoints(p_derivate, p_compute_derivate, ss.str(), error);
1289 44 t += 0.1;
1290 }
1291 4 }
1292 1 } catch (...) {
1293 ✗ error = true;
1294 ✗ std::cout << "Error in CubicHermitePairsPositionDerivativeTest"
1295 ✗ << std::endl;
1296 }
1297 1 }
1298
1299 1 void piecewiseCurveTest(bool& error) {
1300 try {
1301 // TEST WITH POLYNOMIALS
1302 std::string errmsg1(
1303 "in piecewise polynomial curve test, Error While checking value of "
1304
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "point on curve : ");
1305
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 1, 1); // in [0,1[
1306
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 1, 1); // in [1,2[
1307
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 1, 1); // in [2,3]
1308
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t res;
1309
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 t_pointX_t vec1, vec2, vec3;
1310
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec1.push_back(a); // x=1, y=1, z=1
1311
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec2.push_back(b); // x=2, y=1, z=1
1312
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec3.push_back(c); // x=3, y=1, z=1
1313 // Create three polynomials of constant value in the interval of definition
1314 std::shared_ptr<polynomial_t> pol1_ptr =
1315
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<polynomial_t>(vec1.begin(), vec1.end(), 0, 1);
1316 std::shared_ptr<polynomial_t> pol2_ptr =
1317
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<polynomial_t>(vec2.begin(), vec2.end(), 1, 2);
1318 std::shared_ptr<polynomial_t> pol3_ptr =
1319
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<polynomial_t>(vec3.begin(), vec3.end(), 2, 3);
1320 // 1 polynomial in curve
1321
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 piecewise_t pc(pol1_ptr);
1322
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc(0.5);
1323
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(a, res, errmsg1, error);
1324 // 3 polynomials in curve
1325
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pc.add_curve_ptr(pol2_ptr);
1326
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pc.add_curve_ptr(pol3_ptr);
1327 // Check values on piecewise curve
1328 // t in [0,1[ -> res=a
1329
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc(0.);
1330
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(a, res, errmsg1, error);
1331
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc(0.5);
1332
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(a, res, errmsg1, error);
1333 // t in [1,2[ -> res=b
1334
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc(1.0);
1335
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(b, res, errmsg1, error);
1336
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc(1.5);
1337
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(b, res, errmsg1, error);
1338 // t in [2,3] -> res=c
1339
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc(2.0);
1340
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(c, res, errmsg1, error);
1341
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc(3.0);
1342
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(c, res, errmsg1, error);
1343 // Create piecewise curve C0 from bezier
1344
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a0(1, 2, 3);
1345
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b0(2, 3, 4);
1346
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c0(3, 4, 5);
1347
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d0(4, 5, 6);
1348 1 std::vector<point3_t> params0;
1349 1 std::vector<point3_t> params1;
1350
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params0.push_back(a0); // bezier between [0,1]
1351
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params0.push_back(b0);
1352
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params0.push_back(c0);
1353
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params0.push_back(d0);
1354
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params1.push_back(d0); // bezier between [1,2]
1355
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params1.push_back(c0);
1356
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params1.push_back(b0);
1357
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params1.push_back(a0);
1358 std::shared_ptr<bezier_t> bc0_ptr =
1359
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<bezier_t>(params0.begin(), params0.end(), 0., 1.);
1360 std::shared_ptr<bezier_t> bc1_ptr =
1361
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<bezier_t>(params1.begin(), params1.end(), 1., 2.);
1362
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 piecewise_t pc_C0(bc0_ptr);
1363
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pc_C0.add_curve_ptr(bc1_ptr);
1364 // Check value in t=0.5 and t=1.5
1365
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc_C0(0.0);
1366
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(a0, res, errmsg1, error);
1367
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc_C0(1.0);
1368
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(d0, res, errmsg1, error);
1369
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = pc_C0(2.0);
1370
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 ComparePoints(a0, res, errmsg1, error);
1371 // Create piecewise curve C1 from Hermite
1372
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p0(0., 0., 0.);
1373
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p1(1., 2., 3.);
1374
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p2(4., 4., 4.);
1375
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t t0(0.5, 0.5, 0.5);
1376
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t t1(0.1, 0.2, -0.5);
1377
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t t2(0.1, 0.2, 0.3);
1378 1 std::vector<pair_point_tangent_t> control_points_0;
1379
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 control_points_0.push_back(pair_point_tangent_t(p0, t0));
1380
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points_0.push_back(
1381
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pair_point_tangent_t(p1, t1)); // control_points_0 = 1st piece of curve
1382 1 std::vector<pair_point_tangent_t> control_points_1;
1383
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 control_points_1.push_back(pair_point_tangent_t(p1, t1));
1384
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points_1.push_back(
1385
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pair_point_tangent_t(p2, t2)); // control_points_1 = 2nd piece of curve
1386 1 std::vector<double> time_control_points0, time_control_points1;
1387
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points0.push_back(0.);
1388
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points0.push_back(1.); // hermite 0 between [0,1]
1389
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points1.push_back(1.);
1390
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points1.push_back(3.); // hermite 1 between [1,3]
1391 std::shared_ptr<cubic_hermite_spline_t> chs0_ptr =
1392 1 std::make_shared<cubic_hermite_spline_t>(control_points_0.begin(),
1393 1 control_points_0.end(),
1394
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points0);
1395 std::shared_ptr<cubic_hermite_spline_t> chs1_ptr =
1396 1 std::make_shared<cubic_hermite_spline_t>(control_points_1.begin(),
1397 1 control_points_1.end(),
1398
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points1);
1399
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 piecewise_t pc_C1(chs0_ptr);
1400
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pc_C1.add_curve_ptr(chs1_ptr);
1401 // Create piecewise curve C2
1402
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a1(0, 0, 0);
1403
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b1(1, 1, 1);
1404
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 t_pointX_t veca, vecb;
1405 // in [0,1[
1406
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 veca.push_back(a1);
1407
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 veca.push_back(b1); // x=t, y=t, z=t
1408 // in [1,2]
1409
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vecb.push_back(b1);
1410
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vecb.push_back(b1); // x=(t-1)+1, y=(t-1)+1, z=(t-1)+1
1411 std::shared_ptr<polynomial_t> pola_ptr =
1412
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<polynomial_t>(veca.begin(), veca.end(), 0, 1);
1413 std::shared_ptr<polynomial_t> polb_ptr =
1414
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<polynomial_t>(vecb.begin(), vecb.end(), 1, 2);
1415
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 piecewise_t pc_C2(pola_ptr);
1416
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pc_C2.add_curve_ptr(polb_ptr);
1417 // check C0 continuity
1418 std::string errmsg2(
1419 "in piecewise polynomial curve test, Error while checking continuity "
1420
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "C0 on ");
1421 std::string errmsg3(
1422 "in piecewise polynomial curve test, Error while checking continuity "
1423
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "C1 on ");
1424 std::string errmsg4(
1425 "in piecewise polynomial curve test, Error while checking continuity "
1426
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "C2 on ");
1427 // not C0
1428
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bool isC0 = pc.is_continuous(0);
1429
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (isC0) {
1430 ✗ std::cout << errmsg2 << " pc " << std::endl;
1431 ✗ error = true;
1432 }
1433 // C0
1434
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 isC0 = pc_C0.is_continuous(0);
1435
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (not isC0) {
1436 ✗ std::cout << errmsg2 << " pc_C0 " << std::endl;
1437 ✗ error = true;
1438 }
1439 // not C1
1440
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bool isC1 = pc_C0.is_continuous(1);
1441
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (isC1) {
1442 ✗ std::cout << errmsg3 << " pc_C0 " << std::endl;
1443 ✗ error = true;
1444 }
1445 // C1
1446
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 isC1 = pc_C1.is_continuous(1);
1447
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (not isC1) {
1448 ✗ std::cout << errmsg3 << " pc_C1 " << std::endl;
1449 ✗ error = true;
1450 }
1451 // not C2
1452
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bool isC2 = pc_C1.is_continuous(2);
1453
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (isC2) {
1454 ✗ std::cout << errmsg4 << " pc_C1 " << std::endl;
1455 ✗ error = true;
1456 }
1457 // C2
1458
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 isC2 = pc_C2.is_continuous(2);
1459
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (not isC2) {
1460 ✗ std::cout << errmsg4 << " pc_C2 " << std::endl;
1461 ✗ error = true;
1462 }
1463 // CONVERT PIECEWISE POLYNOMIAL CURVES TO BEZIER AND HERMITE
1464 std::string errmsg5(
1465 "in piecewise polynomial curve test, Error while checking piecewise "
1466
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "curve conversion");
1467
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewise_t pc_bezier = pc.convert_piecewise_curve_to_bezier<bezier_t>();
1468
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(pc, pc_bezier, errmsg5, error);
1469 piecewise_t pc_hermite =
1470
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
1471
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(pc, pc_hermite, errmsg5, error);
1472 piecewise_t pc_polynomial_same =
1473
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc.convert_piecewise_curve_to_polynomial<polynomial_t>();
1474
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(pc, pc_polynomial_same, errmsg5,
1475 error);
1476 // CONVERT PIECEWISE BEZIER TO POLYNOMIAL AND HERMITE
1477
1478 std::string errmsg6(
1479 "in piecewise bezier curve test, Error while checking piecewise curve "
1480
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "conversion");
1481 piecewise_t pc_bezier1 =
1482
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc_C0.convert_piecewise_curve_to_bezier<bezier_t>();
1483
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(pc_C0, pc_bezier1, errmsg6, error);
1484 piecewise_t pc_hermite1 =
1485 pc_C0
1486
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 .convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
1487
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(pc_C0, pc_hermite1, errmsg6, error);
1488 piecewise_t pc_polynomial1 =
1489
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc_C0.convert_piecewise_curve_to_polynomial<polynomial_t>();
1490
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(pc_C0, pc_polynomial1, errmsg6,
1491 error);
1492
1493 // compare compute_derivate and derivate results :
1494
1495
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 curve_abc_t* pc_C2_derivate = pc_C2.compute_derivate_ptr(1);
1496
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 curve_abc_t* pc_C2_derivate2 = pc_C2.compute_derivate_ptr(2);
1497
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (pc_C2.min() != pc_C2_derivate->min()) {
1498 ✗ error = true;
1499 ✗ std::cout << "min bounds for curve and it's derivate are not equals."
1500 ✗ << std::endl;
1501 }
1502
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (pc_C2.min() != pc_C2_derivate2->min()) {
1503 ✗ error = true;
1504 std::cout
1505 ✗ << "min bounds for curve and it's second derivate are not equals."
1506 ✗ << std::endl;
1507 }
1508
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (pc_C2.max() != pc_C2_derivate->max()) {
1509 ✗ error = true;
1510 ✗ std::cout << "max bounds for curve and it's derivate are not equals."
1511 ✗ << std::endl;
1512 }
1513
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (pc_C2.max() != pc_C2_derivate2->max()) {
1514 ✗ error = true;
1515 std::cout
1516 ✗ << "max bounds for curve and it's second derivate are not equals."
1517 ✗ << std::endl;
1518 }
1519 1 double t = 0.;
1520
2/2
✓ Branch 1 taken 200 times.
✓ Branch 2 taken 1 times.
 201 while (t < pc_C2.max()) {
1521
6/12
✓ Branch 1 taken 200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 200 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 200 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 200 times.
 200 if (!QuasiEqual(pc_C2.derivate(t, 1), (*pc_C2_derivate)(t))) {
1522 ✗ error = true;
1523 std::cout << "value not equal between derivate and compute_derivate "
1524 ✗ "(order 1) at t = "
1525 ✗ << t << std::endl;
1526 }
1527
6/12
✓ Branch 1 taken 200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 200 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 200 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 200 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 200 times.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✓ Branch 18 taken 200 times.
 200 if (!QuasiEqual(pc_C2.derivate(t, 2), (*pc_C2_derivate2)(t))) {
1528 ✗ error = true;
1529 std::cout << "value not equal between derivate and compute_derivate "
1530 ✗ "(order 2) at t = "
1531 ✗ << t << std::endl;
1532 }
1533 200 t += 0.01;
1534 }
1535
1536 1 } catch (...) {
1537 ✗ error = true;
1538 ✗ std::cout << "Error in piecewiseCurveTest" << std::endl;
1539 }
1540 1 }
1541
1542 1 void curveAbcDimDynamicTest(bool& error) {
1543 typedef curve_abc<double, double, true> curve_abc_test_t;
1544 typedef polynomial<double, double, true> polynomial_test_t;
1545 typedef exact_cubic<double, double, true> exact_cubic_test_t;
1546 typedef exact_cubic_test_t::spline_constraints spline_constraints_test_t;
1547 typedef bezier_curve<double, double, true> bezier_test_t;
1548 typedef cubic_hermite_spline<double, double, true>
1549 cubic_hermite_spline_test_t;
1550 curve_abc_test_t* pt_curve_abc;
1551 // POLYNOMIAL
1552
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 1, 1);
1553
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 2, 2);
1554
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pointX_t vec;
1555
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(a);
1556
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec.push_back(b);
1557
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 polynomial_test_t pol(vec.begin(), vec.end(), 0, 1);
1558 try {
1559
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pol(0);
1560
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pol(1);
1561 ✗ } catch (...) {
1562 ✗ error = false;
1563 }
1564 // BEZIER
1565
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_test_t bc = bezier_from_curve<bezier_test_t>(pol);
1566 try {
1567
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bc(0);
1568
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bc(1);
1569 ✗ } catch (...) {
1570 ✗ error = false;
1571 }
1572 // CUBIC HERMITE
1573 cubic_hermite_spline_test_t chs =
1574
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 hermite_from_curve<cubic_hermite_spline_test_t>(pol);
1575 try {
1576
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 chs(0);
1577
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 chs(1);
1578 ✗ } catch (...) {
1579 ✗ error = false;
1580 }
1581 // EXACT CUBIC : NOT SUPPORTED, problem to fix later
1582 1 ndcurves::T_Waypoint waypoints;
1583
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 1; i = i + 0.2) {
1584
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
1585 }
1586 std::string errmsg(
1587 "Error in ExactCubicVelocityConstraintsTest (1); while checking that "
1588 "given wayPoints are crossed (expected / "
1589
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
1590
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 spline_constraints_test_t constraints(3);
1591
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_vel = point3_t(0, 0, 0);
1592
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_vel = point3_t(0, 0, 0);
1593
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_acc = point3_t(0, 0, 0);
1594
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_acc = point3_t(0, 0, 0);
1595
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exact_cubic_test_t ec(waypoints.begin(), waypoints.end(), constraints);
1596 try {
1597
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ec(0);
1598
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ec(1);
1599 ✗ } catch (...) {
1600 ✗ error = false;
1601 }
1602 // Test with pointer to curve_abc type
1603 try {
1604 1 pt_curve_abc = &pol;
1605
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_curve_abc)(0);
1606
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_curve_abc)(1);
1607 1 pt_curve_abc = &bc;
1608
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_curve_abc)(0);
1609
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_curve_abc)(1);
1610 1 pt_curve_abc = &chs;
1611
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_curve_abc)(0);
1612
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_curve_abc)(1);
1613 1 pt_curve_abc = &ec;
1614
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_curve_abc)(0);
1615
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_curve_abc)(1);
1616 ✗ } catch (...) {
1617 ✗ error = false;
1618 }
1619 1 }
1620
1621 1 void PiecewisePolynomialCurveFromDiscretePoints(bool& error) {
1622 std::string errMsg(
1623 "PiecewisePolynomialCurveFromDiscretePoints, Error, value on curve is "
1624
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "wrong : ");
1625
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p0(0., 0., 0.);
1626
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p1(1., 2., 3.);
1627
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p2(4., 4., 4.);
1628
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p3(10., 10., 10.);
1629
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d0(1., 1., 1.);
1630
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d1(2., 2., 2.);
1631
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d2(3., 3., 3.);
1632
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d3(5., 5., 5.);
1633
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t dd0(1.5, 1.5, 1.5);
1634
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t dd1(2.5, 2.5, 2.5);
1635
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t dd2(3.5, 3.5, 3.5);
1636
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t dd3(5.5, 5.5, 5.5);
1637 1 double t0 = 1.0;
1638 1 double t1 = 1.5;
1639 1 double t2 = 3.0;
1640 1 double t3 = 10.0;
1641
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pointX_t points;
1642
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points.push_back(p0);
1643
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points.push_back(p1);
1644
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points.push_back(p2);
1645
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points.push_back(p3);
1646
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pointX_t points_derivative;
1647
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points_derivative.push_back(d0);
1648
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points_derivative.push_back(d1);
1649
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points_derivative.push_back(d2);
1650
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points_derivative.push_back(d3);
1651
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pointX_t points_second_derivative;
1652
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points_second_derivative.push_back(dd0);
1653
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points_second_derivative.push_back(dd1);
1654
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points_second_derivative.push_back(dd2);
1655
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 points_second_derivative.push_back(dd3);
1656 1 std::vector<double> time_points;
1657
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_points.push_back(t0);
1658
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_points.push_back(t1);
1659
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_points.push_back(t2);
1660
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_points.push_back(t3);
1661
1662 // Piecewise polynomial curve C0 => Linear interpolation between points
1663 piecewise_t ppc_C0 =
1664 piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
1665
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.
 2 points, time_points);
1666
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (!ppc_C0.is_continuous(0)) {
1667 std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise "
1668 ✗ "curve is not C0"
1669 ✗ << std::endl;
1670 ✗ error = true;
1671 }
1672
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
 5 for (std::size_t i = 0; i < points.size(); i++) {
1673
4/8
✓ Branch 3 taken 4 times.
✗ Branch 4 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 ComparePoints(points[i], ppc_C0(time_points[i]), errMsg, error);
1674 }
1675
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 point3_t pos_between_po_and_p1((p1[0] + p0[0]) / 2.0, (p1[1] + p0[1]) / 2.0,
1676
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 (p1[2] + p0[2]) / 2.0);
1677 1 double time_between_po_and_p1 = (t0 + t1) / 2.0;
1678
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 ComparePoints(pos_between_po_and_p1, ppc_C0(time_between_po_and_p1), errMsg,
1679 error);
1680
1681 // Piecewise polynomial curve C1
1682 piecewise_t ppc_C1 =
1683 piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
1684
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 points, points_derivative, time_points);
1685
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (!ppc_C1.is_continuous(1)) {
1686 std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise "
1687 ✗ "curve is not C1"
1688 ✗ << std::endl;
1689 ✗ error = true;
1690 }
1691
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
 5 for (std::size_t i = 0; i < points.size(); i++) {
1692
4/8
✓ Branch 3 taken 4 times.
✗ Branch 4 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 ComparePoints(points[i], ppc_C1(time_points[i]), errMsg, error);
1693
4/8
✓ Branch 3 taken 4 times.
✗ Branch 4 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 ComparePoints(points_derivative[i], ppc_C1.derivate(time_points[i], 1),
1694 errMsg, error);
1695 }
1696
1697 // Piecewise polynomial curve C2
1698 piecewise_t ppc_C2 =
1699 piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
1700
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 points, points_derivative, points_second_derivative, time_points);
1701
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (!ppc_C2.is_continuous(2)) {
1702 std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise "
1703 ✗ "curve is not C1"
1704 ✗ << std::endl;
1705 ✗ error = true;
1706 }
1707
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
 5 for (std::size_t i = 0; i < points.size(); i++) {
1708
4/8
✓ Branch 3 taken 4 times.
✗ Branch 4 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 ComparePoints(points[i], ppc_C2(time_points[i]), errMsg, error);
1709
4/8
✓ Branch 3 taken 4 times.
✗ Branch 4 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 ComparePoints(points_derivative[i], ppc_C2.derivate(time_points[i], 1),
1710 errMsg, error);
1711
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 8 ComparePoints(points_second_derivative[i],
1712
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 ppc_C2.derivate(time_points[i], 2), errMsg, error);
1713 }
1714 1 }
1715
1716 1 void PiecewisePolynomialCurveFromFile(bool& error) {
1717
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string filename_pos(TEST_DATA_PATH "discrete_points_pos.txt");
1718
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string filename_vel(TEST_DATA_PATH "discrete_points_vel.txt");
1719
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string filename_acc(TEST_DATA_PATH "discrete_points_acc.txt");
1720
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string filename_error(TEST_DATA_PATH "discrete_points_error.txt");
1721
1722 piecewise_t c_pos = piecewise_t::load_piecewise_from_text_file<polynomial_t>(
1723
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 filename_pos, 0.01, 3);
1724
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (c_pos.min() != 0.) {
1725 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0"
1726 ✗ << std::endl;
1727 ✗ error = true;
1728 }
1729
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (c_pos.max() != 0.03) {
1730 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 0.03"
1731 ✗ << std::endl;
1732 ✗ error = true;
1733 }
1734
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p0(3), p2(3);
1735
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 p0 << -0.003860389372941039, 0.0012353625242474164, 0.009005041639999767;
1736
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 p2 << -0.0028803627898293283, 0.0011918668401150736, 0.009005041639999767;
1737
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_pos(0.).isApprox(p0)) {
1738 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match"
1739 ✗ << std::endl;
1740 ✗ error = true;
1741 }
1742
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_pos(0.02).isApprox(p2)) {
1743 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match"
1744 ✗ << std::endl;
1745 ✗ error = true;
1746 }
1747
1748 piecewise_t c_vel = piecewise_t::load_piecewise_from_text_file<polynomial_t>(
1749
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 filename_vel, 0.05, 3);
1750
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (c_pos.min() != 0.) {
1751 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0"
1752 ✗ << std::endl;
1753 ✗ error = true;
1754 }
1755
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(c_vel.max(), 0.15)) {
1756 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 0.15"
1757 ✗ << std::endl;
1758 ✗ error = true;
1759 }
1760
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pointX_t p3(3);
1761
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 p3 << 0.2968141884672718, 0.0012916907964522569, 0.00951023474821927;
1762
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_vel(0.).isApprox(p0)) {
1763 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match"
1764 ✗ << std::endl;
1765 ✗ error = true;
1766 }
1767
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_vel(0.15).isApprox(p3)) {
1768 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match"
1769 ✗ << std::endl;
1770 ✗ error = true;
1771 }
1772
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_vel.derivate(0., 1).isZero()) {
1773 std::cout << "PiecewisePolynomialCurveFromFile, Error, c_vel derivative at "
1774 ✗ "0. should be null"
1775 ✗ << std::endl;
1776 ✗ error = true;
1777 }
1778
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_vel.derivate(0.1, 1).isZero()) {
1779 std::cout << "PiecewisePolynomialCurveFromFile, Error, c_vel derivative at "
1780 ✗ "0.1 should be null"
1781 ✗ << std::endl;
1782 ✗ error = true;
1783 }
1784
1785 piecewise_t c_acc = piecewise_t::load_piecewise_from_text_file<polynomial_t>(
1786
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 filename_acc, 0.001, 3);
1787
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (c_acc.min() != 0.) {
1788 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, t_min should be 0"
1789 ✗ << std::endl;
1790 ✗ error = true;
1791 }
1792
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 if (!QuasiEqual(c_acc.max(), 7.85)) {
1793 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, t_max should be 7.85"
1794 ✗ << std::endl;
1795 ✗ error = true;
1796 }
1797
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_acc(0.).isApprox(p0)) {
1798 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match"
1799 ✗ << std::endl;
1800 ✗ error = true;
1801 }
1802
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pointX_t p5200(3);
1803
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 p5200 << 0.30273356072723845, -0.07619420199174821, 0.010015348526727433;
1804
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_acc(5.2).isApprox(p5200)) {
1805 ✗ std::cout << "PiecewisePolynomialCurveFromFile, Error, points do not match"
1806 ✗ << std::endl;
1807 ✗ error = true;
1808 }
1809
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_acc.derivate(0., 1).isZero()) {
1810 std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc derivative at "
1811 ✗ "0 should be null"
1812 ✗ << std::endl;
1813 ✗ error = true;
1814 }
1815
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_acc.derivate(0.5, 1).isZero()) {
1816 std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc derivative "
1817 ✗ "should at 0.5 be null"
1818 ✗ << std::endl;
1819 ✗ error = true;
1820 }
1821
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_acc.derivate(0., 2).isZero()) {
1822 std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc second "
1823 ✗ "derivative at 0 should be null"
1824 ✗ << std::endl;
1825 ✗ error = true;
1826 }
1827
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
 1 if (!c_acc.derivate(5., 2).isZero()) {
1828 std::cout << "PiecewisePolynomialCurveFromFile, Error, c_acc second "
1829 ✗ "derivative at 5 should be null"
1830 ✗ << std::endl;
1831 ✗ error = true;
1832 }
1833
1834 try {
1835 piecewise_t c_error =
1836 piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_acc,
1837
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 0.01, 4);
1838 std::cout << "PiecewisePolynomialCurveFromFile, Error, dimension do not "
1839 ✗ "match, an error should be raised"
1840 ✗ << std::endl;
1841 ✗ error = true;
1842
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
1843 1 }
1844 try {
1845 piecewise_t c_error =
1846 piecewise_t::load_piecewise_from_text_file<polynomial_t>(filename_error,
1847
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 0.01, 3);
1848 std::cout << "PiecewisePolynomialCurveFromFile, Error, "
1849 ✗ "discrete_points_error should not be parsed correctly"
1850 ✗ << std::endl;
1851 ✗ error = true;
1852
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
1853 1 }
1854 1 }
1855
1856 1 void serializationCurvesTest(bool& error) {
1857 try {
1858 std::string errMsg1(
1859
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "in serializationCurveTest, Error While serializing Polynomial : ");
1860 std::string errMsg2(
1861
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "in serializationCurveTest, Error While serializing Bezier : ");
1862 std::string errMsg3(
1863
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "in serializationCurveTest, Error While serializing Cubic Hermite : ");
1864 std::string errMsg4(
1865 "in serializationCurveTest, Error While serializing Piecewise curves "
1866
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 ": ");
1867 std::string errMsg5(
1868
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "in serializationCurveTest, Error While serializing Exact cubic : ");
1869 std::string errMsg6(
1870 "in serializationCurveTest, Error While serializing using abstract "
1871
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "pointers : ");
1872
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 1, 1); // in [0,1[
1873
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 1, 1); // in [1,2[
1874
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 1, 1); // in [2,3]
1875
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t res;
1876
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 t_pointX_t vec1, vec2, vec3;
1877
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec1.push_back(a); // x=1, y=1, z=1
1878
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec2.push_back(b); // x=2, y=1, z=1
1879
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 vec3.push_back(c); // x=3, y=1, z=1
1880
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
1881
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
1882
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
1883
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewise_t ppc;
1884
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ppc.add_curve<polynomial_t>(pol1);
1885
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ppc.add_curve<polynomial_t>(pol2);
1886
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ppc.add_curve<polynomial_t>(pol3);
1887
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string fileName("fileTest.test");
1888
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string fileName1("fileTest1.test");
1889 // Simple curves
1890 // Test serialization on Polynomial
1891
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pol1.saveAsText<polynomial_t>(fileName1);
1892
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t pol_test;
1893
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pol_test.loadFromText<polynomial_t>(fileName1);
1894
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<polynomial_t, polynomial_t>(pol1, pol_test, errMsg1, error);
1895 // Test serialization on Bezier
1896
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t bc = bezier_from_curve<bezier_t>(pol1);
1897
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bc.saveAsText<bezier_t>(fileName);
1898
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t bc_test;
1899
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bc_test.loadFromText<bezier_t>(fileName);
1900
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<polynomial_t, bezier_t>(pol1, bc_test, errMsg2, error);
1901 // Test serialization on Cubic Hermite
1902 cubic_hermite_spline_t chs =
1903
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 hermite_from_curve<cubic_hermite_spline_t>(pol1);
1904
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 chs.saveAsText<cubic_hermite_spline_t>(fileName);
1905
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cubic_hermite_spline_t chs_test;
1906
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 chs_test.loadFromText<cubic_hermite_spline_t>(fileName);
1907
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol1, chs_test, errMsg3,
1908 error);
1909 // Piecewise curves
1910 // Test serialization on Piecewise Polynomial curve
1911
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ppc.saveAsText<piecewise_t>(fileName);
1912
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 piecewise_t ppc_test, ppc_test_binary;
1913
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ppc_test.loadFromText<piecewise_t>(fileName);
1914
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(ppc, ppc_test, errMsg4, error);
1915
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ppc.saveAsBinary<piecewise_t>(fileName);
1916
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ppc_test_binary.loadFromBinary<piecewise_t>(fileName);
1917
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(ppc, ppc_test_binary, errMsg4,
1918 error);
1919
1920 // Test serialization on Piecewise Bezier curve
1921
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewise_t pbc = ppc.convert_piecewise_curve_to_bezier<bezier_t>();
1922
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pbc.saveAsText<piecewise_t>(fileName);
1923
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewise_t pbc_test;
1924
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pbc_test.loadFromText<piecewise_t>(fileName);
1925
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(ppc, pbc_test, errMsg4, error);
1926 // Test serialization on Piecewise Cubic Hermite curve
1927 piecewise_t pchc =
1928
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ppc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
1929
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pchc.saveAsText<piecewise_t>(fileName);
1930
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewise_t pchc_test;
1931
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pchc_test.loadFromText<piecewise_t>(fileName);
1932
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<piecewise_t, piecewise_t>(ppc, pchc_test, errMsg4, error);
1933 // Test serialization on exact cubic
1934 1 ndcurves::T_Waypoint waypoints;
1935
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 1; i = i + 0.2) {
1936
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 6 waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
1937 }
1938
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 spline_constraints_t constraints(3);
1939
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_vel = point3_t(0.1, 0, 0);
1940
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_vel = point3_t(0.2, 0, 0);
1941
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.end_acc = point3_t(0.01, 0, 0);
1942
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 constraints.init_acc = point3_t(0.01, 0, 0);
1943
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exact_cubic_t ec(waypoints.begin(), waypoints.end(), constraints);
1944
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ec.saveAsText<exact_cubic_t>(fileName);
1945
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 exact_cubic_t ec_test;
1946
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ec_test.loadFromText<exact_cubic_t>(fileName);
1947
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 CompareCurves<exact_cubic_t, exact_cubic_t>(ec, ec_test, errMsg5, error);
1948 // Test with pointer on abstract struct curve_abc
1949 // Polynomial
1950 curve_abc_t* pt_0;
1951 curve_abc_t* pt_1;
1952
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pol_test = polynomial_t();
1953 1 pt_0 = &pol1;
1954 1 pt_1 = &pol_test;
1955
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_0).saveAsText<polynomial_t>(fileName);
1956
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_1).loadFromText<polynomial_t>(fileName);
1957
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<polynomial_t, polynomial_t>(
1958
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 pol1, (*dynamic_cast<polynomial_t*>(pt_1)), errMsg6, error);
1959 // Piecewise Polynomial
1960 1 pt_0 = NULL;
1961 1 pt_1 = NULL;
1962
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 ppc_test = piecewise_t();
1963 1 pt_0 = &ppc;
1964 1 pt_1 = &ppc_test;
1965
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_0).saveAsText<piecewise_t>(fileName);
1966
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 (*pt_1).loadFromText<piecewise_t>(fileName);
1967
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<piecewise_t, piecewise_t>(
1968
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 ppc, (*dynamic_cast<piecewise_t*>(pt_1)), errMsg6, error);
1969 1 } catch (...) {
1970 ✗ error = true;
1971 ✗ std::cout << "Error in serializationCurvesTest" << std::endl;
1972 }
1973 1 }
1974
1975 1 void polynomialFromBoundaryConditions(bool& error) {
1976
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t zeros = point3_t(0., 0., 0.);
1977
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p0 = point3_t(0., 1., 0.);
1978
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p1 = point3_t(1., 2., -3.);
1979
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t dp0 = point3_t(-8., 4., 6.);
1980
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t dp1 = point3_t(10., -10., 10.);
1981
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t ddp0 = point3_t(-1., 7., 4.);
1982
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t ddp1 = point3_t(12., -8., 2.5);
1983 1 double min = 0.5;
1984 1 double max = 2.;
1985 // C0 : order 1
1986
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t polC0 = polynomial_t(p0, p1, min, max);
1987
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (polC0.min() != min) {
1988 ✗ error = true;
1989 std::cout
1990 ✗ << "polynomialFromBoundaryConditions C0: min interval not respected."
1991 ✗ << std::endl;
1992 }
1993
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (polC0.max() != max) {
1994 ✗ error = true;
1995 std::cout
1996 ✗ << "polynomialFromBoundaryConditions C0: max interval not respected."
1997 ✗ << std::endl;
1998 }
1999
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (polC0(min) != p0) {
2000 ✗ error = true;
2001 std::cout
2002 ✗ << "polynomialFromBoundaryConditions C0: initial value not respected"
2003 ✗ << std::endl;
2004 }
2005
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (polC0(max) != p1) {
2006 ✗ error = true;
2007 std::cout
2008 ✗ << "polynomialFromBoundaryConditions C0: final value not respected"
2009 ✗ << std::endl;
2010 }
2011
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (polC0.degree_ != 1) {
2012 ✗ error = true;
2013 ✗ std::cout << "polynomialFromBoundaryConditions C0: curve is not degree 1 "
2014 ✗ << std::endl;
2015 }
2016
6/12
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 1 times.
 1 if (polC0((max + min) / 2.) != (p0 * 0.5 + p1 * 0.5)) {
2017 ✗ error = true;
2018 std::cout << "polynomialFromBoundaryConditions C0: middle point doesn't "
2019 ✗ "have the right value' "
2020 ✗ << std::endl;
2021 }
2022 // C1 : order 3
2023
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t polC1 = polynomial_t(p0, dp0, p1, dp1, min, max);
2024
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (polC1.min() != min) {
2025 ✗ error = true;
2026 std::cout
2027 ✗ << "polynomialFromBoundaryConditions C1: min interval not respected."
2028 ✗ << std::endl;
2029 }
2030
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (polC1.max() != max) {
2031 ✗ error = true;
2032 std::cout
2033 ✗ << "polynomialFromBoundaryConditions C1: max interval not respected."
2034 ✗ << std::endl;
2035 }
2036
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (polC1(min) != p0) {
2037 ✗ error = true;
2038 std::cout
2039 ✗ << "polynomialFromBoundaryConditions C1: initial value not respected"
2040 ✗ << std::endl;
2041 }
2042
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (!QuasiEqual(polC1(max), p1)) {
2043 ✗ error = true;
2044 std::cout
2045 ✗ << "polynomialFromBoundaryConditions C1: final value not respected"
2046 ✗ << std::endl;
2047 ✗ std::cout << "p1 = " << p1.transpose()
2048 ✗ << " curve end = " << polC1(max).transpose() << std::endl;
2049 }
2050
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (polC1.derivate(min, 1) != dp0) {
2051 ✗ error = true;
2052 std::cout << "polynomialFromBoundaryConditions C1: initial derivative "
2053 ✗ "value not respected"
2054 ✗ << std::endl;
2055 }
2056
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (!QuasiEqual(polC1.derivate(max, 1), dp1)) {
2057 ✗ error = true;
2058 std::cout << "polynomialFromBoundaryConditions C1: final derivative value "
2059 ✗ "not respected"
2060 ✗ << std::endl;
2061 ✗ std::cout << "dp1 = " << dp1.transpose() << " curve end derivative = "
2062 ✗ << polC1.derivate(max, 1).transpose() << std::endl;
2063 }
2064
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (polC1.degree_ != 3) {
2065 ✗ error = true;
2066 ✗ std::cout << "polynomialFromBoundaryConditions C1: curve is not degree 3 "
2067 ✗ << std::endl;
2068 }
2069 // C2 : order 5
2070
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t polC2 = polynomial_t(p0, dp0, ddp0, p1, dp1, ddp1, min, max);
2071
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (polC2.min() != min) {
2072 ✗ error = true;
2073 std::cout
2074 ✗ << "polynomialFromBoundaryConditions C2: min interval not respected."
2075 ✗ << std::endl;
2076 }
2077
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (polC2.max() != max) {
2078 ✗ error = true;
2079 std::cout
2080 ✗ << "polynomialFromBoundaryConditions C2: max interval not respected."
2081 ✗ << std::endl;
2082 }
2083
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (polC2(min) != p0) {
2084 ✗ error = true;
2085 std::cout
2086 ✗ << "polynomialFromBoundaryConditions C2: initial value not respected"
2087 ✗ << std::endl;
2088 }
2089
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (!QuasiEqual(polC2(max), p1)) {
2090 ✗ error = true;
2091 std::cout
2092 ✗ << "polynomialFromBoundaryConditions C2: final value not respected"
2093 ✗ << std::endl;
2094 }
2095
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (polC2.derivate(min, 1) != dp0) {
2096 ✗ error = true;
2097 std::cout << "polynomialFromBoundaryConditions C2: initial derivative "
2098 ✗ "value not respected"
2099 ✗ << std::endl;
2100 }
2101
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (!QuasiEqual(polC2.derivate(max, 1), dp1)) {
2102 ✗ error = true;
2103 std::cout << "polynomialFromBoundaryConditions C2: final derivative value "
2104 ✗ "not respected"
2105 ✗ << std::endl;
2106 }
2107
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (polC2.derivate(min, 2) != ddp0) {
2108 ✗ error = true;
2109 std::cout << "polynomialFromBoundaryConditions C2: initial second "
2110 ✗ "derivative value not respected"
2111 ✗ << std::endl;
2112 }
2113
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (!QuasiEqual(polC2.derivate(max, 2), ddp1)) {
2114 ✗ error = true;
2115 std::cout << "polynomialFromBoundaryConditions C2: final second derivative "
2116 ✗ "value not respected"
2117 ✗ << std::endl;
2118 }
2119
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (polC2.degree_ != 5) {
2120 ✗ error = true;
2121 ✗ std::cout << "polynomialFromBoundaryConditions C2: curve is not degree 5 "
2122 ✗ << std::endl;
2123 }
2124 // check if the exeptions are correctly raised :
2125 try {
2126
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 polynomial_t polC0Err = polynomial_t(p0, p1, max, min);
2127 ✗ error = true;
2128 ✗ std::cout << "Created a polynomial with tMin > tMax without error. "
2129 ✗ << std::endl;
2130
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 } catch (const invalid_argument& /*e*/) {
2131 1 }
2132 try {
2133
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 polynomial_t polC1Err = polynomial_t(p0, dp0, p1, dp1, max, min);
2134 ✗ error = true;
2135 ✗ std::cout << "Created a polynomial with tMin > tMax without error. "
2136 ✗ << std::endl;
2137
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 } catch (const invalid_argument& /*e*/) {
2138 1 }
2139 try {
2140 polynomial_t polC2Err =
2141
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 polynomial_t(p0, dp0, ddp0, p1, dp1, ddp1, max, min);
2142 ✗ error = true;
2143 ✗ std::cout << "Created a polynomial with tMin > tMax without error. "
2144 ✗ << std::endl;
2145
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 } catch (const invalid_argument& /*e*/) {
2146 1 }
2147 1 }
2148
2149 1 void so3LinearTest(bool& error) {
2150
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q0(1, 0, 0, 0);
2151
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q1(0.7071, 0.7071, 0, 0);
2152 1 const double tMin = 0.;
2153 1 const double tMax = 1.5;
2154
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3Traj(q0, q1, tMin, tMax);
2155
2156
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (so3Traj.min() != tMin) {
2157 ✗ error = true;
2158 ✗ std::cout << "Min bound not respected" << std::endl;
2159 }
2160
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (so3Traj.max() != tMax) {
2161 ✗ error = true;
2162 ✗ std::cout << "Max bound not respected" << std::endl;
2163 }
2164
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!so3Traj.computeAsQuaternion(tMin).isApprox(q0)) {
2165 ✗ error = true;
2166 ✗ std::cout << "evaluate at t=0 is not the init quaternion" << std::endl;
2167 }
2168
4/8
✓ 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.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
 1 if (so3Traj(tMin) != q0.toRotationMatrix()) {
2169 ✗ error = true;
2170 ✗ std::cout << "evaluate at t=0 is not the init rotation" << std::endl;
2171 }
2172
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!so3Traj.computeAsQuaternion(tMax).isApprox(q1)) {
2173 ✗ error = true;
2174 ✗ std::cout << "evaluate at t=max is not the final quaternion" << std::endl;
2175 }
2176
4/8
✓ 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.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
 1 if (so3Traj(tMax) != q1.toRotationMatrix()) {
2177 ✗ error = true;
2178 ✗ std::cout << "evaluate at t=max is not the final rotation" << std::endl;
2179 }
2180 // check derivatives :
2181
4/8
✓ 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.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
 1 if (so3Traj.derivate(tMin, 1) != so3Traj.derivate(1., 1)) {
2182 ✗ error = true;
2183 ✗ std::cout << "first order derivative should be constant." << std::endl;
2184 }
2185
4/8
✓ 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.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
 1 if (so3Traj.derivate(tMin, 2) != point3_t::Zero(3)) {
2186 ✗ error = true;
2187 ✗ std::cout << "second order derivative should be null" << std::endl;
2188 }
2189
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t angular_vel = so3Traj.derivate(tMin, 1);
2190
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (angular_vel[1] != 0. || angular_vel[2] != 0) {
2191 ✗ error = true;
2192 ✗ std::cout << "Angular velocity around y and z axis should be null"
2193 ✗ << std::endl;
2194 }
2195
2196
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constant3_t so3Derivate1 = so3Traj.compute_derivate(1);
2197
4/8
✓ 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.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
 1 if (so3Derivate1(1.) != so3Traj.derivate(1., 1)) {
2198 ✗ error = true;
2199 ✗ std::cout << "compute_derivate curve do not equal derivate call"
2200 ✗ << std::endl;
2201 }
2202
2203
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constant3_t so3Derivate2 = so3Traj.compute_derivate(2);
2204
4/8
✓ 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.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
 1 if (so3Derivate2(1.) != point3_t::Zero(3)) {
2205 ✗ error = true;
2206 ✗ std::cout << "compute_derivate curve do not equal derivate call"
2207 ✗ << std::endl;
2208 }
2209
2210 // check if errors are correctly raised :
2211 try {
2212
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 so3Traj(-0.1);
2213 ✗ error = true;
2214 std::cout << "SO3Linear: calling () with t < tmin should raise an "
2215 ✗ "invalid_argument error"
2216 ✗ << std::endl;
2217
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
2218 1 }
2219 try {
2220
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 so3Traj(1.7);
2221 ✗ error = true;
2222 std::cout << "SO3Linear: calling () with t > tmin should raise an "
2223 ✗ "invalid_argument error"
2224 ✗ << std::endl;
2225
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
2226 1 }
2227 try {
2228
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 so3Traj.derivate(0, 0);
2229 ✗ error = true;
2230 std::cout << "SO3Linear: calling derivate with order = 0 should raise an "
2231 ✗ "invalid_argument error"
2232 ✗ << std::endl;
2233
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
2234 1 }
2235
2236
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 SO3Linear_t so3TrajMatrix(q0.toRotationMatrix(), q1.toRotationMatrix(), tMin,
2237
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 tMax);
2238 std::string errmsg(
2239
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "SO3Linear built from quaternion or from matrix are not identical.");
2240
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CompareCurves(so3Traj, so3TrajMatrix, errmsg, error, 1e-3);
2241 1 }
2242
2243 1 void SO3serializationTest(bool& error) {
2244
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string fileName("fileTest");
2245 std::string errmsg(
2246 "SO3serializationTest : curve serialized is not equivalent to the "
2247
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "original curve.");
2248
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q0(0.544, -0.002, -0.796, 0.265);
2249
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q1(0.7071, 0.7071, 0, 0);
2250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q1.normalize();
2251
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q0.normalize();
2252
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3Traj(q0, q1, 0.5, 2.2);
2253
2254
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 so3Traj.saveAsText<SO3Linear_t>(fileName + ".txt");
2255
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3_from_txt;
2256
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 so3_from_txt.loadFromText<SO3Linear_t>(fileName + ".txt");
2257
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SO3Linear_t, SO3Linear_t>(
2258
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 so3Traj, so3_from_txt, errmsg + " For text serialization", error);
2259
2260
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 so3Traj.saveAsXML<SO3Linear_t>(fileName + ".xml", "so3Curve");
2261
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3_from_xml;
2262
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 so3_from_xml.loadFromXML<SO3Linear_t>(fileName + ".xml", "so3Curve");
2263
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SO3Linear_t, SO3Linear_t>(
2264
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 so3Traj, so3_from_xml, errmsg + " For XML serialization", error);
2265
2266
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 so3Traj.saveAsBinary<SO3Linear_t>(fileName);
2267
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3_from_binary;
2268
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 so3_from_binary.loadFromBinary<SO3Linear_t>(fileName);
2269
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SO3Linear_t, SO3Linear_t>(
2270
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 so3Traj, so3_from_binary, errmsg + " For binary serialization", error);
2271 1 }
2272
2273 1 void se3CurveTest(bool& error) {
2274
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q0(1, 0, 0, 0);
2275
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q1(0., 1., 0, 0);
2276
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p0 = point3_t(1., 1.5, -2.);
2277
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p1 = point3_t(3., 0, 1.);
2278
2279 1 double min = 0.5, max = 2.;
2280
2281 // constructor from init/end position/rotation : automatically create a linear
2282 // interpolation for position and slerp for rotation
2283
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 SE3Curve_t cLinear(p0, p1, q0, q1, min, max);
2284
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 transform_t transformInit = cLinear(min);
2285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 transform_t transformEnd = cLinear(max);
2286
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 transform_t transformMid = cLinear((max + min) / 2.);
2287
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!transformInit.translation().isApprox(p0)) {
2288 ✗ error = true;
2289 ✗ std::cout << "Init position of the curve is not correct." << std::endl;
2290 }
2291
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (!transformInit.rotation().isApprox(q0.toRotationMatrix())) {
2292 ✗ error = true;
2293 ✗ std::cout << "Init rotation of the curve is not correct." << std::endl;
2294 }
2295
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!transformEnd.translation().isApprox(p1)) {
2296 ✗ error = true;
2297 ✗ std::cout << "End position of the curve is not correct." << std::endl;
2298 }
2299
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (!transformEnd.rotation().isApprox(q1.toRotationMatrix())) {
2300 ✗ error = true;
2301 ✗ std::cout << "End rotation of the curve is not correct." << std::endl;
2302 }
2303
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t qMid(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0);
2304
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 point3_t pMid = (p0 + p1) / 2.;
2305
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!transformMid.translation().isApprox(pMid)) {
2306 ✗ error = true;
2307 ✗ std::cout << "Mid position of the curve is not correct." << std::endl;
2308 }
2309
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (!transformMid.rotation().isApprox(qMid.toRotationMatrix())) {
2310 ✗ error = true;
2311 ✗ std::cout << "Mid rotation of the curve is not correct." << std::endl;
2312 }
2313
2314 // constructor with specific translation curve
2315
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t cBezier;
2316 { // inner scope to check what happen when translation_bezier is out of scope
2317
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
2318
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
2319
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
2320
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
2321 1 std::vector<point3_t> params;
2322
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
2323
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
2324
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
2325
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(d);
2326 std::shared_ptr<bezier3_t> translation_bezier =
2327
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<bezier3_t>(params.begin(), params.end(), min, max);
2328
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 cBezier = SE3Curve_t(translation_bezier, q0.toRotationMatrix(),
2329
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 q1.toRotationMatrix());
2330
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 p0 = (*translation_bezier)(min);
2331
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 p1 = (*translation_bezier)(max);
2332
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pMid = (*translation_bezier)((max + min) / 2.);
2333
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (cBezier.min() != min) {
2334 ✗ error = true;
2335 std::cout << "SE3 constructor from translation bezier do not respect the "
2336 ✗ "min time interval"
2337 ✗ << std::endl;
2338 }
2339
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (cBezier.max() != max) {
2340 ✗ error = true;
2341 std::cout << "SE3 constructor from translation bezier do not respect the "
2342 ✗ "max time interval"
2343 ✗ << std::endl;
2344 }
2345 1 double t = min;
2346
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 1 times.
 16 while (t < max) {
2347
5/10
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 15 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 15 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 15 times.
 15 if (!cBezier(t).translation().isApprox((*translation_bezier)(t))) {
2348 ✗ error = true;
2349 ✗ std::cout << "SE3 translation is not equivalent to bezier for t = " << t
2350 ✗ << std::endl;
2351 }
2352 15 t += 0.1;
2353 }
2354 // check the derivatives for translation:
2355
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 for (size_t i = 1; i < 3; i++) {
2356 2 t = min;
2357
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 2 times.
 32 while (t < max) {
2358
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 30 times.
✗ Branch 9 not taken.
 60 if (!cBezier.derivate(t, i).head<3>().isApprox(
2359
2/4
✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 30 times.
 60 translation_bezier->derivate(t, i))) {
2360 ✗ error = true;
2361 std::cout
2362 ✗ << "SE3 curve derivative is not equivalent to bezier for t = "
2363 ✗ << t << " and order = " << i << std::endl;
2364 }
2365 30 t += 0.1;
2366 }
2367 }
2368 1 }
2369 // check the rotation
2370
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 transformInit = cBezier(min);
2371
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 transformEnd = cBezier(max);
2372
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 transformMid = cBezier((max + min) / 2.);
2373
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!transformInit.translation().isApprox(p0)) {
2374 ✗ error = true;
2375 ✗ std::cout << "Init position of the curve is not correct." << std::endl;
2376 }
2377
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (!transformInit.rotation().isApprox(q0.toRotationMatrix())) {
2378 ✗ error = true;
2379 ✗ std::cout << "Init rotation of the curve is not correct." << std::endl;
2380 }
2381
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!transformEnd.translation().isApprox(p1)) {
2382 ✗ error = true;
2383 ✗ std::cout << "End position of the curve is not correct." << std::endl;
2384 }
2385
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (!transformEnd.rotation().isApprox(q1.toRotationMatrix())) {
2386 ✗ error = true;
2387 ✗ std::cout << "End rotation of the curve is not correct." << std::endl;
2388 }
2389
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
 1 if (!transformMid.translation().isApprox(pMid)) {
2390 ✗ error = true;
2391 ✗ std::cout << "Mid position of the curve is not correct." << std::endl;
2392 }
2393
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 if (!transformMid.rotation().isApprox(qMid.toRotationMatrix())) {
2394 ✗ error = true;
2395 ✗ std::cout << "Mid rotation of the curve is not correct." << std::endl;
2396 }
2397
2398 // check derivatives for rotation:
2399
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 if (cBezier.derivate(min, 1).tail<3>() !=
2400
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 2 cBezier.derivate(max, 1).tail<3>()) {
2401 ✗ error = true;
2402 std::cout
2403 ✗ << "SE3 curve : first order derivative for rotation should be constant."
2404 ✗ << std::endl;
2405 }
2406
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
 1 if (cBezier.derivate(min, 2).tail<3>() != point3_t::Zero(3)) {
2407 ✗ error = true;
2408 std::cout
2409 ✗ << "SE3 curve : second order derivative for rotation should be null"
2410 ✗ << std::endl;
2411 }
2412
2413 // check accessor to translation curves :
2414 1 curve_translation_ptr_t translation = cBezier.translation_curve();
2415
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (translation->operator()(min) != cBezier(min).translation()) {
2416 ✗ error = true;
2417 ✗ std::cout << "SE3 curve : translation curve not equal to se3.translation"
2418 ✗ << std::endl;
2419 }
2420
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 1 times.
 1 if (translation->operator()(max) != cBezier(max).translation()) {
2421 ✗ error = true;
2422 ✗ std::cout << "SE3 curve : translation curve not equal to se3.translation"
2423 ✗ << std::endl;
2424 }
2425
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 if (translation->operator()((max + min) / 2.) !=
2426
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 2 cBezier((max + min) / 2.).translation()) {
2427 ✗ error = true;
2428 ✗ std::cout << "SE3 curve : translation curve not equal to se3.translation"
2429 ✗ << std::endl;
2430 }
2431 // check accessor to rotation curves :
2432 1 curve_rotation_ptr_t rotation = cBezier.rotation_curve();
2433
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
 1 if (!rotation->operator()(min).isApprox(cBezier(min).rotation())) {
2434 ✗ error = true;
2435 ✗ std::cout << "SE3 curve : rotation curve not equal to se3.rotation"
2436 ✗ << std::endl;
2437 }
2438
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
 1 if (!rotation->operator()(max).isApprox(cBezier(max).rotation())) {
2439 ✗ error = true;
2440 ✗ std::cout << "SE3 curve : rotation curve not equal to se3.rotation"
2441 ✗ << std::endl;
2442 }
2443
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 if (!rotation->operator()((max + min) / 2.)
2444
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 1 times.
 1 .isApprox(cBezier((max + min) / 2.).rotation())) {
2445 ✗ error = true;
2446 ✗ std::cout << "SE3 curve : rotation curve not equal to se3.rotation"
2447 ✗ << std::endl;
2448 }
2449
2450 // check if errors are correctly raised
2451 try {
2452
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cBezier(0.1);
2453 ✗ error = true;
2454 std::cout << "SE3 curve: calling () with t < tmin should raise an "
2455 ✗ "invalid_argument error"
2456 ✗ << std::endl;
2457
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
2458 1 }
2459 try {
2460
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cBezier(2.3);
2461 ✗ error = true;
2462 std::cout << "SE3 curve: calling () with t > tmin should raise an "
2463 ✗ "invalid_argument error"
2464 ✗ << std::endl;
2465
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
2466 1 }
2467 try {
2468
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cBezier.derivate(0.6, 0);
2469 ✗ error = true;
2470 std::cout << "SE3 curve: calling derivate with order = 0 should raise an "
2471 ✗ "invalid_argument error"
2472 ✗ << std::endl;
2473
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 } catch (std::invalid_argument& /*e*/) {
2474 1 }
2475 1 }
2476
2477 1 void Se3serializationTest(bool& error) {
2478
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string fileName("fileTest");
2479 std::string errmsg(
2480 "SE3serializationTest : curve serialized is not equivalent to the "
2481
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "original curve.");
2482
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q0(1, 0, 0, 0);
2483
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q1(0., 1., 0, 0);
2484
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p0 = point3_t(1., 1.5, -2.);
2485
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p1 = point3_t(3., 0, 1.);
2486 1 double min = 0.5, max = 2.;
2487 // constructor from init/end position/rotation : automatically create a linear
2488 // interpolation for position and slerp for rotation
2489
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 SE3Curve_t cLinear(p0, p1, q0, q1, min, max);
2490
2491
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 cLinear.saveAsText<SE3Curve_t>(fileName + ".txt");
2492
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t se3_from_txt;
2493
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 se3_from_txt.loadFromText<SE3Curve_t>(fileName + ".txt");
2494
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SE3Curve_t, SE3Curve_t>(
2495
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cLinear, se3_from_txt, errmsg + " For text serialization", error);
2496
2497
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 cLinear.saveAsXML<SE3Curve_t>(fileName + ".xml", "se3Curve");
2498
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t se3_from_xml;
2499
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 se3_from_xml.loadFromXML<SE3Curve_t>(fileName + ".xml", "se3Curve");
2500
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SE3Curve_t, SE3Curve_t>(
2501
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cLinear, se3_from_xml, errmsg + " For XML serialization", error);
2502
2503
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cLinear.saveAsBinary<SE3Curve_t>(fileName);
2504
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t se3_from_binary;
2505
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 se3_from_binary.loadFromBinary<SE3Curve_t>(fileName);
2506
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SE3Curve_t, SE3Curve_t>(
2507
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cLinear, se3_from_binary, errmsg + " For binary serialization", error);
2508
2509 // constructor with specific translation curve
2510
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t cBezier;
2511 { // inner scope to check what happen when translation_bezier is out of scope
2512
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q0(0.544, -0.002, -0.796, 0.265);
2513
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q1(0.7071, 0.7071, 0, 0);
2514
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q1.normalize();
2515
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q0.normalize();
2516
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
2517
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
2518
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
2519
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
2520 1 std::vector<point3_t> params;
2521
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(a);
2522
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(b);
2523
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(c);
2524
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params.push_back(d);
2525 std::shared_ptr<bezier3_t> translation_bezier =
2526
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<bezier3_t>(params.begin(), params.end(), min, max);
2527
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 cBezier = SE3Curve_t(translation_bezier, q0, q1);
2528 1 }
2529
2530
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 cBezier.saveAsText<SE3Curve_t>(fileName + ".txt");
2531
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t se3_from_txt_bezier;
2532
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 se3_from_txt_bezier.loadFromText<SE3Curve_t>(fileName + ".txt");
2533
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SE3Curve_t, SE3Curve_t>(
2534
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cBezier, se3_from_txt_bezier, errmsg + " For text serialization", error);
2535
2536
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 cBezier.saveAsXML<SE3Curve_t>(fileName + ".xml", "se3Curve");
2537
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t se3_from_xml_bezier;
2538
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 se3_from_xml_bezier.loadFromXML<SE3Curve_t>(fileName + ".xml", "se3Curve");
2539
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SE3Curve_t, SE3Curve_t>(
2540
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cBezier, se3_from_xml_bezier, errmsg + " For XML serialization", error);
2541
2542
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cBezier.saveAsBinary<SE3Curve_t>(fileName);
2543
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t se3_from_binary_bezier;
2544
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 se3_from_binary_bezier.loadFromBinary<SE3Curve_t>(fileName);
2545
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 CompareCurves<SE3Curve_t, SE3Curve_t>(cBezier, se3_from_binary_bezier,
2546
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 errmsg + " For binary serialization",
2547 error);
2548 1 }
2549
2550 /**
2551 * @brief BezierLinearProblemTests test the generation of linear / quadratic
2552 * problems with variable control points bezier curves
2553 * @param error
2554 */
2555
2556 using namespace ndcurves::optimization;
2557
2558 22 var_pair_t setup_control_points(
2559 const std::size_t degree, const constraint_flag flag,
2560 const point3_t& initPos = point3_t(), const point3_t& endPos = point3_t(),
2561 const constraint_linear& constraints = constraint_linear(3),
2562 const double totalTime = 1.) {
2563
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 problem_definition_t pDef(constraints);
2564
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 pDef.init_pos = initPos;
2565
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 pDef.end_pos = endPos;
2566 22 pDef.flag = flag;
2567 22 pDef.totalTime = totalTime;
2568 22 pDef.degree = degree;
2569
2/2
✓ Branch 1 taken 19 times.
✓ Branch 2 taken 3 times.
 22 problem_data_t pData = setup_control_points<point3_t, double, true>(pDef);
2570 return std::make_pair(
2571 pData.variables_,
2572
2/4
✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19 times.
✗ Branch 5 not taken.
 38 std::make_pair(pData.startVariableIndex, pData.numVariables));
2573 22 }
2574
2575 enum vartype { variable, constant };
2576
2577 86 bool isVar(const linear_variable_t& var) {
2578 86 return !var.isZero() &&
2579
4/8
✓ Branch 0 taken 86 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 86 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 86 times.
✗ Branch 10 not taken.
 172 var.B() == linear_variable_t::matrix_x_t::Identity(3, 3) &&
2580
3/6
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 86 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 86 times.
✗ Branch 8 not taken.
 172 var.c() == linear_variable_t::vector_x_t::Zero(3);
2581 }
2582
2583 31 bool isConstant(const linear_variable_t& var) {
2584
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 62 return var.isZero() ||
2585
3/6
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 31 times.
✗ Branch 8 not taken.
 31 (var.B() == linear_variable_t::matrix_x_t::Zero(3, 3) &&
2586
3/6
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 31 times.
✗ Branch 8 not taken.
 62 var.c() != linear_variable_t::vector_x_t::Zero(3));
2587 }
2588
2589 /*bool isMixed(const linear_variable_t& var)
2590 {
2591 return var.A_ != linear_variable_t::matrix_t::Zero() &&
2592 var.b_ != linear_variable_t::point_t::Zero();
2593 }*/
2594
2595 117 bool checkValue(const linear_variable_t& var, const vartype vart) {
2596
2/2
✓ Branch 0 taken 31 times.
✓ Branch 1 taken 86 times.
 117 if (vart == constant)
2597 31 return isConstant(var);
2598 else
2599 86 return isVar(var);
2600 }
2601
2602 19 void checksequence(const T_linear_variable_t& vars, vartype* expected,
2603 const std::string testname, bool& error) {
2604 19 int i = 0;
2605
2/2
✓ Branch 3 taken 117 times.
✓ Branch 4 taken 19 times.
 136 for (CIT_linear_variable_t cit = vars.begin(); cit != vars.end();
2606 117 ++cit, ++i) {
2607
2/4
✓ Branch 2 taken 117 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 117 times.
 117 if (!checkValue(*cit, expected[i])) {
2608 std::cout << "in test: " << testname
2609 ✗ << ": wrong type for variable at position " << i << std::endl;
2610 ✗ error = true;
2611 }
2612 }
2613 19 }
2614
2615 19 void checkNumVar(const T_linear_variable_t& vars, const std::size_t expected,
2616 const std::string testname, bool& error) {
2617
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 19 times.
 19 if (vars.size() != expected) {
2618 ✗ error = true;
2619 ✗ std::cout << "incorrect number of variables in " << testname << "("
2620 ✗ << expected << "," << vars.size() << ")" << std::endl;
2621 }
2622 19 }
2623
2624 18 void checkPair(const pair_size_t pair, const std::size_t start_index,
2625 const std::size_t num_vars, const std::string testname,
2626 bool& error) {
2627
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
 18 if (pair.first != start_index) {
2628 ✗ error = true;
2629 std::cout << "incorrect starting index for variablesin "
2630
2631 ✗ << testname << "(" << start_index << "," << pair.first << ")"
2632 ✗ << std::endl;
2633 }
2634
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 18 times.
 18 if (pair.second != num_vars) {
2635 ✗ error = true;
2636 std::cout << "incorrect number of identified variablesin " << testname
2637 ✗ << "(" << num_vars << "," << pair.second << ")" << std::endl;
2638 }
2639 18 }
2640
2641 1 void BezierLinearProblemsetup_control_pointsNoConstraint(bool& error) {
2642 1 constraint_flag flag = optimization::NONE;
2643
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 var_pair_t res_no_constraints = setup_control_points(5, flag);
2644 1 T_linear_variable_t& vars = res_no_constraints.first;
2645 1 vartype exptecdvars[] = {variable, variable, variable,
2646 variable, variable, variable};
2647
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "setup_control_pointsNoConstraint", error);
2648
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvars, "setup_control_pointsNoConstraint", error);
2649 1 }
2650
2651 3 constraint_linear makeConstraint() {
2652
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 point3_t init_pos = point3_t(1., 1., 1.);
2653
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 constraint_linear cl(3);
2654
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 init_pos *= 2;
2655
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 cl.init_vel = init_pos;
2656
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 init_pos *= 2;
2657
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 cl.init_acc = init_pos;
2658
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 init_pos *= 2;
2659
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 cl.end_acc = init_pos;
2660
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 init_pos *= 2;
2661
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 cl.end_vel = init_pos;
2662 6 return cl;
2663 }
2664
2665 1 void BezierLinearProblemsetup_control_pointsVarCombinatorialInit(bool& error) {
2666 1 constraint_flag flag = optimization::INIT_POS;
2667
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t init_pos = point3_t(1., 1., 1.);
2668
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 var_pair_t res = setup_control_points(5, flag, init_pos);
2669 1 T_linear_variable_t& vars = res.first;
2670 1 vartype exptecdvars[] = {constant, variable, variable,
2671 variable, variable, variable};
2672
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialInit", error);
2673
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvars, "VarCombinatorialInit", error);
2674
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 1, 5, "VarCombinatorialInit", error);
2675
2676
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraint_linear constraints = makeConstraint();
2677 1 flag = INIT_POS | INIT_VEL;
2678
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = setup_control_points(5, flag, init_pos, point3_t(), constraints);
2679
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2680 1 vartype exptecdvar1[] = {constant, constant, variable,
2681 variable, variable, variable};
2682
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialInit", error);
2683
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar1, "VarCombinatorialInit", error);
2684
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 2, 4, "VarCombinatorialInit", error);
2685
2686 1 flag = INIT_POS | INIT_VEL | INIT_ACC;
2687
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = setup_control_points(5, flag, init_pos, point3_t(), constraints);
2688
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2689 1 vartype exptecdvar2[] = {constant, constant, constant,
2690 variable, variable, variable};
2691
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialInit", error);
2692
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar2, "VarCombinatorialInit", error);
2693
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 3, 3, "VarCombinatorialInit", error);
2694
2695 1 flag = INIT_VEL;
2696
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = setup_control_points(5, flag, init_pos, point3_t(), constraints);
2697
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2698 1 vartype exptecdvar3[] = {variable, variable, variable,
2699 variable, variable, variable};
2700
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialInit", error);
2701
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar3, "VarCombinatorialInit", error);
2702
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
2703
2704 1 flag = INIT_ACC;
2705
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = setup_control_points(5, flag, init_pos, point3_t(), constraints);
2706
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2707 1 vartype exptecdvar4[] = {variable, variable, variable,
2708 variable, variable, variable};
2709
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialInit", error);
2710
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar4, "VarCombinatorialInit", error);
2711
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
2712
2713 1 flag = INIT_ACC | INIT_VEL;
2714
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res = setup_control_points(5, flag, init_pos, point3_t(), constraints);
2715
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2716 1 vartype exptecdvar5[] = {variable, variable, variable,
2717 variable, variable, variable};
2718
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialInit", error);
2719
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar5, "VarCombinatorialInit", error);
2720
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
2721
2722 1 bool err = true;
2723 try {
2724 1 flag = INIT_POS | INIT_VEL;
2725
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 res = setup_control_points(1, flag, init_pos, point3_t(), constraints);
2726 1 } catch (...) {
2727 1 err = false;
2728
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
2729
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (err) {
2730 ✗ error = true;
2731 std::cout << "exception should be raised when degree of bezier curve is "
2732 ✗ "not high enough to handle constraints "
2733 ✗ << std::endl;
2734 }
2735 1 }
2736
2737 1 void BezierLinearProblemsetup_control_pointsVarCombinatorialEnd(bool& error) {
2738 1 constraint_flag flag = optimization::END_POS;
2739
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t init_pos = point3_t(1., 1., 1.);
2740
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 var_pair_t res = setup_control_points(5, flag, init_pos);
2741 1 T_linear_variable_t& vars = res.first;
2742 1 vartype exptecdvars[] = {variable, variable, variable,
2743 variable, variable, constant};
2744
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialEnd", error);
2745
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvars, "VarCombinatorialEnd", error);
2746
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 5, "VarCombinatorialEnd", error);
2747
2748
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraint_linear constraints = makeConstraint();
2749 1 flag = END_POS | END_VEL;
2750
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2751
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2752 1 vartype exptecdvar1[] = {variable, variable, variable,
2753 variable, constant, constant};
2754
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialEnd", error);
2755
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar1, "VarCombinatorialEnd", error);
2756
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 4, "VarCombinatorialEnd", error);
2757
2758 1 flag = END_POS | END_VEL | END_ACC;
2759
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2760
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2761 1 vartype exptecdvar2[] = {variable, variable, variable,
2762 constant, constant, constant};
2763
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialEnd", error);
2764
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar2, "VarCombinatorialEnd", error);
2765
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 3, "VarCombinatorialEnd", error);
2766
2767 1 flag = END_VEL;
2768
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2769
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2770 1 vartype exptecdvar3[] = {variable, variable, variable,
2771 variable, variable, variable};
2772
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialEnd", error);
2773
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar3, "VarCombinatorialEnd", error);
2774
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
2775
2776 1 flag = END_ACC;
2777
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2778
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2779 1 vartype exptecdvar4[] = {variable, variable, variable,
2780 variable, variable, variable};
2781
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialEnd", error);
2782
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar4, "VarCombinatorialEnd", error);
2783
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
2784
2785 1 flag = END_ACC | END_VEL;
2786
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2787
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2788 1 vartype exptecdvar5[] = {variable, variable, variable,
2789 variable, variable, variable};
2790
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialEnd", error);
2791
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar5, "VarCombinatorialEnd", error);
2792
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
2793
2794 1 bool err = true;
2795 try {
2796 1 flag = END_ACC | END_VEL;
2797
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 res = setup_control_points(1, flag, init_pos, point3_t(), constraints);
2798 1 } catch (...) {
2799 1 err = false;
2800
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
2801
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (err) {
2802 ✗ error = true;
2803 std::cout << "exception should be raised when degree of bezier curve is "
2804 ✗ "not high enough to handle constraints "
2805 ✗ << std::endl;
2806 }
2807 1 }
2808
2809 1 void BezierLinearProblemsetup_control_pointsVarCombinatorialMix(bool& error) {
2810 1 constraint_flag flag = END_POS | INIT_POS;
2811
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t init_pos = point3_t(1., 1., 1.);
2812
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 var_pair_t res = setup_control_points(5, flag, init_pos);
2813 1 T_linear_variable_t& vars = res.first;
2814 1 vartype exptecdvars[] = {constant, variable, variable,
2815 variable, variable, constant};
2816
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialMix", error);
2817
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvars, "VarCombinatorialMix", error);
2818
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 1, 4, "VarCombinatorialMix", error);
2819
2820
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraint_linear constraints = makeConstraint();
2821 1 flag = END_POS | END_VEL | INIT_VEL | INIT_POS;
2822
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2823
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2824 1 vartype exptecdvar1[] = {constant, constant, variable,
2825 variable, constant, constant};
2826
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialMix", error);
2827
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar1, "VarCombinatorialMix", error);
2828
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 2, 2, "VarCombinatorialMix", error);
2829
2830 1 flag = END_POS | END_VEL | END_ACC | INIT_VEL | INIT_POS;
2831
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2832
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2833 1 vartype exptecdvar2[] = {constant, constant, variable,
2834 constant, constant, constant};
2835
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialMix", error);
2836
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar2, "VarCombinatorialMix", error);
2837
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 2, 1, "VarCombinatorialMix", error);
2838
2839 1 flag = ALL;
2840
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(8, flag, init_pos, init_pos, constraints);
2841
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2842 1 vartype exptecdvar3[] = {constant, constant, constant, constant, variable,
2843 constant, constant, constant, constant};
2844
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 9, "VarCombinatorialMix", error);
2845
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar3, "VarCombinatorialMix", error);
2846
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 4, 1, "VarCombinatorialMix", error);
2847
2848 1 flag = END_VEL | END_ACC | INIT_VEL;
2849
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2850
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2851 1 vartype exptecdvar4[] = {variable, variable, variable,
2852 variable, variable, variable};
2853
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialMix", error);
2854
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar4, "VarCombinatorialMix", error);
2855
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
2856
2857 1 flag = END_VEL | INIT_VEL;
2858
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2859
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vars = res.first;
2860 1 vartype exptecdvar5[] = {variable, variable, variable,
2861 variable, variable, variable};
2862
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkNumVar(vars, 6, "VarCombinatorialMix", error);
2863
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checksequence(vars, exptecdvar5, "VarCombinatorialMix", error);
2864
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
2865
2866 1 bool err = true;
2867 try {
2868 1 flag = ALL;
2869
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2870 1 } catch (...) {
2871 1 err = false;
2872
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
2873
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (err) {
2874 ✗ error = true;
2875 std::cout << "exception should be raised when degree of bezier curve is "
2876 ✗ "not high enough to handle constraints "
2877 ✗ << std::endl;
2878 }
2879 1 }
2880
2881 ✗ void BezierLinearProblemInitInequalities(bool& error) {
2882 ✗ constraint_flag flag = INIT_POS | END_POS;
2883 ✗ point3_t init_pos = point3_t(1., 1., 1.);
2884 ✗ var_pair_t res = setup_control_points(5, flag, init_pos);
2885 ✗ T_linear_variable_t& vars = res.first;
2886 ✗ vartype exptecdvars[] = {constant, variable, variable,
2887 variable, variable, constant};
2888 ✗ checkNumVar(vars, 6, "VarCombinatorialMix", error);
2889 ✗ checksequence(vars, exptecdvars, "VarCombinatorialMix", error);
2890 ✗ checkPair(res.second, 1, 4, "VarCombinatorialMix", error);
2891
2892 ✗ constraint_linear constraints = makeConstraint();
2893 ✗ flag = END_POS | END_VEL | INIT_VEL | INIT_POS;
2894 ✗ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2895 ✗ vars = res.first;
2896 ✗ vartype exptecdvar1[] = {constant, constant, variable,
2897 variable, constant, constant};
2898 ✗ checkNumVar(vars, 6, "VarCombinatorialMix", error);
2899 ✗ checksequence(vars, exptecdvar1, "VarCombinatorialMix", error);
2900 ✗ checkPair(res.second, 2, 2, "VarCombinatorialMix", error);
2901
2902 ✗ flag = END_POS | END_VEL | END_ACC | INIT_VEL | INIT_POS;
2903 ✗ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2904 ✗ vars = res.first;
2905 ✗ vartype exptecdvar2[] = {constant, constant, variable,
2906 constant, constant, constant};
2907 ✗ checkNumVar(vars, 6, "VarCombinatorialMix", error);
2908 ✗ checksequence(vars, exptecdvar2, "VarCombinatorialMix", error);
2909 ✗ checkPair(res.second, 2, 1, "VarCombinatorialMix", error);
2910
2911 ✗ flag = ALL;
2912 ✗ res = setup_control_points(6, flag, init_pos, init_pos, constraints);
2913 ✗ vars = res.first;
2914 ✗ vartype exptecdvar3[] = {constant, constant, constant, variable,
2915 constant, constant, constant};
2916 ✗ checkNumVar(vars, 7, "VarCombinatorialMix", error);
2917 ✗ checksequence(vars, exptecdvar3, "VarCombinatorialMix", error);
2918 ✗ checkPair(res.second, 3, 1, "VarCombinatorialMix", error);
2919
2920 ✗ flag = END_VEL | END_ACC | INIT_VEL;
2921 ✗ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2922 ✗ vars = res.first;
2923 ✗ vartype exptecdvar4[] = {variable, variable, variable,
2924 variable, variable, variable};
2925 ✗ checkNumVar(vars, 6, "VarCombinatorialMix", error);
2926 ✗ checksequence(vars, exptecdvar4, "VarCombinatorialMix", error);
2927 ✗ checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
2928
2929 ✗ flag = END_VEL | INIT_VEL;
2930 ✗ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2931 ✗ vars = res.first;
2932 ✗ vartype exptecdvar5[] = {variable, variable, variable,
2933 variable, variable, variable};
2934 ✗ checkNumVar(vars, 6, "VarCombinatorialMix", error);
2935 ✗ checksequence(vars, exptecdvar5, "VarCombinatorialMix", error);
2936 ✗ checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
2937
2938 ✗ bool err = true;
2939 try {
2940 ✗ flag = ALL;
2941 ✗ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
2942 ✗ } catch (...) {
2943 ✗ err = false;
2944 }
2945 ✗ if (err) {
2946 ✗ error = true;
2947 std::cout << "exception should be raised when degree of bezier curve is "
2948 ✗ "not high enough to handle constraints "
2949 ✗ << std::endl;
2950 }
2951 }
2952
2953 1 void BezierLinearProblemsetupLoadProblem(bool& /*error*/) {
2954
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 problem_definition_t pDef = loadproblem(TEST_DATA_PATH "test.pb");
2955 // problem_data_t pData = setup_control_points<point_t, 3, double>(pDef);
2956
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 generate_problem<point3_t, double, true>(pDef, VELOCITY);
2957 // initInequalityMatrix<point_t,3,double>(pDef,pData,prob);
2958 1 }
2959
2960 1 void testOperatorEqual(bool& error) {
2961 // test with a C2 polynomial :
2962
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t zeros = point3_t(0., 0., 0.);
2963
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p0 = point3_t(0., 1., 0.);
2964
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t p1 = point3_t(1., 2., -3.);
2965
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t dp0 = point3_t(-8., 4., 6.);
2966
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t dp1 = point3_t(10., -10., 10.);
2967
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t ddp0 = point3_t(-1., 7., 4.);
2968
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 pointX_t ddp1 = point3_t(12., -8., 2.5);
2969 1 double min = 0.5;
2970 1 double max = 2.;
2971
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t polC2_1 = polynomial_t(p0, dp0, ddp0, p1, dp1, ddp1, min, max);
2972
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t polC2_2 = polynomial_t(p0, dp0, ddp0, p1, dp1, ddp1, min, max);
2973
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t polC2_3(polC2_1);
2974 // std::cout<<"Should call polynomial method : "<<std::endl;
2975
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (polC2_1 != polC2_2) {
2976 ✗ std::cout << "polC2_1 and polC2_2 should be equals" << std::endl;
2977 ✗ error = true;
2978 }
2979
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (polC2_1 != polC2_3) {
2980 ✗ std::cout << "polC2_1 and polC2_3 should be equals" << std::endl;
2981 ✗ error = true;
2982 }
2983
2984
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t polC2_4 = polynomial_t(p1, dp0, ddp0, p1, dp1, ddp0, min, max);
2985
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (polC2_1 == polC2_4) {
2986 ✗ std::cout << "polC2_1 and polC2_4 should not be equals" << std::endl;
2987 ✗ error = true;
2988 }
2989
2990 // test with bezier
2991
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a(1, 2, 3);
2992
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b(2, 3, 4);
2993
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c(3, 4, 5);
2994
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d(3, 6, 7);
2995
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t e(3, 61, 7);
2996
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t f(3, 56, 7);
2997
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t g(3, 36, 7);
2998
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t h(43, 6, 7);
2999
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t i(3, 6, 77);
3000 1 std::vector<point3_t> control_points;
3001
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(a);
3002
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(b);
3003
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(c);
3004
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(d);
3005
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(e);
3006
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(f);
3007
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(g);
3008
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(h);
3009
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points.push_back(i);
3010 1 bezier_t::num_t T_min = 1.0;
3011 1 bezier_t::num_t T_max = 3.0;
3012
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t bc_0(control_points.begin(), control_points.end(), T_min, T_max);
3013
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier3_t bc3_0(control_points.begin(), control_points.end(), T_min, T_max);
3014
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t bc_1(bc_0);
3015 // std::cout<<"Should call Bezier method : "<<std::endl;
3016
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (bc_1 != bc_0) {
3017 ✗ std::cout << "bc_0 and bc_1 should be equals" << std::endl;
3018 ✗ error = true;
3019 }
3020 1 std::vector<point3_t> control_points2;
3021
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points2.push_back(a);
3022
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points2.push_back(b);
3023
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points2.push_back(c);
3024
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points2.push_back(d);
3025
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t bc_2(control_points2.begin(), control_points2.end(), T_min, T_max);
3026
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier3_t bc3_2(control_points2.begin(), control_points2.end(), T_min, T_max);
3027
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (bc_2 == bc_0) {
3028 ✗ std::cout << "bc_0 and bc_2 should not be equals" << std::endl;
3029 ✗ error = true;
3030 }
3031
3032
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t e3(3, 61.9, 7);
3033
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t g3(-3, 36, 7);
3034 1 std::vector<point3_t> control_points3;
3035
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(a);
3036
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(b);
3037
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(c);
3038
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(d);
3039
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(e3);
3040
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(f);
3041
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(g3);
3042
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(h);
3043
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_points3.push_back(i);
3044
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 bezier_t bc_0_3(control_points3.begin(), control_points3.end(), T_min, T_max);
3045
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (bc_0_3 == bc_0) {
3046 ✗ std::cout << "bc_0_3 and bc_0 should not be equals" << std::endl;
3047 ✗ error = true;
3048 }
3049
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t pol_2 = polynomial_from_curve<polynomial_t>(bc_2);
3050
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial3_t pol3_2 = polynomial_from_curve<polynomial3_t>(bc3_2);
3051
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t bc_3 = bezier_from_curve<bezier_t>(pol_2);
3052
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (bc_2 != bc_3) {
3053 ✗ std::cout << "bc_2 and bc_3 should be equals" << std::endl;
3054 ✗ error = true;
3055 }
3056
3057 // test bezier / polynomial
3058
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t pol_0 = polynomial_from_curve<polynomial_t>(bc_0);
3059
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 CompareCurves<polynomial_t, bezier_t>(pol_0, bc_0, "compare pol_0 and bc_0",
3060 error);
3061 // std::cout<<"Should call curve_abc method : "<<std::endl;
3062
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (!bc_0.isEquivalent(&pol_0)) {
3063 ✗ std::cout << "bc_0 and pol_0 should be equivalent" << std::endl;
3064 ✗ error = true;
3065 }
3066
3067 // test with hermite :
3068
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t ch_p0(1, 2, 3);
3069
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t ch_m0(2, 3, 4);
3070
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t ch_p1(3, 4, 5);
3071
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t ch_m1(3, 6, 7);
3072
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pair_point_tangent_t pair0(ch_p0, ch_m0);
3073
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pair_point_tangent_t pair1(ch_p1, ch_m1);
3074
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pair_point_tangent_t ch_control_points;
3075
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ch_control_points.push_back(pair0);
3076
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ch_control_points.push_back(pair1);
3077 1 std::vector<double> time_control_points;
3078
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(T_min);
3079
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 time_control_points.push_back(T_max);
3080 cubic_hermite_spline_t chs0(ch_control_points.begin(),
3081
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 ch_control_points.end(), time_control_points);
3082 cubic_hermite_spline_t chs1(ch_control_points.begin(),
3083
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 ch_control_points.end(), time_control_points);
3084
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cubic_hermite_spline_t chs2(chs0);
3085
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t ch_p2(3.1, 4, 5);
3086
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t ch_m2(3, 6.5, 6.);
3087
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pair_point_tangent_t pair2(ch_p2, ch_m2);
3088
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_pair_point_tangent_t ch_control_points2;
3089
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ch_control_points2.push_back(pair0);
3090
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ch_control_points2.push_back(pair2);
3091 cubic_hermite_spline_t chs3(ch_control_points2.begin(),
3092
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 ch_control_points2.end(), time_control_points);
3093 // std::cout<<"Should call hermite method : "<<std::endl;
3094
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (chs0 != chs1) {
3095 ✗ std::cout << "chs0 and chs1 should be equals" << std::endl;
3096 ✗ error = true;
3097 }
3098
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (chs0 != chs2) {
3099 ✗ std::cout << "chs0 and chs2 should be equals" << std::endl;
3100 ✗ error = true;
3101 }
3102
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (chs0 == chs3) {
3103 ✗ std::cout << "chs0 and chs3 should not be equals" << std::endl;
3104 ✗ error = true;
3105 }
3106
3107 // // test bezier / hermite
3108
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 bezier_t bc_ch = bezier_from_curve<bezier_t>(chs0);
3109 // std::cout<<"Should call curve_abc method : "<<std::endl;
3110
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (!chs0.isEquivalent(&bc_ch)) {
3111 ✗ std::cout << "chs0 and bc_ch should be equivalent" << std::endl;
3112 ✗ error = true;
3113 }
3114 // test polynomial / hermite
3115
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial_t pol_ch = polynomial_from_curve<polynomial_t>(chs0);
3116
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (!chs0.isEquivalent(&pol_ch)) {
3117 ✗ std::cout << "chs0 and pol_ch should be equivalent" << std::endl;
3118 ✗ error = true;
3119 }
3120
3121 // SO3
3122
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q0(1, 0, 0, 0);
3123
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q1(0.7071, 0.7071, 0, 0);
3124
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q0.normalize();
3125
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q1.normalize();
3126 1 const double tMin = 0.;
3127 1 const double tMax = 1.5;
3128
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3Traj1(q0, q1, tMin, tMax);
3129
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3Traj2(q0, q1, tMin, tMax);
3130
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 SO3Linear_t so3TrajMatrix1(q0.toRotationMatrix(), q1.toRotationMatrix(), tMin,
3131
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 tMax);
3132
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3TrajMatrix2(so3TrajMatrix1);
3133
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t q2(0.7071, 0., 0.7071, 0);
3134
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 q2.normalize();
3135
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3Linear_t so3Traj3(q0, q2, tMin, tMax);
3136 // std::cout<<"Should call SO3 method : "<<std::endl;
3137
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (so3Traj1 != so3Traj2) {
3138 ✗ std::cout << "so3Traj1 and so3Traj2 should be equals" << std::endl;
3139 ✗ error = true;
3140 }
3141
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (so3Traj1 != so3TrajMatrix1) {
3142 ✗ std::cout << "so3Traj1 and so3TrajMatrix1 should be equals" << std::endl;
3143 ✗ error = true;
3144 }
3145
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (so3Traj1 != so3TrajMatrix2) {
3146 ✗ std::cout << "so3Traj1 and so3TrajMatrix2 should be equals" << std::endl;
3147 ✗ error = true;
3148 }
3149
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (so3Traj1 == so3Traj3) {
3150 ✗ std::cout << "so3Traj1 and so3Traj3 should not be equals" << std::endl;
3151 ✗ error = true;
3152 }
3153
3154 // test from pointer :
3155
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 curve_ptr_t c_ptr1(new bezier_t(bc_0));
3156
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 curve_abc_t* c_ptr2 = new bezier_t(bc_0);
3157
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 curve_ptr_t c_ptr3(new polynomial_t(pol_0));
3158 // std::cout<<"Should call bezier method : "<<std::endl;
3159
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (!c_ptr1->isApprox(c_ptr2)) {
3160 ✗ std::cout << "c_ptr1 and c_ptr2 should be approx" << std::endl;
3161 ✗ error = true;
3162 }
3163 // std::cout<<"Should call curve_abc method : "<<std::endl;
3164
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
 1 if (!c_ptr2->isEquivalent(c_ptr3.get())) {
3165 ✗ std::cout << "c_ptr2 and c_ptr3 should be equivalent" << std::endl;
3166 ✗ error = true;
3167 }
3168
2/4
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 1 if (c_ptr1->isApprox(c_ptr3.get())) {
3169 ✗ std::cout << "c_ptr1 and c_ptr3 should not be approx" << std::endl;
3170 ✗ error = true;
3171 }
3172
3173 // SE3
3174
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomial3_t pol3_0 = polynomial_from_curve<polynomial3_t>(bc3_0);
3175 std::shared_ptr<bezier3_t> translation_bezier =
3176
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::make_shared<bezier3_t>(bc3_0);
3177 std::shared_ptr<bezier3_t> translation_bezier2 =
3178
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::make_shared<bezier3_t>(bc3_0);
3179 std::shared_ptr<polynomial3_t> translation_polynomial =
3180
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::make_shared<polynomial3_t>(pol3_0);
3181
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t se3_bezier1 = SE3Curve_t(translation_bezier, q0.toRotationMatrix(),
3182
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 q1.toRotationMatrix());
3183 SE3Curve_t se3_bezier12 = SE3Curve_t(
3184
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 translation_bezier2, q0.toRotationMatrix(), q1.toRotationMatrix());
3185 SE3Curve_t se3_pol1 = SE3Curve_t(
3186
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 translation_polynomial, q0.toRotationMatrix(), q1.toRotationMatrix());
3187 1 SE3Curve_t se3_bezier2(se3_bezier1);
3188
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SE3Curve_t se3_bezier3 = SE3Curve_t(translation_bezier, q0.toRotationMatrix(),
3189
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 q2.toRotationMatrix());
3190 std::shared_ptr<polynomial3_t> translation_polynomial2 =
3191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::make_shared<polynomial3_t>(pol3_2);
3192 SE3Curve_t se3_pol2 = SE3Curve_t(
3193
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 translation_polynomial2, q0.toRotationMatrix(), q1.toRotationMatrix());
3194 // std::cout<<"Should call se3 method : "<<std::endl;
3195
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (se3_bezier1 == se3_pol1) {
3196 ✗ std::cout << "se3_bezier1 and se3_pol1 should not be equals" << std::endl;
3197 ✗ error = true;
3198 }
3199
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (se3_bezier1.isApprox(se3_pol1)) {
3200 ✗ std::cout << "se3_bezier1 and se3_pol1 should not be approx" << std::endl;
3201 ✗ error = true;
3202 }
3203 // std::cout<<"Should call curve_abc : "<<std::endl;
3204
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (!se3_bezier1.isEquivalent(&se3_pol1)) {
3205 ✗ std::cout << "se3_bezier1 and se3_pol1 should be equivalent" << std::endl;
3206 ✗ error = true;
3207 }
3208 // std::cout<<"Should call se3 method : "<<std::endl;
3209
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (se3_bezier1 != se3_bezier2) {
3210 ✗ std::cout << "se3_bezier1 and se3_bezier2 should be equals" << std::endl;
3211 ✗ error = true;
3212 }
3213 // std::cout<<"Should call se3 -> bezier / SO3 method : "<<std::endl;
3214
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (se3_bezier1 != se3_bezier12) {
3215 ✗ std::cout << "se3_bezier1 and se3_bezier12 should be equals" << std::endl;
3216 ✗ error = true;
3217 }
3218 // std::cout<<"Should call se3 -> curve_abc : "<<std::endl;
3219
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (se3_bezier1 == se3_pol2) {
3220 ✗ std::cout << "se3_bezier1 and se3_pol2 should not be equals" << std::endl;
3221 ✗ error = true;
3222 }
3223 // std::cout<<"Should call se3 -> so3 method : "<<std::endl;
3224
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (se3_bezier1 == se3_bezier3) {
3225 ✗ std::cout << "se3_bezier1 and se3_bezier3 should not be equals"
3226 ✗ << std::endl;
3227 ✗ error = true;
3228 }
3229
3230 // Piecewises
3231
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t a0(1, 2, 3);
3232
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t b0(2, 3, 4);
3233
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t c0(3, 4, 5);
3234
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t d0(4, 5, 6);
3235 1 std::vector<point3_t> params0;
3236 1 std::vector<point3_t> params1;
3237
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params0.push_back(a0); // bezier between [0,1]
3238
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params0.push_back(b0);
3239
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params0.push_back(c0);
3240
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params0.push_back(d0);
3241
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params1.push_back(d0); // bezier between [1,2]
3242
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params1.push_back(c0);
3243
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params1.push_back(b0);
3244
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 params1.push_back(a0);
3245 std::shared_ptr<bezier_t> bc0_ptr =
3246
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<bezier_t>(params0.begin(), params0.end(), 0., 1.);
3247 std::shared_ptr<bezier_t> bc1_ptr =
3248
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<bezier_t>(params1.begin(), params1.end(), 1., 2.);
3249
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 piecewise_t pc_C0(bc0_ptr);
3250
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pc_C0.add_curve_ptr(bc1_ptr);
3251
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 piecewise_t pc_C1(bc0_ptr);
3252
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pc_C1.add_curve_ptr(bc1_ptr);
3253
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewise_t pc_C2(pc_C0);
3254
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 piecewise_t pc_C3(bc0_ptr);
3255
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 piecewise_t pc_C4(bc0_ptr);
3256 std::shared_ptr<bezier_t> bc2_ptr =
3257
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 std::make_shared<bezier_t>(params0.begin(), params0.end(), 1., 2.);
3258
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 pc_C4.add_curve_ptr(bc2_ptr);
3259 // std::cout<<"Should call piecewise method -> bezier , bezier: "<<std::endl;
3260
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (pc_C0 != pc_C1) {
3261 ✗ std::cout << "pc_C0 and pc_C1 should be equals" << std::endl;
3262 ✗ error = true;
3263 }
3264 // std::cout<<"Should call piecewise method -> bezier , bezier: "<<std::endl;
3265
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (pc_C0 != pc_C2) {
3266 ✗ std::cout << "pc_C0 and pc_C2 should be equals" << std::endl;
3267 ✗ error = true;
3268 }
3269 // std::cout<<"Should call piecewise method: "<<std::endl;
3270
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (pc_C0 == pc_C3) {
3271 ✗ std::cout << "pc_C0 and pc_C3 should not be equals" << std::endl;
3272 ✗ error = true;
3273 }
3274 // std::cout<<"Should call piecewise method -> bezier , bezier: "<<std::endl;
3275
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (pc_C0 == pc_C4) {
3276 ✗ std::cout << "pc_C0 and pc_C4 should not be equals" << std::endl;
3277 ✗ error = true;
3278 }
3279 // piecewise with mixed curves types
3280 // std::cout<<"Should call piecewise method: "<<std::endl;
3281 piecewise_t pc_C5 =
3282
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc_C0.convert_piecewise_curve_to_polynomial<polynomial_t>();
3283
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (pc_C0 == pc_C5) {
3284 ✗ std::cout << "pc_C0 and pc_C5 should be not equals" << std::endl;
3285 ✗ error = true;
3286 }
3287 // std::cout<<"Should call curve_abc method: "<<std::endl;
3288
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 1 if (!pc_C0.isEquivalent(&pc_C5)) {
3289 ✗ std::cout << "pc_C0 and pc_C5 should be equivalents" << std::endl;
3290 ✗ error = true;
3291 }
3292
3293 // piecewise se3 :
3294
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewise_SE3_t pc_se3_1;
3295
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc_se3_1.add_curve(se3_pol1);
3296 point3_t p_init_se3(
3297
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 translation_polynomial->operator()(translation_polynomial->max()));
3298 point3_t dp_init_se3(
3299
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 translation_polynomial->derivate(translation_polynomial->max(), 1));
3300
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t p_end_se3(1, -2, 6);
3301
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point3_t dp_end_se3(3.5, 2.5, -9);
3302 std::shared_ptr<polynomial3_t> translation_pol3 =
3303 std::make_shared<polynomial3_t>(p_init_se3, dp_init_se3, p_end_se3,
3304
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dp_end_se3, translation_polynomial->max(),
3305
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 translation_polynomial->max() + 2.5);
3306 curve_SE3_ptr_t se3_pol_3(new SE3Curve_t(
3307
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 translation_pol3, q1.toRotationMatrix(), q2.toRotationMatrix()));
3308
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc_se3_1.add_curve_ptr(se3_pol_3);
3309
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewise_SE3_t pc_se3_2(pc_se3_1);
3310
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 piecewise_SE3_t pc_se3_3(std::make_shared<SE3Curve_t>(se3_pol1));
3311
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc_se3_3.add_curve_ptr(se3_pol_3);
3312
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 piecewise_SE3_t pc_se3_4(std::make_shared<SE3Curve_t>(se3_pol2));
3313
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pc_se3_4.add_curve_ptr(se3_pol_3);
3314 // std::cout<<"Should call piecewise method -> SE3 , SE3: "<<std::endl;
3315
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (pc_se3_1 != pc_se3_2) {
3316 ✗ std::cout << "pc_se3_1 and pc_se3_2 should be equals" << std::endl;
3317 ✗ error = true;
3318 }
3319 // std::cout<<"Should call piecewise method -> SE3 , SE3: "<<std::endl;
3320
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (pc_se3_1 != pc_se3_3) {
3321 ✗ std::cout << "pc_se3_1 and pc_se3_3 should be equals" << std::endl;
3322 ✗ error = true;
3323 }
3324 // std::cout<<"Should call piecewise method -> SE3 -> polynomial :
3325 // "<<std::endl;
3326
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (pc_se3_1 == pc_se3_4) {
3327 ✗ std::cout << "pc_se3_1 and pc_se3_3 should not be equals" << std::endl;
3328 ✗ error = true;
3329 }
3330 1 }
3331
3332 1 int main(int /*argc*/, char** /*argv[]*/) {
3333
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "performing tests... \n";
3334 1 bool error = false;
3335
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 PolynomialCubicFunctionTest(error);
3336
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicNoErrorTest(error);
3337
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicPointsCrossedTest(
3338 error); // checks that given wayPoints are crossed
3339
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicTwoPointsTest(error);
3340
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicOneDimTest(error);
3341
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicVelocityConstraintsTest(error);
3342
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 EffectorTrajectoryTest(error);
3343
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 EffectorSplineRotationNoRotationTest(error);
3344
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 EffectorSplineRotationRotationTest(error);
3345
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 TestReparametrization(error);
3346
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 EffectorSplineRotationWayPointRotationTest(error);
3347
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierCurveTest(error);
3348
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierDerivativeCurveTest(error);
3349
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierDerivativeCurveConstraintTest(error);
3350
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierCurveTestCompareHornerAndBernstein(error);
3351
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierDerivativeCurveTimeReparametrizationTest(error);
3352
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierEvalDeCasteljau(error);
3353
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierSplitCurve(error);
3354
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierElevate(error);
3355
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CubicHermitePairsPositionDerivativeTest(error);
3356
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 piecewiseCurveTest(error);
3357
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 PiecewisePolynomialCurveFromDiscretePoints(error);
3358
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 PiecewisePolynomialCurveFromFile(error);
3359
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 toPolynomialConversionTest(error);
3360
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cubicConversionTest(error);
3361
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 curveAbcDimDynamicTest(error);
3362
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 serializationCurvesTest(error);
3363
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynomialFromBoundaryConditions(error);
3364
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 so3LinearTest(error);
3365
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 SO3serializationTest(error);
3366
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 se3CurveTest(error);
3367
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Se3serializationTest(error);
3368
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierLinearProblemsetup_control_pointsNoConstraint(error);
3369
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierLinearProblemsetup_control_pointsVarCombinatorialInit(error);
3370
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierLinearProblemsetup_control_pointsVarCombinatorialEnd(error);
3371
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierLinearProblemsetup_control_pointsVarCombinatorialMix(error);
3372
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 BezierLinearProblemsetupLoadProblem(error);
3373
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 testOperatorEqual(error);
3374
3375
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
3376 ✗ std::cout << "There were some errors\n";
3377 ✗ return -1;
3378 } else {
3379
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "no errors found \n";
3380 1 return 0;
3381 }
3382 }
3383