GCC Code Coverage Report

Directory: ./
File: src/BV/RSS.cpp
Date: 2025-05-02 10:16:21
Exec Total Coverage
Lines: 448 595 75.3%
Branches: 958 2044 46.9%
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 "coal/BV/RSS.h"
39 #include "coal/BVH/BVH_utility.h"
40 #include "coal/internal/tools.h"
41 #include "coal/collision_data.h"
42
43 #include <iostream>
44
45 namespace coal {
46
47 /// @brief Clip value between a and b
48 3719480 void clipToRange(Scalar& val, Scalar a, Scalar b) {
49
2/2
✓ Branch 0 taken 1363170 times.
✓ Branch 1 taken 2356310 times.
 3719480 if (val < a)
50 1363170 val = a;
51
2/2
✓ Branch 0 taken 1406331 times.
✓ Branch 1 taken 949979 times.
 2356310 else if (val > b)
52 1406331 val = b;
53 3719480 }
54
55 /// @brief Finds the parameters t & u corresponding to the two closest points on
56 /// a pair of line segments. The first segment is defined as Pa + A*t, 0 <= t <=
57 /// a, where "Pa" is one endpoint of the segment, "A" is a unit vector pointing
58 /// to the other endpoint, and t is a scalar that produces all the points
59 /// between the two endpoints. Since "A" is a unit vector, "a" is the segment's
60 /// length. The second segment is defined as Pb + B*u, 0 <= u <= b. Many of the
61 /// terms needed by the algorithm are already computed for other purposes,so we
62 /// just pass these terms into the function instead of complete specifications
63 /// of each segment. "T" in the dot products is the vector betweeen Pa and Pb.
64 /// Reference: "On fast computation of distance between line segments." Vladimir
65 /// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985.
66 1556383 void segCoords(Scalar& t, Scalar& u, Scalar a, Scalar b, Scalar A_dot_B,
67 Scalar A_dot_T, Scalar B_dot_T) {
68 1556383 Scalar denom = 1 - A_dot_B * A_dot_B;
69
70
2/2
✓ Branch 0 taken 91889 times.
✓ Branch 1 taken 1464494 times.
 1556383 if (denom == 0)
71 91889 t = 0;
72 else {
73 1464494 t = (A_dot_T - B_dot_T * A_dot_B) / denom;
74 1464494 clipToRange(t, 0, a);
75 }
76
77 1556383 u = t * A_dot_B - B_dot_T;
78
2/2
✓ Branch 0 taken 650048 times.
✓ Branch 1 taken 906335 times.
 1556383 if (u < 0) {
79 650048 u = 0;
80 650048 t = A_dot_T;
81 650048 clipToRange(t, 0, a);
82
2/2
✓ Branch 0 taken 685598 times.
✓ Branch 1 taken 220737 times.
 906335 } else if (u > b) {
83 685598 u = b;
84 685598 t = u * A_dot_B + A_dot_T;
85 685598 clipToRange(t, 0, a);
86 }
87 1556383 }
88
89 /// @brief Returns whether the nearest point on rectangle edge
90 /// Pb + B*u, 0 <= u <= b, to the rectangle edge,
91 /// Pa + A*t, 0 <= t <= a, is within the half space
92 /// determined by the point Pa and the direction Anorm.
93 /// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb.
94 459700 bool inVoronoi(Scalar a, Scalar b, Scalar Anorm_dot_B, Scalar Anorm_dot_T,
95 Scalar A_dot_B, Scalar A_dot_T, Scalar B_dot_T) {
96
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 459670 times.
 459700 if (fabs(Anorm_dot_B) < 1e-7) return false;
97
98 Scalar t, u, v;
99
100 459670 u = -Anorm_dot_T / Anorm_dot_B;
101
1/2
✓ Branch 1 taken 459670 times.
✗ Branch 2 not taken.
 459670 clipToRange(u, 0, b);
102
103 459670 t = u * A_dot_B + A_dot_T;
104
1/2
✓ Branch 1 taken 459670 times.
✗ Branch 2 not taken.
 459670 clipToRange(t, 0, a);
105
106 459670 v = t * A_dot_B - B_dot_T;
107
108
2/2
✓ Branch 0 taken 228254 times.
✓ Branch 1 taken 231416 times.
 459670 if (Anorm_dot_B > 0) {
109
2/2
✓ Branch 0 taken 99093 times.
✓ Branch 1 taken 129161 times.
 228254 if (v > (u + 1e-7)) return true;
110 } else {
111
2/2
✓ Branch 0 taken 86453 times.
✓ Branch 1 taken 144963 times.
 231416 if (v < (u - 1e-7)) return true;
112 }
113 274124 return false;
114 }
115
116 /// @brief Distance between two oriented rectangles; P and Q (optional return
117 /// values) are the closest points in the rectangles, both are in the local
118 /// frame of the first rectangle.
119 1620713 Scalar rectDistance(const Matrix3s& Rab, Vec3s const& Tab, const Scalar a[2],
120 const Scalar b[2], Vec3s* P = NULL, Vec3s* Q = NULL) {
121 Scalar A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
122
123
1/2
✓ Branch 1 taken 1620713 times.
✗ Branch 2 not taken.
 1620713 A0_dot_B0 = Rab(0, 0);
124
1/2
✓ Branch 1 taken 1620713 times.
✗ Branch 2 not taken.
 1620713 A0_dot_B1 = Rab(0, 1);
125
1/2
✓ Branch 1 taken 1620713 times.
✗ Branch 2 not taken.
 1620713 A1_dot_B0 = Rab(1, 0);
126
1/2
✓ Branch 1 taken 1620713 times.
✗ Branch 2 not taken.
 1620713 A1_dot_B1 = Rab(1, 1);
127
128 Scalar aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
129 Scalar bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
130
131 1620713 aA0_dot_B0 = a[0] * A0_dot_B0;
132 1620713 aA0_dot_B1 = a[0] * A0_dot_B1;
133 1620713 aA1_dot_B0 = a[1] * A1_dot_B0;
134 1620713 aA1_dot_B1 = a[1] * A1_dot_B1;
135 1620713 bA0_dot_B0 = b[0] * A0_dot_B0;
136 1620713 bA1_dot_B0 = b[0] * A1_dot_B0;
137 1620713 bA0_dot_B1 = b[1] * A0_dot_B1;
138 1620713 bA1_dot_B1 = b[1] * A1_dot_B1;
139
140
3/6
✓ Branch 1 taken 1620713 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1620713 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1620713 times.
✗ Branch 8 not taken.
 1620713 Vec3s Tba(Rab.transpose() * Tab);
141
142
1/2
✓ Branch 1 taken 1620713 times.
✗ Branch 2 not taken.
 1620713 Vec3s S;
143 Scalar t, u;
144
145 // determine if any edge pair contains the closest points
146
147 Scalar ALL_x, ALU_x, AUL_x, AUU_x;
148 Scalar BLL_x, BLU_x, BUL_x, BUU_x;
149 Scalar LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
150
151
1/2
✓ Branch 1 taken 1620713 times.
✗ Branch 2 not taken.
 1620713 ALL_x = -Tba[0];
152 1620713 ALU_x = ALL_x + aA1_dot_B0;
153 1620713 AUL_x = ALL_x + aA0_dot_B0;
154 1620713 AUU_x = ALU_x + aA0_dot_B0;
155
156
2/2
✓ Branch 0 taken 857644 times.
✓ Branch 1 taken 763069 times.
 1620713 if (ALL_x < ALU_x) {
157 857644 LA1_lx = ALL_x;
158 857644 LA1_ux = ALU_x;
159 857644 UA1_lx = AUL_x;
160 857644 UA1_ux = AUU_x;
161 } else {
162 763069 LA1_lx = ALU_x;
163 763069 LA1_ux = ALL_x;
164 763069 UA1_lx = AUU_x;
165 763069 UA1_ux = AUL_x;
166 }
167
168
1/2
✓ Branch 1 taken 1620713 times.
✗ Branch 2 not taken.
 1620713 BLL_x = Tab[0];
169 1620713 BLU_x = BLL_x + bA0_dot_B1;
170 1620713 BUL_x = BLL_x + bA0_dot_B0;
171 1620713 BUU_x = BLU_x + bA0_dot_B0;
172
173
2/2
✓ Branch 0 taken 865597 times.
✓ Branch 1 taken 755116 times.
 1620713 if (BLL_x < BLU_x) {
174 865597 LB1_lx = BLL_x;
175 865597 LB1_ux = BLU_x;
176 865597 UB1_lx = BUL_x;
177 865597 UB1_ux = BUU_x;
178 } else {
179 755116 LB1_lx = BLU_x;
180 755116 LB1_ux = BLL_x;
181 755116 UB1_lx = BUU_x;
182 755116 UB1_ux = BUL_x;
183 }
184
185 // UA1, UB1
186
187
4/4
✓ Branch 0 taken 824175 times.
✓ Branch 1 taken 796538 times.
✓ Branch 2 taken 394701 times.
✓ Branch 3 taken 429474 times.
 1620713 if ((UA1_ux > b[0]) && (UB1_ux > a[0])) {
188 809854 if (((UA1_lx > b[0]) ||
189
3/4
✓ Branch 1 taken 20452 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9615 times.
✓ Branch 4 taken 10837 times.
 20452 inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], A1_dot_B1,
190
7/10
✓ Branch 0 taken 20452 times.
✓ Branch 1 taken 374249 times.
✓ Branch 3 taken 20452 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 20452 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 20452 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 367989 times.
✓ Branch 12 taken 26712 times.
 789402 aA0_dot_B1 - Tba[1], -Tab[1] - bA1_dot_B0)) &&
191
2/2
✓ Branch 0 taken 26709 times.
✓ Branch 1 taken 357155 times.
 383864 ((UB1_lx > a[0]) ||
192
3/4
✓ Branch 1 taken 26709 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10834 times.
✓ Branch 4 taken 15875 times.
 26709 inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], A1_dot_B1,
193
3/6
✓ Branch 1 taken 26709 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26709 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26709 times.
✗ Branch 8 not taken.
 26709 Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) {
194
1/2
✓ Branch 1 taken 367989 times.
✗ Branch 2 not taken.
 367989 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
195
2/4
✓ Branch 1 taken 367989 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367989 times.
✗ Branch 5 not taken.
 367989 Tba[1] - aA0_dot_B1);
196
197
4/8
✓ Branch 1 taken 367989 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367989 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367989 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 367989 times.
✗ Branch 11 not taken.
 367989 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0];
198
4/8
✓ Branch 1 taken 367989 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367989 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367989 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 367989 times.
✗ Branch 11 not taken.
 367989 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
199
4/8
✓ Branch 1 taken 367989 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367989 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367989 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 367989 times.
✗ Branch 11 not taken.
 367989 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
200
201
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 367989 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 367989 if (P && Q) {
202 ✗ *P << a[0], t, 0;
203 ✗ *Q = S + (*P);
204 }
205
206
1/2
✓ Branch 1 taken 367989 times.
✗ Branch 2 not taken.
 367989 return S.norm();
207 }
208 }
209
210 // UA1, LB1
211
212
4/4
✓ Branch 0 taken 692040 times.
✓ Branch 1 taken 560684 times.
✓ Branch 2 taken 318595 times.
✓ Branch 3 taken 373445 times.
 1252724 if ((UA1_lx < 0) && (LB1_ux > a[0])) {
213
3/4
✓ Branch 1 taken 19949 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7709 times.
✓ Branch 4 taken 12240 times.
 19949 if (((UA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
214
7/10
✓ Branch 0 taken 19949 times.
✓ Branch 1 taken 298646 times.
✓ Branch 3 taken 19949 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19949 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19949 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 296180 times.
✓ Branch 12 taken 22415 times.
 637190 A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) &&
215
2/2
✓ Branch 0 taken 23116 times.
✓ Branch 1 taken 283239 times.
 306355 ((LB1_lx > a[0]) ||
216
4/6
✓ Branch 1 taken 23116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23116 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 12941 times.
✓ Branch 7 taken 10175 times.
 23116 inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], A1_dot_B1, Tab[1],
217
2/4
✓ Branch 1 taken 23116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23116 times.
✗ Branch 5 not taken.
 23116 Tba[1] - aA0_dot_B1))) {
218
3/6
✓ Branch 1 taken 296180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 296180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 296180 times.
✗ Branch 8 not taken.
 296180 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
219
220
3/6
✓ Branch 1 taken 296180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 296180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 296180 times.
✗ Branch 8 not taken.
 296180 S[0] = Tab[0] + Rab(0, 1) * u - a[0];
221
3/6
✓ Branch 1 taken 296180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 296180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 296180 times.
✗ Branch 8 not taken.
 296180 S[1] = Tab[1] + Rab(1, 1) * u - t;
222
3/6
✓ Branch 1 taken 296180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 296180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 296180 times.
✗ Branch 8 not taken.
 296180 S[2] = Tab[2] + Rab(2, 1) * u;
223
224
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 296180 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 296180 if (P && Q) {
225 ✗ *P << a[0], t, 0;
226 ✗ *Q = S + (*P);
227 }
228
229
1/2
✓ Branch 1 taken 296180 times.
✗ Branch 2 not taken.
 296180 return S.norm();
230 }
231 }
232
233 // LA1, UB1
234
235
4/4
✓ Branch 0 taken 443299 times.
✓ Branch 1 taken 513245 times.
✓ Branch 2 taken 328625 times.
✓ Branch 3 taken 114674 times.
 956544 if ((LA1_ux > b[0]) && (UB1_lx < 0)) {
236 676340 if (((LA1_lx > b[0]) ||
237
4/6
✓ Branch 1 taken 19090 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19090 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 9809 times.
✓ Branch 7 taken 9281 times.
 19090 inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], A1_dot_B1, -Tba[1],
238
8/10
✓ Branch 0 taken 19090 times.
✓ Branch 1 taken 309535 times.
✓ Branch 3 taken 19090 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19090 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 26342 times.
✓ Branch 9 taken 293002 times.
✓ Branch 10 taken 307058 times.
✓ Branch 11 taken 21567 times.
 374057 -Tab[1] - bA1_dot_B0)) &&
239
3/4
✓ Branch 1 taken 26342 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 14056 times.
✓ Branch 4 taken 12286 times.
 26342 ((UB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
240
3/6
✓ Branch 1 taken 26342 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26342 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26342 times.
✗ Branch 8 not taken.
 26342 A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) {
241
3/6
✓ Branch 1 taken 307058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 307058 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 307058 times.
✗ Branch 8 not taken.
 307058 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
242
243
4/8
✓ Branch 1 taken 307058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 307058 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 307058 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 307058 times.
✗ Branch 11 not taken.
 307058 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u;
244
4/8
✓ Branch 1 taken 307058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 307058 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 307058 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 307058 times.
✗ Branch 11 not taken.
 307058 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
245
4/8
✓ Branch 1 taken 307058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 307058 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 307058 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 307058 times.
✗ Branch 11 not taken.
 307058 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
246
247
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 307058 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 307058 if (P && Q) {
248 ✗ *P << 0, t, 0;
249 ✗ *Q = S + (*P);
250 }
251
252
1/2
✓ Branch 1 taken 307058 times.
✗ Branch 2 not taken.
 307058 return S.norm();
253 }
254 }
255
256 // LA1, LB1
257
258
4/4
✓ Branch 0 taken 414355 times.
✓ Branch 1 taken 235131 times.
✓ Branch 2 taken 304950 times.
✓ Branch 3 taken 109405 times.
 649486 if ((LA1_lx < 0) && (LB1_lx < 0)) {
259
3/4
✓ Branch 1 taken 19068 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6160 times.
✓ Branch 4 taken 12908 times.
 19068 if (((LA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
260
9/12
✓ Branch 0 taken 19068 times.
✓ Branch 1 taken 285882 times.
✓ Branch 3 taken 19068 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19068 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19068 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 17359 times.
✓ Branch 12 taken 274683 times.
✓ Branch 13 taken 281888 times.
✓ Branch 14 taken 23062 times.
 341377 -Tba[1], -Tab[1])) &&
261
3/4
✓ Branch 1 taken 17359 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7205 times.
✓ Branch 4 taken 10154 times.
 17359 ((LB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
262
3/6
✓ Branch 1 taken 17359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17359 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17359 times.
✗ Branch 8 not taken.
 17359 Tab[1], Tba[1]))) {
263
3/6
✓ Branch 1 taken 281888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281888 times.
✗ Branch 8 not taken.
 281888 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
264
265
3/6
✓ Branch 1 taken 281888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281888 times.
✗ Branch 8 not taken.
 281888 S[0] = Tab[0] + Rab(0, 1) * u;
266
3/6
✓ Branch 1 taken 281888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281888 times.
✗ Branch 8 not taken.
 281888 S[1] = Tab[1] + Rab(1, 1) * u - t;
267
3/6
✓ Branch 1 taken 281888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281888 times.
✗ Branch 8 not taken.
 281888 S[2] = Tab[2] + Rab(2, 1) * u;
268
269
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 281888 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 281888 if (P && Q) {
270 ✗ *P << 0, t, 0;
271 ✗ *Q = S + (*P);
272 }
273
274
1/2
✓ Branch 1 taken 281888 times.
✗ Branch 2 not taken.
 281888 return S.norm();
275 }
276 }
277
278 Scalar ALL_y, ALU_y, AUL_y, AUU_y;
279
280
1/2
✓ Branch 1 taken 367598 times.
✗ Branch 2 not taken.
 367598 ALL_y = -Tba[1];
281 367598 ALU_y = ALL_y + aA1_dot_B1;
282 367598 AUL_y = ALL_y + aA0_dot_B1;
283 367598 AUU_y = ALU_y + aA0_dot_B1;
284
285 Scalar LA1_ly, LA1_uy, UA1_ly, UA1_uy, LB0_lx, LB0_ux, UB0_lx, UB0_ux;
286
287
2/2
✓ Branch 0 taken 196629 times.
✓ Branch 1 taken 170969 times.
 367598 if (ALL_y < ALU_y) {
288 196629 LA1_ly = ALL_y;
289 196629 LA1_uy = ALU_y;
290 196629 UA1_ly = AUL_y;
291 196629 UA1_uy = AUU_y;
292 } else {
293 170969 LA1_ly = ALU_y;
294 170969 LA1_uy = ALL_y;
295 170969 UA1_ly = AUU_y;
296 170969 UA1_uy = AUL_y;
297 }
298
299
2/2
✓ Branch 0 taken 190196 times.
✓ Branch 1 taken 177402 times.
 367598 if (BLL_x < BUL_x) {
300 190196 LB0_lx = BLL_x;
301 190196 LB0_ux = BUL_x;
302 190196 UB0_lx = BLU_x;
303 190196 UB0_ux = BUU_x;
304 } else {
305 177402 LB0_lx = BUL_x;
306 177402 LB0_ux = BLL_x;
307 177402 UB0_lx = BUU_x;
308 177402 UB0_ux = BLU_x;
309 }
310
311 // UA1, UB0
312
313
4/4
✓ Branch 0 taken 163443 times.
✓ Branch 1 taken 204155 times.
✓ Branch 2 taken 47142 times.
✓ Branch 3 taken 116301 times.
 367598 if ((UA1_uy > b[1]) && (UB0_ux > a[0])) {
314 101279 if (((UA1_ly > b[1]) ||
315
3/4
✓ Branch 1 taken 6995 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3513 times.
✓ Branch 4 taken 3482 times.
 6995 inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], A1_dot_B0,
316
7/10
✓ Branch 0 taken 6995 times.
✓ Branch 1 taken 40147 times.
✓ Branch 3 taken 6995 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6995 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6995 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 18879 times.
✓ Branch 12 taken 28263 times.
 94284 aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) &&
317
2/2
✓ Branch 0 taken 28821 times.
✓ Branch 1 taken 14839 times.
 43660 ((UB0_lx > a[0]) ||
318
3/4
✓ Branch 1 taken 28821 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4040 times.
✓ Branch 4 taken 24781 times.
 28821 inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, A1_dot_B0,
319
3/6
✓ Branch 1 taken 28821 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28821 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28821 times.
✗ Branch 8 not taken.
 28821 Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) {
320
1/2
✓ Branch 1 taken 18879 times.
✗ Branch 2 not taken.
 18879 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
321
2/4
✓ Branch 1 taken 18879 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18879 times.
✗ Branch 5 not taken.
 18879 Tba[0] - aA0_dot_B0);
322
323
4/8
✓ Branch 1 taken 18879 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18879 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18879 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18879 times.
✗ Branch 11 not taken.
 18879 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0];
324
4/8
✓ Branch 1 taken 18879 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18879 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18879 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18879 times.
✗ Branch 11 not taken.
 18879 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
325
4/8
✓ Branch 1 taken 18879 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18879 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18879 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18879 times.
✗ Branch 11 not taken.
 18879 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
326
327
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 18879 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 18879 if (P && Q) {
328 ✗ *P << a[0], t, 0;
329 ✗ *Q = S + (*P);
330 }
331
332
1/2
✓ Branch 1 taken 18879 times.
✗ Branch 2 not taken.
 18879 return S.norm();
333 }
334 }
335
336 // UA1, LB0
337
338
4/4
✓ Branch 0 taken 196906 times.
✓ Branch 1 taken 151813 times.
✓ Branch 2 taken 58553 times.
✓ Branch 3 taken 138353 times.
 348719 if ((UA1_ly < 0) && (LB0_ux > a[0])) {
339
3/4
✓ Branch 1 taken 7898 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4650 times.
✓ Branch 4 taken 3248 times.
 7898 if (((UA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1,
340
7/10
✓ Branch 0 taken 7898 times.
✓ Branch 1 taken 50655 times.
✓ Branch 3 taken 7898 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7898 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7898 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 37034 times.
✓ Branch 12 taken 21519 times.
 117106 A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1])) &&
341
2/2
✓ Branch 0 taken 23980 times.
✓ Branch 1 taken 31325 times.
 55305 ((LB0_lx > a[0]) ||
342
4/6
✓ Branch 1 taken 23980 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23980 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5709 times.
✓ Branch 7 taken 18271 times.
 23980 inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], A1_dot_B0, Tab[1],
343
2/4
✓ Branch 1 taken 23980 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23980 times.
✗ Branch 5 not taken.
 23980 Tba[0] - aA0_dot_B0))) {
344
3/6
✓ Branch 1 taken 37034 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37034 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37034 times.
✗ Branch 8 not taken.
 37034 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
345
346
3/6
✓ Branch 1 taken 37034 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37034 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37034 times.
✗ Branch 8 not taken.
 37034 S[0] = Tab[0] + Rab(0, 0) * u - a[0];
347
3/6
✓ Branch 1 taken 37034 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37034 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37034 times.
✗ Branch 8 not taken.
 37034 S[1] = Tab[1] + Rab(1, 0) * u - t;
348
3/6
✓ Branch 1 taken 37034 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37034 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37034 times.
✗ Branch 8 not taken.
 37034 S[2] = Tab[2] + Rab(2, 0) * u;
349
350
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 37034 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 37034 if (P && Q) {
351 ✗ *P << a[0], t, 0;
352 ✗ *Q = S + (*P);
353 }
354
355
1/2
✓ Branch 1 taken 37034 times.
✗ Branch 2 not taken.
 37034 return S.norm();
356 }
357 }
358
359 // LA1, UB0
360
361
4/4
✓ Branch 0 taken 134245 times.
✓ Branch 1 taken 177440 times.
✓ Branch 2 taken 40070 times.
✓ Branch 3 taken 94175 times.
 311685 if ((LA1_uy > b[1]) && (UB0_lx < 0)) {
362 87693 if (((LA1_ly > b[1]) ||
363
4/6
✓ Branch 1 taken 7553 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7553 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4151 times.
✓ Branch 7 taken 3402 times.
 7553 inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], A1_dot_B0, -Tba[0],
364
8/10
✓ Branch 0 taken 7553 times.
✓ Branch 1 taken 32517 times.
✓ Branch 3 taken 7553 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7553 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 22713 times.
✓ Branch 9 taken 13955 times.
✓ Branch 10 taken 19883 times.
✓ Branch 11 taken 20187 times.
 70336 -Tab[1] - bA1_dot_B1)) &&
365
366
3/4
✓ Branch 1 taken 22713 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5928 times.
✓ Branch 4 taken 16785 times.
 22713 ((UB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1,
367
3/6
✓ Branch 1 taken 22713 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22713 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22713 times.
✗ Branch 8 not taken.
 22713 A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]))) {
368
3/6
✓ Branch 1 taken 19883 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19883 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19883 times.
✗ Branch 8 not taken.
 19883 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
369
370
4/8
✓ Branch 1 taken 19883 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19883 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19883 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19883 times.
✗ Branch 11 not taken.
 19883 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u;
371
4/8
✓ Branch 1 taken 19883 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19883 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19883 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19883 times.
✗ Branch 11 not taken.
 19883 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
372
4/8
✓ Branch 1 taken 19883 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19883 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19883 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19883 times.
✗ Branch 11 not taken.
 19883 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
373
374
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 19883 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 19883 if (P && Q) {
375 ✗ *P << 0, t, 0;
376 ✗ *Q = S + (*P);
377 }
378
379
1/2
✓ Branch 1 taken 19883 times.
✗ Branch 2 not taken.
 19883 return S.norm();
380 }
381 }
382
383 // LA1, LB0
384
385
4/4
✓ Branch 0 taken 170536 times.
✓ Branch 1 taken 121266 times.
✓ Branch 2 taken 76779 times.
✓ Branch 3 taken 93757 times.
 291802 if ((LA1_ly < 0) && (LB0_lx < 0)) {
386
3/4
✓ Branch 1 taken 7327 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3084 times.
✓ Branch 4 taken 4243 times.
 7327 if (((LA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
387
9/12
✓ Branch 0 taken 7327 times.
✓ Branch 1 taken 69452 times.
✓ Branch 3 taken 7327 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7327 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7327 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 28587 times.
✓ Branch 12 taken 43949 times.
✓ Branch 13 taken 50557 times.
✓ Branch 14 taken 26222 times.
 112693 -Tba[0], -Tab[1])) &&
388
3/4
✓ Branch 1 taken 28587 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6608 times.
✓ Branch 4 taken 21979 times.
 28587 ((LB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
389
3/6
✓ Branch 1 taken 28587 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28587 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28587 times.
✗ Branch 8 not taken.
 28587 Tab[1], Tba[0]))) {
390
3/6
✓ Branch 1 taken 50557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50557 times.
✗ Branch 8 not taken.
 50557 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
391
392
3/6
✓ Branch 1 taken 50557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50557 times.
✗ Branch 8 not taken.
 50557 S[0] = Tab[0] + Rab(0, 0) * u;
393
3/6
✓ Branch 1 taken 50557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50557 times.
✗ Branch 8 not taken.
 50557 S[1] = Tab[1] + Rab(1, 0) * u - t;
394
3/6
✓ Branch 1 taken 50557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50557 times.
✗ Branch 8 not taken.
 50557 S[2] = Tab[2] + Rab(2, 0) * u;
395
396
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 50557 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 50557 if (P && Q) {
397 ✗ *P << 0, t, 0;
398 ✗ *Q = S + (*P);
399 }
400
401
1/2
✓ Branch 1 taken 50557 times.
✗ Branch 2 not taken.
 50557 return S.norm();
402 }
403 }
404
405 Scalar BLL_y, BLU_y, BUL_y, BUU_y;
406
407
1/2
✓ Branch 1 taken 241245 times.
✗ Branch 2 not taken.
 241245 BLL_y = Tab[1];
408 241245 BLU_y = BLL_y + bA1_dot_B1;
409 241245 BUL_y = BLL_y + bA1_dot_B0;
410 241245 BUU_y = BLU_y + bA1_dot_B0;
411
412 Scalar LA0_lx, LA0_ux, UA0_lx, UA0_ux, LB1_ly, LB1_uy, UB1_ly, UB1_uy;
413
414
2/2
✓ Branch 0 taken 124198 times.
✓ Branch 1 taken 117047 times.
 241245 if (ALL_x < AUL_x) {
415 124198 LA0_lx = ALL_x;
416 124198 LA0_ux = AUL_x;
417 124198 UA0_lx = ALU_x;
418 124198 UA0_ux = AUU_x;
419 } else {
420 117047 LA0_lx = AUL_x;
421 117047 LA0_ux = ALL_x;
422 117047 UA0_lx = AUU_x;
423 117047 UA0_ux = ALU_x;
424 }
425
426
2/2
✓ Branch 0 taken 131126 times.
✓ Branch 1 taken 110119 times.
 241245 if (BLL_y < BLU_y) {
427 131126 LB1_ly = BLL_y;
428 131126 LB1_uy = BLU_y;
429 131126 UB1_ly = BUL_y;
430 131126 UB1_uy = BUU_y;
431 } else {
432 110119 LB1_ly = BLU_y;
433 110119 LB1_uy = BLL_y;
434 110119 UB1_ly = BUU_y;
435 110119 UB1_uy = BUL_y;
436 }
437
438 // UA0, UB1
439
440
4/4
✓ Branch 0 taken 132524 times.
✓ Branch 1 taken 108721 times.
✓ Branch 2 taken 47538 times.
✓ Branch 3 taken 84986 times.
 241245 if ((UA0_ux > b[0]) && (UB1_uy > a[1])) {
441 111645 if (((UA0_lx > b[0]) ||
442
3/4
✓ Branch 1 taken 16569 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7131 times.
✓ Branch 4 taken 9438 times.
 16569 inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], A0_dot_B1,
443
7/10
✓ Branch 0 taken 16569 times.
✓ Branch 1 taken 30969 times.
✓ Branch 3 taken 16569 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16569 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16569 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 35801 times.
✓ Branch 12 taken 11737 times.
 95076 aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) &&
444
2/2
✓ Branch 0 taken 4518 times.
✓ Branch 1 taken 33582 times.
 38100 ((UB1_ly > a[1]) ||
445
3/4
✓ Branch 1 taken 4518 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2219 times.
✓ Branch 4 taken 2299 times.
 4518 inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, A0_dot_B1,
446
3/6
✓ Branch 1 taken 4518 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4518 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4518 times.
✗ Branch 8 not taken.
 4518 Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) {
447
1/2
✓ Branch 1 taken 35801 times.
✗ Branch 2 not taken.
 35801 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
448
2/4
✓ Branch 1 taken 35801 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35801 times.
✗ Branch 5 not taken.
 35801 Tba[1] - aA1_dot_B1);
449
450
4/8
✓ Branch 1 taken 35801 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35801 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35801 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35801 times.
✗ Branch 11 not taken.
 35801 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
451
4/8
✓ Branch 1 taken 35801 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35801 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35801 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35801 times.
✗ Branch 11 not taken.
 35801 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1];
452
4/8
✓ Branch 1 taken 35801 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35801 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35801 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35801 times.
✗ Branch 11 not taken.
 35801 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
453
454
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 35801 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 35801 if (P && Q) {
455 ✗ *P << t, a[1], 0;
456 ✗ *Q = S + (*P);
457 }
458
459
1/2
✓ Branch 1 taken 35801 times.
✗ Branch 2 not taken.
 35801 return S.norm();
460 }
461 }
462
463 // UA0, LB1
464
465
4/4
✓ Branch 0 taken 110676 times.
✓ Branch 1 taken 94768 times.
✓ Branch 2 taken 45219 times.
✓ Branch 3 taken 65457 times.
 205444 if ((UA0_lx < 0) && (LB1_uy > a[1])) {
466
3/4
✓ Branch 1 taken 16284 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6956 times.
✓ Branch 4 taken 9328 times.
 16284 if (((UA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0,
467
7/10
✓ Branch 0 taken 16284 times.
✓ Branch 1 taken 28935 times.
✓ Branch 3 taken 16284 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16284 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16284 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 34322 times.
✓ Branch 12 taken 10897 times.
 90438 A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0])) &&
468
2/2
✓ Branch 0 taken 5159 times.
✓ Branch 1 taken 30732 times.
 35891 ((LB1_ly > a[1]) ||
469
4/6
✓ Branch 1 taken 5159 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5159 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3590 times.
✓ Branch 7 taken 1569 times.
 5159 inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
470
2/4
✓ Branch 1 taken 5159 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5159 times.
✗ Branch 5 not taken.
 5159 Tba[1] - aA1_dot_B1))) {
471
3/6
✓ Branch 1 taken 34322 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34322 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34322 times.
✗ Branch 8 not taken.
 34322 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
472
473
3/6
✓ Branch 1 taken 34322 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34322 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34322 times.
✗ Branch 8 not taken.
 34322 S[0] = Tab[0] + Rab(0, 1) * u - t;
474
3/6
✓ Branch 1 taken 34322 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34322 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34322 times.
✗ Branch 8 not taken.
 34322 S[1] = Tab[1] + Rab(1, 1) * u - a[1];
475
3/6
✓ Branch 1 taken 34322 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34322 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34322 times.
✗ Branch 8 not taken.
 34322 S[2] = Tab[2] + Rab(2, 1) * u;
476
477
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 34322 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 34322 if (P && Q) {
478 ✗ *P << t, a[1], 0;
479 ✗ *Q = S + (*P);
480 }
481
482
1/2
✓ Branch 1 taken 34322 times.
✗ Branch 2 not taken.
 34322 return S.norm();
483 }
484 }
485
486 // LA0, UB1
487
488
4/4
✓ Branch 0 taken 94551 times.
✓ Branch 1 taken 76571 times.
✓ Branch 2 taken 63961 times.
✓ Branch 3 taken 30590 times.
 171122 if ((LA0_ux > b[0]) && (UB1_ly < 0)) {
489 146124 if (((LA0_lx > b[0]) ||
490
4/6
✓ Branch 1 taken 18202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18202 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8884 times.
✓ Branch 7 taken 9318 times.
 18202 inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
491
8/10
✓ Branch 0 taken 18202 times.
✓ Branch 1 taken 45759 times.
✓ Branch 3 taken 18202 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18202 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 5680 times.
✓ Branch 9 taken 48963 times.
✓ Branch 10 taken 52668 times.
✓ Branch 11 taken 11293 times.
 87843 -bA0_dot_B0 - Tab[0])) &&
492
3/4
✓ Branch 1 taken 5680 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3705 times.
✓ Branch 4 taken 1975 times.
 5680 ((UB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0,
493
3/6
✓ Branch 1 taken 5680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5680 times.
✗ Branch 8 not taken.
 5680 A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]))) {
494
3/6
✓ Branch 1 taken 52668 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52668 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52668 times.
✗ Branch 8 not taken.
 52668 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
495
496
4/8
✓ Branch 1 taken 52668 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52668 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52668 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52668 times.
✗ Branch 11 not taken.
 52668 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
497
4/8
✓ Branch 1 taken 52668 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52668 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52668 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52668 times.
✗ Branch 11 not taken.
 52668 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u;
498
4/8
✓ Branch 1 taken 52668 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52668 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52668 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52668 times.
✗ Branch 11 not taken.
 52668 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
499
500
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 52668 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 52668 if (P && Q) {
501 ✗ *P << t, 0, 0;
502 ✗ *Q = S + (*P);
503 }
504
505
1/2
✓ Branch 1 taken 52668 times.
✗ Branch 2 not taken.
 52668 return S.norm();
506 }
507 }
508
509 // LA0, LB1
510
511
4/4
✓ Branch 0 taken 78350 times.
✓ Branch 1 taken 40104 times.
✓ Branch 2 taken 42741 times.
✓ Branch 3 taken 35609 times.
 118454 if ((LA0_lx < 0) && (LB1_ly < 0)) {
512
3/4
✓ Branch 1 taken 15444 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8462 times.
✓ Branch 4 taken 6982 times.
 15444 if (((LA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1,
513
9/12
✓ Branch 0 taken 15444 times.
✓ Branch 1 taken 27297 times.
✓ Branch 3 taken 15444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15444 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 15444 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 3717 times.
✓ Branch 12 taken 32042 times.
✓ Branch 13 taken 33639 times.
✓ Branch 14 taken 9102 times.
 61902 -Tba[1], -Tab[0])) &&
514
3/4
✓ Branch 1 taken 3717 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1597 times.
✓ Branch 4 taken 2120 times.
 3717 ((LB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
515
3/6
✓ Branch 1 taken 3717 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3717 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3717 times.
✗ Branch 8 not taken.
 3717 Tab[0], Tba[1]))) {
516
3/6
✓ Branch 1 taken 33639 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33639 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33639 times.
✗ Branch 8 not taken.
 33639 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
517
518
3/6
✓ Branch 1 taken 33639 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33639 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33639 times.
✗ Branch 8 not taken.
 33639 S[0] = Tab[0] + Rab(0, 1) * u - t;
519
3/6
✓ Branch 1 taken 33639 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33639 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33639 times.
✗ Branch 8 not taken.
 33639 S[1] = Tab[1] + Rab(1, 1) * u;
520
3/6
✓ Branch 1 taken 33639 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33639 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33639 times.
✗ Branch 8 not taken.
 33639 S[2] = Tab[2] + Rab(2, 1) * u;
521
522
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 33639 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 33639 if (P && Q) {
523 ✗ *P << t, 0, 0;
524 ✗ *Q = S + (*P);
525 }
526
527
1/2
✓ Branch 1 taken 33639 times.
✗ Branch 2 not taken.
 33639 return S.norm();
528 }
529 }
530
531 Scalar LA0_ly, LA0_uy, UA0_ly, UA0_uy, LB0_ly, LB0_uy, UB0_ly, UB0_uy;
532
533
2/2
✓ Branch 0 taken 42800 times.
✓ Branch 1 taken 42015 times.
 84815 if (ALL_y < AUL_y) {
534 42800 LA0_ly = ALL_y;
535 42800 LA0_uy = AUL_y;
536 42800 UA0_ly = ALU_y;
537 42800 UA0_uy = AUU_y;
538 } else {
539 42015 LA0_ly = AUL_y;
540 42015 LA0_uy = ALL_y;
541 42015 UA0_ly = AUU_y;
542 42015 UA0_uy = ALU_y;
543 }
544
545
2/2
✓ Branch 0 taken 43467 times.
✓ Branch 1 taken 41348 times.
 84815 if (BLL_y < BUL_y) {
546 43467 LB0_ly = BLL_y;
547 43467 LB0_uy = BUL_y;
548 43467 UB0_ly = BLU_y;
549 43467 UB0_uy = BUU_y;
550 } else {
551 41348 LB0_ly = BUL_y;
552 41348 LB0_uy = BLL_y;
553 41348 UB0_ly = BUU_y;
554 41348 UB0_uy = BLU_y;
555 }
556
557 // UA0, UB0
558
559
4/4
✓ Branch 0 taken 51112 times.
✓ Branch 1 taken 33703 times.
✓ Branch 2 taken 18330 times.
✓ Branch 3 taken 32782 times.
 84815 if ((UA0_uy > b[1]) && (UB0_uy > a[1])) {
560 50128 if (((UA0_ly > b[1]) ||
561
3/4
✓ Branch 1 taken 13468 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5695 times.
✓ Branch 4 taken 7773 times.
 13468 inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], A0_dot_B0,
562
7/10
✓ Branch 0 taken 13468 times.
✓ Branch 1 taken 4862 times.
✓ Branch 3 taken 13468 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13468 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13468 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4413 times.
✓ Branch 12 taken 13917 times.
 36660 aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) &&
563
2/2
✓ Branch 0 taken 8355 times.
✓ Branch 1 taken 2202 times.
 10557 ((UB0_ly > a[1]) ||
564
3/4
✓ Branch 1 taken 8355 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2211 times.
✓ Branch 4 taken 6144 times.
 8355 inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
565
3/6
✓ Branch 1 taken 8355 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8355 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8355 times.
✗ Branch 8 not taken.
 8355 Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) {
566
1/2
✓ Branch 1 taken 4413 times.
✗ Branch 2 not taken.
 4413 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
567
2/4
✓ Branch 1 taken 4413 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4413 times.
✗ Branch 5 not taken.
 4413 Tba[0] - aA1_dot_B0);
568
569
4/8
✓ Branch 1 taken 4413 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4413 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4413 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4413 times.
✗ Branch 11 not taken.
 4413 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
570
4/8
✓ Branch 1 taken 4413 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4413 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4413 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4413 times.
✗ Branch 11 not taken.
 4413 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1];
571
4/8
✓ Branch 1 taken 4413 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4413 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4413 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4413 times.
✗ Branch 11 not taken.
 4413 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
572
573
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4413 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4413 if (P && Q) {
574 ✗ *P << t, a[1], 0;
575 ✗ *Q = S + (*P);
576 }
577
578
1/2
✓ Branch 1 taken 4413 times.
✗ Branch 2 not taken.
 4413 return S.norm();
579 }
580 }
581
582 // UA0, LB0
583
584
4/4
✓ Branch 0 taken 46410 times.
✓ Branch 1 taken 33992 times.
✓ Branch 2 taken 17091 times.
✓ Branch 3 taken 29319 times.
 80402 if ((UA0_ly < 0) && (LB0_uy > a[1])) {
585
3/4
✓ Branch 1 taken 9785 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4208 times.
✓ Branch 4 taken 5577 times.
 9785 if (((UA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1,
586
7/10
✓ Branch 0 taken 9785 times.
✓ Branch 1 taken 7306 times.
✓ Branch 3 taken 9785 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9785 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9785 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 7840 times.
✓ Branch 12 taken 9251 times.
 34182 A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0])) &&
587
2/2
✓ Branch 0 taken 5959 times.
✓ Branch 1 taken 5555 times.
 11514 ((LB0_ly > a[1]) ||
588
4/6
✓ Branch 1 taken 5959 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5959 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2285 times.
✓ Branch 7 taken 3674 times.
 5959 inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], A0_dot_B0, Tab[0],
589
2/4
✓ Branch 1 taken 5959 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5959 times.
✗ Branch 5 not taken.
 5959 Tba[0] - aA1_dot_B0))) {
590
3/6
✓ Branch 1 taken 7840 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7840 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7840 times.
✗ Branch 8 not taken.
 7840 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
591
592
3/6
✓ Branch 1 taken 7840 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7840 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7840 times.
✗ Branch 8 not taken.
 7840 S[0] = Tab[0] + Rab(0, 0) * u - t;
593
3/6
✓ Branch 1 taken 7840 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7840 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7840 times.
✗ Branch 8 not taken.
 7840 S[1] = Tab[1] + Rab(1, 0) * u - a[1];
594
3/6
✓ Branch 1 taken 7840 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7840 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7840 times.
✗ Branch 8 not taken.
 7840 S[2] = Tab[2] + Rab(2, 0) * u;
595
596
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 7840 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 7840 if (P && Q) {
597 ✗ *P << t, a[1], 0;
598 ✗ *Q = S + (*P);
599 }
600
601
1/2
✓ Branch 1 taken 7840 times.
✗ Branch 2 not taken.
 7840 return S.norm();
602 }
603 }
604
605 // LA0, UB0
606
607
4/4
✓ Branch 0 taken 43762 times.
✓ Branch 1 taken 28800 times.
✓ Branch 2 taken 14681 times.
✓ Branch 3 taken 29081 times.
 72562 if ((LA0_uy > b[1]) && (UB0_ly < 0)) {
608 40139 if (((LA0_ly > b[1]) ||
609
4/6
✓ Branch 1 taken 10777 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10777 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4680 times.
✓ Branch 7 taken 6097 times.
 10777 inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
610
8/10
✓ Branch 0 taken 10777 times.
✓ Branch 1 taken 3904 times.
✓ Branch 3 taken 10777 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10777 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6317 times.
✓ Branch 9 taken 2267 times.
✓ Branch 10 taken 4898 times.
✓ Branch 11 taken 9783 times.
 31775 -Tab[0] - bA0_dot_B1)) &&
611
612
3/4
✓ Branch 1 taken 6317 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2631 times.
✓ Branch 4 taken 3686 times.
 6317 ((UB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1,
613
3/6
✓ Branch 1 taken 6317 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6317 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6317 times.
✗ Branch 8 not taken.
 6317 A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]))) {
614
3/6
✓ Branch 1 taken 4898 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4898 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4898 times.
✗ Branch 8 not taken.
 4898 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
615
616
4/8
✓ Branch 1 taken 4898 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4898 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4898 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4898 times.
✗ Branch 11 not taken.
 4898 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
617
4/8
✓ Branch 1 taken 4898 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4898 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4898 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4898 times.
✗ Branch 11 not taken.
 4898 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u;
618
4/8
✓ Branch 1 taken 4898 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4898 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4898 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4898 times.
✗ Branch 11 not taken.
 4898 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
619
620
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4898 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4898 if (P && Q) {
621 ✗ *P << t, 0, 0;
622 ✗ *Q = S + (*P);
623 }
624
625
1/2
✓ Branch 1 taken 4898 times.
✗ Branch 2 not taken.
 4898 return S.norm();
626 }
627 }
628
629 // LA0, LB0
630
631
4/4
✓ Branch 0 taken 39477 times.
✓ Branch 1 taken 28187 times.
✓ Branch 2 taken 11561 times.
✓ Branch 3 taken 27916 times.
 67664 if ((LA0_ly < 0) && (LB0_ly < 0)) {
632
3/4
✓ Branch 1 taken 8558 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3769 times.
✓ Branch 4 taken 4789 times.
 8558 if (((LA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
633
9/12
✓ Branch 0 taken 8558 times.
✓ Branch 1 taken 3003 times.
✓ Branch 3 taken 8558 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8558 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8558 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4949 times.
✓ Branch 12 taken 1823 times.
✓ Branch 13 taken 3334 times.
✓ Branch 14 taken 8227 times.
 25068 -Tba[0], -Tab[0])) &&
634
3/4
✓ Branch 1 taken 4949 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1511 times.
✓ Branch 4 taken 3438 times.
 4949 ((LB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
635
3/6
✓ Branch 1 taken 4949 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4949 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4949 times.
✗ Branch 8 not taken.
 4949 Tab[0], Tba[0]))) {
636
3/6
✓ Branch 1 taken 3334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3334 times.
✗ Branch 8 not taken.
 3334 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
637
638
3/6
✓ Branch 1 taken 3334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3334 times.
✗ Branch 8 not taken.
 3334 S[0] = Tab[0] + Rab(0, 0) * u - t;
639
3/6
✓ Branch 1 taken 3334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3334 times.
✗ Branch 8 not taken.
 3334 S[1] = Tab[1] + Rab(1, 0) * u;
640
3/6
✓ Branch 1 taken 3334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3334 times.
✗ Branch 8 not taken.
 3334 S[2] = Tab[2] + Rab(2, 0) * u;
641
642
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3334 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3334 if (P && Q) {
643 ✗ *P << t, 0, 0;
644 ✗ *Q = S + (*P);
645 }
646
647
1/2
✓ Branch 1 taken 3334 times.
✗ Branch 2 not taken.
 3334 return S.norm();
648 }
649 }
650
651 // no edges passed, take max separation along face normals
652
653 Scalar sep1, sep2;
654
655
3/4
✓ Branch 1 taken 64330 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33757 times.
✓ Branch 4 taken 30573 times.
 64330 if (Tab[2] > 0.0) {
656
1/2
✓ Branch 1 taken 33757 times.
✗ Branch 2 not taken.
 33757 sep1 = Tab[2];
657
4/6
✓ Branch 1 taken 33757 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20108 times.
✓ Branch 4 taken 13649 times.
✓ Branch 6 taken 20108 times.
✗ Branch 7 not taken.
 33757 if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
658
4/6
✓ Branch 1 taken 33757 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18551 times.
✓ Branch 4 taken 15206 times.
✓ Branch 6 taken 18551 times.
✗ Branch 7 not taken.
 33757 if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
659 } else {
660
1/2
✓ Branch 1 taken 30573 times.
✗ Branch 2 not taken.
 30573 sep1 = -Tab[2];
661
4/6
✓ Branch 1 taken 30573 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18305 times.
✓ Branch 4 taken 12268 times.
✓ Branch 6 taken 18305 times.
✗ Branch 7 not taken.
 30573 if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
662
4/6
✓ Branch 1 taken 30573 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17222 times.
✓ Branch 4 taken 13351 times.
✓ Branch 6 taken 17222 times.
✗ Branch 7 not taken.
 30573 if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
663 }
664
665
3/4
✓ Branch 1 taken 64330 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33469 times.
✓ Branch 4 taken 30861 times.
 64330 if (Tba[2] < 0) {
666
1/2
✓ Branch 1 taken 33469 times.
✗ Branch 2 not taken.
 33469 sep2 = -Tba[2];
667
4/6
✓ Branch 1 taken 33469 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23294 times.
✓ Branch 4 taken 10175 times.
✓ Branch 6 taken 23294 times.
✗ Branch 7 not taken.
 33469 if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
668
4/6
✓ Branch 1 taken 33469 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 19984 times.
✓ Branch 4 taken 13485 times.
✓ Branch 6 taken 19984 times.
✗ Branch 7 not taken.
 33469 if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
669 } else {
670
1/2
✓ Branch 1 taken 30861 times.
✗ Branch 2 not taken.
 30861 sep2 = Tba[2];
671
4/6
✓ Branch 1 taken 30861 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23192 times.
✓ Branch 4 taken 7669 times.
✓ Branch 6 taken 23192 times.
✗ Branch 7 not taken.
 30861 if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
672
4/6
✓ Branch 1 taken 30861 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18728 times.
✓ Branch 4 taken 12133 times.
✓ Branch 6 taken 18728 times.
✗ Branch 7 not taken.
 30861 if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
673 }
674
675
4/4
✓ Branch 0 taken 44197 times.
✓ Branch 1 taken 20133 times.
✓ Branch 2 taken 34111 times.
✓ Branch 3 taken 10086 times.
 64330 if (sep1 >= sep2 && sep1 >= 0) {
676
3/4
✓ Branch 1 taken 34111 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17471 times.
✓ Branch 4 taken 16640 times.
 34111 if (Tab[2] > 0)
677
3/6
✓ Branch 1 taken 17471 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17471 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17471 times.
✗ Branch 8 not taken.
 17471 S << 0, 0, sep1;
678 else
679
3/6
✓ Branch 1 taken 16640 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16640 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16640 times.
✗ Branch 8 not taken.
 16640 S << 0, 0, -sep1;
680
681
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 34111 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 34111 if (P && Q) {
682 ✗ *Q = S;
683 ✗ P->setZero();
684 }
685 }
686
687
4/4
✓ Branch 0 taken 20298 times.
✓ Branch 1 taken 44032 times.
✓ Branch 2 taken 15212 times.
✓ Branch 3 taken 5086 times.
 64330 if (sep2 >= sep1 && sep2 >= 0) {
688
4/8
✓ Branch 1 taken 15212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15212 times.
✗ Branch 11 not taken.
 15212 Vec3s Q_(Tab[0], Tab[1], Tab[2]);
689
1/2
✓ Branch 1 taken 15212 times.
✗ Branch 2 not taken.
 15212 Vec3s P_;
690
3/4
✓ Branch 1 taken 15212 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8607 times.
✓ Branch 4 taken 6605 times.
 15212 if (Tba[2] < 0) {
691
3/6
✓ Branch 1 taken 8607 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8607 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8607 times.
✗ Branch 8 not taken.
 8607 P_[0] = Rab(0, 2) * sep2 + Tab[0];
692
3/6
✓ Branch 1 taken 8607 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8607 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8607 times.
✗ Branch 8 not taken.
 8607 P_[1] = Rab(1, 2) * sep2 + Tab[1];
693
3/6
✓ Branch 1 taken 8607 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8607 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8607 times.
✗ Branch 8 not taken.
 8607 P_[2] = Rab(2, 2) * sep2 + Tab[2];
694 } else {
695
3/6
✓ Branch 1 taken 6605 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6605 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6605 times.
✗ Branch 8 not taken.
 6605 P_[0] = -Rab(0, 2) * sep2 + Tab[0];
696
3/6
✓ Branch 1 taken 6605 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6605 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6605 times.
✗ Branch 8 not taken.
 6605 P_[1] = -Rab(1, 2) * sep2 + Tab[1];
697
3/6
✓ Branch 1 taken 6605 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6605 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6605 times.
✗ Branch 8 not taken.
 6605 P_[2] = -Rab(2, 2) * sep2 + Tab[2];
698 }
699
700
2/4
✓ Branch 1 taken 15212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15212 times.
✗ Branch 5 not taken.
 15212 S = Q_ - P_;
701
702
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 15212 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 15212 if (P && Q) {
703 ✗ *P = P_;
704 ✗ *Q = Q_;
705 }
706 }
707
708
2/2
✓ Branch 0 taken 44032 times.
✓ Branch 1 taken 20298 times.
 64330 Scalar sep = (sep1 > sep2 ? sep1 : sep2);
709
2/2
✓ Branch 0 taken 49156 times.
✓ Branch 1 taken 15174 times.
 64330 return (sep > 0 ? sep : 0);
710 }
711
712 15645 bool RSS::overlap(const RSS& other) const {
713 /// compute what transform [R,T] that takes us from cs1 to cs2.
714 /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
715 /// First compute the rotation part, then translation part
716
717 /// Then compute R1'(T2 - T1)
718
4/8
✓ Branch 1 taken 15645 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15645 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15645 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15645 times.
✗ Branch 11 not taken.
 15645 Vec3s T(axes.transpose() * (other.Tr - Tr));
719
720 /// Now compute R1'R2
721
3/6
✓ Branch 1 taken 15645 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15645 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15645 times.
✗ Branch 8 not taken.
 15645 Matrix3s R(axes.transpose() * other.axes);
722
723
1/2
✓ Branch 1 taken 15645 times.
✗ Branch 2 not taken.
 15645 Scalar dist = rectDistance(R, T, length, other.length);
724 15645 return (dist <= (radius + other.radius));
725 }
726
727 ✗ bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
728 const RSS& b2) {
729 // ROb2 = R0 . b2
730 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
731
732 // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
733 // R = b2^T RO^T b1
734 ✗ Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
735 ✗ Vec3s T(b1.axes.transpose() * Ttemp);
736 ✗ Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
737
738 ✗ Scalar dist = rectDistance(R, T, b1.length, b2.length);
739 ✗ return (dist <= (b1.radius + b2.radius));
740 }
741
742 9048 bool overlap(const Matrix3s& R0, const Vec3s& T0, const RSS& b1, const RSS& b2,
743 const CollisionRequest& request, Scalar& sqrDistLowerBound) {
744 // ROb2 = R0 . b2
745 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
746
747 // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
748 // R = b2^T RO^T b1
749
5/10
✓ Branch 1 taken 9048 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9048 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9048 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9048 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9048 times.
✗ Branch 14 not taken.
 9048 Vec3s Ttemp(R0.transpose() * (b2.Tr - T0) - b1.Tr);
750
3/6
✓ Branch 1 taken 9048 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9048 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9048 times.
✗ Branch 8 not taken.
 9048 Vec3s T(b1.axes.transpose() * Ttemp);
751
5/10
✓ Branch 1 taken 9048 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9048 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9048 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9048 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9048 times.
✗ Branch 14 not taken.
 9048 Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
752
753
1/2
✓ Branch 1 taken 9048 times.
✗ Branch 2 not taken.
 9048 Scalar dist = rectDistance(R, T, b1.length, b2.length) - b1.radius -
754 9048 b2.radius - request.security_margin;
755
2/2
✓ Branch 0 taken 5096 times.
✓ Branch 1 taken 3952 times.
 9048 if (dist <= 0) return true;
756 3952 sqrDistLowerBound = dist * dist;
757 3952 return false;
758 }
759
760 ✗ bool RSS::contain(const Vec3s& p) const {
761 ✗ Vec3s local_p = p - Tr;
762 // FIXME: Vec3s proj (axes.transpose() * local_p);
763 ✗ Scalar proj0 = local_p.dot(axes.col(0));
764 ✗ Scalar proj1 = local_p.dot(axes.col(1));
765 ✗ Scalar proj2 = local_p.dot(axes.col(2));
766 ✗ Scalar abs_proj2 = fabs(proj2);
767 ✗ Vec3s proj(proj0, proj1, proj2);
768
769 /// projection is within the rectangle
770 ✗ if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) &&
771 ✗ (proj1 > 0)) {
772 ✗ return (abs_proj2 < radius);
773 ✗ } else if ((proj0 < length[0]) && (proj0 > 0) &&
774 ✗ ((proj1 < 0) || (proj1 > length[1]))) {
775 ✗ Scalar y = (proj1 > 0) ? length[1] : 0;
776 ✗ Vec3s v(proj0, y, 0);
777 ✗ return ((proj - v).squaredNorm() < radius * radius);
778 ✗ } else if ((proj1 < length[1]) && (proj1 > 0) &&
779 ✗ ((proj0 < 0) || (proj0 > length[0]))) {
780 ✗ Scalar x = (proj0 > 0) ? length[0] : 0;
781 ✗ Vec3s v(x, proj1, 0);
782 ✗ return ((proj - v).squaredNorm() < radius * radius);
783 } else {
784 ✗ Scalar x = (proj0 > 0) ? length[0] : 0;
785 ✗ Scalar y = (proj1 > 0) ? length[1] : 0;
786 ✗ Vec3s v(x, y, 0);
787 ✗ return ((proj - v).squaredNorm() < radius * radius);
788 }
789 }
790
791 ✗ RSS& RSS::operator+=(const Vec3s& p) {
792 ✗ Vec3s local_p = p - Tr;
793 ✗ Scalar proj0 = local_p.dot(axes.col(0));
794 ✗ Scalar proj1 = local_p.dot(axes.col(1));
795 ✗ Scalar proj2 = local_p.dot(axes.col(2));
796 ✗ Scalar abs_proj2 = fabs(proj2);
797 ✗ Vec3s proj(proj0, proj1, proj2);
798
799 // projection is within the rectangle
800 ✗ if ((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) &&
801 ✗ (proj1 > 0)) {
802 ✗ if (abs_proj2 < radius)
803 ; // do nothing
804 else {
805 ✗ const Scalar half = Scalar(0.5);
806 ✗ radius = half * (radius + abs_proj2); // enlarge the r
807 // change RSS origin position
808 ✗ if (proj2 > 0)
809 ✗ Tr[2] += half * (abs_proj2 - radius);
810 else
811 ✗ Tr[2] -= half * (abs_proj2 - radius);
812 }
813 ✗ } else if ((proj0 < length[0]) && (proj0 > 0) &&
814 ✗ ((proj1 < 0) || (proj1 > length[1]))) {
815 ✗ Scalar y = (proj1 > 0) ? length[1] : 0;
816 ✗ Vec3s v(proj0, y, 0);
817 ✗ Scalar new_r_sqr = (proj - v).squaredNorm();
818 ✗ if (new_r_sqr < radius * radius)
819 ; // do nothing
820 else {
821 ✗ if (abs_proj2 < radius) {
822 Scalar delta_y =
823 ✗ -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y);
824 ✗ length[1] += delta_y;
825 ✗ if (proj1 < 0) Tr[1] -= delta_y;
826 } else {
827 ✗ Scalar delta_y = fabs(proj1 - y);
828 ✗ length[1] += delta_y;
829 ✗ if (proj1 < 0) Tr[1] -= delta_y;
830
831 ✗ const Scalar half = Scalar(0.5);
832 ✗ if (proj2 > 0)
833 ✗ Tr[2] += half * (abs_proj2 - radius);
834 else
835 ✗ Tr[2] -= half * (abs_proj2 - radius);
836 }
837 }
838 ✗ } else if ((proj1 < length[1]) && (proj1 > 0) &&
839 ✗ ((proj0 < 0) || (proj0 > length[0]))) {
840 ✗ Scalar x = (proj0 > 0) ? length[0] : 0;
841 ✗ Vec3s v(x, proj1, 0);
842 ✗ Scalar new_r_sqr = (proj - v).squaredNorm();
843 ✗ if (new_r_sqr < radius * radius)
844 ; // do nothing
845 else {
846 ✗ if (abs_proj2 < radius) {
847 Scalar delta_x =
848 ✗ -std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x);
849 ✗ length[0] += delta_x;
850 ✗ if (proj0 < 0) Tr[0] -= delta_x;
851 } else {
852 ✗ Scalar delta_x = fabs(proj0 - x);
853 ✗ length[0] += delta_x;
854 ✗ if (proj0 < 0) Tr[0] -= delta_x;
855
856 ✗ const Scalar half = Scalar(0.5);
857 ✗ if (proj2 > 0)
858 ✗ Tr[2] += half * (abs_proj2 - radius);
859 else
860 ✗ Tr[2] -= half * (abs_proj2 - radius);
861 }
862 }
863 ✗ } else {
864 ✗ Scalar x = (proj0 > 0) ? length[0] : 0;
865 ✗ Scalar y = (proj1 > 0) ? length[1] : 0;
866 ✗ Vec3s v(x, y, 0);
867 ✗ Scalar new_r_sqr = (proj - v).squaredNorm();
868 ✗ if (new_r_sqr < radius * radius)
869 ; // do nothing
870 else {
871 ✗ if (abs_proj2 < radius) {
872 ✗ Scalar diag = std::sqrt(new_r_sqr - proj2 * proj2);
873 ✗ Scalar delta_diag = -std::sqrt(radius * radius - proj2 * proj2) + diag;
874
875 ✗ Scalar delta_x = delta_diag / diag * fabs(proj0 - x);
876 ✗ Scalar delta_y = delta_diag / diag * fabs(proj1 - y);
877 ✗ length[0] += delta_x;
878 ✗ length[1] += delta_y;
879
880 ✗ if (proj0 < 0 && proj1 < 0) {
881 ✗ Tr[0] -= delta_x;
882 ✗ Tr[1] -= delta_y;
883 }
884 } else {
885 ✗ Scalar delta_x = fabs(proj0 - x);
886 ✗ Scalar delta_y = fabs(proj1 - y);
887
888 ✗ length[0] += delta_x;
889 ✗ length[1] += delta_y;
890
891 ✗ if (proj0 < 0 && proj1 < 0) {
892 ✗ Tr[0] -= delta_x;
893 ✗ Tr[1] -= delta_y;
894 }
895
896 ✗ const Scalar half = Scalar(0.5);
897 ✗ if (proj2 > 0)
898 ✗ Tr[2] += half * (abs_proj2 - radius);
899 else
900 ✗ Tr[2] -= half * (abs_proj2 - radius);
901 }
902 }
903 }
904
905 ✗ return *this;
906 }
907
908 13074 RSS RSS::operator+(const RSS& other) const {
909
1/2
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
 13074 RSS bv;
910
911
3/4
✓ Branch 1 taken 209184 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 209184 times.
✓ Branch 4 taken 13074 times.
 222258 Vec3s v[16];
912
3/6
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
 13074 Vec3s d0_pos(other.axes.col(0) * (other.length[0] + other.radius));
913
3/6
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
 13074 Vec3s d1_pos(other.axes.col(1) * (other.length[1] + other.radius));
914
3/6
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
 13074 Vec3s d0_neg(other.axes.col(0) * (-other.radius));
915
3/6
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
 13074 Vec3s d1_neg(other.axes.col(1) * (-other.radius));
916
3/6
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
 13074 Vec3s d2_pos(other.axes.col(2) * other.radius);
917
3/6
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
 13074 Vec3s d2_neg(other.axes.col(2) * (-other.radius));
918
919
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos;
920
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg;
921
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[2].noalias() = other.Tr + d0_pos + d1_neg + d2_pos;
922
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[3].noalias() = other.Tr + d0_pos + d1_neg + d2_neg;
923
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[4].noalias() = other.Tr + d0_neg + d1_pos + d2_pos;
924
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[5].noalias() = other.Tr + d0_neg + d1_pos + d2_neg;
925
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[6].noalias() = other.Tr + d0_neg + d1_neg + d2_pos;
926
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[7].noalias() = other.Tr + d0_neg + d1_neg + d2_neg;
927
928
4/8
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
 13074 d0_pos.noalias() = axes.col(0) * (length[0] + radius);
929
4/8
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
 13074 d1_pos.noalias() = axes.col(1) * (length[1] + radius);
930
4/8
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
 13074 d0_neg.noalias() = axes.col(0) * (-radius);
931
4/8
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
 13074 d1_neg.noalias() = axes.col(1) * (-radius);
932
4/8
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
 13074 d2_pos.noalias() = axes.col(2) * radius;
933
4/8
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
 13074 d2_neg.noalias() = axes.col(2) * (-radius);
934
935
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[8].noalias() = Tr + d0_pos + d1_pos + d2_pos;
936
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[9].noalias() = Tr + d0_pos + d1_pos + d2_neg;
937
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[10].noalias() = Tr + d0_pos + d1_neg + d2_pos;
938
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[11].noalias() = Tr + d0_pos + d1_neg + d2_neg;
939
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[12].noalias() = Tr + d0_neg + d1_pos + d2_pos;
940
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[13].noalias() = Tr + d0_neg + d1_pos + d2_neg;
941
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[14].noalias() = Tr + d0_neg + d1_neg + d2_pos;
942
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 v[15].noalias() = Tr + d0_neg + d1_neg + d2_neg;
943
944
1/2
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
 13074 Matrix3s M; // row first matrix
945
3/4
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 39222 times.
✓ Branch 4 taken 13074 times.
 52296 Vec3s E[3]; // row first eigen-vectors
946 13074 Scalar s[3] = {0, 0, 0};
947
948
1/2
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
 13074 getCovariance(v, NULL, NULL, NULL, 16, M);
949
1/2
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
 13074 eigen(M, s, E);
950
951 int min, mid, max;
952
2/2
✓ Branch 0 taken 1269 times.
✓ Branch 1 taken 11805 times.
 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
2/2
✓ Branch 0 taken 5051 times.
✓ Branch 1 taken 8023 times.
 13074 if (s[2] < s[min]) {
960 5051 mid = min;
961 5051 min = 2;
962
2/2
✓ Branch 0 taken 916 times.
✓ Branch 1 taken 7107 times.
 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
7/14
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13074 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13074 times.
✗ Branch 20 not taken.
 13074 bv.axes.col(0) << E[0][max], E[1][max], E[2][max];
971
7/14
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13074 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13074 times.
✗ Branch 20 not taken.
 13074 bv.axes.col(1) << E[0][mid], E[1][mid], E[2][mid];
972
6/12
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13074 times.
✗ Branch 17 not taken.
 13074 bv.axes.col(2) << E[1][max] * E[2][mid] - E[1][mid] * E[2][max],
973
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 13074 E[0][mid] * E[2][max] - E[0][max] * E[2][mid],
974
5/10
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13074 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13074 times.
✗ Branch 14 not taken.
 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
1/2
✓ Branch 1 taken 13074 times.
✗ Branch 2 not taken.
 13074 bv.length, bv.radius);
979
980 26148 return bv;
981 }
982
983 964 Scalar RSS::distance(const RSS& other, Vec3s* P, Vec3s* 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
3/6
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
 964 Matrix3s R(axes.transpose() * other.axes);
988
4/8
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 964 times.
✗ Branch 11 not taken.
 964 Vec3s T(axes.transpose() * (other.Tr - Tr));
989
990
1/2
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
 964 Scalar dist = rectDistance(R, T, length, other.length, P, Q);
991 964 dist -= (radius + other.radius);
992
2/2
✓ Branch 0 taken 686 times.
✓ Branch 1 taken 278 times.
 964 return (dist < (Scalar)0.0) ? (Scalar)0.0 : dist;
993 }
994
995 1595056 Scalar distance(const Matrix3s& R0, const Vec3s& T0, const RSS& b1,
996 const RSS& b2, Vec3s* P, Vec3s* Q) {
997
4/8
✓ Branch 1 taken 1595056 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1595056 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1595056 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1595056 times.
✗ Branch 11 not taken.
 1595056 Matrix3s R(b1.axes.transpose() * R0 * b2.axes);
998
4/8
✓ Branch 1 taken 1595056 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1595056 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1595056 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1595056 times.
✗ Branch 11 not taken.
 1595056 Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr);
999
1000
3/6
✓ Branch 1 taken 1595056 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1595056 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1595056 times.
✗ Branch 8 not taken.
 1595056 Vec3s T(b1.axes.transpose() * Ttemp);
1001
1002
1/2
✓ Branch 1 taken 1595056 times.
✗ Branch 2 not taken.
 1595056 Scalar dist = rectDistance(R, T, b1.length, b2.length, P, Q);
1003 1595056 dist -= (b1.radius + b2.radius);
1004
2/2
✓ Branch 0 taken 17791 times.
✓ Branch 1 taken 1577265 times.
 1595056 return (dist < (Scalar)0.0) ? (Scalar)0.0 : dist;
1005 }
1006
1007 ✗ RSS translate(const RSS& bv, const Vec3s& t) {
1008 ✗ RSS res(bv);
1009 ✗ res.Tr += t;
1010 ✗ return res;
1011 }
1012
1013 } // namespace coal
1014