GCC Code Coverage Report

Directory: ./
File: src/reeds-shepp-path.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 293 400 73.2%
Branches: 496 1020 48.6%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016 CNRS
3 // Authors: Joseph Mirabel
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <boost/math/constants/constants.hpp>
31 #include <boost/serialization/weak_ptr.hpp>
32 #include <hpp/core/path-vector.hh>
33 #include <hpp/core/steering-method/constant-curvature.hh>
34 #include <hpp/pinocchio/configuration.hh>
35 #include <hpp/pinocchio/device.hh>
36 #include <hpp/pinocchio/joint.hh>
37 #include <hpp/pinocchio/liegroup.hh>
38 #include <hpp/pinocchio/serialization.hh>
39 #include <hpp/util/debug.hh>
40 #include <hpp/util/serialization.hh>
41 #include <pinocchio/serialization/eigen.hpp>
42
43 namespace hpp {
44 namespace core {
45 namespace {
46 using steeringMethod::ConstantCurvature;
47 using steeringMethod::ConstantCurvaturePtr_t;
48
49 struct Data {
50 6 Data(const value_type& rho_)
51 6 : rsLength(std::numeric_limits<value_type>::infinity()), rho(rho_) {}
52 typedef Eigen::Matrix<value_type, 5, 1> Lengths_t;
53 std::size_t typeId;
54 Lengths_t lengths;
55 value_type rsLength;
56 value_type rho;
57 }; // struct Data
58
59 value_type precision(sqrt(std::numeric_limits<value_type>::epsilon()));
60
61 // The comments, variable names, etc. use the nomenclature from the Reeds &
62 // Shepp paper.
63
64 const value_type pi = boost::math::constants::pi<value_type>();
65 const value_type twopi = 2. * pi;
66 const value_type RS_EPS = 1e-6;
67 const value_type ZERO = 10 * std::numeric_limits<value_type>::epsilon();
68 const vector2_t oneMP = (vector2_t() << -1, 1).finished();
69 const vector2_t onePM = (vector2_t() << 1, -1).finished();
70
71 enum SegmentType { RS_NOP = 0, RS_LEFT = 1, RS_STRAIGHT = 2, RS_RIGHT = 3 };
72 // static const SegmentType types[18][5];
73 const SegmentType types[18][5] = {
74 {RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 0
75 {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP}, // 1
76 {RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 2
77 {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP}, // 3
78 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 4
79 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 5
80 {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 6
81 {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 7
82 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP}, // 8
83 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP}, // 9
84 {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP}, // 10
85 {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP}, // 11
86 {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 12
87 {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 13
88 {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP}, // 14
89 {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP}, // 15
90 {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT}, // 16
91 {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT} // 17
92 };
93
94 template <typename D1, typename D2>
95 24 inline vector2_t rotate(const Eigen::MatrixBase<D1>& U,
96 const Eigen::MatrixBase<D2>& CS) {
97 24 const D1& _U = U.derived();
98 24 const D2& _CS = CS.derived();
99
7/14
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 12 times.
✗ Branch 21 not taken.
 48 return (vector2_t() << _U.dot(_CS), -_U(0) * _CS(1) + _U(1) * _CS(0))
100
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 48 .finished();
101 }
102
103 386 inline value_type mod2pi(const value_type& x) {
104 386 value_type v = fmod(x, twopi);
105
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 362 times.
 386 if (v < -pi)
106 24 v += twopi;
107
2/2
✓ Branch 0 taken 26 times.
✓ Branch 1 taken 336 times.
 362 else if (v > pi)
108 26 v -= twopi;
109 386 return v;
110 }
111 inline value_type angle(const vector2_t& cs) {
112 if (cs(1) < 0)
113 return -std::acos(cs(0));
114 else
115 return std::acos(cs(0));
116 }
117 180 inline void polar(const value_type& x, const value_type& y, value_type& r,
118 value_type& theta) {
119 180 r = std::sqrt(x * x + y * y);
120 180 theta = atan2(y, x);
121 180 }
122 8 inline void tauOmega(value_type u, value_type v, value_type xi, value_type eta,
123 value_type phi, value_type& tau, value_type& omega) {
124 8 value_type delta = mod2pi(u - v), A = sin(u) - sin(delta),
125 8 B = cos(u) - cos(delta) - 1.;
126 8 value_type t1 = atan2(eta * A - xi * B, xi * A + eta * B),
127 8 t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
128
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 tau = (t2 < 0) ? mod2pi(t1 + pi) : mod2pi(t1);
129 8 omega = mod2pi(tau - u + v - phi);
130 8 }
131
132 // formula 8.1 in Reeds-Shepp paper
133 20 inline bool LpSpLp(const vector2_t& xy, const vector2_t& csPhi,
134 const value_type& phi, value_type& t, value_type& u,
135 value_type& v) {
136
2/4
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
 20 polar(xy(0) - csPhi(1), xy(1) - 1. + csPhi(0), u, t);
137
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 14 times.
 20 if (t >= -ZERO) {
138 6 v = mod2pi(phi - t);
139
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
 6 if (v >= -ZERO) {
140
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 4 assert(fabs(u * cos(t) + csPhi(1) - xy(0)) < RS_EPS);
141
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 4 assert(fabs(u * sin(t) - csPhi(0) + 1 - xy(1)) < RS_EPS);
142
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
 4 assert(fabs(mod2pi(t + v - phi)) < RS_EPS);
143 4 return true;
144 }
145 }
146 16 return false;
147 }
148 // formula 8.2
149 20 inline bool LpSpRp(const vector2_t& xy, const vector2_t& csPhi,
150 const value_type& phi, value_type& t, value_type& u,
151 value_type& v) {
152 value_type t1, u1;
153
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
 20 polar(xy(0) + csPhi(1), xy(1) - 1. - csPhi(0), u1, t1);
154 20 u1 = u1 * u1;
155
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 20 if (u1 >= 4.) {
156 value_type theta;
157 20 u = sqrt(u1 - 4.);
158 20 theta = atan2(2., u);
159 20 t = mod2pi(t1 + theta);
160 20 v = mod2pi(t - phi);
161
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 20 times.
 20 assert(fabs(2 * sin(t) + u * cos(t) - csPhi(1) - xy(0)) < RS_EPS);
162
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 20 times.
 20 assert(fabs(-2 * cos(t) + u * sin(t) + csPhi(0) + 1 - xy(1)) < RS_EPS);
163
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
 20 assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
164
4/4
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 6 times.
 20 return t >= -ZERO && v >= -ZERO;
165 }
166 ✗ return false;
167 }
168
169 9 void setupPath(Data& d, const std::size_t& typeId, double t, double u = 0,
170 double v = 0, double w = 0, double x = 0) {
171 9 d.typeId = typeId;
172 9 d.lengths(0) = t;
173 9 d.lengths(1) = u;
174 9 d.lengths(2) = v;
175 9 d.lengths(3) = w;
176 9 d.lengths(4) = x;
177 9 d.rsLength = d.rho * d.lengths.lpNorm<1>();
178 hppDout(info, "lengths = " << d.lengths.transpose());
179 9 }
180
181 5 void CSC(Data& d, const vector2_t& xy, const vector2_t& csPhi,
182 const value_type& phi) {
183 5 value_type t, u, v, Lmin = d.rsLength, L;
184
5/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 4 times.
✓ Branch 5 taken 1 times.
✓ Branch 6 taken 4 times.
 6 if (LpSpLp(xy, csPhi, phi, t, u, v) &&
185
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
186
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 setupPath(d, 14, t, u, v);
187 1 Lmin = L;
188 }
189
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpSpLp(xy.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
190
4/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 3 times.
 7 v) &&
191
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
192 {
193
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 setupPath(d, 14, -t, -u, -v);
194 2 Lmin = L;
195 }
196
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpSpLp(xy.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
197
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
198 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
199 {
200 ✗ setupPath(d, 15, t, u, v);
201 ✗ Lmin = L;
202 }
203
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 6 if (LpSpLp(-xy, csPhi, phi, t, u, v) &&
204
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
205 {
206 ✗ setupPath(d, 15, -t, -u, -v);
207 ✗ Lmin = L;
208 }
209
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 5 if (LpSpRp(xy, csPhi, phi, t, u, v) &&
210 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
211 ✗ setupPath(d, 12, t, u, v);
212 ✗ Lmin = L;
213 }
214
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpSpRp(xy.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
215
4/4
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 3 times.
 8 v) &&
216
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
217 {
218
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 setupPath(d, 12, -t, -u, -v);
219 2 Lmin = L;
220 }
221
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpSpRp(xy.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
222
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
223 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
224 {
225 ✗ setupPath(d, 13, t, u, v);
226 ✗ Lmin = L;
227 }
228
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 6 if (LpSpRp(-xy, csPhi, phi, t, u, v) &&
229
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
230 ✗ setupPath(d, 13, -t, -u, -v);
231 5 }
232 // formula 8.3 / 8.4 *** TYPO IN PAPER ***
233 40 inline bool LpRmL(const vector2_t& xy, const vector2_t& csPhi,
234 const value_type& phi, value_type& t, value_type& u,
235 value_type& v) {
236
4/8
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 40 times.
✗ Branch 11 not taken.
 40 value_type xi = xy(0) - csPhi(1), eta = xy(1) - 1. + csPhi(0), u1, theta;
237 40 polar(xi, eta, u1, theta);
238
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 24 times.
 40 if (u1 <= 4.) {
239 16 u = -2. * asin(.25 * u1);
240 16 t = mod2pi(theta + .5 * u + pi);
241 16 v = mod2pi(phi - t + u);
242
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 16 times.
 16 assert(fabs(2 * (sin(t) - sin(t - u)) + csPhi(1) - xy(0)) < RS_EPS);
243
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 16 times.
 16 assert(fabs(2 * (-cos(t) + cos(t - u)) - csPhi(0) + 1 - xy(1)) < RS_EPS);
244
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 16 assert(fabs(mod2pi(t - u + v - phi)) < RS_EPS);
245
3/4
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 16 return t >= -ZERO && u <= ZERO;
246 }
247 24 return false;
248 }
249 5 void CCC(Data& d, const vector2_t& xy, const vector2_t& csPhi,
250 const value_type& phi) {
251 5 value_type t, u, v, Lmin = d.rsLength, L;
252
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 5 if (LpRmL(xy, csPhi, phi, t, u, v) &&
253 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
254 ✗ setupPath(d, 0, t, u, v);
255 ✗ Lmin = L;
256 }
257
8/14
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 5 times.
 6 if (LpRmL(xy.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u, v) &&
258
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
259 {
260 ✗ setupPath(d, 0, -t, -u, -v);
261 ✗ Lmin = L;
262 }
263
7/14
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 5 times.
 5 if (LpRmL(xy.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u, v) &&
264 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
265 {
266 ✗ setupPath(d, 1, t, u, v);
267 ✗ Lmin = L;
268 }
269
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 3 times.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 8 if (LpRmL(-xy, csPhi, phi, t, u, v) &&
270
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
271 {
272 ✗ setupPath(d, 1, -t, -u, -v);
273 ✗ Lmin = L;
274 }
275
276 // backwards
277
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 vector2_t xyb;
278
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 xyb << xy(0) * csPhi(0) + xy(1) * csPhi(1),
279
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 xy(0) * csPhi(1) - xy(1) * csPhi(0);
280 // value_type xb = xy(0)*csPhi(0) + xy(1)*csPhi(1), yb = xy(0)*csPhi(1) -
281 // xy(1)*csPhi(0);
282
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 5 if (LpRmL(xyb, csPhi, phi, t, u, v) &&
283 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
284 ✗ setupPath(d, 0, v, u, t);
285 ✗ Lmin = L;
286 }
287
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmL(xyb.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
288
3/4
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 6 v) &&
289
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
290 {
291 ✗ setupPath(d, 0, -v, -u, -t);
292 ✗ Lmin = L;
293 }
294
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmL(xyb.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
295
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
296 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
297 {
298 ✗ setupPath(d, 1, v, u, t);
299 ✗ Lmin = L;
300 }
301
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 6 if (LpRmL(-xyb, csPhi, phi, t, u, v) &&
302
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
303 ✗ setupPath(d, 1, -v, -u, -t);
304 5 }
305 // formula 8.7
306 20 inline bool LpRupLumRm(const vector2_t& xy, const vector2_t& csPhi,
307 const value_type& phi, value_type& t, value_type& u,
308 value_type& v) {
309 20 value_type xi = xy(0) + csPhi(1), eta = xy(1) - 1. - csPhi(0),
310 20 rho = .25 * (2. + sqrt(xi * xi + eta * eta));
311
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 20 times.
 20 if (rho <= 1.) {
312 ✗ u = acos(rho);
313 ✗ tauOmega(u, -u, xi, eta, phi, t, v);
314 ✗ assert(fabs(2 * (sin(t) - sin(t - u) + sin(t - 2 * u)) - csPhi(1) - xy(0)) <
315 RS_EPS);
316 ✗ assert(fabs(2 * (-cos(t) + cos(t - u) - cos(t - 2 * u)) + csPhi(0) + 1 -
317 xy(1)) < RS_EPS);
318 ✗ assert(fabs(mod2pi(t - 2 * u - v - phi)) < RS_EPS);
319 ✗ return t >= -ZERO && v <= ZERO;
320 }
321 20 return false;
322 }
323 // formula 8.8
324 20 inline bool LpRumLumRp(const vector2_t& xy, const vector2_t& csPhi,
325 const value_type& phi, value_type& t, value_type& u,
326 value_type& v) {
327 20 value_type xi = xy(0) + csPhi(1), eta = xy(1) - 1. - csPhi(0),
328 20 rho = (20. - xi * xi - eta * eta) / 16.;
329
3/4
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 20 if (rho >= 0 && rho <= 1) {
330 8 u = -acos(rho);
331
1/2
✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
 8 if (u >= -.5 * pi) {
332 8 tauOmega(u, u, xi, eta, phi, t, v);
333
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
 8 assert(fabs(4 * sin(t) - 2 * sin(t - u) - csPhi(1) - xy(0)) < RS_EPS);
334
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
 8 assert(fabs(-4 * cos(t) + 2 * cos(t - u) + csPhi(0) + 1 - xy(1)) <
335 RS_EPS);
336
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
 8 assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
337
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 2 times.
 8 return t >= -ZERO && v >= -ZERO;
338 }
339 }
340 12 return false;
341 }
342
343 5 void CCCC(Data& d, const vector2_t& xy, const vector2_t& csPhi,
344 const value_type& phi) {
345 5 value_type t, u, v, Lmin = d.rsLength, L;
346
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 5 if (LpRupLumRm(xy, csPhi, phi, t, u, v) &&
347 ✗ Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) {
348 ✗ setupPath(d, 2, t, u, -u, v);
349 ✗ Lmin = L;
350 }
351
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRupLumRm(xy.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
352
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
353 ✗ Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
354 {
355 ✗ setupPath(d, 2, -t, -u, u, -v);
356 ✗ Lmin = L;
357 }
358
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRupLumRm(xy.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
359
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
360 ✗ Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
361 {
362 ✗ setupPath(d, 3, t, u, -u, v);
363 ✗ Lmin = L;
364 }
365
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 5 if (LpRupLumRm(-xy, csPhi, phi, t, u, v) &&
366 ✗ Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
367 {
368 ✗ setupPath(d, 3, -t, -u, u, -v);
369 ✗ Lmin = L;
370 }
371
372
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 5 if (LpRumLumRp(xy, csPhi, phi, t, u, v) &&
373 ✗ Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) {
374 ✗ setupPath(d, 2, t, u, u, v);
375 ✗ Lmin = L;
376 }
377
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRumLumRp(xy.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
378
3/4
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 6 v) &&
379
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip
380 {
381 ✗ setupPath(d, 2, -t, -u, -u, -v);
382 ✗ Lmin = L;
383 }
384
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRumLumRp(xy.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
385
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
386 ✗ Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // reflect
387 {
388 ✗ setupPath(d, 3, t, u, u, v);
389 ✗ Lmin = L;
390 }
391
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 6 if (LpRumLumRp(-xy, csPhi, phi, t, u, v) &&
392
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v))) // timeflip + reflect
393 ✗ setupPath(d, 3, -t, -u, -u, -v);
394 5 }
395 // formula 8.9
396 40 inline bool LpRmSmLm(const vector2_t& xy, const vector2_t& csPhi,
397 const value_type& phi, value_type& t, value_type& u,
398 value_type& v) {
399
4/8
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 40 times.
✗ Branch 11 not taken.
 40 value_type xi = xy(0) - csPhi(1), eta = xy(1) - 1. + csPhi(0), rho, theta;
400 40 polar(xi, eta, rho, theta);
401
1/2
✓ Branch 0 taken 40 times.
✗ Branch 1 not taken.
 40 if (rho >= 2.) {
402 40 value_type r = sqrt(rho * rho - 4.);
403 40 u = 2. - r;
404 40 t = mod2pi(theta + atan2(r, -2.));
405 40 v = mod2pi(phi - .5 * pi - t);
406
3/6
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 40 times.
 40 assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + csPhi(1) - xy(0)) <
407 RS_EPS);
408
3/6
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 40 times.
 40 assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - csPhi(0) + 1 - xy(1)) <
409 RS_EPS);
410
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 40 times.
 40 assert(fabs(mod2pi(t + pi / 2 + v - phi)) < RS_EPS);
411
6/6
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 7 times.
✓ Branch 5 taken 9 times.
 40 return t >= -ZERO && u <= ZERO && v <= ZERO;
412 }
413 ✗ return false;
414 }
415 // formula 8.10
416 40 inline bool LpRmSmRm(const vector2_t& xy, const vector2_t& csPhi,
417 const value_type& phi, value_type& t, value_type& u,
418 value_type& v) {
419
4/8
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 40 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 40 times.
✗ Branch 11 not taken.
 40 value_type xi = xy(0) + csPhi(1), eta = xy(1) - 1. - csPhi(0), rho, theta;
420 40 polar(-eta, xi, rho, theta);
421
1/2
✓ Branch 0 taken 40 times.
✗ Branch 1 not taken.
 40 if (rho >= 2.) {
422 40 t = theta;
423 40 u = 2. - rho;
424 40 v = mod2pi(t + .5 * pi - phi);
425
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 40 times.
 40 assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - xy(0)) < RS_EPS);
426
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 40 times.
 40 assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - xy(1)) < RS_EPS);
427
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 40 times.
 40 assert(fabs(mod2pi(t + pi / 2 - v - phi)) < RS_EPS);
428
5/6
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 14 times.
 40 return t >= -ZERO && u <= ZERO && v <= ZERO;
429 }
430 ✗ return false;
431 }
432 5 void CCSC(Data& d, const vector2_t& xy, const vector2_t& csPhi,
433 const value_type& phi) {
434 5 value_type t, u, v, Lmin = d.rsLength - .5 * pi, L;
435
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 5 if (LpRmSmLm(xy, csPhi, phi, t, u, v) &&
436 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
437 ✗ setupPath(d, 4, t, -.5 * pi, u, v);
438 ✗ Lmin = L;
439 }
440
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSmLm(xy.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
441
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
442 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
443 {
444 ✗ setupPath(d, 4, -t, .5 * pi, -u, -v);
445 ✗ Lmin = L;
446 }
447
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSmLm(xy.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
448
4/4
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 4 times.
 6 v) &&
449
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
450 {
451
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 setupPath(d, 5, t, -.5 * pi, u, v);
452 1 Lmin = L;
453 }
454
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 7 if (LpRmSmLm(-xy, csPhi, phi, t, u, v) &&
455
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
456 {
457 ✗ setupPath(d, 5, -t, .5 * pi, -u, -v);
458 ✗ Lmin = L;
459 }
460
461
4/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 6 if (LpRmSmRm(xy, csPhi, phi, t, u, v) &&
462
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
463 ✗ setupPath(d, 8, t, -.5 * pi, u, v);
464 ✗ Lmin = L;
465 }
466
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSmRm(xy.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
467
3/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 7 v) &&
468
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
469 {
470 ✗ setupPath(d, 8, -t, .5 * pi, -u, -v);
471 ✗ Lmin = L;
472 }
473
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSmRm(xy.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
474
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
475 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
476 {
477 ✗ setupPath(d, 9, t, -.5 * pi, u, v);
478 ✗ Lmin = L;
479 }
480
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1 times.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 6 if (LpRmSmRm(-xy, csPhi, phi, t, u, v) &&
481
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
482 {
483 ✗ setupPath(d, 9, -t, .5 * pi, -u, -v);
484 ✗ Lmin = L;
485 }
486
487 // backwards
488
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 vector2_t xyb;
489
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 xyb << xy(0) * csPhi(0) + xy(1) * csPhi(1),
490
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 xy(0) * csPhi(1) - xy(1) * csPhi(0);
491 // std::cout << xyb << std::endl;
492 // value_type xb = xy(0)*csPhi(0) + xy(1)*csPhi(1), yb = xy(0)*csPhi(1) -
493 // xy(1)*csPhi(0); std::cout << xy(0)*csPhi(0) + xy(1)*csPhi(1) << " " <<
494 // xy(0)*csPhi(1) - xy(1)*csPhi(0) << std::endl;
495
5/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 3 times.
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 3 times.
 7 if (LpRmSmLm(xyb, csPhi, phi, t, u, v) &&
496
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
497
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 setupPath(d, 6, v, u, -.5 * pi, t);
498 2 Lmin = L;
499 }
500
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSmLm(xyb.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
501
4/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 4 times.
 7 v) &&
502
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
503 {
504
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 setupPath(d, 6, -v, -u, .5 * pi, -t);
505 1 Lmin = L;
506 }
507
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSmLm(xyb.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
508
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
509 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
510 {
511 ✗ setupPath(d, 7, v, u, -.5 * pi, t);
512 ✗ Lmin = L;
513 }
514
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 5 if (LpRmSmLm(-xyb, csPhi, phi, t, u, v) &&
515 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
516 {
517 ✗ setupPath(d, 7, -v, -u, .5 * pi, -t);
518 ✗ Lmin = L;
519 }
520
521
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 5 if (LpRmSmRm(xyb, csPhi, phi, t, u, v) &&
522 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
523 ✗ setupPath(d, 10, v, u, -.5 * pi, t);
524 ✗ Lmin = L;
525 }
526
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSmRm(xyb.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
527
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
528 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
529 {
530 ✗ setupPath(d, 10, -v, -u, .5 * pi, -t);
531 ✗ Lmin = L;
532 }
533
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSmRm(xyb.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
534
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
535 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
536 {
537 ✗ setupPath(d, 11, v, u, -.5 * pi, t);
538 ✗ Lmin = L;
539 }
540
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2 times.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 7 if (LpRmSmRm(-xyb, csPhi, phi, t, u, v) &&
541
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
542 ✗ setupPath(d, 11, -v, -u, .5 * pi, -t);
543 5 }
544 // formula 8.11 *** TYPO IN PAPER ***
545 20 inline bool LpRmSLmRp(const vector2_t& xy, const vector2_t& csPhi,
546 const value_type& phi, value_type& t, value_type& u,
547 value_type& v) {
548
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
 20 value_type xi = xy(0) + csPhi(1), eta = xy(1) - 1. - csPhi(0), rho, theta;
549 20 polar(xi, eta, rho, theta);
550
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 20 if (rho >= 2.) {
551 20 u = 4. - sqrt(rho * rho - 4.);
552
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 8 times.
 20 if (u <= ZERO) {
553 12 t = mod2pi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
554 12 v = mod2pi(t - phi);
555
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 12 times.
 12 assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - csPhi(1) - xy(0)) <
556 RS_EPS);
557
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 12 times.
 12 assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + csPhi(0) + 1 -
558 xy(1)) < RS_EPS);
559
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
 12 assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
560
4/4
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 2 times.
 12 return t >= -ZERO && v >= -ZERO;
561 }
562 }
563 8 return false;
564 }
565 5 void CCSCC(Data& d, const vector2_t& xy, const vector2_t& csPhi,
566 const value_type& phi) {
567 5 value_type t, u, v, Lmin = d.rsLength - pi, L;
568
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
 5 if (LpRmSLmRp(xy, csPhi, phi, t, u, v) &&
569 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) {
570 ✗ setupPath(d, 16, t, -.5 * pi, u, -.5 * pi, v);
571 ✗ Lmin = L;
572 }
573
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSLmRp(xy.cwiseProduct(oneMP), csPhi.cwiseProduct(onePM), -phi, t, u,
574
3/4
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 6 v) &&
575
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
576 {
577 ✗ setupPath(d, 16, -t, .5 * pi, -u, .5 * pi, -v);
578 ✗ Lmin = L;
579 }
580
5/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
 5 if (LpRmSLmRp(xy.cwiseProduct(onePM), csPhi.cwiseProduct(onePM), -phi, t, u,
581
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 v) &&
582 ✗ Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
583 {
584 ✗ setupPath(d, 17, t, -.5 * pi, u, -.5 * pi, v);
585 ✗ Lmin = L;
586 }
587
6/10
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 3 times.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 12 taken 5 times.
 8 if (LpRmSLmRp(-xy, csPhi, phi, t, u, v) &&
588
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
 3 Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
589 ✗ setupPath(d, 17, -t, .5 * pi, -u, .5 * pi, -v);
590 5 }
591
592 inline value_type meanBounds(const JointPtr_t& j, const size_type& i) {
593 return (j->upperBound(i) + j->lowerBound(i)) / 2;
594 }
595
596 inline value_type saturate(const value_type& v, const JointPtr_t& j,
597 const size_type& i) {
598 return std::min(j->upperBound(i), std::max(j->lowerBound(i), v));
599 }
600
601 } // namespace
602 namespace steeringMethod {
603 6 PathVectorPtr_t reedsSheppPathOrDistance(
604 const DevicePtr_t& device, ConfigurationIn_t init, ConfigurationIn_t end,
605 value_type extraLength, value_type rho, size_type xyId, size_type rzId,
606 const std::vector<JointPtr_t> wheels, ConstraintSetPtr_t constraints,
607 bool computeDistance, value_type& distance) {
608
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 Data d(rho);
609 6 PathVectorPtr_t res;
610 6 distance = 0;
611
1/2
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
 6 if (!computeDistance)
612
3/6
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
 6 res = PathVector::create(device->configSize(), device->numberDof());
613 // Find rank of translation and rotation in velocity vectors
614 // Hypothesis: degrees of freedom all belong to a planar joint or
615 // xyId_ belong to a tranlation joint, rzId belongs to a SO2 joint.
616
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 JointPtr_t xy(device->getJointAtConfigRank(xyId));
617
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 JointPtr_t rz(device->getJointAtConfigRank(rzId));
618 // rotate
619
3/6
✓ 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.
 6 vector2_t XY = rotate(end.segment<2>(xyId) - init.segment<2>(xyId),
620
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 init.segment<2>(rzId));
621
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 XY /= d.rho;
622
3/6
✓ 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.
 6 vector2_t csPhi = rotate(end.segment<2>(rzId), init.segment<2>(rzId));
623
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 value_type phi = atan2(csPhi(1), csPhi(0));
624
625
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 Configuration_t qInit(init), qEnd(device->configSize());
626
627
3/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 5 times.
 6 if (XY.squaredNorm() + phi * phi < 1e-8) {
628
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (computeDistance) {
629 ✗ distance = extraLength;
630 } else {
631 ConstantCurvaturePtr_t segment(
632 ConstantCurvature::create(device, qInit, end, 0, extraLength, 0, xyId,
633
4/8
✓ 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.
 2 rzId, rz, wheels, ConstraintSetPtr_t()));
634
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 res->appendPath(segment);
635 1 }
636 1 return res;
637 }
638
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 CSC(d, XY, csPhi, phi);
639
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 CCC(d, XY, csPhi, phi);
640
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 CCCC(d, XY, csPhi, phi);
641
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 CCSC(d, XY, csPhi, phi);
642
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 CCSCC(d, XY, csPhi, phi);
643 // build path vector
644 5 value_type L(d.rsLength), s(0.);
645
2/2
✓ Branch 0 taken 25 times.
✓ Branch 1 taken 5 times.
 30 for (unsigned int i = 0; i < 5; ++i) {
646
3/4
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 9 times.
 25 if (fabs(d.lengths[i]) > precision) {
647
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 value_type l = d.rho * fabs(d.lengths[i]);
648 16 s += l;
649
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (types[d.typeId][i] == RS_NOP) break;
650 value_type curvature;
651
3/4
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 16 switch (types[d.typeId][i]) {
652 8 case RS_LEFT:
653 8 curvature = 1. / d.rho;
654 8 break;
655 3 case RS_RIGHT:
656 3 curvature = -1. / d.rho;
657 3 break;
658 5 case RS_STRAIGHT:
659 5 curvature = 0;
660 5 break;
661 ✗ case RS_NOP:
662 default:
663 ✗ abort();
664 }
665
4/8
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
 16 pinocchio::interpolate(device, init, end, s / L, qEnd);
666
1/2
✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
 16 if (!computeDistance) {
667 ConstantCurvaturePtr_t segment(
668 32 ConstantCurvature::create(device, qInit, qEnd, d.rho * d.lengths[i],
669 16 l * (1 + extraLength / L), curvature,
670
5/10
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 16 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 16 times.
✗ Branch 15 not taken.
 32 xyId, rzId, rz, wheels, constraints));
671
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 res->appendPath(segment);
672
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 16 qInit = segment->end();
673 16 }
674 }
675 }
676
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (computeDistance) {
677 ✗ distance = d.rsLength + extraLength;
678 } else {
679
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 5 times.
 5 assert(res->numberPaths() > 0);
680 }
681 5 return res;
682 6 }
683 } // namespace steeringMethod
684 } // namespace core
685 } // namespace hpp
686