Directory: | ./ |
---|---|
File: | src/reeds-shepp-path.cc |
Date: | 2024-08-10 11:29:48 |
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 |