GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2011-2014, Willow Garage, Inc. |
||
5 |
* Copyright (c) 2014-2015, Open Source Robotics Foundation |
||
6 |
* All rights reserved. |
||
7 |
* |
||
8 |
* Redistribution and use in source and binary forms, with or without |
||
9 |
* modification, are permitted provided that the following conditions |
||
10 |
* are met: |
||
11 |
* |
||
12 |
* * Redistributions of source code must retain the above copyright |
||
13 |
* notice, this list of conditions and the following disclaimer. |
||
14 |
* * Redistributions in binary form must reproduce the above |
||
15 |
* copyright notice, this list of conditions and the following |
||
16 |
* disclaimer in the documentation and/or other materials provided |
||
17 |
* with the distribution. |
||
18 |
* * Neither the name of Open Source Robotics Foundation nor the names of its |
||
19 |
* contributors may be used to endorse or promote products derived |
||
20 |
* from this software without specific prior written permission. |
||
21 |
* |
||
22 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
23 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
24 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
25 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
26 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
27 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
28 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
29 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
30 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
31 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
32 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
33 |
* POSSIBILITY OF SUCH DAMAGE. |
||
34 |
*/ |
||
35 |
|||
36 |
/** \author Jia Pan */ |
||
37 |
|||
38 |
#include <hpp/fcl/BV/RSS.h> |
||
39 |
#include <hpp/fcl/BVH/BVH_utility.h> |
||
40 |
#include <hpp/fcl/internal/tools.h> |
||
41 |
#include <hpp/fcl/collision_data.h> |
||
42 |
|||
43 |
#include <iostream> |
||
44 |
|||
45 |
namespace hpp { |
||
46 |
namespace fcl { |
||
47 |
|||
48 |
/// @brief Clip value between a and b |
||
49 |
3472014 |
void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b) { |
|
50 |
✓✓ | 3472014 |
if (val < a) |
51 |
1279535 |
val = a; |
|
52 |
✓✓ | 2192479 |
else if (val > b) |
53 |
1309020 |
val = b; |
|
54 |
3472014 |
} |
|
55 |
|||
56 |
/// @brief Finds the parameters t & u corresponding to the two closest points on |
||
57 |
/// a pair of line segments. The first segment is defined as Pa + A*t, 0 <= t <= |
||
58 |
/// a, where "Pa" is one endpoint of the segment, "A" is a unit vector pointing |
||
59 |
/// to the other endpoint, and t is a scalar that produces all the points |
||
60 |
/// between the two endpoints. Since "A" is a unit vector, "a" is the segment's |
||
61 |
/// length. The second segment is defined as Pb + B*u, 0 <= u <= b. Many of the |
||
62 |
/// terms needed by the algorithm are already computed for other purposes,so we |
||
63 |
/// just pass these terms into the function instead of complete specifications |
||
64 |
/// of each segment. "T" in the dot products is the vector betweeen Pa and Pb. |
||
65 |
/// Reference: "On fast computation of distance between line segments." Vladimir |
||
66 |
/// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. |
||
67 |
1455870 |
void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, |
|
68 |
FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T) { |
||
69 |
1455870 |
FCL_REAL denom = 1 - A_dot_B * A_dot_B; |
|
70 |
|||
71 |
✓✓ | 1455870 |
if (denom == 0) |
72 |
91931 |
t = 0; |
|
73 |
else { |
||
74 |
1363939 |
t = (A_dot_T - B_dot_T * A_dot_B) / denom; |
|
75 |
1363939 |
clipToRange(t, 0, a); |
|
76 |
} |
||
77 |
|||
78 |
1455870 |
u = t * A_dot_B - B_dot_T; |
|
79 |
✓✓ | 1455870 |
if (u < 0) { |
80 |
608582 |
u = 0; |
|
81 |
608582 |
t = A_dot_T; |
|
82 |
608582 |
clipToRange(t, 0, a); |
|
83 |
✓✓ | 847288 |
} else if (u > b) { |
84 |
645925 |
u = b; |
|
85 |
645925 |
t = u * A_dot_B + A_dot_T; |
|
86 |
645925 |
clipToRange(t, 0, a); |
|
87 |
} |
||
88 |
1455870 |
} |
|
89 |
|||
90 |
/// @brief Returns whether the nearest point on rectangle edge |
||
91 |
/// Pb + B*u, 0 <= u <= b, to the rectangle edge, |
||
92 |
/// Pa + A*t, 0 <= t <= a, is within the half space |
||
93 |
/// determined by the point Pa and the direction Anorm. |
||
94 |
/// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. |
||
95 |
426814 |
bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, |
|
96 |
FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, |
||
97 |
FCL_REAL B_dot_T) { |
||
98 |
✓✓ | 426814 |
if (fabs(Anorm_dot_B) < 1e-7) return false; |
99 |
|||
100 |
FCL_REAL t, u, v; |
||
101 |
|||
102 |
426784 |
u = -Anorm_dot_T / Anorm_dot_B; |
|
103 |
✓✗ | 426784 |
clipToRange(u, 0, b); |
104 |
|||
105 |
426784 |
t = u * A_dot_B + A_dot_T; |
|
106 |
✓✗ | 426784 |
clipToRange(t, 0, a); |
107 |
|||
108 |
426784 |
v = t * A_dot_B - B_dot_T; |
|
109 |
|||
110 |
✓✓ | 426784 |
if (Anorm_dot_B > 0) { |
111 |
✓✓ | 211677 |
if (v > (u + 1e-7)) return true; |
112 |
} else { |
||
113 |
✓✓ | 215107 |
if (v < (u - 1e-7)) return true; |
114 |
} |
||
115 |
255506 |
return false; |
|
116 |
} |
||
117 |
|||
118 |
/// @brief Distance between two oriented rectangles; P and Q (optional return |
||
119 |
/// values) are the closest points in the rectangles, both are in the local |
||
120 |
/// frame of the first rectangle. |
||
121 |
1517715 |
FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, |
|
122 |
const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, |
||
123 |
Vec3f* Q = NULL) { |
||
124 |
FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1; |
||
125 |
|||
126 |
✓✗ | 1517715 |
A0_dot_B0 = Rab(0, 0); |
127 |
✓✗ | 1517715 |
A0_dot_B1 = Rab(0, 1); |
128 |
✓✗ | 1517715 |
A1_dot_B0 = Rab(1, 0); |
129 |
✓✗ | 1517715 |
A1_dot_B1 = Rab(1, 1); |
130 |
|||
131 |
FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1; |
||
132 |
FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1; |
||
133 |
|||
134 |
1517715 |
aA0_dot_B0 = a[0] * A0_dot_B0; |
|
135 |
1517715 |
aA0_dot_B1 = a[0] * A0_dot_B1; |
|
136 |
1517715 |
aA1_dot_B0 = a[1] * A1_dot_B0; |
|
137 |
1517715 |
aA1_dot_B1 = a[1] * A1_dot_B1; |
|
138 |
1517715 |
bA0_dot_B0 = b[0] * A0_dot_B0; |
|
139 |
1517715 |
bA1_dot_B0 = b[0] * A1_dot_B0; |
|
140 |
1517715 |
bA0_dot_B1 = b[1] * A0_dot_B1; |
|
141 |
1517715 |
bA1_dot_B1 = b[1] * A1_dot_B1; |
|
142 |
|||
143 |
✓✗✓✗ ✓✗ |
1517715 |
Vec3f Tba(Rab.transpose() * Tab); |
144 |
|||
145 |
✓✗ | 1517715 |
Vec3f S; |
146 |
FCL_REAL t, u; |
||
147 |
|||
148 |
// determine if any edge pair contains the closest points |
||
149 |
|||
150 |
FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x; |
||
151 |
FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x; |
||
152 |
FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux; |
||
153 |
|||
154 |
✓✗ | 1517715 |
ALL_x = -Tba[0]; |
155 |
1517715 |
ALU_x = ALL_x + aA1_dot_B0; |
|
156 |
1517715 |
AUL_x = ALL_x + aA0_dot_B0; |
|
157 |
1517715 |
AUU_x = ALU_x + aA0_dot_B0; |
|
158 |
|||
159 |
✓✓ | 1517715 |
if (ALL_x < ALU_x) { |
160 |
800461 |
LA1_lx = ALL_x; |
|
161 |
800461 |
LA1_ux = ALU_x; |
|
162 |
800461 |
UA1_lx = AUL_x; |
|
163 |
800461 |
UA1_ux = AUU_x; |
|
164 |
} else { |
||
165 |
717254 |
LA1_lx = ALU_x; |
|
166 |
717254 |
LA1_ux = ALL_x; |
|
167 |
717254 |
UA1_lx = AUU_x; |
|
168 |
717254 |
UA1_ux = AUL_x; |
|
169 |
} |
||
170 |
|||
171 |
✓✗ | 1517715 |
BLL_x = Tab[0]; |
172 |
1517715 |
BLU_x = BLL_x + bA0_dot_B1; |
|
173 |
1517715 |
BUL_x = BLL_x + bA0_dot_B0; |
|
174 |
1517715 |
BUU_x = BLU_x + bA0_dot_B0; |
|
175 |
|||
176 |
✓✓ | 1517715 |
if (BLL_x < BLU_x) { |
177 |
810036 |
LB1_lx = BLL_x; |
|
178 |
810036 |
LB1_ux = BLU_x; |
|
179 |
810036 |
UB1_lx = BUL_x; |
|
180 |
810036 |
UB1_ux = BUU_x; |
|
181 |
} else { |
||
182 |
707679 |
LB1_lx = BLU_x; |
|
183 |
707679 |
LB1_ux = BLL_x; |
|
184 |
707679 |
UB1_lx = BUU_x; |
|
185 |
707679 |
UB1_ux = BUL_x; |
|
186 |
} |
||
187 |
|||
188 |
// UA1, UB1 |
||
189 |
|||
190 |
✓✓✓✓ |
1517715 |
if ((UA1_ux > b[0]) && (UB1_ux > a[0])) { |
191 |
742881 |
if (((UA1_lx > b[0]) || |
|
192 |
✓✗✓✓ |
18155 |
inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], A1_dot_B1, |
193 |
✓✓✓✗ ✓✗✓✗ ✓✓ |
724726 |
aA0_dot_B1 - Tba[1], -Tab[1] - bA1_dot_B0)) && |
194 |
✓✓ | 352534 |
((UB1_lx > a[0]) || |
195 |
✓✗✓✓ |
24101 |
inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], A1_dot_B1, |
196 |
✓✗✓✗ ✓✗ |
24101 |
Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) { |
197 |
✓✗ | 338063 |
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, |
198 |
✓✗✓✗ |
338063 |
Tba[1] - aA0_dot_B1); |
199 |
|||
200 |
✓✗✓✗ ✓✗✓✗ |
338063 |
S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0]; |
201 |
✓✗✓✗ ✓✗✓✗ |
338063 |
S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; |
202 |
✓✗✓✗ ✓✗✓✗ |
338063 |
S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; |
203 |
|||
204 |
✗✓✗✗ |
338063 |
if (P && Q) { |
205 |
*P << a[0], t, 0; |
||
206 |
*Q = S + (*P); |
||
207 |
} |
||
208 |
|||
209 |
✓✗ | 338063 |
return S.norm(); |
210 |
} |
||
211 |
} |
||
212 |
|||
213 |
// UA1, LB1 |
||
214 |
|||
215 |
✓✓✓✓ |
1179652 |
if ((UA1_lx < 0) && (LB1_ux > a[0])) { |
216 |
✓✗✓✓ |
17823 |
if (((UA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0, |
217 |
✓✓✓✗ ✓✗✓✗ ✓✓ |
606408 |
A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) && |
218 |
✓✓ | 292077 |
((LB1_lx > a[0]) || |
219 |
✓✗✓✗ ✓✓ |
21707 |
inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], A1_dot_B1, Tab[1], |
220 |
✓✗✓✗ |
21707 |
Tba[1] - aA0_dot_B1))) { |
221 |
✓✗✓✗ ✓✗ |
282672 |
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1); |
222 |
|||
223 |
✓✗✓✗ ✓✗ |
282672 |
S[0] = Tab[0] + Rab(0, 1) * u - a[0]; |
224 |
✓✗✓✗ ✓✗ |
282672 |
S[1] = Tab[1] + Rab(1, 1) * u - t; |
225 |
✓✗✓✗ ✓✗ |
282672 |
S[2] = Tab[2] + Rab(2, 1) * u; |
226 |
|||
227 |
✗✓✗✗ |
282672 |
if (P && Q) { |
228 |
*P << a[0], t, 0; |
||
229 |
*Q = S + (*P); |
||
230 |
} |
||
231 |
|||
232 |
✓✗ | 282672 |
return S.norm(); |
233 |
} |
||
234 |
} |
||
235 |
|||
236 |
// LA1, UB1 |
||
237 |
|||
238 |
✓✓✓✓ |
896980 |
if ((LA1_ux > b[0]) && (UB1_lx < 0)) { |
239 |
616767 |
if (((LA1_lx > b[0]) || |
|
240 |
✓✗✓✗ ✓✓ |
17085 |
inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], A1_dot_B1, -Tba[1], |
241 |
✓✓✓✗ ✓✗✓✓ ✓✓ |
340879 |
-Tab[1] - bA1_dot_B0)) && |
242 |
✓✗✓✓ |
23953 |
((UB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0, |
243 |
✓✗✓✗ ✓✗ |
23953 |
A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) { |
244 |
✓✗✓✗ ✓✗ |
280279 |
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]); |
245 |
|||
246 |
✓✗✓✗ ✓✗✓✗ |
280279 |
S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u; |
247 |
✓✗✓✗ ✓✗✓✗ |
280279 |
S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t; |
248 |
✓✗✓✗ ✓✗✓✗ |
280279 |
S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; |
249 |
|||
250 |
✗✓✗✗ |
280279 |
if (P && Q) { |
251 |
*P << 0, t, 0; |
||
252 |
*Q = S + (*P); |
||
253 |
} |
||
254 |
|||
255 |
✓✗ | 280279 |
return S.norm(); |
256 |
} |
||
257 |
} |
||
258 |
|||
259 |
// LA1, LB1 |
||
260 |
|||
261 |
✓✓✓✓ |
616701 |
if ((LA1_lx < 0) && (LB1_lx < 0)) { |
262 |
✓✗✓✓ |
17067 |
if (((LA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1, |
263 |
✓✓✓✗ ✓✗✓✗ ✓✓✓✓ |
327637 |
-Tba[1], -Tab[1])) && |
264 |
✓✗✓✓ |
16087 |
((LB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1, |
265 |
✓✗✓✗ ✓✗ |
16087 |
Tab[1], Tba[1]))) { |
266 |
✓✗✓✗ ✓✗ |
273097 |
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]); |
267 |
|||
268 |
✓✗✓✗ ✓✗ |
273097 |
S[0] = Tab[0] + Rab(0, 1) * u; |
269 |
✓✗✓✗ ✓✗ |
273097 |
S[1] = Tab[1] + Rab(1, 1) * u - t; |
270 |
✓✗✓✗ ✓✗ |
273097 |
S[2] = Tab[2] + Rab(2, 1) * u; |
271 |
|||
272 |
✗✓✗✗ |
273097 |
if (P && Q) { |
273 |
*P << 0, t, 0; |
||
274 |
*Q = S + (*P); |
||
275 |
} |
||
276 |
|||
277 |
✓✗ | 273097 |
return S.norm(); |
278 |
} |
||
279 |
} |
||
280 |
|||
281 |
FCL_REAL ALL_y, ALU_y, AUL_y, AUU_y; |
||
282 |
|||
283 |
✓✗ | 343604 |
ALL_y = -Tba[1]; |
284 |
343604 |
ALU_y = ALL_y + aA1_dot_B1; |
|
285 |
343604 |
AUL_y = ALL_y + aA0_dot_B1; |
|
286 |
343604 |
AUU_y = ALU_y + aA0_dot_B1; |
|
287 |
|||
288 |
FCL_REAL LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux; |
||
289 |
|||
290 |
✓✓ | 343604 |
if (ALL_y < ALU_y) { |
291 |
182370 |
LA1_ly = ALL_y; |
|
292 |
182370 |
LA1_uy = ALU_y; |
|
293 |
182370 |
UA1_ly = AUL_y; |
|
294 |
182370 |
UA1_uy = AUU_y; |
|
295 |
} else { |
||
296 |
161234 |
LA1_ly = ALU_y; |
|
297 |
161234 |
LA1_uy = ALL_y; |
|
298 |
161234 |
UA1_ly = AUU_y; |
|
299 |
161234 |
UA1_uy = AUL_y; |
|
300 |
} |
||
301 |
|||
302 |
✓✓ | 343604 |
if (BLL_x < BUL_x) { |
303 |
179023 |
LB0_lx = BLL_x; |
|
304 |
179023 |
LB0_ux = BUL_x; |
|
305 |
179023 |
UB0_lx = BLU_x; |
|
306 |
179023 |
UB0_ux = BUU_x; |
|
307 |
} else { |
||
308 |
164581 |
LB0_lx = BUL_x; |
|
309 |
164581 |
LB0_ux = BLL_x; |
|
310 |
164581 |
UB0_lx = BUU_x; |
|
311 |
164581 |
UB0_ux = BLU_x; |
|
312 |
} |
||
313 |
|||
314 |
// UA1, UB0 |
||
315 |
|||
316 |
✓✓✓✓ |
343604 |
if ((UA1_uy > b[1]) && (UB0_ux > a[0])) { |
317 |
92372 |
if (((UA1_ly > b[1]) || |
|
318 |
✓✗✓✓ |
6462 |
inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], A1_dot_B0, |
319 |
✓✓✓✗ ✓✗✓✗ ✓✓ |
85910 |
aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) && |
320 |
✓✓ | 39743 |
((UB0_lx > a[0]) || |
321 |
✓✗✓✓ |
27281 |
inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, A1_dot_B0, |
322 |
✓✗✓✗ ✓✗ |
27281 |
Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) { |
323 |
✓✗ | 16120 |
segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, |
324 |
✓✗✓✗ |
16120 |
Tba[0] - aA0_dot_B0); |
325 |
|||
326 |
✓✗✓✗ ✓✗✓✗ |
16120 |
S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0]; |
327 |
✓✗✓✗ ✓✗✓✗ |
16120 |
S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; |
328 |
✓✗✓✗ ✓✗✓✗ |
16120 |
S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; |
329 |
|||
330 |
✗✓✗✗ |
16120 |
if (P && Q) { |
331 |
*P << a[0], t, 0; |
||
332 |
*Q = S + (*P); |
||
333 |
} |
||
334 |
|||
335 |
✓✗ | 16120 |
return S.norm(); |
336 |
} |
||
337 |
} |
||
338 |
|||
339 |
// UA1, LB0 |
||
340 |
|||
341 |
✓✓✓✓ |
327484 |
if ((UA1_ly < 0) && (LB0_ux > a[0])) { |
342 |
✓✗✓✓ |
7345 |
if (((UA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1, |
343 |
✓✓✓✗ ✓✗✓✗ ✓✓ |
107868 |
A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1])) && |
344 |
✓✓ | 50929 |
((LB0_lx > a[0]) || |
345 |
✓✗✓✗ ✓✓ |
22371 |
inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], A1_dot_B0, Tab[1], |
346 |
✓✗✓✗ |
22371 |
Tba[0] - aA0_dot_B0))) { |
347 |
✓✗✓✗ ✓✗ |
33936 |
segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0); |
348 |
|||
349 |
✓✗✓✗ ✓✗ |
33936 |
S[0] = Tab[0] + Rab(0, 0) * u - a[0]; |
350 |
✓✗✓✗ ✓✗ |
33936 |
S[1] = Tab[1] + Rab(1, 0) * u - t; |
351 |
✓✗✓✗ ✓✗ |
33936 |
S[2] = Tab[2] + Rab(2, 0) * u; |
352 |
|||
353 |
✗✓✗✗ |
33936 |
if (P && Q) { |
354 |
*P << a[0], t, 0; |
||
355 |
*Q = S + (*P); |
||
356 |
} |
||
357 |
|||
358 |
✓✗ | 33936 |
return S.norm(); |
359 |
} |
||
360 |
} |
||
361 |
|||
362 |
// LA1, UB0 |
||
363 |
|||
364 |
✓✓✓✓ |
293548 |
if ((LA1_uy > b[1]) && (UB0_lx < 0)) { |
365 |
77867 |
if (((LA1_ly > b[1]) || |
|
366 |
✓✗✓✗ ✓✓ |
7029 |
inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], A1_dot_B0, -Tba[0], |
367 |
✓✓✓✗ ✓✗✓✓ ✓✓ |
62936 |
-Tab[1] - bA1_dot_B1)) && |
368 |
|||
369 |
✓✗✓✓ |
20488 |
((UB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1, |
370 |
✓✗✓✗ ✓✗ |
20488 |
A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]))) { |
371 |
✓✗✓✗ ✓✗ |
17214 |
segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]); |
372 |
|||
373 |
✓✗✓✗ ✓✗✓✗ |
17214 |
S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u; |
374 |
✓✗✓✗ ✓✗✓✗ |
17214 |
S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t; |
375 |
✓✗✓✗ ✓✗✓✗ |
17214 |
S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; |
376 |
|||
377 |
✗✓✗✗ |
17214 |
if (P && Q) { |
378 |
*P << 0, t, 0; |
||
379 |
*Q = S + (*P); |
||
380 |
} |
||
381 |
|||
382 |
✓✗ | 17214 |
return S.norm(); |
383 |
} |
||
384 |
} |
||
385 |
|||
386 |
// LA1, LB0 |
||
387 |
|||
388 |
✓✓✓✓ |
276334 |
if ((LA1_ly < 0) && (LB0_lx < 0)) { |
389 |
✓✗✓✓ |
6756 |
if (((LA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0, |
390 |
✓✓✓✗ ✓✗✓✗ ✓✓✓✓ |
105225 |
-Tba[0], -Tab[1])) && |
391 |
✓✗✓✓ |
26389 |
((LB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0, |
392 |
✓✗✓✗ ✓✗ |
26389 |
Tab[1], Tba[0]))) { |
393 |
✓✗✓✗ ✓✗ |
47935 |
segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]); |
394 |
|||
395 |
✓✗✓✗ ✓✗ |
47935 |
S[0] = Tab[0] + Rab(0, 0) * u; |
396 |
✓✗✓✗ ✓✗ |
47935 |
S[1] = Tab[1] + Rab(1, 0) * u - t; |
397 |
✓✗✓✗ ✓✗ |
47935 |
S[2] = Tab[2] + Rab(2, 0) * u; |
398 |
|||
399 |
✗✓✗✗ |
47935 |
if (P && Q) { |
400 |
*P << 0, t, 0; |
||
401 |
*Q = S + (*P); |
||
402 |
} |
||
403 |
|||
404 |
✓✗ | 47935 |
return S.norm(); |
405 |
} |
||
406 |
} |
||
407 |
|||
408 |
FCL_REAL BLL_y, BLU_y, BUL_y, BUU_y; |
||
409 |
|||
410 |
✓✗ | 228399 |
BLL_y = Tab[1]; |
411 |
228399 |
BLU_y = BLL_y + bA1_dot_B1; |
|
412 |
228399 |
BUL_y = BLL_y + bA1_dot_B0; |
|
413 |
228399 |
BUU_y = BLU_y + bA1_dot_B0; |
|
414 |
|||
415 |
FCL_REAL LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy; |
||
416 |
|||
417 |
✓✓ | 228399 |
if (ALL_x < AUL_x) { |
418 |
118187 |
LA0_lx = ALL_x; |
|
419 |
118187 |
LA0_ux = AUL_x; |
|
420 |
118187 |
UA0_lx = ALU_x; |
|
421 |
118187 |
UA0_ux = AUU_x; |
|
422 |
} else { |
||
423 |
110212 |
LA0_lx = AUL_x; |
|
424 |
110212 |
LA0_ux = ALL_x; |
|
425 |
110212 |
UA0_lx = AUU_x; |
|
426 |
110212 |
UA0_ux = ALU_x; |
|
427 |
} |
||
428 |
|||
429 |
✓✓ | 228399 |
if (BLL_y < BLU_y) { |
430 |
123882 |
LB1_ly = BLL_y; |
|
431 |
123882 |
LB1_uy = BLU_y; |
|
432 |
123882 |
UB1_ly = BUL_y; |
|
433 |
123882 |
UB1_uy = BUU_y; |
|
434 |
} else { |
||
435 |
104517 |
LB1_ly = BLU_y; |
|
436 |
104517 |
LB1_uy = BLL_y; |
|
437 |
104517 |
UB1_ly = BUU_y; |
|
438 |
104517 |
UB1_uy = BUL_y; |
|
439 |
} |
||
440 |
|||
441 |
// UA0, UB1 |
||
442 |
|||
443 |
✓✓✓✓ |
228399 |
if ((UA0_ux > b[0]) && (UB1_uy > a[1])) { |
444 |
104618 |
if (((UA0_lx > b[0]) || |
|
445 |
✓✗✓✓ |
16148 |
inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], A0_dot_B1, |
446 |
✓✓✓✗ ✓✗✓✗ ✓✓ |
88470 |
aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) && |
447 |
✓✓ | 34915 |
((UB1_ly > a[1]) || |
448 |
✓✗✓✓ |
4036 |
inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, A0_dot_B1, |
449 |
✓✗✓✗ ✓✗ |
4036 |
Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) { |
450 |
✓✗ | 32897 |
segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, |
451 |
✓✗✓✗ |
32897 |
Tba[1] - aA1_dot_B1); |
452 |
|||
453 |
✓✗✓✗ ✓✗✓✗ |
32897 |
S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; |
454 |
✓✗✓✗ ✓✗✓✗ |
32897 |
S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1]; |
455 |
✓✗✓✗ ✓✗✓✗ |
32897 |
S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; |
456 |
|||
457 |
✗✓✗✗ |
32897 |
if (P && Q) { |
458 |
*P << t, a[1], 0; |
||
459 |
*Q = S + (*P); |
||
460 |
} |
||
461 |
|||
462 |
✓✗ | 32897 |
return S.norm(); |
463 |
} |
||
464 |
} |
||
465 |
|||
466 |
// UA0, LB1 |
||
467 |
|||
468 |
✓✓✓✓ |
195502 |
if ((UA0_lx < 0) && (LB1_uy > a[1])) { |
469 |
✓✗✓✓ |
15781 |
if (((UA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0, |
470 |
✓✓✓✗ ✓✗✓✗ ✓✓ |
87146 |
A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0])) && |
471 |
✓✓ | 34487 |
((LB1_ly > a[1]) || |
472 |
✓✗✓✗ ✓✓ |
4891 |
inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0], |
473 |
✓✗✓✗ |
4891 |
Tba[1] - aA1_dot_B1))) { |
474 |
✓✗✓✗ ✓✗ |
33023 |
segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1); |
475 |
|||
476 |
✓✗✓✗ ✓✗ |
33023 |
S[0] = Tab[0] + Rab(0, 1) * u - t; |
477 |
✓✗✓✗ ✓✗ |
33023 |
S[1] = Tab[1] + Rab(1, 1) * u - a[1]; |
478 |
✓✗✓✗ ✓✗ |
33023 |
S[2] = Tab[2] + Rab(2, 1) * u; |
479 |
|||
480 |
✗✓✗✗ |
33023 |
if (P && Q) { |
481 |
*P << t, a[1], 0; |
||
482 |
*Q = S + (*P); |
||
483 |
} |
||
484 |
|||
485 |
✓✗ | 33023 |
return S.norm(); |
486 |
} |
||
487 |
} |
||
488 |
|||
489 |
// LA0, UB1 |
||
490 |
|||
491 |
✓✓✓✓ |
162479 |
if ((LA0_ux > b[0]) && (UB1_ly < 0)) { |
492 |
138032 |
if (((LA0_lx > b[0]) || |
|
493 |
✓✗✓✗ ✓✓ |
17512 |
inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1], |
494 |
✓✓✓✗ ✓✗✓✓ ✓✓ |
82966 |
-bA0_dot_B0 - Tab[0])) && |
495 |
✓✗✓✓ |
5194 |
((UB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0, |
496 |
✓✗✓✗ ✓✗ |
5194 |
A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]))) { |
497 |
✓✗✓✗ ✓✗ |
49423 |
segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]); |
498 |
|||
499 |
✓✗✓✗ ✓✗✓✗ |
49423 |
S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t; |
500 |
✓✗✓✗ ✓✗✓✗ |
49423 |
S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u; |
501 |
✓✗✓✗ ✓✗✓✗ |
49423 |
S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u; |
502 |
|||
503 |
✗✓✗✗ |
49423 |
if (P && Q) { |
504 |
*P << t, 0, 0; |
||
505 |
*Q = S + (*P); |
||
506 |
} |
||
507 |
|||
508 |
✓✗ | 49423 |
return S.norm(); |
509 |
} |
||
510 |
} |
||
511 |
|||
512 |
// LA0, LB1 |
||
513 |
|||
514 |
✓✓✓✓ |
113056 |
if ((LA0_lx < 0) && (LB1_ly < 0)) { |
515 |
✓✗✓✓ |
14958 |
if (((LA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1, |
516 |
✓✓✓✗ ✓✗✓✗ ✓✓✓✓ |
59051 |
-Tba[1], -Tab[0])) && |
517 |
✓✗✓✓ |
3470 |
((LB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1, |
518 |
✓✗✓✗ ✓✗ |
3470 |
Tab[0], Tba[1]))) { |
519 |
✓✗✓✗ ✓✗ |
31845 |
segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]); |
520 |
|||
521 |
✓✗✓✗ ✓✗ |
31845 |
S[0] = Tab[0] + Rab(0, 1) * u - t; |
522 |
✓✗✓✗ ✓✗ |
31845 |
S[1] = Tab[1] + Rab(1, 1) * u; |
523 |
✓✗✓✗ ✓✗ |
31845 |
S[2] = Tab[2] + Rab(2, 1) * u; |
524 |
|||
525 |
✗✓✗✗ |
31845 |
if (P && Q) { |
526 |
*P << t, 0, 0; |
||
527 |
*Q = S + (*P); |
||
528 |
} |
||
529 |
|||
530 |
✓✗ | 31845 |
return S.norm(); |
531 |
} |
||
532 |
} |
||
533 |
|||
534 |
FCL_REAL LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy; |
||
535 |
|||
536 |
✓✓ | 81211 |
if (ALL_y < AUL_y) { |
537 |
41447 |
LA0_ly = ALL_y; |
|
538 |
41447 |
LA0_uy = AUL_y; |
|
539 |
41447 |
UA0_ly = ALU_y; |
|
540 |
41447 |
UA0_uy = AUU_y; |
|
541 |
} else { |
||
542 |
39764 |
LA0_ly = AUL_y; |
|
543 |
39764 |
LA0_uy = ALL_y; |
|
544 |
39764 |
UA0_ly = AUU_y; |
|
545 |
39764 |
UA0_uy = ALU_y; |
|
546 |
} |
||
547 |
|||
548 |
✓✓ | 81211 |
if (BLL_y < BUL_y) { |
549 |
42198 |
LB0_ly = BLL_y; |
|
550 |
42198 |
LB0_uy = BUL_y; |
|
551 |
42198 |
UB0_ly = BLU_y; |
|
552 |
42198 |
UB0_uy = BUU_y; |
|
553 |
} else { |
||
554 |
39013 |
LB0_ly = BUL_y; |
|
555 |
39013 |
LB0_uy = BLL_y; |
|
556 |
39013 |
UB0_ly = BUU_y; |
|
557 |
39013 |
UB0_uy = BLU_y; |
|
558 |
} |
||
559 |
|||
560 |
// UA0, UB0 |
||
561 |
|||
562 |
✓✓✓✓ |
81211 |
if ((UA0_uy > b[1]) && (UB0_uy > a[1])) { |
563 |
47699 |
if (((UA0_ly > b[1]) || |
|
564 |
✓✗✓✓ |
12919 |
inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], A0_dot_B0, |
565 |
✓✓✓✗ ✓✗✓✗ ✓✓ |
34780 |
aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) && |
566 |
✓✓ | 9978 |
((UB0_ly > a[1]) || |
567 |
✓✗✓✓ |
7946 |
inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0, |
568 |
✓✗✓✗ ✓✗ |
7946 |
Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) { |
569 |
✓✗ | 4118 |
segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, |
570 |
✓✗✓✗ |
4118 |
Tba[0] - aA1_dot_B0); |
571 |
|||
572 |
✓✗✓✗ ✓✗✓✗ |
4118 |
S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; |
573 |
✓✗✓✗ ✓✗✓✗ |
4118 |
S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1]; |
574 |
✓✗✓✗ ✓✗✓✗ |
4118 |
S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; |
575 |
|||
576 |
✗✓✗✗ |
4118 |
if (P && Q) { |
577 |
*P << t, a[1], 0; |
||
578 |
*Q = S + (*P); |
||
579 |
} |
||
580 |
|||
581 |
✓✗ | 4118 |
return S.norm(); |
582 |
} |
||
583 |
} |
||
584 |
|||
585 |
// UA0, LB0 |
||
586 |
|||
587 |
✓✓✓✓ |
77093 |
if ((UA0_ly < 0) && (LB0_uy > a[1])) { |
588 |
✓✗✓✓ |
9363 |
if (((UA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1, |
589 |
✓✓✓✗ ✓✗✓✗ ✓✓ |
32736 |
A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0])) && |
590 |
✓✓ | 10982 |
((LB0_ly > a[1]) || |
591 |
✓✗✓✗ ✓✓ |
5550 |
inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], A0_dot_B0, Tab[0], |
592 |
✓✗✓✗ |
5550 |
Tba[0] - aA1_dot_B0))) { |
593 |
✓✗✓✗ ✓✗ |
7588 |
segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0); |
594 |
|||
595 |
✓✗✓✗ ✓✗ |
7588 |
S[0] = Tab[0] + Rab(0, 0) * u - t; |
596 |
✓✗✓✗ ✓✗ |
7588 |
S[1] = Tab[1] + Rab(1, 0) * u - a[1]; |
597 |
✓✗✓✗ ✓✗ |
7588 |
S[2] = Tab[2] + Rab(2, 0) * u; |
598 |
|||
599 |
✗✓✗✗ |
7588 |
if (P && Q) { |
600 |
*P << t, a[1], 0; |
||
601 |
*Q = S + (*P); |
||
602 |
} |
||
603 |
|||
604 |
✓✗ | 7588 |
return S.norm(); |
605 |
} |
||
606 |
} |
||
607 |
|||
608 |
// LA0, UB0 |
||
609 |
|||
610 |
✓✓✓✓ |
69505 |
if ((LA0_uy > b[1]) && (UB0_ly < 0)) { |
611 |
37980 |
if (((LA0_ly > b[1]) || |
|
612 |
✓✗✓✗ ✓✓ |
10312 |
inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0], |
613 |
✓✓✓✗ ✓✗✓✓ ✓✓ |
30037 |
-Tab[0] - bA0_dot_B1)) && |
614 |
|||
615 |
✓✗✓✓ |
5891 |
((UB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1, |
616 |
✓✗✓✗ ✓✗ |
5891 |
A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]))) { |
617 |
✓✗✓✗ ✓✗ |
4557 |
segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]); |
618 |
|||
619 |
✓✗✓✗ ✓✗✓✗ |
4557 |
S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t; |
620 |
✓✗✓✗ ✓✗✓✗ |
4557 |
S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u; |
621 |
✓✗✓✗ ✓✗✓✗ |
4557 |
S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u; |
622 |
|||
623 |
✗✓✗✗ |
4557 |
if (P && Q) { |
624 |
*P << t, 0, 0; |
||
625 |
*Q = S + (*P); |
||
626 |
} |
||
627 |
|||
628 |
✓✗ | 4557 |
return S.norm(); |
629 |
} |
||
630 |
} |
||
631 |
|||
632 |
// LA0, LB0 |
||
633 |
|||
634 |
✓✓✓✓ |
64948 |
if ((LA0_ly < 0) && (LB0_ly < 0)) { |
635 |
✓✗✓✓ |
8203 |
if (((LA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0, |
636 |
✓✓✓✗ ✓✗✓✗ ✓✓✓✓ |
23588 |
-Tba[0], -Tab[0])) && |
637 |
✓✗✓✓ |
4541 |
((LB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0, |
638 |
✓✗✓✗ ✓✗ |
4541 |
Tab[0], Tba[0]))) { |
639 |
✓✗✓✗ ✓✗ |
3103 |
segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]); |
640 |
|||
641 |
✓✗✓✗ ✓✗ |
3103 |
S[0] = Tab[0] + Rab(0, 0) * u - t; |
642 |
✓✗✓✗ ✓✗ |
3103 |
S[1] = Tab[1] + Rab(1, 0) * u; |
643 |
✓✗✓✗ ✓✗ |
3103 |
S[2] = Tab[2] + Rab(2, 0) * u; |
644 |
|||
645 |
✗✓✗✗ |
3103 |
if (P && Q) { |
646 |
*P << t, 0, 0; |
||
647 |
*Q = S + (*P); |
||
648 |
} |
||
649 |
|||
650 |
✓✗ | 3103 |
return S.norm(); |
651 |
} |
||
652 |
} |
||
653 |
|||
654 |
// no edges passed, take max separation along face normals |
||
655 |
|||
656 |
FCL_REAL sep1, sep2; |
||
657 |
|||
658 |
✓✗✓✓ |
61845 |
if (Tab[2] > 0.0) { |
659 |
✓✗ | 32491 |
sep1 = Tab[2]; |
660 |
✓✗✓✓ ✓✗ |
32491 |
if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0); |
661 |
✓✗✓✓ ✓✗ |
32491 |
if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1); |
662 |
} else { |
||
663 |
✓✗ | 29354 |
sep1 = -Tab[2]; |
664 |
✓✗✓✓ ✓✗ |
29354 |
if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0); |
665 |
✓✗✓✓ ✓✗ |
29354 |
if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1); |
666 |
} |
||
667 |
|||
668 |
✓✗✓✓ |
61845 |
if (Tba[2] < 0) { |
669 |
✓✗ | 32049 |
sep2 = -Tba[2]; |
670 |
✓✗✓✓ ✓✗ |
32049 |
if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2); |
671 |
✓✗✓✓ ✓✗ |
32049 |
if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2); |
672 |
} else { |
||
673 |
✓✗ | 29796 |
sep2 = Tba[2]; |
674 |
✓✗✓✓ ✓✗ |
29796 |
if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2); |
675 |
✓✗✓✓ ✓✗ |
29796 |
if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2); |
676 |
} |
||
677 |
|||
678 |
✓✓✓✓ |
61845 |
if (sep1 >= sep2 && sep1 >= 0) { |
679 |
✓✗✓✓ |
32743 |
if (Tab[2] > 0) |
680 |
✓✗✓✗ ✓✗ |
16666 |
S << 0, 0, sep1; |
681 |
else |
||
682 |
✓✗✓✗ ✓✗ |
16077 |
S << 0, 0, -sep1; |
683 |
|||
684 |
✗✓✗✗ |
32743 |
if (P && Q) { |
685 |
*Q = S; |
||
686 |
P->setZero(); |
||
687 |
} |
||
688 |
} |
||
689 |
|||
690 |
✓✓✓✓ |
61845 |
if (sep2 >= sep1 && sep2 >= 0) { |
691 |
✓✗✓✗ ✓✗✓✗ |
14119 |
Vec3f Q_(Tab[0], Tab[1], Tab[2]); |
692 |
✓✗ | 14119 |
Vec3f P_; |
693 |
✓✗✓✓ |
14119 |
if (Tba[2] < 0) { |
694 |
✓✗✓✗ ✓✗ |
7955 |
P_[0] = Rab(0, 2) * sep2 + Tab[0]; |
695 |
✓✗✓✗ ✓✗ |
7955 |
P_[1] = Rab(1, 2) * sep2 + Tab[1]; |
696 |
✓✗✓✗ ✓✗ |
7955 |
P_[2] = Rab(2, 2) * sep2 + Tab[2]; |
697 |
} else { |
||
698 |
✓✗✓✗ ✓✗ |
6164 |
P_[0] = -Rab(0, 2) * sep2 + Tab[0]; |
699 |
✓✗✓✗ ✓✗ |
6164 |
P_[1] = -Rab(1, 2) * sep2 + Tab[1]; |
700 |
✓✗✓✗ ✓✗ |
6164 |
P_[2] = -Rab(2, 2) * sep2 + Tab[2]; |
701 |
} |
||
702 |
|||
703 |
✓✗✓✗ |
14119 |
S = Q_ - P_; |
704 |
|||
705 |
✗✓✗✗ |
14119 |
if (P && Q) { |
706 |
*P = P_; |
||
707 |
*Q = Q_; |
||
708 |
} |
||
709 |
} |
||
710 |
|||
711 |
✓✓ | 61845 |
FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2); |
712 |
✓✓ | 61845 |
return (sep > 0 ? sep : 0); |
713 |
} |
||
714 |
|||
715 |
15645 |
bool RSS::overlap(const RSS& other) const { |
|
716 |
/// compute what transform [R,T] that takes us from cs1 to cs2. |
||
717 |
/// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] |
||
718 |
/// First compute the rotation part, then translation part |
||
719 |
|||
720 |
/// Then compute R1'(T2 - T1) |
||
721 |
✓✗✓✗ ✓✗✓✗ |
15645 |
Vec3f T(axes.transpose() * (other.Tr - Tr)); |
722 |
|||
723 |
/// Now compute R1'R2 |
||
724 |
✓✗✓✗ ✓✗ |
15645 |
Matrix3f R(axes.transpose() * other.axes); |
725 |
|||
726 |
✓✗ | 15645 |
FCL_REAL dist = rectDistance(R, T, length, other.length); |
727 |
15645 |
return (dist <= (radius + other.radius)); |
|
728 |
} |
||
729 |
|||
730 |
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, |
||
731 |
const RSS& b2) { |
||
732 |
// ROb2 = R0 . b2 |
||
733 |
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] |
||
734 |
|||
735 |
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] |
||
736 |
// R = b2^T RO^T b1 |
||
737 |
Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); |
||
738 |
Vec3f T(b1.axes.transpose() * Ttemp); |
||
739 |
Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); |
||
740 |
|||
741 |
FCL_REAL dist = rectDistance(R, T, b1.length, b2.length); |
||
742 |
return (dist <= (b1.radius + b2.radius)); |
||
743 |
} |
||
744 |
|||
745 |
9048 |
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, |
|
746 |
const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { |
||
747 |
// ROb2 = R0 . b2 |
||
748 |
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] |
||
749 |
|||
750 |
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0] |
||
751 |
// R = b2^T RO^T b1 |
||
752 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
9048 |
Vec3f Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr); |
753 |
✓✗✓✗ ✓✗ |
9048 |
Vec3f T(b1.axes.transpose() * Ttemp); |
754 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
9048 |
Matrix3f R(b1.axes.transpose() * R0.transpose() * b2.axes); |
755 |
|||
756 |
✓✗ | 9048 |
FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - |
757 |
9048 |
b2.radius - request.security_margin; |
|
758 |
✓✓ | 9048 |
if (dist <= 0) return true; |
759 |
3952 |
sqrDistLowerBound = dist * dist; |
|
760 |
3952 |
return false; |
|
761 |
} |
||
762 |
|||
763 |
bool RSS::contain(const Vec3f& p) const { |
||
764 |
Vec3f local_p = p - Tr; |
||
765 |
// FIXME: Vec3f proj (axes.transpose() * local_p); |
||
766 |
FCL_REAL proj0 = local_p.dot(axes.col(0)); |
||
767 |
FCL_REAL proj1 = local_p.dot(axes.col(1)); |
||
768 |
FCL_REAL proj2 = local_p.dot(axes.col(2)); |
||
769 |
FCL_REAL abs_proj2 = fabs(proj2); |
||
770 |
Vec3f proj(proj0, proj1, proj2); |
||
771 |
|||
772 |
/// projection is within the rectangle |
||
773 |
if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && |
||
774 |
(proj1 > 0)) { |
||
775 |
return (abs_proj2 < radius); |
||
776 |
} else if ((proj0 < length[0]) && (proj0 > 0) && |
||
777 |
((proj1 < 0) || (proj1 > length[1]))) { |
||
778 |
FCL_REAL y = (proj1 > 0) ? length[1] : 0; |
||
779 |
Vec3f v(proj0, y, 0); |
||
780 |
return ((proj - v).squaredNorm() < radius * radius); |
||
781 |
} else if ((proj1 < length[1]) && (proj1 > 0) && |
||
782 |
((proj0 < 0) || (proj0 > length[0]))) { |
||
783 |
FCL_REAL x = (proj0 > 0) ? length[0] : 0; |
||
784 |
Vec3f v(x, proj1, 0); |
||
785 |
return ((proj - v).squaredNorm() < radius * radius); |
||
786 |
} else { |
||
787 |
FCL_REAL x = (proj0 > 0) ? length[0] : 0; |
||
788 |
FCL_REAL y = (proj1 > 0) ? length[1] : 0; |
||
789 |
Vec3f v(x, y, 0); |
||
790 |
return ((proj - v).squaredNorm() < radius * radius); |
||
791 |
} |
||
792 |
} |
||
793 |
|||
794 |
RSS& RSS::operator+=(const Vec3f& p) { |
||
795 |
Vec3f local_p = p - Tr; |
||
796 |
FCL_REAL proj0 = local_p.dot(axes.col(0)); |
||
797 |
FCL_REAL proj1 = local_p.dot(axes.col(1)); |
||
798 |
FCL_REAL proj2 = local_p.dot(axes.col(2)); |
||
799 |
FCL_REAL abs_proj2 = fabs(proj2); |
||
800 |
Vec3f proj(proj0, proj1, proj2); |
||
801 |
|||
802 |
// projection is within the rectangle |
||
803 |
if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && |
||
804 |
(proj1 > 0)) { |
||
805 |
if (abs_proj2 < radius) |
||
806 |
; // do nothing |
||
807 |
else { |
||
808 |
radius = 0.5 * (radius + abs_proj2); // enlarge the r |
||
809 |
// change RSS origin position |
||
810 |
if (proj2 > 0) |
||
811 |
Tr[2] += 0.5 * (abs_proj2 - radius); |
||
812 |
else |
||
813 |
Tr[2] -= 0.5 * (abs_proj2 - radius); |
||
814 |
} |
||
815 |
} else if ((proj0 < length[0]) && (proj0 > 0) && |
||
816 |
((proj1 < 0) || (proj1 > length[1]))) { |
||
817 |
FCL_REAL y = (proj1 > 0) ? length[1] : 0; |
||
818 |
Vec3f v(proj0, y, 0); |
||
819 |
FCL_REAL new_r_sqr = (proj - v).squaredNorm(); |
||
820 |
if (new_r_sqr < radius * radius) |
||
821 |
; // do nothing |
||
822 |
else { |
||
823 |
if (abs_proj2 < radius) { |
||
824 |
FCL_REAL delta_y = |
||
825 |
-std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y); |
||
826 |
length[1] += delta_y; |
||
827 |
if (proj1 < 0) Tr[1] -= delta_y; |
||
828 |
} else { |
||
829 |
FCL_REAL delta_y = fabs(proj1 - y); |
||
830 |
length[1] += delta_y; |
||
831 |
if (proj1 < 0) Tr[1] -= delta_y; |
||
832 |
|||
833 |
if (proj2 > 0) |
||
834 |
Tr[2] += 0.5 * (abs_proj2 - radius); |
||
835 |
else |
||
836 |
Tr[2] -= 0.5 * (abs_proj2 - radius); |
||
837 |
} |
||
838 |
} |
||
839 |
} else if ((proj1 < length[1]) && (proj1 > 0) && |
||
840 |
((proj0 < 0) || (proj0 > length[0]))) { |
||
841 |
FCL_REAL x = (proj0 > 0) ? length[0] : 0; |
||
842 |
Vec3f v(x, proj1, 0); |
||
843 |
FCL_REAL new_r_sqr = (proj - v).squaredNorm(); |
||
844 |
if (new_r_sqr < radius * radius) |
||
845 |
; // do nothing |
||
846 |
else { |
||
847 |
if (abs_proj2 < radius) { |
||
848 |
FCL_REAL delta_x = |
||
849 |
-std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x); |
||
850 |
length[0] += delta_x; |
||
851 |
if (proj0 < 0) Tr[0] -= delta_x; |
||
852 |
} else { |
||
853 |
FCL_REAL delta_x = fabs(proj0 - x); |
||
854 |
length[0] += delta_x; |
||
855 |
if (proj0 < 0) Tr[0] -= delta_x; |
||
856 |
|||
857 |
if (proj2 > 0) |
||
858 |
Tr[2] += 0.5 * (abs_proj2 - radius); |
||
859 |
else |
||
860 |
Tr[2] -= 0.5 * (abs_proj2 - radius); |
||
861 |
} |
||
862 |
} |
||
863 |
} else { |
||
864 |
FCL_REAL x = (proj0 > 0) ? length[0] : 0; |
||
865 |
FCL_REAL y = (proj1 > 0) ? length[1] : 0; |
||
866 |
Vec3f v(x, y, 0); |
||
867 |
FCL_REAL new_r_sqr = (proj - v).squaredNorm(); |
||
868 |
if (new_r_sqr < radius * radius) |
||
869 |
; // do nothing |
||
870 |
else { |
||
871 |
if (abs_proj2 < radius) { |
||
872 |
FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2); |
||
873 |
FCL_REAL delta_diag = |
||
874 |
-std::sqrt(radius * radius - proj2 * proj2) + diag; |
||
875 |
|||
876 |
FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x); |
||
877 |
FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y); |
||
878 |
length[0] += delta_x; |
||
879 |
length[1] += delta_y; |
||
880 |
|||
881 |
if (proj0 < 0 && proj1 < 0) { |
||
882 |
Tr[0] -= delta_x; |
||
883 |
Tr[1] -= delta_y; |
||
884 |
} |
||
885 |
} else { |
||
886 |
FCL_REAL delta_x = fabs(proj0 - x); |
||
887 |
FCL_REAL delta_y = fabs(proj1 - y); |
||
888 |
|||
889 |
length[0] += delta_x; |
||
890 |
length[1] += delta_y; |
||
891 |
|||
892 |
if (proj0 < 0 && proj1 < 0) { |
||
893 |
Tr[0] -= delta_x; |
||
894 |
Tr[1] -= delta_y; |
||
895 |
} |
||
896 |
|||
897 |
if (proj2 > 0) |
||
898 |
Tr[2] += 0.5 * (abs_proj2 - radius); |
||
899 |
else |
||
900 |
Tr[2] -= 0.5 * (abs_proj2 - radius); |
||
901 |
} |
||
902 |
} |
||
903 |
} |
||
904 |
|||
905 |
return *this; |
||
906 |
} |
||
907 |
|||
908 |
13074 |
RSS RSS::operator+(const RSS& other) const { |
|
909 |
✓✗ | 13074 |
RSS bv; |
910 |
|||
911 |
✓✓✓✗ |
222258 |
Vec3f v[16]; |
912 |
✓✗✓✗ ✓✗ |
13074 |
Vec3f d0_pos(other.axes.col(0) * (other.length[0] + other.radius)); |
913 |
✓✗✓✗ ✓✗ |
13074 |
Vec3f d1_pos(other.axes.col(1) * (other.length[1] + other.radius)); |
914 |
✓✗✓✗ ✓✗ |
13074 |
Vec3f d0_neg(other.axes.col(0) * (-other.radius)); |
915 |
✓✗✓✗ ✓✗ |
13074 |
Vec3f d1_neg(other.axes.col(1) * (-other.radius)); |
916 |
✓✗✓✗ ✓✗ |
13074 |
Vec3f d2_pos(other.axes.col(2) * other.radius); |
917 |
✓✗✓✗ ✓✗ |
13074 |
Vec3f d2_neg(other.axes.col(2) * (-other.radius)); |
918 |
|||
919 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos; |
920 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg; |
921 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[2].noalias() = other.Tr + d0_pos + d1_neg + d2_pos; |
922 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[3].noalias() = other.Tr + d0_pos + d1_neg + d2_neg; |
923 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[4].noalias() = other.Tr + d0_neg + d1_pos + d2_pos; |
924 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[5].noalias() = other.Tr + d0_neg + d1_pos + d2_neg; |
925 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[6].noalias() = other.Tr + d0_neg + d1_neg + d2_pos; |
926 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[7].noalias() = other.Tr + d0_neg + d1_neg + d2_neg; |
927 |
|||
928 |
✓✗✓✗ ✓✗✓✗ |
13074 |
d0_pos.noalias() = axes.col(0) * (length[0] + radius); |
929 |
✓✗✓✗ ✓✗✓✗ |
13074 |
d1_pos.noalias() = axes.col(1) * (length[1] + radius); |
930 |
✓✗✓✗ ✓✗✓✗ |
13074 |
d0_neg.noalias() = axes.col(0) * (-radius); |
931 |
✓✗✓✗ ✓✗✓✗ |
13074 |
d1_neg.noalias() = axes.col(1) * (-radius); |
932 |
✓✗✓✗ ✓✗✓✗ |
13074 |
d2_pos.noalias() = axes.col(2) * radius; |
933 |
✓✗✓✗ ✓✗✓✗ |
13074 |
d2_neg.noalias() = axes.col(2) * (-radius); |
934 |
|||
935 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[8].noalias() = Tr + d0_pos + d1_pos + d2_pos; |
936 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[9].noalias() = Tr + d0_pos + d1_pos + d2_neg; |
937 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[10].noalias() = Tr + d0_pos + d1_neg + d2_pos; |
938 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[11].noalias() = Tr + d0_pos + d1_neg + d2_neg; |
939 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[12].noalias() = Tr + d0_neg + d1_pos + d2_pos; |
940 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[13].noalias() = Tr + d0_neg + d1_pos + d2_neg; |
941 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos; |
942 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg; |
943 |
|||
944 |
✓✗ | 13074 |
Matrix3f M; // row first matrix |
945 |
✓✓✓✗ |
52296 |
Vec3f E[3]; // row first eigen-vectors |
946 |
13074 |
Matrix3f::Scalar s[3] = {0, 0, 0}; |
|
947 |
|||
948 |
✓✗ | 13074 |
getCovariance(v, NULL, NULL, NULL, 16, M); |
949 |
✓✗ | 13074 |
eigen(M, s, E); |
950 |
|||
951 |
int min, mid, max; |
||
952 |
✓✓ | 13074 |
if (s[0] > s[1]) { |
953 |
1269 |
max = 0; |
|
954 |
1269 |
min = 1; |
|
955 |
} else { |
||
956 |
11805 |
min = 0; |
|
957 |
11805 |
max = 1; |
|
958 |
} |
||
959 |
✓✓ | 13074 |
if (s[2] < s[min]) { |
960 |
5051 |
mid = min; |
|
961 |
5051 |
min = 2; |
|
962 |
✓✓ | 8023 |
} else if (s[2] > s[max]) { |
963 |
916 |
mid = max; |
|
964 |
916 |
max = 2; |
|
965 |
} else { |
||
966 |
7107 |
mid = 2; |
|
967 |
} |
||
968 |
|||
969 |
// column first matrix, as the axis in RSS |
||
970 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
bv.axes.col(0) << E[0][max], E[1][max], E[2][max]; |
971 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
bv.axes.col(1) << E[0][mid], E[1][mid], E[2][mid]; |
972 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
13074 |
bv.axes.col(2) << E[1][max] * E[2][mid] - E[1][mid] * E[2][max], |
973 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
E[0][mid] * E[2][max] - E[0][max] * E[2][mid], |
974 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13074 |
E[0][max] * E[1][mid] - E[0][mid] * E[1][max]; |
975 |
|||
976 |
// set rss origin, rectangle size and radius |
||
977 |
13074 |
getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, |
|
978 |
✓✗ | 13074 |
bv.length, bv.radius); |
979 |
|||
980 |
26148 |
return bv; |
|
981 |
} |
||
982 |
|||
983 |
964 |
FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const { |
|
984 |
// compute what transform [R,T] that takes us from cs1 to cs2. |
||
985 |
// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)] |
||
986 |
// First compute the rotation part, then translation part |
||
987 |
✓✗✓✗ ✓✗ |
964 |
Matrix3f R(axes.transpose() * other.axes); |
988 |
✓✗✓✗ ✓✗✓✗ |
964 |
Vec3f T(axes.transpose() * (other.Tr - Tr)); |
989 |
|||
990 |
✓✗ | 964 |
FCL_REAL dist = rectDistance(R, T, length, other.length, P, Q); |
991 |
964 |
dist -= (radius + other.radius); |
|
992 |
✓✓ | 964 |
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; |
993 |
} |
||
994 |
|||
995 |
1492058 |
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, |
|
996 |
const RSS& b2, Vec3f* P, Vec3f* Q) { |
||
997 |
✓✗✓✗ ✓✗✓✗ |
1492058 |
Matrix3f R(b1.axes.transpose() * R0 * b2.axes); |
998 |
✓✗✓✗ ✓✗✓✗ |
1492058 |
Vec3f Ttemp(R0 * b2.Tr + T0 - b1.Tr); |
999 |
|||
1000 |
✓✗✓✗ ✓✗ |
1492058 |
Vec3f T(b1.axes.transpose() * Ttemp); |
1001 |
|||
1002 |
✓✗ | 1492058 |
FCL_REAL dist = rectDistance(R, T, b1.length, b2.length, P, Q); |
1003 |
1492058 |
dist -= (b1.radius + b2.radius); |
|
1004 |
✓✓ | 1492058 |
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist; |
1005 |
} |
||
1006 |
|||
1007 |
RSS translate(const RSS& bv, const Vec3f& t) { |
||
1008 |
RSS res(bv); |
||
1009 |
res.Tr += t; |
||
1010 |
return res; |
||
1011 |
} |
||
1012 |
|||
1013 |
} // namespace fcl |
||
1014 |
|||
1015 |
} // namespace hpp |
Generated by: GCOVR (Version 4.2) |