| 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 |
6/12✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 35 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 35 times.
✗ Branch 17 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 |
6/12✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 35 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 35 times.
✗ Branch 17 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "checking degree of bezier curve :"); |
| 458 | std::string errMsg2( | ||
| 459 | "In test BezierDerivativeCurveConstraintTest, Error While checking " | ||
| 460 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "value of point on curve : "); |
| 507 | std::string errMsg1( | ||
| 508 | "In test CubicConversionTest - convert bezier to, Error While checking " | ||
| 509 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "value of point on curve : "); |
| 510 | std::string errMsg2( | ||
| 511 | "In test CubicConversionTest - convert polynomial to, Error While " | ||
| 512 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "obtained)"); |
| 821 | std::string errmsg2( | ||
| 822 | "Error in EffectorTrajectoryTest; while checking derivative (expected / " | ||
| 823 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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.
|
6 | 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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.
|
1 | 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "correspond to the original"); |
| 1088 | std::string errMsg1( | ||
| 1089 | "BezierSplitCurve, ERROR splitting point of the splitted curve doesn't " | ||
| 1090 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "correspond to the original"); |
| 1091 | std::string errMsg2( | ||
| 1092 | "BezierSplitCurve, ERROR final point of the splitted curve doesn't " | ||
| 1093 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "correspond to the original"); |
| 1094 | std::string errMsg3( | ||
| 1095 | "BezierSplitCurve, ERROR while checking value on curve and curves " | ||
| 1096 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "splitted"); |
| 1097 | std::string errMsg4( | ||
| 1098 | "BezierSplitCurve, ERROR Degree of the splitted curve are not the same " | ||
| 1099 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "as the original curve"); |
| 1100 | std::string errMsg5( | ||
| 1101 | "BezierSplitCurve, ERROR duration of the splitted curve doesn't " | ||
| 1102 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "correspond to the original"); |
| 1103 | std::string errMsg6( | ||
| 1104 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "obtained) : "); |
| 1182 | std::string errmsg2( | ||
| 1183 | "in Cubic Hermite 2 points, Error While checking value of point on " | ||
| 1184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "curve : "); |
| 1185 | std::string errmsg3( | ||
| 1186 | "in Cubic Hermite 2 points, Error While checking value of tangent on " | ||
| 1187 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "C0 on "); |
| 1421 | std::string errmsg3( | ||
| 1422 | "in piecewise polynomial curve test, Error while checking continuity " | ||
| 1423 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "C1 on "); |
| 1424 | std::string errmsg4( | ||
| 1425 | "in piecewise polynomial curve test, Error while checking continuity " | ||
| 1426 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 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.
|
1 | 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.
|
1 | 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.
|
1 | 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::string filename_pos(TEST_DATA_PATH "discrete_points_pos.txt"); |
| 1718 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::string filename_vel(TEST_DATA_PATH "discrete_points_vel.txt"); |
| 1719 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::string filename_acc(TEST_DATA_PATH "discrete_points_acc.txt"); |
| 1720 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "in serializationCurveTest, Error While serializing Polynomial : "); |
| 1860 | std::string errMsg2( | ||
| 1861 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "in serializationCurveTest, Error While serializing Bezier : "); |
| 1862 | std::string errMsg3( | ||
| 1863 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "in serializationCurveTest, Error While serializing Cubic Hermite : "); |
| 1864 | std::string errMsg4( | ||
| 1865 | "in serializationCurveTest, Error While serializing Piecewise curves " | ||
| 1866 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ": "); |
| 1867 | std::string errMsg5( | ||
| 1868 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "in serializationCurveTest, Error While serializing Exact cubic : "); |
| 1869 | std::string errMsg6( | ||
| 1870 | "in serializationCurveTest, Error While serializing using abstract " | ||
| 1871 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::string fileName("fileTest.test"); |
| 1888 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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.
|
2 | 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 1 taken 1 times.
✗ Branch 2 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::string fileName("fileTest"); |
| 2245 | std::string errmsg( | ||
| 2246 | "SO3serializationTest : curve serialized is not equivalent to the " | ||
| 2247 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 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 | 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 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 | 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 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::string fileName("fileTest"); |
| 2479 | std::string errmsg( | ||
| 2480 | "SE3serializationTest : curve serialized is not equivalent to the " | ||
| 2481 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 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 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 | 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 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 | 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 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 | 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 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 | 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "setup_control_pointsNoConstraint", error); |
| 2648 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialInit", error); |
| 2673 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvars, "VarCombinatorialInit", error); |
| 2674 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialInit", error); |
| 2683 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar1, "VarCombinatorialInit", error); |
| 2684 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialInit", error); |
| 2692 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar2, "VarCombinatorialInit", error); |
| 2693 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialInit", error); |
| 2701 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar3, "VarCombinatorialInit", error); |
| 2702 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialInit", error); |
| 2710 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar4, "VarCombinatorialInit", error); |
| 2711 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialInit", error); |
| 2719 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar5, "VarCombinatorialInit", error); |
| 2720 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialEnd", error); |
| 2745 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvars, "VarCombinatorialEnd", error); |
| 2746 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialEnd", error); |
| 2755 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar1, "VarCombinatorialEnd", error); |
| 2756 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialEnd", error); |
| 2764 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar2, "VarCombinatorialEnd", error); |
| 2765 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialEnd", error); |
| 2773 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar3, "VarCombinatorialEnd", error); |
| 2774 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialEnd", error); |
| 2782 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar4, "VarCombinatorialEnd", error); |
| 2783 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialEnd", error); |
| 2791 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar5, "VarCombinatorialEnd", error); |
| 2792 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialMix", error); |
| 2817 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvars, "VarCombinatorialMix", error); |
| 2818 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialMix", error); |
| 2827 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar1, "VarCombinatorialMix", error); |
| 2828 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialMix", error); |
| 2836 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar2, "VarCombinatorialMix", error); |
| 2837 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 9, "VarCombinatorialMix", error); |
| 2845 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar3, "VarCombinatorialMix", error); |
| 2846 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialMix", error); |
| 2854 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar4, "VarCombinatorialMix", error); |
| 2855 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checkNumVar(vars, 6, "VarCombinatorialMix", error); |
| 2863 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | checksequence(vars, exptecdvar5, "VarCombinatorialMix", error); |
| 2864 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | 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 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | 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/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 not taken.
|
1 | curve_ptr_t c_ptr1(new bezier_t(bc_0)); |
| 3156 |
2/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 not taken.
|
1 | curve_abc_t* c_ptr2 = new bezier_t(bc_0); |
| 3157 |
3/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 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/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 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✗ Branch 18 not taken.
✗ Branch 19 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.
|
1 | 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.
|
1 | 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 |