GCC Code Coverage Report

Directory: ./
File: src/BV/RSS.cpp
Date: 2025-04-01 09:23:31
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 3725914 void clipToRange(Scalar& val, Scalar a, Scalar b) {
49
2/2
✓ Branch 0 taken 1366094 times.
✓ Branch 1 taken 2359820 times.
 3725914 if (val < a)
50 1366094 val = a;
51
2/2
✓ Branch 0 taken 1410418 times.
✓ Branch 1 taken 949402 times.
 2359820 else if (val > b)
52 1410418 val = b;
53 3725914 }
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 1559992 void segCoords(Scalar& t, Scalar& u, Scalar a, Scalar b, Scalar A_dot_B,
67 Scalar A_dot_T, Scalar B_dot_T) {
68 1559992 Scalar denom = 1 - A_dot_B * A_dot_B;
69
70
2/2
✓ Branch 0 taken 91908 times.
✓ Branch 1 taken 1468084 times.
 1559992 if (denom == 0)
71 91908 t = 0;
72 else {
73 1468084 t = (A_dot_T - B_dot_T * A_dot_B) / denom;
74 1468084 clipToRange(t, 0, a);
75 }
76
77 1559992 u = t * A_dot_B - B_dot_T;
78
2/2
✓ Branch 0 taken 652096 times.
✓ Branch 1 taken 907896 times.
 1559992 if (u < 0) {
79 652096 u = 0;
80 652096 t = A_dot_T;
81 652096 clipToRange(t, 0, a);
82
2/2
✓ Branch 0 taken 686480 times.
✓ Branch 1 taken 221416 times.
 907896 } else if (u > b) {
83 686480 u = b;
84 686480 t = u * A_dot_B + A_dot_T;
85 686480 clipToRange(t, 0, a);
86 }
87 1559992 }
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 459657 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 459627 times.
 459657 if (fabs(Anorm_dot_B) < 1e-7) return false;
97
98 Scalar t, u, v;
99
100 459627 u = -Anorm_dot_T / Anorm_dot_B;
101
1/2
✓ Branch 1 taken 459627 times.
✗ Branch 2 not taken.
 459627 clipToRange(u, 0, b);
102
103 459627 t = u * A_dot_B + A_dot_T;
104
1/2
✓ Branch 1 taken 459627 times.
✗ Branch 2 not taken.
 459627 clipToRange(t, 0, a);
105
106 459627 v = t * A_dot_B - B_dot_T;
107
108
2/2
✓ Branch 0 taken 228351 times.
✓ Branch 1 taken 231276 times.
 459627 if (Anorm_dot_B > 0) {
109
2/2
✓ Branch 0 taken 99020 times.
✓ Branch 1 taken 129331 times.
 228351 if (v > (u + 1e-7)) return true;
110 } else {
111
2/2
✓ Branch 0 taken 86463 times.
✓ Branch 1 taken 144813 times.
 231276 if (v < (u - 1e-7)) return true;
112 }
113 274144 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 1624301 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 1624301 times.
✗ Branch 2 not taken.
 1624301 A0_dot_B0 = Rab(0, 0);
124
1/2
✓ Branch 1 taken 1624301 times.
✗ Branch 2 not taken.
 1624301 A0_dot_B1 = Rab(0, 1);
125
1/2
✓ Branch 1 taken 1624301 times.
✗ Branch 2 not taken.
 1624301 A1_dot_B0 = Rab(1, 0);
126
1/2
✓ Branch 1 taken 1624301 times.
✗ Branch 2 not taken.
 1624301 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 1624301 aA0_dot_B0 = a[0] * A0_dot_B0;
132 1624301 aA0_dot_B1 = a[0] * A0_dot_B1;
133 1624301 aA1_dot_B0 = a[1] * A1_dot_B0;
134 1624301 aA1_dot_B1 = a[1] * A1_dot_B1;
135 1624301 bA0_dot_B0 = b[0] * A0_dot_B0;
136 1624301 bA1_dot_B0 = b[0] * A1_dot_B0;
137 1624301 bA0_dot_B1 = b[1] * A0_dot_B1;
138 1624301 bA1_dot_B1 = b[1] * A1_dot_B1;
139
140
3/6
✓ Branch 1 taken 1624301 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1624301 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1624301 times.
✗ Branch 8 not taken.
 1624301 Vec3s Tba(Rab.transpose() * Tab);
141
142
1/2
✓ Branch 1 taken 1624301 times.
✗ Branch 2 not taken.
 1624301 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 1624301 times.
✗ Branch 2 not taken.
 1624301 ALL_x = -Tba[0];
152 1624301 ALU_x = ALL_x + aA1_dot_B0;
153 1624301 AUL_x = ALL_x + aA0_dot_B0;
154 1624301 AUU_x = ALU_x + aA0_dot_B0;
155
156
2/2
✓ Branch 0 taken 859425 times.
✓ Branch 1 taken 764876 times.
 1624301 if (ALL_x < ALU_x) {
157 859425 LA1_lx = ALL_x;
158 859425 LA1_ux = ALU_x;
159 859425 UA1_lx = AUL_x;
160 859425 UA1_ux = AUU_x;
161 } else {
162 764876 LA1_lx = ALU_x;
163 764876 LA1_ux = ALL_x;
164 764876 UA1_lx = AUU_x;
165 764876 UA1_ux = AUL_x;
166 }
167
168
1/2
✓ Branch 1 taken 1624301 times.
✗ Branch 2 not taken.
 1624301 BLL_x = Tab[0];
169 1624301 BLU_x = BLL_x + bA0_dot_B1;
170 1624301 BUL_x = BLL_x + bA0_dot_B0;
171 1624301 BUU_x = BLU_x + bA0_dot_B0;
172
173
2/2
✓ Branch 0 taken 867273 times.
✓ Branch 1 taken 757028 times.
 1624301 if (BLL_x < BLU_x) {
174 867273 LB1_lx = BLL_x;
175 867273 LB1_ux = BLU_x;
176 867273 UB1_lx = BUL_x;
177 867273 UB1_ux = BUU_x;
178 } else {
179 757028 LB1_lx = BLU_x;
180 757028 LB1_ux = BLL_x;
181 757028 UB1_lx = BUU_x;
182 757028 UB1_ux = BUL_x;
183 }
184
185 // UA1, UB1
186
187
4/4
✓ Branch 0 taken 825553 times.
✓ Branch 1 taken 798748 times.
✓ Branch 2 taken 395908 times.
✓ Branch 3 taken 429645 times.
 1624301 if ((UA1_ux > b[0]) && (UB1_ux > a[0])) {
188 812303 if (((UA1_lx > b[0]) ||
189
3/4
✓ Branch 1 taken 20487 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9622 times.
✓ Branch 4 taken 10865 times.
 20487 inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], A1_dot_B1,
190
7/10
✓ Branch 0 taken 20487 times.
✓ Branch 1 taken 375421 times.
✓ Branch 3 taken 20487 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 20487 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 20487 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 369174 times.
✓ Branch 12 taken 26734 times.
 791816 aA0_dot_B1 - Tba[1], -Tab[1] - bA1_dot_B0)) &&
191
2/2
✓ Branch 0 taken 26610 times.
✓ Branch 1 taken 358433 times.
 385043 ((UB1_lx > a[0]) ||
192
3/4
✓ Branch 1 taken 26610 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10741 times.
✓ Branch 4 taken 15869 times.
 26610 inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], A1_dot_B1,
193
3/6
✓ Branch 1 taken 26610 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26610 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26610 times.
✗ Branch 8 not taken.
 26610 Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) {
194
1/2
✓ Branch 1 taken 369174 times.
✗ Branch 2 not taken.
 369174 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
195
2/4
✓ Branch 1 taken 369174 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 369174 times.
✗ Branch 5 not taken.
 369174 Tba[1] - aA0_dot_B1);
196
197
4/8
✓ Branch 1 taken 369174 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 369174 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 369174 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 369174 times.
✗ Branch 11 not taken.
 369174 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0];
198
4/8
✓ Branch 1 taken 369174 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 369174 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 369174 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 369174 times.
✗ Branch 11 not taken.
 369174 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
199
4/8
✓ Branch 1 taken 369174 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 369174 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 369174 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 369174 times.
✗ Branch 11 not taken.
 369174 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
200
201
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 369174 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 369174 if (P && Q) {
202 ✗ *P << a[0], t, 0;
203 ✗ *Q = S + (*P);
204 }
205
206
1/2
✓ Branch 1 taken 369174 times.
✗ Branch 2 not taken.
 369174 return S.norm();
207 }
208 }
209
210 // UA1, LB1
211
212
4/4
✓ Branch 0 taken 694038 times.
✓ Branch 1 taken 561089 times.
✓ Branch 2 taken 319667 times.
✓ Branch 3 taken 374371 times.
 1255127 if ((UA1_lx < 0) && (LB1_ux > a[0])) {
213
3/4
✓ Branch 1 taken 20106 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7801 times.
✓ Branch 4 taken 12305 times.
 20106 if (((UA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
214
7/10
✓ Branch 0 taken 20106 times.
✓ Branch 1 taken 299561 times.
✓ Branch 3 taken 20106 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 20106 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 20106 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 297177 times.
✓ Branch 12 taken 22490 times.
 639334 A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) &&
215
2/2
✓ Branch 0 taken 23198 times.
✓ Branch 1 taken 284164 times.
 307362 ((LB1_lx > a[0]) ||
216
4/6
✓ Branch 1 taken 23198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23198 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 13013 times.
✓ Branch 7 taken 10185 times.
 23198 inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], A1_dot_B1, Tab[1],
217
2/4
✓ Branch 1 taken 23198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23198 times.
✗ Branch 5 not taken.
 23198 Tba[1] - aA0_dot_B1))) {
218
3/6
✓ Branch 1 taken 297177 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 297177 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 297177 times.
✗ Branch 8 not taken.
 297177 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
219
220
3/6
✓ Branch 1 taken 297177 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 297177 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 297177 times.
✗ Branch 8 not taken.
 297177 S[0] = Tab[0] + Rab(0, 1) * u - a[0];
221
3/6
✓ Branch 1 taken 297177 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 297177 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 297177 times.
✗ Branch 8 not taken.
 297177 S[1] = Tab[1] + Rab(1, 1) * u - t;
222
3/6
✓ Branch 1 taken 297177 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 297177 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 297177 times.
✗ Branch 8 not taken.
 297177 S[2] = Tab[2] + Rab(2, 1) * u;
223
224
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 297177 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 297177 if (P && Q) {
225 ✗ *P << a[0], t, 0;
226 ✗ *Q = S + (*P);
227 }
228
229
1/2
✓ Branch 1 taken 297177 times.
✗ Branch 2 not taken.
 297177 return S.norm();
230 }
231 }
232
233 // LA1, UB1
234
235
4/4
✓ Branch 0 taken 443382 times.
✓ Branch 1 taken 514568 times.
✓ Branch 2 taken 328925 times.
✓ Branch 3 taken 114457 times.
 957950 if ((LA1_ux > b[0]) && (UB1_lx < 0)) {
236 676874 if (((LA1_lx > b[0]) ||
237
4/6
✓ Branch 1 taken 19024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19024 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 9742 times.
✓ Branch 7 taken 9282 times.
 19024 inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], A1_dot_B1, -Tba[1],
238
8/10
✓ Branch 0 taken 19024 times.
✓ Branch 1 taken 309901 times.
✓ Branch 3 taken 19024 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19024 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 26244 times.
✓ Branch 9 taken 293399 times.
✓ Branch 10 taken 307418 times.
✓ Branch 11 taken 21507 times.
 374193 -Tab[1] - bA1_dot_B0)) &&
239
3/4
✓ Branch 1 taken 26244 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 14019 times.
✓ Branch 4 taken 12225 times.
 26244 ((UB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
240
3/6
✓ Branch 1 taken 26244 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26244 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26244 times.
✗ Branch 8 not taken.
 26244 A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) {
241
3/6
✓ Branch 1 taken 307418 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 307418 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 307418 times.
✗ Branch 8 not taken.
 307418 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
242
243
4/8
✓ Branch 1 taken 307418 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 307418 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 307418 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 307418 times.
✗ Branch 11 not taken.
 307418 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u;
244
4/8
✓ Branch 1 taken 307418 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 307418 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 307418 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 307418 times.
✗ Branch 11 not taken.
 307418 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
245
4/8
✓ Branch 1 taken 307418 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 307418 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 307418 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 307418 times.
✗ Branch 11 not taken.
 307418 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
246
247
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 307418 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 307418 if (P && Q) {
248 ✗ *P << 0, t, 0;
249 ✗ *Q = S + (*P);
250 }
251
252
1/2
✓ Branch 1 taken 307418 times.
✗ Branch 2 not taken.
 307418 return S.norm();
253 }
254 }
255
256 // LA1, LB1
257
258
4/4
✓ Branch 0 taken 415561 times.
✓ Branch 1 taken 234971 times.
✓ Branch 2 taken 305971 times.
✓ Branch 3 taken 109590 times.
 650532 if ((LA1_lx < 0) && (LB1_lx < 0)) {
259
3/4
✓ Branch 1 taken 19135 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6185 times.
✓ Branch 4 taken 12950 times.
 19135 if (((LA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
260
9/12
✓ Branch 0 taken 19135 times.
✓ Branch 1 taken 286836 times.
✓ Branch 3 taken 19135 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19135 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19135 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 17431 times.
✓ Branch 12 taken 275590 times.
✓ Branch 13 taken 282847 times.
✓ Branch 14 taken 23124 times.
 342537 -Tba[1], -Tab[1])) &&
261
3/4
✓ Branch 1 taken 17431 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7257 times.
✓ Branch 4 taken 10174 times.
 17431 ((LB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
262
3/6
✓ Branch 1 taken 17431 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17431 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17431 times.
✗ Branch 8 not taken.
 17431 Tab[1], Tba[1]))) {
263
3/6
✓ Branch 1 taken 282847 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 282847 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 282847 times.
✗ Branch 8 not taken.
 282847 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
264
265
3/6
✓ Branch 1 taken 282847 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 282847 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 282847 times.
✗ Branch 8 not taken.
 282847 S[0] = Tab[0] + Rab(0, 1) * u;
266
3/6
✓ Branch 1 taken 282847 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 282847 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 282847 times.
✗ Branch 8 not taken.
 282847 S[1] = Tab[1] + Rab(1, 1) * u - t;
267
3/6
✓ Branch 1 taken 282847 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 282847 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 282847 times.
✗ Branch 8 not taken.
 282847 S[2] = Tab[2] + Rab(2, 1) * u;
268
269
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 282847 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 282847 if (P && Q) {
270 ✗ *P << 0, t, 0;
271 ✗ *Q = S + (*P);
272 }
273
274
1/2
✓ Branch 1 taken 282847 times.
✗ Branch 2 not taken.
 282847 return S.norm();
275 }
276 }
277
278 Scalar ALL_y, ALU_y, AUL_y, AUU_y;
279
280
1/2
✓ Branch 1 taken 367685 times.
✗ Branch 2 not taken.
 367685 ALL_y = -Tba[1];
281 367685 ALU_y = ALL_y + aA1_dot_B1;
282 367685 AUL_y = ALL_y + aA0_dot_B1;
283 367685 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 196502 times.
✓ Branch 1 taken 171183 times.
 367685 if (ALL_y < ALU_y) {
288 196502 LA1_ly = ALL_y;
289 196502 LA1_uy = ALU_y;
290 196502 UA1_ly = AUL_y;
291 196502 UA1_uy = AUU_y;
292 } else {
293 171183 LA1_ly = ALU_y;
294 171183 LA1_uy = ALL_y;
295 171183 UA1_ly = AUU_y;
296 171183 UA1_uy = AUL_y;
297 }
298
299
2/2
✓ Branch 0 taken 190294 times.
✓ Branch 1 taken 177391 times.
 367685 if (BLL_x < BUL_x) {
300 190294 LB0_lx = BLL_x;
301 190294 LB0_ux = BUL_x;
302 190294 UB0_lx = BLU_x;
303 190294 UB0_ux = BUU_x;
304 } else {
305 177391 LB0_lx = BUL_x;
306 177391 LB0_ux = BLL_x;
307 177391 UB0_lx = BUU_x;
308 177391 UB0_ux = BLU_x;
309 }
310
311 // UA1, UB0
312
313
4/4
✓ Branch 0 taken 163332 times.
✓ Branch 1 taken 204353 times.
✓ Branch 2 taken 47165 times.
✓ Branch 3 taken 116167 times.
 367685 if ((UA1_uy > b[1]) && (UB0_ux > a[0])) {
314 101328 if (((UA1_ly > b[1]) ||
315
3/4
✓ Branch 1 taken 6998 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3502 times.
✓ Branch 4 taken 3496 times.
 6998 inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], A1_dot_B0,
316
7/10
✓ Branch 0 taken 6998 times.
✓ Branch 1 taken 40167 times.
✓ Branch 3 taken 6998 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6998 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6998 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 18945 times.
✓ Branch 12 taken 28220 times.
 94330 aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) &&
317
2/2
✓ Branch 0 taken 28745 times.
✓ Branch 1 taken 14924 times.
 43669 ((UB0_lx > a[0]) ||
318
3/4
✓ Branch 1 taken 28745 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4021 times.
✓ Branch 4 taken 24724 times.
 28745 inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, A1_dot_B0,
319
3/6
✓ Branch 1 taken 28745 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28745 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28745 times.
✗ Branch 8 not taken.
 28745 Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) {
320
1/2
✓ Branch 1 taken 18945 times.
✗ Branch 2 not taken.
 18945 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
321
2/4
✓ Branch 1 taken 18945 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18945 times.
✗ Branch 5 not taken.
 18945 Tba[0] - aA0_dot_B0);
322
323
4/8
✓ Branch 1 taken 18945 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18945 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18945 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18945 times.
✗ Branch 11 not taken.
 18945 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0];
324
4/8
✓ Branch 1 taken 18945 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18945 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18945 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18945 times.
✗ Branch 11 not taken.
 18945 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
325
4/8
✓ Branch 1 taken 18945 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18945 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18945 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18945 times.
✗ Branch 11 not taken.
 18945 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
326
327
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 18945 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 18945 if (P && Q) {
328 ✗ *P << a[0], t, 0;
329 ✗ *Q = S + (*P);
330 }
331
332
1/2
✓ Branch 1 taken 18945 times.
✗ Branch 2 not taken.
 18945 return S.norm();
333 }
334 }
335
336 // UA1, LB0
337
338
4/4
✓ Branch 0 taken 197079 times.
✓ Branch 1 taken 151661 times.
✓ Branch 2 taken 58762 times.
✓ Branch 3 taken 138317 times.
 348740 if ((UA1_ly < 0) && (LB0_ux > a[0])) {
339
3/4
✓ Branch 1 taken 7921 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4663 times.
✓ Branch 4 taken 3258 times.
 7921 if (((UA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1,
340
7/10
✓ Branch 0 taken 7921 times.
✓ Branch 1 taken 50841 times.
✓ Branch 3 taken 7921 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7921 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7921 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 37193 times.
✓ Branch 12 taken 21569 times.
 117524 A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1])) &&
341
2/2
✓ Branch 0 taken 24020 times.
✓ Branch 1 taken 31484 times.
 55504 ((LB0_lx > a[0]) ||
342
4/6
✓ Branch 1 taken 24020 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24020 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5709 times.
✓ Branch 7 taken 18311 times.
 24020 inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], A1_dot_B0, Tab[1],
343
2/4
✓ Branch 1 taken 24020 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24020 times.
✗ Branch 5 not taken.
 24020 Tba[0] - aA0_dot_B0))) {
344
3/6
✓ Branch 1 taken 37193 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37193 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37193 times.
✗ Branch 8 not taken.
 37193 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
345
346
3/6
✓ Branch 1 taken 37193 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37193 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37193 times.
✗ Branch 8 not taken.
 37193 S[0] = Tab[0] + Rab(0, 0) * u - a[0];
347
3/6
✓ Branch 1 taken 37193 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37193 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37193 times.
✗ Branch 8 not taken.
 37193 S[1] = Tab[1] + Rab(1, 0) * u - t;
348
3/6
✓ Branch 1 taken 37193 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37193 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37193 times.
✗ Branch 8 not taken.
 37193 S[2] = Tab[2] + Rab(2, 0) * u;
349
350
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 37193 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 37193 if (P && Q) {
351 ✗ *P << a[0], t, 0;
352 ✗ *Q = S + (*P);
353 }
354
355
1/2
✓ Branch 1 taken 37193 times.
✗ Branch 2 not taken.
 37193 return S.norm();
356 }
357 }
358
359 // LA1, UB0
360
361
4/4
✓ Branch 0 taken 133972 times.
✓ Branch 1 taken 177575 times.
✓ Branch 2 taken 40062 times.
✓ Branch 3 taken 93910 times.
 311547 if ((LA1_uy > b[1]) && (UB0_lx < 0)) {
362 87660 if (((LA1_ly > b[1]) ||
363
4/6
✓ Branch 1 taken 7536 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7536 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4145 times.
✓ Branch 7 taken 3391 times.
 7536 inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], A1_dot_B0, -Tba[0],
364
8/10
✓ Branch 0 taken 7536 times.
✓ Branch 1 taken 32526 times.
✓ Branch 3 taken 7536 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7536 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 22680 times.
✓ Branch 9 taken 13991 times.
✓ Branch 10 taken 19913 times.
✓ Branch 11 taken 20149 times.
 70278 -Tab[1] - bA1_dot_B1)) &&
365
366
3/4
✓ Branch 1 taken 22680 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5922 times.
✓ Branch 4 taken 16758 times.
 22680 ((UB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1,
367
3/6
✓ Branch 1 taken 22680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22680 times.
✗ Branch 8 not taken.
 22680 A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]))) {
368
3/6
✓ Branch 1 taken 19913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19913 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19913 times.
✗ Branch 8 not taken.
 19913 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
369
370
4/8
✓ Branch 1 taken 19913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19913 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19913 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19913 times.
✗ Branch 11 not taken.
 19913 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u;
371
4/8
✓ Branch 1 taken 19913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19913 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19913 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19913 times.
✗ Branch 11 not taken.
 19913 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
372
4/8
✓ Branch 1 taken 19913 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19913 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19913 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19913 times.
✗ Branch 11 not taken.
 19913 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
373
374
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 19913 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 19913 if (P && Q) {
375 ✗ *P << 0, t, 0;
376 ✗ *Q = S + (*P);
377 }
378
379
1/2
✓ Branch 1 taken 19913 times.
✗ Branch 2 not taken.
 19913 return S.norm();
380 }
381 }
382
383 // LA1, LB0
384
385
4/4
✓ Branch 0 taken 170675 times.
✓ Branch 1 taken 120959 times.
✓ Branch 2 taken 76882 times.
✓ Branch 3 taken 93793 times.
 291634 if ((LA1_ly < 0) && (LB0_lx < 0)) {
386
3/4
✓ Branch 1 taken 7348 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3098 times.
✓ Branch 4 taken 4250 times.
 7348 if (((LA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
387
9/12
✓ Branch 0 taken 7348 times.
✓ Branch 1 taken 69534 times.
✓ Branch 3 taken 7348 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7348 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7348 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 28623 times.
✓ Branch 12 taken 44009 times.
✓ Branch 13 taken 50655 times.
✓ Branch 14 taken 26227 times.
 112853 -Tba[0], -Tab[1])) &&
388
3/4
✓ Branch 1 taken 28623 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6646 times.
✓ Branch 4 taken 21977 times.
 28623 ((LB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
389
3/6
✓ Branch 1 taken 28623 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28623 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28623 times.
✗ Branch 8 not taken.
 28623 Tab[1], Tba[0]))) {
390
3/6
✓ Branch 1 taken 50655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50655 times.
✗ Branch 8 not taken.
 50655 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
391
392
3/6
✓ Branch 1 taken 50655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50655 times.
✗ Branch 8 not taken.
 50655 S[0] = Tab[0] + Rab(0, 0) * u;
393
3/6
✓ Branch 1 taken 50655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50655 times.
✗ Branch 8 not taken.
 50655 S[1] = Tab[1] + Rab(1, 0) * u - t;
394
3/6
✓ Branch 1 taken 50655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50655 times.
✗ Branch 8 not taken.
 50655 S[2] = Tab[2] + Rab(2, 0) * u;
395
396
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 50655 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 50655 if (P && Q) {
397 ✗ *P << 0, t, 0;
398 ✗ *Q = S + (*P);
399 }
400
401
1/2
✓ Branch 1 taken 50655 times.
✗ Branch 2 not taken.
 50655 return S.norm();
402 }
403 }
404
405 Scalar BLL_y, BLU_y, BUL_y, BUU_y;
406
407
1/2
✓ Branch 1 taken 240979 times.
✗ Branch 2 not taken.
 240979 BLL_y = Tab[1];
408 240979 BLU_y = BLL_y + bA1_dot_B1;
409 240979 BUL_y = BLL_y + bA1_dot_B0;
410 240979 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 124178 times.
✓ Branch 1 taken 116801 times.
 240979 if (ALL_x < AUL_x) {
415 124178 LA0_lx = ALL_x;
416 124178 LA0_ux = AUL_x;
417 124178 UA0_lx = ALU_x;
418 124178 UA0_ux = AUU_x;
419 } else {
420 116801 LA0_lx = AUL_x;
421 116801 LA0_ux = ALL_x;
422 116801 UA0_lx = AUU_x;
423 116801 UA0_ux = ALU_x;
424 }
425
426
2/2
✓ Branch 0 taken 130867 times.
✓ Branch 1 taken 110112 times.
 240979 if (BLL_y < BLU_y) {
427 130867 LB1_ly = BLL_y;
428 130867 LB1_uy = BLU_y;
429 130867 UB1_ly = BUL_y;
430 130867 UB1_uy = BUU_y;
431 } else {
432 110112 LB1_ly = BLU_y;
433 110112 LB1_uy = BLL_y;
434 110112 UB1_ly = BUU_y;
435 110112 UB1_uy = BUL_y;
436 }
437
438 // UA0, UB1
439
440
4/4
✓ Branch 0 taken 132124 times.
✓ Branch 1 taken 108855 times.
✓ Branch 2 taken 47416 times.
✓ Branch 3 taken 84708 times.
 240979 if ((UA0_ux > b[0]) && (UB1_uy > a[1])) {
441 111371 if (((UA0_lx > b[0]) ||
442
3/4
✓ Branch 1 taken 16539 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7108 times.
✓ Branch 4 taken 9431 times.
 16539 inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], A0_dot_B1,
443
7/10
✓ Branch 0 taken 16539 times.
✓ Branch 1 taken 30877 times.
✓ Branch 3 taken 16539 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16539 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16539 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 35712 times.
✓ Branch 12 taken 11704 times.
 94832 aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) &&
444
2/2
✓ Branch 0 taken 4488 times.
✓ Branch 1 taken 33497 times.
 37985 ((UB1_ly > a[1]) ||
445
3/4
✓ Branch 1 taken 4488 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2215 times.
✓ Branch 4 taken 2273 times.
 4488 inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, A0_dot_B1,
446
3/6
✓ Branch 1 taken 4488 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4488 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4488 times.
✗ Branch 8 not taken.
 4488 Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) {
447
1/2
✓ Branch 1 taken 35712 times.
✗ Branch 2 not taken.
 35712 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
448
2/4
✓ Branch 1 taken 35712 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35712 times.
✗ Branch 5 not taken.
 35712 Tba[1] - aA1_dot_B1);
449
450
4/8
✓ Branch 1 taken 35712 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35712 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35712 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35712 times.
✗ Branch 11 not taken.
 35712 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
451
4/8
✓ Branch 1 taken 35712 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35712 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35712 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35712 times.
✗ Branch 11 not taken.
 35712 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1];
452
4/8
✓ Branch 1 taken 35712 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35712 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35712 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35712 times.
✗ Branch 11 not taken.
 35712 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
453
454
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 35712 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 35712 if (P && Q) {
455 ✗ *P << t, a[1], 0;
456 ✗ *Q = S + (*P);
457 }
458
459
1/2
✓ Branch 1 taken 35712 times.
✗ Branch 2 not taken.
 35712 return S.norm();
460 }
461 }
462
463 // UA0, LB1
464
465
4/4
✓ Branch 0 taken 110765 times.
✓ Branch 1 taken 94502 times.
✓ Branch 2 taken 45252 times.
✓ Branch 3 taken 65513 times.
 205267 if ((UA0_lx < 0) && (LB1_uy > a[1])) {
466
3/4
✓ Branch 1 taken 16277 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6959 times.
✓ Branch 4 taken 9318 times.
 16277 if (((UA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0,
467
7/10
✓ Branch 0 taken 16277 times.
✓ Branch 1 taken 28975 times.
✓ Branch 3 taken 16277 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16277 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16277 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 34359 times.
✓ Branch 12 taken 10893 times.
 90504 A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0])) &&
468
2/2
✓ Branch 0 taken 5178 times.
✓ Branch 1 taken 30756 times.
 35934 ((LB1_ly > a[1]) ||
469
4/6
✓ Branch 1 taken 5178 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5178 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3603 times.
✓ Branch 7 taken 1575 times.
 5178 inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
470
2/4
✓ Branch 1 taken 5178 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5178 times.
✗ Branch 5 not taken.
 5178 Tba[1] - aA1_dot_B1))) {
471
3/6
✓ Branch 1 taken 34359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34359 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34359 times.
✗ Branch 8 not taken.
 34359 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
472
473
3/6
✓ Branch 1 taken 34359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34359 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34359 times.
✗ Branch 8 not taken.
 34359 S[0] = Tab[0] + Rab(0, 1) * u - t;
474
3/6
✓ Branch 1 taken 34359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34359 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34359 times.
✗ Branch 8 not taken.
 34359 S[1] = Tab[1] + Rab(1, 1) * u - a[1];
475
3/6
✓ Branch 1 taken 34359 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34359 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34359 times.
✗ Branch 8 not taken.
 34359 S[2] = Tab[2] + Rab(2, 1) * u;
476
477
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 34359 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 34359 if (P && Q) {
478 ✗ *P << t, a[1], 0;
479 ✗ *Q = S + (*P);
480 }
481
482
1/2
✓ Branch 1 taken 34359 times.
✗ Branch 2 not taken.
 34359 return S.norm();
483 }
484 }
485
486 // LA0, UB1
487
488
4/4
✓ Branch 0 taken 94233 times.
✓ Branch 1 taken 76675 times.
✓ Branch 2 taken 63688 times.
✓ Branch 3 taken 30545 times.
 170908 if ((LA0_ux > b[0]) && (UB1_ly < 0)) {
489 145462 if (((LA0_lx > b[0]) ||
490
4/6
✓ Branch 1 taken 18086 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18086 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8780 times.
✓ Branch 7 taken 9306 times.
 18086 inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
491
8/10
✓ Branch 0 taken 18086 times.
✓ Branch 1 taken 45602 times.
✓ Branch 3 taken 18086 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18086 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 5635 times.
✓ Branch 9 taken 48747 times.
✓ Branch 10 taken 52411 times.
✓ Branch 11 taken 11277 times.
 87409 -bA0_dot_B0 - Tab[0])) &&
492
3/4
✓ Branch 1 taken 5635 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3664 times.
✓ Branch 4 taken 1971 times.
 5635 ((UB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0,
493
3/6
✓ Branch 1 taken 5635 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5635 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5635 times.
✗ Branch 8 not taken.
 5635 A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]))) {
494
3/6
✓ Branch 1 taken 52411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52411 times.
✗ Branch 8 not taken.
 52411 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
495
496
4/8
✓ Branch 1 taken 52411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52411 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52411 times.
✗ Branch 11 not taken.
 52411 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
497
4/8
✓ Branch 1 taken 52411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52411 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52411 times.
✗ Branch 11 not taken.
 52411 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u;
498
4/8
✓ Branch 1 taken 52411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52411 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52411 times.
✗ Branch 11 not taken.
 52411 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
499
500
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 52411 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 52411 if (P && Q) {
501 ✗ *P << t, 0, 0;
502 ✗ *Q = S + (*P);
503 }
504
505
1/2
✓ Branch 1 taken 52411 times.
✗ Branch 2 not taken.
 52411 return S.norm();
506 }
507 }
508
509 // LA0, LB1
510
511
4/4
✓ Branch 0 taken 78410 times.
✓ Branch 1 taken 40087 times.
✓ Branch 2 taken 42805 times.
✓ Branch 3 taken 35605 times.
 118497 if ((LA0_lx < 0) && (LB1_ly < 0)) {
512
3/4
✓ Branch 1 taken 15444 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8466 times.
✓ Branch 4 taken 6978 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 27361 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 3729 times.
✓ Branch 12 taken 32098 times.
✓ Branch 13 taken 33703 times.
✓ Branch 14 taken 9102 times.
 61978 -Tba[1], -Tab[0])) &&
514
3/4
✓ Branch 1 taken 3729 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1605 times.
✓ Branch 4 taken 2124 times.
 3729 ((LB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
515
3/6
✓ Branch 1 taken 3729 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3729 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3729 times.
✗ Branch 8 not taken.
 3729 Tab[0], Tba[1]))) {
516
3/6
✓ Branch 1 taken 33703 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33703 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33703 times.
✗ Branch 8 not taken.
 33703 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
517
518
3/6
✓ Branch 1 taken 33703 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33703 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33703 times.
✗ Branch 8 not taken.
 33703 S[0] = Tab[0] + Rab(0, 1) * u - t;
519
3/6
✓ Branch 1 taken 33703 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33703 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33703 times.
✗ Branch 8 not taken.
 33703 S[1] = Tab[1] + Rab(1, 1) * u;
520
3/6
✓ Branch 1 taken 33703 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33703 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33703 times.
✗ Branch 8 not taken.
 33703 S[2] = Tab[2] + Rab(2, 1) * u;
521
522
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 33703 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 33703 if (P && Q) {
523 ✗ *P << t, 0, 0;
524 ✗ *Q = S + (*P);
525 }
526
527
1/2
✓ Branch 1 taken 33703 times.
✗ Branch 2 not taken.
 33703 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 42846 times.
✓ Branch 1 taken 41948 times.
 84794 if (ALL_y < AUL_y) {
534 42846 LA0_ly = ALL_y;
535 42846 LA0_uy = AUL_y;
536 42846 UA0_ly = ALU_y;
537 42846 UA0_uy = AUU_y;
538 } else {
539 41948 LA0_ly = AUL_y;
540 41948 LA0_uy = ALL_y;
541 41948 UA0_ly = AUU_y;
542 41948 UA0_uy = ALU_y;
543 }
544
545
2/2
✓ Branch 0 taken 43447 times.
✓ Branch 1 taken 41347 times.
 84794 if (BLL_y < BUL_y) {
546 43447 LB0_ly = BLL_y;
547 43447 LB0_uy = BUL_y;
548 43447 UB0_ly = BLU_y;
549 43447 UB0_uy = BUU_y;
550 } else {
551 41347 LB0_ly = BUL_y;
552 41347 LB0_uy = BLL_y;
553 41347 UB0_ly = BUU_y;
554 41347 UB0_uy = BLU_y;
555 }
556
557 // UA0, UB0
558
559
4/4
✓ Branch 0 taken 51091 times.
✓ Branch 1 taken 33703 times.
✓ Branch 2 taken 18323 times.
✓ Branch 3 taken 32768 times.
 84794 if ((UA0_uy > b[1]) && (UB0_uy > a[1])) {
560 50105 if (((UA0_ly > b[1]) ||
561
3/4
✓ Branch 1 taken 13459 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5692 times.
✓ Branch 4 taken 7767 times.
 13459 inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], A0_dot_B0,
562
7/10
✓ Branch 0 taken 13459 times.
✓ Branch 1 taken 4864 times.
✓ Branch 3 taken 13459 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13459 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13459 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4410 times.
✓ Branch 12 taken 13913 times.
 36646 aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) &&
563
2/2
✓ Branch 0 taken 8357 times.
✓ Branch 1 taken 2199 times.
 10556 ((UB0_ly > a[1]) ||
564
3/4
✓ Branch 1 taken 8357 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2211 times.
✓ Branch 4 taken 6146 times.
 8357 inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
565
3/6
✓ Branch 1 taken 8357 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8357 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8357 times.
✗ Branch 8 not taken.
 8357 Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) {
566
1/2
✓ Branch 1 taken 4410 times.
✗ Branch 2 not taken.
 4410 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
567
2/4
✓ Branch 1 taken 4410 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4410 times.
✗ Branch 5 not taken.
 4410 Tba[0] - aA1_dot_B0);
568
569
4/8
✓ Branch 1 taken 4410 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4410 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4410 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4410 times.
✗ Branch 11 not taken.
 4410 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
570
4/8
✓ Branch 1 taken 4410 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4410 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4410 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4410 times.
✗ Branch 11 not taken.
 4410 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1];
571
4/8
✓ Branch 1 taken 4410 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4410 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4410 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4410 times.
✗ Branch 11 not taken.
 4410 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
572
573
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4410 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4410 if (P && Q) {
574 ✗ *P << t, a[1], 0;
575 ✗ *Q = S + (*P);
576 }
577
578
1/2
✓ Branch 1 taken 4410 times.
✗ Branch 2 not taken.
 4410 return S.norm();
579 }
580 }
581
582 // UA0, LB0
583
584
4/4
✓ Branch 0 taken 46407 times.
✓ Branch 1 taken 33977 times.
✓ Branch 2 taken 17082 times.
✓ Branch 3 taken 29325 times.
 80384 if ((UA0_ly < 0) && (LB0_uy > a[1])) {
585
3/4
✓ Branch 1 taken 9784 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4211 times.
✓ Branch 4 taken 5573 times.
 9784 if (((UA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1,
586
7/10
✓ Branch 0 taken 9784 times.
✓ Branch 1 taken 7298 times.
✓ Branch 3 taken 9784 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9784 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9784 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 7838 times.
✓ Branch 12 taken 9244 times.
 34164 A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0])) &&
587
2/2
✓ Branch 0 taken 5955 times.
✓ Branch 1 taken 5554 times.
 11509 ((LB0_ly > a[1]) ||
588
4/6
✓ Branch 1 taken 5955 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5955 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2284 times.
✓ Branch 7 taken 3671 times.
 5955 inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], A0_dot_B0, Tab[0],
589
2/4
✓ Branch 1 taken 5955 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5955 times.
✗ Branch 5 not taken.
 5955 Tba[0] - aA1_dot_B0))) {
590
3/6
✓ Branch 1 taken 7838 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7838 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7838 times.
✗ Branch 8 not taken.
 7838 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
591
592
3/6
✓ Branch 1 taken 7838 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7838 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7838 times.
✗ Branch 8 not taken.
 7838 S[0] = Tab[0] + Rab(0, 0) * u - t;
593
3/6
✓ Branch 1 taken 7838 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7838 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7838 times.
✗ Branch 8 not taken.
 7838 S[1] = Tab[1] + Rab(1, 0) * u - a[1];
594
3/6
✓ Branch 1 taken 7838 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7838 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7838 times.
✗ Branch 8 not taken.
 7838 S[2] = Tab[2] + Rab(2, 0) * u;
595
596
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 7838 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 7838 if (P && Q) {
597 ✗ *P << t, a[1], 0;
598 ✗ *Q = S + (*P);
599 }
600
601
1/2
✓ Branch 1 taken 7838 times.
✗ Branch 2 not taken.
 7838 return S.norm();
602 }
603 }
604
605 // LA0, UB0
606
607
4/4
✓ Branch 0 taken 43747 times.
✓ Branch 1 taken 28799 times.
✓ Branch 2 taken 14685 times.
✓ Branch 3 taken 29062 times.
 72546 if ((LA0_uy > b[1]) && (UB0_ly < 0)) {
608 40166 if (((LA0_ly > b[1]) ||
609
4/6
✓ Branch 1 taken 10796 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10796 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4687 times.
✓ Branch 7 taken 6109 times.
 10796 inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
610
8/10
✓ Branch 0 taken 10796 times.
✓ Branch 1 taken 3889 times.
✓ Branch 3 taken 10796 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10796 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6315 times.
✓ Branch 9 taken 2261 times.
✓ Branch 10 taken 4894 times.
✓ Branch 11 taken 9791 times.
 31796 -Tab[0] - bA0_dot_B1)) &&
611
612
3/4
✓ Branch 1 taken 6315 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2633 times.
✓ Branch 4 taken 3682 times.
 6315 ((UB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1,
613
3/6
✓ Branch 1 taken 6315 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6315 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6315 times.
✗ Branch 8 not taken.
 6315 A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]))) {
614
3/6
✓ Branch 1 taken 4894 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4894 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4894 times.
✗ Branch 8 not taken.
 4894 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
615
616
4/8
✓ Branch 1 taken 4894 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4894 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4894 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4894 times.
✗ Branch 11 not taken.
 4894 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
617
4/8
✓ Branch 1 taken 4894 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4894 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4894 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4894 times.
✗ Branch 11 not taken.
 4894 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u;
618
4/8
✓ Branch 1 taken 4894 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4894 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4894 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4894 times.
✗ Branch 11 not taken.
 4894 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
619
620
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4894 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4894 if (P && Q) {
621 ✗ *P << t, 0, 0;
622 ✗ *Q = S + (*P);
623 }
624
625
1/2
✓ Branch 1 taken 4894 times.
✗ Branch 2 not taken.
 4894 return S.norm();
626 }
627 }
628
629 // LA0, LB0
630
631
4/4
✓ Branch 0 taken 39468 times.
✓ Branch 1 taken 28184 times.
✓ Branch 2 taken 11573 times.
✓ Branch 3 taken 27895 times.
 67652 if ((LA0_ly < 0) && (LB0_ly < 0)) {
632
3/4
✓ Branch 1 taken 8570 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3767 times.
✓ Branch 4 taken 4803 times.
 8570 if (((LA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
633
9/12
✓ Branch 0 taken 8570 times.
✓ Branch 1 taken 3003 times.
✓ Branch 3 taken 8570 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8570 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8570 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4939 times.
✓ Branch 12 taken 1831 times.
✓ Branch 13 taken 3343 times.
✓ Branch 14 taken 8230 times.
 25082 -Tba[0], -Tab[0])) &&
634
3/4
✓ Branch 1 taken 4939 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1512 times.
✓ Branch 4 taken 3427 times.
 4939 ((LB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
635
3/6
✓ Branch 1 taken 4939 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4939 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4939 times.
✗ Branch 8 not taken.
 4939 Tab[0], Tba[0]))) {
636
3/6
✓ Branch 1 taken 3343 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3343 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3343 times.
✗ Branch 8 not taken.
 3343 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
637
638
3/6
✓ Branch 1 taken 3343 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3343 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3343 times.
✗ Branch 8 not taken.
 3343 S[0] = Tab[0] + Rab(0, 0) * u - t;
639
3/6
✓ Branch 1 taken 3343 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3343 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3343 times.
✗ Branch 8 not taken.
 3343 S[1] = Tab[1] + Rab(1, 0) * u;
640
3/6
✓ Branch 1 taken 3343 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3343 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3343 times.
✗ Branch 8 not taken.
 3343 S[2] = Tab[2] + Rab(2, 0) * u;
641
642
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3343 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3343 if (P && Q) {
643 ✗ *P << t, 0, 0;
644 ✗ *Q = S + (*P);
645 }
646
647
1/2
✓ Branch 1 taken 3343 times.
✗ Branch 2 not taken.
 3343 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 64309 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33718 times.
✓ Branch 4 taken 30591 times.
 64309 if (Tab[2] > 0.0) {
656
1/2
✓ Branch 1 taken 33718 times.
✗ Branch 2 not taken.
 33718 sep1 = Tab[2];
657
4/6
✓ Branch 1 taken 33718 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20088 times.
✓ Branch 4 taken 13630 times.
✓ Branch 6 taken 20088 times.
✗ Branch 7 not taken.
 33718 if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
658
4/6
✓ Branch 1 taken 33718 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18531 times.
✓ Branch 4 taken 15187 times.
✓ Branch 6 taken 18531 times.
✗ Branch 7 not taken.
 33718 if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
659 } else {
660
1/2
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
 30591 sep1 = -Tab[2];
661
4/6
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18262 times.
✓ Branch 4 taken 12329 times.
✓ Branch 6 taken 18262 times.
✗ Branch 7 not taken.
 30591 if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
662
4/6
✓ Branch 1 taken 30591 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17230 times.
✓ Branch 4 taken 13361 times.
✓ Branch 6 taken 17230 times.
✗ Branch 7 not taken.
 30591 if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
663 }
664
665
3/4
✓ Branch 1 taken 64309 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33451 times.
✓ Branch 4 taken 30858 times.
 64309 if (Tba[2] < 0) {
666
1/2
✓ Branch 1 taken 33451 times.
✗ Branch 2 not taken.
 33451 sep2 = -Tba[2];
667
4/6
✓ Branch 1 taken 33451 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23276 times.
✓ Branch 4 taken 10175 times.
✓ Branch 6 taken 23276 times.
✗ Branch 7 not taken.
 33451 if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
668
4/6
✓ Branch 1 taken 33451 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 19971 times.
✓ Branch 4 taken 13480 times.
✓ Branch 6 taken 19971 times.
✗ Branch 7 not taken.
 33451 if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
669 } else {
670
1/2
✓ Branch 1 taken 30858 times.
✗ Branch 2 not taken.
 30858 sep2 = Tba[2];
671
4/6
✓ Branch 1 taken 30858 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23189 times.
✓ Branch 4 taken 7669 times.
✓ Branch 6 taken 23189 times.
✗ Branch 7 not taken.
 30858 if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
672
4/6
✓ Branch 1 taken 30858 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18719 times.
✓ Branch 4 taken 12139 times.
✓ Branch 6 taken 18719 times.
✗ Branch 7 not taken.
 30858 if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
673 }
674
675
4/4
✓ Branch 0 taken 44159 times.
✓ Branch 1 taken 20150 times.
✓ Branch 2 taken 34073 times.
✓ Branch 3 taken 10086 times.
 64309 if (sep1 >= sep2 && sep1 >= 0) {
676
3/4
✓ Branch 1 taken 34073 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17424 times.
✓ Branch 4 taken 16649 times.
 34073 if (Tab[2] > 0)
677
3/6
✓ Branch 1 taken 17424 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17424 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17424 times.
✗ Branch 8 not taken.
 17424 S << 0, 0, sep1;
678 else
679
3/6
✓ Branch 1 taken 16649 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16649 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16649 times.
✗ Branch 8 not taken.
 16649 S << 0, 0, -sep1;
680
681
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 34073 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 34073 if (P && Q) {
682 ✗ *Q = S;
683 ✗ P->setZero();
684 }
685 }
686
687
4/4
✓ Branch 0 taken 20315 times.
✓ Branch 1 taken 43994 times.
✓ Branch 2 taken 15229 times.
✓ Branch 3 taken 5086 times.
 64309 if (sep2 >= sep1 && sep2 >= 0) {
688
4/8
✓ Branch 1 taken 15229 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15229 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15229 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15229 times.
✗ Branch 11 not taken.
 15229 Vec3s Q_(Tab[0], Tab[1], Tab[2]);
689
1/2
✓ Branch 1 taken 15229 times.
✗ Branch 2 not taken.
 15229 Vec3s P_;
690
3/4
✓ Branch 1 taken 15229 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8618 times.
✓ Branch 4 taken 6611 times.
 15229 if (Tba[2] < 0) {
691
3/6
✓ Branch 1 taken 8618 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8618 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8618 times.
✗ Branch 8 not taken.
 8618 P_[0] = Rab(0, 2) * sep2 + Tab[0];
692
3/6
✓ Branch 1 taken 8618 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8618 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8618 times.
✗ Branch 8 not taken.
 8618 P_[1] = Rab(1, 2) * sep2 + Tab[1];
693
3/6
✓ Branch 1 taken 8618 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8618 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8618 times.
✗ Branch 8 not taken.
 8618 P_[2] = Rab(2, 2) * sep2 + Tab[2];
694 } else {
695
3/6
✓ Branch 1 taken 6611 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6611 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6611 times.
✗ Branch 8 not taken.
 6611 P_[0] = -Rab(0, 2) * sep2 + Tab[0];
696
3/6
✓ Branch 1 taken 6611 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6611 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6611 times.
✗ Branch 8 not taken.
 6611 P_[1] = -Rab(1, 2) * sep2 + Tab[1];
697
3/6
✓ Branch 1 taken 6611 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6611 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6611 times.
✗ Branch 8 not taken.
 6611 P_[2] = -Rab(2, 2) * sep2 + Tab[2];
698 }
699
700
2/4
✓ Branch 1 taken 15229 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15229 times.
✗ Branch 5 not taken.
 15229 S = Q_ - P_;
701
702
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 15229 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 15229 if (P && Q) {
703 ✗ *P = P_;
704 ✗ *Q = Q_;
705 }
706 }
707
708
2/2
✓ Branch 0 taken 43994 times.
✓ Branch 1 taken 20315 times.
 64309 Scalar sep = (sep1 > sep2 ? sep1 : sep2);
709
2/2
✓ Branch 0 taken 49135 times.
✓ Branch 1 taken 15174 times.
 64309 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 1598644 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 1598644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1598644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1598644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1598644 times.
✗ Branch 11 not taken.
 1598644 Matrix3s R(b1.axes.transpose() * R0 * b2.axes);
998
4/8
✓ Branch 1 taken 1598644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1598644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1598644 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1598644 times.
✗ Branch 11 not taken.
 1598644 Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr);
999
1000
3/6
✓ Branch 1 taken 1598644 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1598644 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1598644 times.
✗ Branch 8 not taken.
 1598644 Vec3s T(b1.axes.transpose() * Ttemp);
1001
1002
1/2
✓ Branch 1 taken 1598644 times.
✗ Branch 2 not taken.
 1598644 Scalar dist = rectDistance(R, T, b1.length, b2.length, P, Q);
1003 1598644 dist -= (b1.radius + b2.radius);
1004
2/2
✓ Branch 0 taken 17786 times.
✓ Branch 1 taken 1580858 times.
 1598644 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