GCC Code Coverage Report

Directory: ./
File: src/BV/RSS.cpp
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 448 595 75.3%
Functions: 9 13 69.2%
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 3707184 void clipToRange(Scalar& val, Scalar a, Scalar b) {
49
2/2
✓ Branch 0 taken 1358723 times.
✓ Branch 1 taken 2348461 times.
 3707184 if (val < a)
50 1358723 val = a;
51
2/2
✓ Branch 0 taken 1401656 times.
✓ Branch 1 taken 946805 times.
 2348461 else if (val > b)
52 1401656 val = b;
53 3707184 }
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 1551440 void segCoords(Scalar& t, Scalar& u, Scalar a, Scalar b, Scalar A_dot_B,
67 Scalar A_dot_T, Scalar B_dot_T) {
68 1551440 Scalar denom = 1 - A_dot_B * A_dot_B;
69
70
2/2
✓ Branch 0 taken 91931 times.
✓ Branch 1 taken 1459509 times.
 1551440 if (denom == 0)
71 91931 t = 0;
72 else {
73 1459509 t = (A_dot_T - B_dot_T * A_dot_B) / denom;
74 1459509 clipToRange(t, 0, a);
75 }
76
77 1551440 u = t * A_dot_B - B_dot_T;
78
2/2
✓ Branch 0 taken 648047 times.
✓ Branch 1 taken 903393 times.
 1551440 if (u < 0) {
79 648047 u = 0;
80 648047 t = A_dot_T;
81 648047 clipToRange(t, 0, a);
82
2/2
✓ Branch 0 taken 683200 times.
✓ Branch 1 taken 220193 times.
 903393 } else if (u > b) {
83 683200 u = b;
84 683200 t = u * A_dot_B + A_dot_T;
85 683200 clipToRange(t, 0, a);
86 }
87 1551440 }
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 458244 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 458214 times.
 458244 if (fabs(Anorm_dot_B) < 1e-7) return false;
97
98 Scalar t, u, v;
99
100 458214 u = -Anorm_dot_T / Anorm_dot_B;
101
1/2
✓ Branch 1 taken 458214 times.
✗ Branch 2 not taken.
 458214 clipToRange(u, 0, b);
102
103 458214 t = u * A_dot_B + A_dot_T;
104
1/2
✓ Branch 1 taken 458214 times.
✗ Branch 2 not taken.
 458214 clipToRange(t, 0, a);
105
106 458214 v = t * A_dot_B - B_dot_T;
107
108
2/2
✓ Branch 0 taken 227610 times.
✓ Branch 1 taken 230604 times.
 458214 if (Anorm_dot_B > 0) {
109
2/2
✓ Branch 0 taken 98701 times.
✓ Branch 1 taken 128909 times.
 227610 if (v > (u + 1e-7)) return true;
110 } else {
111
2/2
✓ Branch 0 taken 85962 times.
✓ Branch 1 taken 144642 times.
 230604 if (v < (u - 1e-7)) return true;
112 }
113 273551 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 1615643 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 1615643 times.
✗ Branch 2 not taken.
 1615643 A0_dot_B0 = Rab(0, 0);
124
1/2
✓ Branch 1 taken 1615643 times.
✗ Branch 2 not taken.
 1615643 A0_dot_B1 = Rab(0, 1);
125
1/2
✓ Branch 1 taken 1615643 times.
✗ Branch 2 not taken.
 1615643 A1_dot_B0 = Rab(1, 0);
126
1/2
✓ Branch 1 taken 1615643 times.
✗ Branch 2 not taken.
 1615643 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 1615643 aA0_dot_B0 = a[0] * A0_dot_B0;
132 1615643 aA0_dot_B1 = a[0] * A0_dot_B1;
133 1615643 aA1_dot_B0 = a[1] * A1_dot_B0;
134 1615643 aA1_dot_B1 = a[1] * A1_dot_B1;
135 1615643 bA0_dot_B0 = b[0] * A0_dot_B0;
136 1615643 bA1_dot_B0 = b[0] * A1_dot_B0;
137 1615643 bA0_dot_B1 = b[1] * A0_dot_B1;
138 1615643 bA1_dot_B1 = b[1] * A1_dot_B1;
139
140
3/6
✓ Branch 1 taken 1615643 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1615643 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1615643 times.
✗ Branch 8 not taken.
 1615643 Vec3s Tba(Rab.transpose() * Tab);
141
142
1/2
✓ Branch 1 taken 1615643 times.
✗ Branch 2 not taken.
 1615643 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 1615643 times.
✗ Branch 2 not taken.
 1615643 ALL_x = -Tba[0];
152 1615643 ALU_x = ALL_x + aA1_dot_B0;
153 1615643 AUL_x = ALL_x + aA0_dot_B0;
154 1615643 AUU_x = ALU_x + aA0_dot_B0;
155
156
2/2
✓ Branch 0 taken 855394 times.
✓ Branch 1 taken 760249 times.
 1615643 if (ALL_x < ALU_x) {
157 855394 LA1_lx = ALL_x;
158 855394 LA1_ux = ALU_x;
159 855394 UA1_lx = AUL_x;
160 855394 UA1_ux = AUU_x;
161 } else {
162 760249 LA1_lx = ALU_x;
163 760249 LA1_ux = ALL_x;
164 760249 UA1_lx = AUU_x;
165 760249 UA1_ux = AUL_x;
166 }
167
168
1/2
✓ Branch 1 taken 1615643 times.
✗ Branch 2 not taken.
 1615643 BLL_x = Tab[0];
169 1615643 BLU_x = BLL_x + bA0_dot_B1;
170 1615643 BUL_x = BLL_x + bA0_dot_B0;
171 1615643 BUU_x = BLU_x + bA0_dot_B0;
172
173
2/2
✓ Branch 0 taken 862565 times.
✓ Branch 1 taken 753078 times.
 1615643 if (BLL_x < BLU_x) {
174 862565 LB1_lx = BLL_x;
175 862565 LB1_ux = BLU_x;
176 862565 UB1_lx = BUL_x;
177 862565 UB1_ux = BUU_x;
178 } else {
179 753078 LB1_lx = BLU_x;
180 753078 LB1_ux = BLL_x;
181 753078 UB1_lx = BUU_x;
182 753078 UB1_ux = BUL_x;
183 }
184
185 // UA1, UB1
186
187
4/4
✓ Branch 0 taken 821379 times.
✓ Branch 1 taken 794264 times.
✓ Branch 2 taken 393773 times.
✓ Branch 3 taken 427606 times.
 1615643 if ((UA1_ux > b[0]) && (UB1_ux > a[0])) {
188 807914 if (((UA1_lx > b[0]) ||
189
3/4
✓ Branch 1 taken 20368 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9558 times.
✓ Branch 4 taken 10810 times.
 20368 inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], A1_dot_B1,
190
7/10
✓ Branch 0 taken 20368 times.
✓ Branch 1 taken 373405 times.
✓ Branch 3 taken 20368 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 20368 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 20368 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 367088 times.
✓ Branch 12 taken 26685 times.
 787546 aA0_dot_B1 - Tba[1], -Tab[1] - bA1_dot_B0)) &&
191
2/2
✓ Branch 0 taken 26605 times.
✓ Branch 1 taken 356358 times.
 382963 ((UB1_lx > a[0]) ||
192
3/4
✓ Branch 1 taken 26605 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10730 times.
✓ Branch 4 taken 15875 times.
 26605 inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], A1_dot_B1,
193
3/6
✓ Branch 1 taken 26605 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26605 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26605 times.
✗ Branch 8 not taken.
 26605 Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) {
194
1/2
✓ Branch 1 taken 367088 times.
✗ Branch 2 not taken.
 367088 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
195
2/4
✓ Branch 1 taken 367088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367088 times.
✗ Branch 5 not taken.
 367088 Tba[1] - aA0_dot_B1);
196
197
4/8
✓ Branch 1 taken 367088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367088 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367088 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 367088 times.
✗ Branch 11 not taken.
 367088 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0];
198
4/8
✓ Branch 1 taken 367088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367088 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367088 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 367088 times.
✗ Branch 11 not taken.
 367088 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
199
4/8
✓ Branch 1 taken 367088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367088 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367088 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 367088 times.
✗ Branch 11 not taken.
 367088 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
200
201
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 367088 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 367088 if (P && Q) {
202 ✗ *P << a[0], t, 0;
203 ✗ *Q = S + (*P);
204 }
205
206
1/2
✓ Branch 1 taken 367088 times.
✗ Branch 2 not taken.
 367088 return S.norm();
207 }
208 }
209
210 // UA1, LB1
211
212
4/4
✓ Branch 0 taken 690075 times.
✓ Branch 1 taken 558480 times.
✓ Branch 2 taken 317841 times.
✓ Branch 3 taken 372234 times.
 1248555 if ((UA1_lx < 0) && (LB1_ux > a[0])) {
213
3/4
✓ Branch 1 taken 19936 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7713 times.
✓ Branch 4 taken 12223 times.
 19936 if (((UA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
214
7/10
✓ Branch 0 taken 19936 times.
✓ Branch 1 taken 297905 times.
✓ Branch 3 taken 19936 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19936 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19936 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 295455 times.
✓ Branch 12 taken 22386 times.
 635682 A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) &&
215
2/2
✓ Branch 0 taken 23086 times.
✓ Branch 1 taken 282532 times.
 305618 ((LB1_lx > a[0]) ||
216
4/6
✓ Branch 1 taken 23086 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23086 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 12923 times.
✓ Branch 7 taken 10163 times.
 23086 inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], A1_dot_B1, Tab[1],
217
2/4
✓ Branch 1 taken 23086 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23086 times.
✗ Branch 5 not taken.
 23086 Tba[1] - aA0_dot_B1))) {
218
3/6
✓ Branch 1 taken 295455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 295455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 295455 times.
✗ Branch 8 not taken.
 295455 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
219
220
3/6
✓ Branch 1 taken 295455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 295455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 295455 times.
✗ Branch 8 not taken.
 295455 S[0] = Tab[0] + Rab(0, 1) * u - a[0];
221
3/6
✓ Branch 1 taken 295455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 295455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 295455 times.
✗ Branch 8 not taken.
 295455 S[1] = Tab[1] + Rab(1, 1) * u - t;
222
3/6
✓ Branch 1 taken 295455 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 295455 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 295455 times.
✗ Branch 8 not taken.
 295455 S[2] = Tab[2] + Rab(2, 1) * u;
223
224
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 295455 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 295455 if (P && Q) {
225 ✗ *P << a[0], t, 0;
226 ✗ *Q = S + (*P);
227 }
228
229
1/2
✓ Branch 1 taken 295455 times.
✗ Branch 2 not taken.
 295455 return S.norm();
230 }
231 }
232
233 // LA1, UB1
234
235
4/4
✓ Branch 0 taken 441110 times.
✓ Branch 1 taken 511990 times.
✓ Branch 2 taken 326792 times.
✓ Branch 3 taken 114318 times.
 953100 if ((LA1_ux > b[0]) && (UB1_lx < 0)) {
236 672533 if (((LA1_lx > b[0]) ||
237
4/6
✓ Branch 1 taken 18949 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18949 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 9725 times.
✓ Branch 7 taken 9224 times.
 18949 inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], A1_dot_B1, -Tba[1],
238
8/10
✓ Branch 0 taken 18949 times.
✓ Branch 1 taken 307843 times.
✓ Branch 3 taken 18949 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18949 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 26217 times.
✓ Branch 9 taken 291351 times.
✓ Branch 10 taken 305325 times.
✓ Branch 11 taken 21467 times.
 371958 -Tab[1] - bA1_dot_B0)) &&
239
3/4
✓ Branch 1 taken 26217 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 13974 times.
✓ Branch 4 taken 12243 times.
 26217 ((UB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
240
3/6
✓ Branch 1 taken 26217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26217 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26217 times.
✗ Branch 8 not taken.
 26217 A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) {
241
3/6
✓ Branch 1 taken 305325 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 305325 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 305325 times.
✗ Branch 8 not taken.
 305325 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
242
243
4/8
✓ Branch 1 taken 305325 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 305325 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 305325 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 305325 times.
✗ Branch 11 not taken.
 305325 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u;
244
4/8
✓ Branch 1 taken 305325 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 305325 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 305325 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 305325 times.
✗ Branch 11 not taken.
 305325 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
245
4/8
✓ Branch 1 taken 305325 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 305325 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 305325 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 305325 times.
✗ Branch 11 not taken.
 305325 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
246
247
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 305325 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 305325 if (P && Q) {
248 ✗ *P << 0, t, 0;
249 ✗ *Q = S + (*P);
250 }
251
252
1/2
✓ Branch 1 taken 305325 times.
✗ Branch 2 not taken.
 305325 return S.norm();
253 }
254 }
255
256 // LA1, LB1
257
258
4/4
✓ Branch 0 taken 413383 times.
✓ Branch 1 taken 234392 times.
✓ Branch 2 taken 303930 times.
✓ Branch 3 taken 109453 times.
 647775 if ((LA1_lx < 0) && (LB1_lx < 0)) {
259
3/4
✓ Branch 1 taken 19014 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6127 times.
✓ Branch 4 taken 12887 times.
 19014 if (((LA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
260
9/12
✓ Branch 0 taken 19014 times.
✓ Branch 1 taken 284916 times.
✓ Branch 3 taken 19014 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19014 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19014 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 17412 times.
✓ Branch 12 taken 273631 times.
✓ Branch 13 taken 280859 times.
✓ Branch 14 taken 23071 times.
 340356 -Tba[1], -Tab[1])) &&
261
3/4
✓ Branch 1 taken 17412 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7228 times.
✓ Branch 4 taken 10184 times.
 17412 ((LB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
262
3/6
✓ Branch 1 taken 17412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17412 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17412 times.
✗ Branch 8 not taken.
 17412 Tab[1], Tba[1]))) {
263
3/6
✓ Branch 1 taken 280859 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 280859 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 280859 times.
✗ Branch 8 not taken.
 280859 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
264
265
3/6
✓ Branch 1 taken 280859 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 280859 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 280859 times.
✗ Branch 8 not taken.
 280859 S[0] = Tab[0] + Rab(0, 1) * u;
266
3/6
✓ Branch 1 taken 280859 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 280859 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 280859 times.
✗ Branch 8 not taken.
 280859 S[1] = Tab[1] + Rab(1, 1) * u - t;
267
3/6
✓ Branch 1 taken 280859 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 280859 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 280859 times.
✗ Branch 8 not taken.
 280859 S[2] = Tab[2] + Rab(2, 1) * u;
268
269
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 280859 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 280859 if (P && Q) {
270 ✗ *P << 0, t, 0;
271 ✗ *Q = S + (*P);
272 }
273
274
1/2
✓ Branch 1 taken 280859 times.
✗ Branch 2 not taken.
 280859 return S.norm();
275 }
276 }
277
278 Scalar ALL_y, ALU_y, AUL_y, AUU_y;
279
280
1/2
✓ Branch 1 taken 366916 times.
✗ Branch 2 not taken.
 366916 ALL_y = -Tba[1];
281 366916 ALU_y = ALL_y + aA1_dot_B1;
282 366916 AUL_y = ALL_y + aA0_dot_B1;
283 366916 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 196274 times.
✓ Branch 1 taken 170642 times.
 366916 if (ALL_y < ALU_y) {
288 196274 LA1_ly = ALL_y;
289 196274 LA1_uy = ALU_y;
290 196274 UA1_ly = AUL_y;
291 196274 UA1_uy = AUU_y;
292 } else {
293 170642 LA1_ly = ALU_y;
294 170642 LA1_uy = ALL_y;
295 170642 UA1_ly = AUU_y;
296 170642 UA1_uy = AUL_y;
297 }
298
299
2/2
✓ Branch 0 taken 190265 times.
✓ Branch 1 taken 176651 times.
 366916 if (BLL_x < BUL_x) {
300 190265 LB0_lx = BLL_x;
301 190265 LB0_ux = BUL_x;
302 190265 UB0_lx = BLU_x;
303 190265 UB0_ux = BUU_x;
304 } else {
305 176651 LB0_lx = BUL_x;
306 176651 LB0_ux = BLL_x;
307 176651 UB0_lx = BUU_x;
308 176651 UB0_ux = BLU_x;
309 }
310
311 // UA1, UB0
312
313
4/4
✓ Branch 0 taken 162908 times.
✓ Branch 1 taken 204008 times.
✓ Branch 2 taken 47013 times.
✓ Branch 3 taken 115895 times.
 366916 if ((UA1_uy > b[1]) && (UB0_ux > a[0])) {
314 100985 if (((UA1_ly > b[1]) ||
315
3/4
✓ Branch 1 taken 6959 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3490 times.
✓ Branch 4 taken 3469 times.
 6959 inVoronoi(b[0], a[1], A1_dot_B1, aA0_dot_B1 - Tba[1] - b[1], A1_dot_B0,
316
7/10
✓ Branch 0 taken 6959 times.
✓ Branch 1 taken 40054 times.
✓ Branch 3 taken 6959 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6959 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6959 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 18821 times.
✓ Branch 12 taken 28192 times.
 94026 aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) &&
317
2/2
✓ Branch 0 taken 28736 times.
✓ Branch 1 taken 14808 times.
 43544 ((UB0_lx > a[0]) ||
318
3/4
✓ Branch 1 taken 28736 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4013 times.
✓ Branch 4 taken 24723 times.
 28736 inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, A1_dot_B0,
319
3/6
✓ Branch 1 taken 28736 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28736 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28736 times.
✗ Branch 8 not taken.
 28736 Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) {
320
1/2
✓ Branch 1 taken 18821 times.
✗ Branch 2 not taken.
 18821 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
321
2/4
✓ Branch 1 taken 18821 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18821 times.
✗ Branch 5 not taken.
 18821 Tba[0] - aA0_dot_B0);
322
323
4/8
✓ Branch 1 taken 18821 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18821 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18821 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18821 times.
✗ Branch 11 not taken.
 18821 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0];
324
4/8
✓ Branch 1 taken 18821 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18821 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18821 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18821 times.
✗ Branch 11 not taken.
 18821 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
325
4/8
✓ Branch 1 taken 18821 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18821 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18821 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18821 times.
✗ Branch 11 not taken.
 18821 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
326
327
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 18821 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 18821 if (P && Q) {
328 ✗ *P << a[0], t, 0;
329 ✗ *Q = S + (*P);
330 }
331
332
1/2
✓ Branch 1 taken 18821 times.
✗ Branch 2 not taken.
 18821 return S.norm();
333 }
334 }
335
336 // UA1, LB0
337
338
4/4
✓ Branch 0 taken 196778 times.
✓ Branch 1 taken 151317 times.
✓ Branch 2 taken 58541 times.
✓ Branch 3 taken 138237 times.
 348095 if ((UA1_ly < 0) && (LB0_ux > a[0])) {
339
3/4
✓ Branch 1 taken 7891 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4638 times.
✓ Branch 4 taken 3253 times.
 7891 if (((UA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1,
340
7/10
✓ Branch 0 taken 7891 times.
✓ Branch 1 taken 50650 times.
✓ Branch 3 taken 7891 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7891 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7891 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 36991 times.
✓ Branch 12 taken 21550 times.
 117082 A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1])) &&
341
2/2
✓ Branch 0 taken 23974 times.
✓ Branch 1 taken 31314 times.
 55288 ((LB0_lx > a[0]) ||
342
4/6
✓ Branch 1 taken 23974 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23974 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5677 times.
✓ Branch 7 taken 18297 times.
 23974 inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], A1_dot_B0, Tab[1],
343
2/4
✓ Branch 1 taken 23974 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23974 times.
✗ Branch 5 not taken.
 23974 Tba[0] - aA0_dot_B0))) {
344
3/6
✓ Branch 1 taken 36991 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36991 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36991 times.
✗ Branch 8 not taken.
 36991 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
345
346
3/6
✓ Branch 1 taken 36991 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36991 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36991 times.
✗ Branch 8 not taken.
 36991 S[0] = Tab[0] + Rab(0, 0) * u - a[0];
347
3/6
✓ Branch 1 taken 36991 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36991 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36991 times.
✗ Branch 8 not taken.
 36991 S[1] = Tab[1] + Rab(1, 0) * u - t;
348
3/6
✓ Branch 1 taken 36991 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36991 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36991 times.
✗ Branch 8 not taken.
 36991 S[2] = Tab[2] + Rab(2, 0) * u;
349
350
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 36991 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 36991 if (P && Q) {
351 ✗ *P << a[0], t, 0;
352 ✗ *Q = S + (*P);
353 }
354
355
1/2
✓ Branch 1 taken 36991 times.
✗ Branch 2 not taken.
 36991 return S.norm();
356 }
357 }
358
359 // LA1, UB0
360
361
4/4
✓ Branch 0 taken 133816 times.
✓ Branch 1 taken 177288 times.
✓ Branch 2 taken 39834 times.
✓ Branch 3 taken 93982 times.
 311104 if ((LA1_uy > b[1]) && (UB0_lx < 0)) {
362 87153 if (((LA1_ly > b[1]) ||
363
4/6
✓ Branch 1 taken 7485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7485 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4115 times.
✓ Branch 7 taken 3370 times.
 7485 inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], A1_dot_B0, -Tba[0],
364
8/10
✓ Branch 0 taken 7485 times.
✓ Branch 1 taken 32349 times.
✓ Branch 3 taken 7485 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7485 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 22579 times.
✓ Branch 9 taken 13885 times.
✓ Branch 10 taken 19736 times.
✓ Branch 11 taken 20098 times.
 69898 -Tab[1] - bA1_dot_B1)) &&
365
366
3/4
✓ Branch 1 taken 22579 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5851 times.
✓ Branch 4 taken 16728 times.
 22579 ((UB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1,
367
3/6
✓ Branch 1 taken 22579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22579 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22579 times.
✗ Branch 8 not taken.
 22579 A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]))) {
368
3/6
✓ Branch 1 taken 19736 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19736 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19736 times.
✗ Branch 8 not taken.
 19736 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
369
370
4/8
✓ Branch 1 taken 19736 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19736 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19736 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19736 times.
✗ Branch 11 not taken.
 19736 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u;
371
4/8
✓ Branch 1 taken 19736 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19736 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19736 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19736 times.
✗ Branch 11 not taken.
 19736 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
372
4/8
✓ Branch 1 taken 19736 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19736 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19736 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19736 times.
✗ Branch 11 not taken.
 19736 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
373
374
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 19736 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 19736 if (P && Q) {
375 ✗ *P << 0, t, 0;
376 ✗ *Q = S + (*P);
377 }
378
379
1/2
✓ Branch 1 taken 19736 times.
✗ Branch 2 not taken.
 19736 return S.norm();
380 }
381 }
382
383 // LA1, LB0
384
385
4/4
✓ Branch 0 taken 170354 times.
✓ Branch 1 taken 121014 times.
✓ Branch 2 taken 76597 times.
✓ Branch 3 taken 93757 times.
 291368 if ((LA1_ly < 0) && (LB0_lx < 0)) {
386
3/4
✓ Branch 1 taken 7272 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3047 times.
✓ Branch 4 taken 4225 times.
 7272 if (((LA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
387
9/12
✓ Branch 0 taken 7272 times.
✓ Branch 1 taken 69325 times.
✓ Branch 3 taken 7272 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7272 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7272 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 28523 times.
✓ Branch 12 taken 43849 times.
✓ Branch 13 taken 50424 times.
✓ Branch 14 taken 26173 times.
 112392 -Tba[0], -Tab[1])) &&
388
3/4
✓ Branch 1 taken 28523 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6575 times.
✓ Branch 4 taken 21948 times.
 28523 ((LB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
389
3/6
✓ Branch 1 taken 28523 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28523 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28523 times.
✗ Branch 8 not taken.
 28523 Tab[1], Tba[0]))) {
390
3/6
✓ Branch 1 taken 50424 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50424 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50424 times.
✗ Branch 8 not taken.
 50424 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
391
392
3/6
✓ Branch 1 taken 50424 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50424 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50424 times.
✗ Branch 8 not taken.
 50424 S[0] = Tab[0] + Rab(0, 0) * u;
393
3/6
✓ Branch 1 taken 50424 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50424 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50424 times.
✗ Branch 8 not taken.
 50424 S[1] = Tab[1] + Rab(1, 0) * u - t;
394
3/6
✓ Branch 1 taken 50424 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50424 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50424 times.
✗ Branch 8 not taken.
 50424 S[2] = Tab[2] + Rab(2, 0) * u;
395
396
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 50424 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 50424 if (P && Q) {
397 ✗ *P << 0, t, 0;
398 ✗ *Q = S + (*P);
399 }
400
401
1/2
✓ Branch 1 taken 50424 times.
✗ Branch 2 not taken.
 50424 return S.norm();
402 }
403 }
404
405 Scalar BLL_y, BLU_y, BUL_y, BUU_y;
406
407
1/2
✓ Branch 1 taken 240944 times.
✗ Branch 2 not taken.
 240944 BLL_y = Tab[1];
408 240944 BLU_y = BLL_y + bA1_dot_B1;
409 240944 BUL_y = BLL_y + bA1_dot_B0;
410 240944 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 124342 times.
✓ Branch 1 taken 116602 times.
 240944 if (ALL_x < AUL_x) {
415 124342 LA0_lx = ALL_x;
416 124342 LA0_ux = AUL_x;
417 124342 UA0_lx = ALU_x;
418 124342 UA0_ux = AUU_x;
419 } else {
420 116602 LA0_lx = AUL_x;
421 116602 LA0_ux = ALL_x;
422 116602 UA0_lx = AUU_x;
423 116602 UA0_ux = ALU_x;
424 }
425
426
2/2
✓ Branch 0 taken 130984 times.
✓ Branch 1 taken 109960 times.
 240944 if (BLL_y < BLU_y) {
427 130984 LB1_ly = BLL_y;
428 130984 LB1_uy = BLU_y;
429 130984 UB1_ly = BUL_y;
430 130984 UB1_uy = BUU_y;
431 } else {
432 109960 LB1_ly = BLU_y;
433 109960 LB1_uy = BLL_y;
434 109960 UB1_ly = BUU_y;
435 109960 UB1_uy = BUL_y;
436 }
437
438 // UA0, UB1
439
440
4/4
✓ Branch 0 taken 132180 times.
✓ Branch 1 taken 108764 times.
✓ Branch 2 taken 47560 times.
✓ Branch 3 taken 84620 times.
 240944 if ((UA0_ux > b[0]) && (UB1_uy > a[1])) {
441 111654 if (((UA0_lx > b[0]) ||
442
3/4
✓ Branch 1 taken 16534 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7129 times.
✓ Branch 4 taken 9405 times.
 16534 inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], A0_dot_B1,
443
7/10
✓ Branch 0 taken 16534 times.
✓ Branch 1 taken 31026 times.
✓ Branch 3 taken 16534 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16534 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16534 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 35886 times.
✓ Branch 12 taken 11674 times.
 95120 aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) &&
444
2/2
✓ Branch 0 taken 4472 times.
✓ Branch 1 taken 33683 times.
 38155 ((UB1_ly > a[1]) ||
445
3/4
✓ Branch 1 taken 4472 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2203 times.
✓ Branch 4 taken 2269 times.
 4472 inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, A0_dot_B1,
446
3/6
✓ Branch 1 taken 4472 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4472 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4472 times.
✗ Branch 8 not taken.
 4472 Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) {
447
1/2
✓ Branch 1 taken 35886 times.
✗ Branch 2 not taken.
 35886 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
448
2/4
✓ Branch 1 taken 35886 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35886 times.
✗ Branch 5 not taken.
 35886 Tba[1] - aA1_dot_B1);
449
450
4/8
✓ Branch 1 taken 35886 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35886 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35886 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35886 times.
✗ Branch 11 not taken.
 35886 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
451
4/8
✓ Branch 1 taken 35886 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35886 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35886 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35886 times.
✗ Branch 11 not taken.
 35886 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1];
452
4/8
✓ Branch 1 taken 35886 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35886 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35886 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35886 times.
✗ Branch 11 not taken.
 35886 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
453
454
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 35886 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 35886 if (P && Q) {
455 ✗ *P << t, a[1], 0;
456 ✗ *Q = S + (*P);
457 }
458
459
1/2
✓ Branch 1 taken 35886 times.
✗ Branch 2 not taken.
 35886 return S.norm();
460 }
461 }
462
463 // UA0, LB1
464
465
4/4
✓ Branch 0 taken 110660 times.
✓ Branch 1 taken 94398 times.
✓ Branch 2 taken 45381 times.
✓ Branch 3 taken 65279 times.
 205058 if ((UA0_lx < 0) && (LB1_uy > a[1])) {
466
3/4
✓ Branch 1 taken 16282 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6952 times.
✓ Branch 4 taken 9330 times.
 16282 if (((UA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0,
467
7/10
✓ Branch 0 taken 16282 times.
✓ Branch 1 taken 29099 times.
✓ Branch 3 taken 16282 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16282 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16282 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 34485 times.
✓ Branch 12 taken 10896 times.
 90762 A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0])) &&
468
2/2
✓ Branch 0 taken 5150 times.
✓ Branch 1 taken 30901 times.
 36051 ((LB1_ly > a[1]) ||
469
4/6
✓ Branch 1 taken 5150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5150 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3584 times.
✓ Branch 7 taken 1566 times.
 5150 inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
470
2/4
✓ Branch 1 taken 5150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5150 times.
✗ Branch 5 not taken.
 5150 Tba[1] - aA1_dot_B1))) {
471
3/6
✓ Branch 1 taken 34485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34485 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34485 times.
✗ Branch 8 not taken.
 34485 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
472
473
3/6
✓ Branch 1 taken 34485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34485 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34485 times.
✗ Branch 8 not taken.
 34485 S[0] = Tab[0] + Rab(0, 1) * u - t;
474
3/6
✓ Branch 1 taken 34485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34485 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34485 times.
✗ Branch 8 not taken.
 34485 S[1] = Tab[1] + Rab(1, 1) * u - a[1];
475
3/6
✓ Branch 1 taken 34485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34485 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34485 times.
✗ Branch 8 not taken.
 34485 S[2] = Tab[2] + Rab(2, 1) * u;
476
477
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 34485 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 34485 if (P && Q) {
478 ✗ *P << t, a[1], 0;
479 ✗ *Q = S + (*P);
480 }
481
482
1/2
✓ Branch 1 taken 34485 times.
✗ Branch 2 not taken.
 34485 return S.norm();
483 }
484 }
485
486 // LA0, UB1
487
488
4/4
✓ Branch 0 taken 94142 times.
✓ Branch 1 taken 76431 times.
✓ Branch 2 taken 63611 times.
✓ Branch 3 taken 30531 times.
 170573 if ((LA0_ux > b[0]) && (UB1_ly < 0)) {
489 145248 if (((LA0_lx > b[0]) ||
490
4/6
✓ Branch 1 taken 18026 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18026 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8740 times.
✓ Branch 7 taken 9286 times.
 18026 inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
491
8/10
✓ Branch 0 taken 18026 times.
✓ Branch 1 taken 45585 times.
✓ Branch 3 taken 18026 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18026 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 5619 times.
✓ Branch 9 taken 48706 times.
✓ Branch 10 taken 52366 times.
✓ Branch 11 taken 11245 times.
 87256 -bA0_dot_B0 - Tab[0])) &&
492
3/4
✓ Branch 1 taken 5619 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3660 times.
✓ Branch 4 taken 1959 times.
 5619 ((UB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0,
493
3/6
✓ Branch 1 taken 5619 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5619 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5619 times.
✗ Branch 8 not taken.
 5619 A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]))) {
494
3/6
✓ Branch 1 taken 52366 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52366 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52366 times.
✗ Branch 8 not taken.
 52366 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
495
496
4/8
✓ Branch 1 taken 52366 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52366 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52366 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52366 times.
✗ Branch 11 not taken.
 52366 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
497
4/8
✓ Branch 1 taken 52366 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52366 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52366 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52366 times.
✗ Branch 11 not taken.
 52366 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u;
498
4/8
✓ Branch 1 taken 52366 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52366 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52366 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52366 times.
✗ Branch 11 not taken.
 52366 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
499
500
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 52366 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 52366 if (P && Q) {
501 ✗ *P << t, 0, 0;
502 ✗ *Q = S + (*P);
503 }
504
505
1/2
✓ Branch 1 taken 52366 times.
✗ Branch 2 not taken.
 52366 return S.norm();
506 }
507 }
508
509 // LA0, LB1
510
511
4/4
✓ Branch 0 taken 78192 times.
✓ Branch 1 taken 40015 times.
✓ Branch 2 taken 42571 times.
✓ Branch 3 taken 35621 times.
 118207 if ((LA0_lx < 0) && (LB1_ly < 0)) {
512
3/4
✓ Branch 1 taken 15350 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8424 times.
✓ Branch 4 taken 6926 times.
 15350 if (((LA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1,
513
9/12
✓ Branch 0 taken 15350 times.
✓ Branch 1 taken 27221 times.
✓ Branch 3 taken 15350 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15350 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 15350 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 3715 times.
✓ Branch 12 taken 31930 times.
✓ Branch 13 taken 33529 times.
✓ Branch 14 taken 9042 times.
 61636 -Tba[1], -Tab[0])) &&
514
3/4
✓ Branch 1 taken 3715 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1599 times.
✓ Branch 4 taken 2116 times.
 3715 ((LB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
515
3/6
✓ Branch 1 taken 3715 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3715 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3715 times.
✗ Branch 8 not taken.
 3715 Tab[0], Tba[1]))) {
516
3/6
✓ Branch 1 taken 33529 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33529 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33529 times.
✗ Branch 8 not taken.
 33529 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
517
518
3/6
✓ Branch 1 taken 33529 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33529 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33529 times.
✗ Branch 8 not taken.
 33529 S[0] = Tab[0] + Rab(0, 1) * u - t;
519
3/6
✓ Branch 1 taken 33529 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33529 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33529 times.
✗ Branch 8 not taken.
 33529 S[1] = Tab[1] + Rab(1, 1) * u;
520
3/6
✓ Branch 1 taken 33529 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33529 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33529 times.
✗ Branch 8 not taken.
 33529 S[2] = Tab[2] + Rab(2, 1) * u;
521
522
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 33529 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 33529 if (P && Q) {
523 ✗ *P << t, 0, 0;
524 ✗ *Q = S + (*P);
525 }
526
527
1/2
✓ Branch 1 taken 33529 times.
✗ Branch 2 not taken.
 33529 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 42747 times.
✓ Branch 1 taken 41931 times.
 84678 if (ALL_y < AUL_y) {
534 42747 LA0_ly = ALL_y;
535 42747 LA0_uy = AUL_y;
536 42747 UA0_ly = ALU_y;
537 42747 UA0_uy = AUU_y;
538 } else {
539 41931 LA0_ly = AUL_y;
540 41931 LA0_uy = ALL_y;
541 41931 UA0_ly = AUU_y;
542 41931 UA0_uy = ALU_y;
543 }
544
545
2/2
✓ Branch 0 taken 43394 times.
✓ Branch 1 taken 41284 times.
 84678 if (BLL_y < BUL_y) {
546 43394 LB0_ly = BLL_y;
547 43394 LB0_uy = BUL_y;
548 43394 UB0_ly = BLU_y;
549 43394 UB0_uy = BUU_y;
550 } else {
551 41284 LB0_ly = BUL_y;
552 41284 LB0_uy = BLL_y;
553 41284 UB0_ly = BUU_y;
554 41284 UB0_uy = BLU_y;
555 }
556
557 // UA0, UB0
558
559
4/4
✓ Branch 0 taken 51029 times.
✓ Branch 1 taken 33649 times.
✓ Branch 2 taken 18331 times.
✓ Branch 3 taken 32698 times.
 84678 if ((UA0_uy > b[1]) && (UB0_uy > a[1])) {
560 50142 if (((UA0_ly > b[1]) ||
561
3/4
✓ Branch 1 taken 13480 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5705 times.
✓ Branch 4 taken 7775 times.
 13480 inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], A0_dot_B0,
562
7/10
✓ Branch 0 taken 13480 times.
✓ Branch 1 taken 4851 times.
✓ Branch 3 taken 13480 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13480 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13480 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4426 times.
✓ Branch 12 taken 13905 times.
 36662 aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) &&
563
2/2
✓ Branch 0 taken 8338 times.
✓ Branch 1 taken 2218 times.
 10556 ((UB0_ly > a[1]) ||
564
3/4
✓ Branch 1 taken 8338 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2208 times.
✓ Branch 4 taken 6130 times.
 8338 inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
565
3/6
✓ Branch 1 taken 8338 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8338 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8338 times.
✗ Branch 8 not taken.
 8338 Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) {
566
1/2
✓ Branch 1 taken 4426 times.
✗ Branch 2 not taken.
 4426 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
567
2/4
✓ Branch 1 taken 4426 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4426 times.
✗ Branch 5 not taken.
 4426 Tba[0] - aA1_dot_B0);
568
569
4/8
✓ Branch 1 taken 4426 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4426 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4426 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4426 times.
✗ Branch 11 not taken.
 4426 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
570
4/8
✓ Branch 1 taken 4426 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4426 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4426 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4426 times.
✗ Branch 11 not taken.
 4426 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1];
571
4/8
✓ Branch 1 taken 4426 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4426 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4426 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4426 times.
✗ Branch 11 not taken.
 4426 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
572
573
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4426 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4426 if (P && Q) {
574 ✗ *P << t, a[1], 0;
575 ✗ *Q = S + (*P);
576 }
577
578
1/2
✓ Branch 1 taken 4426 times.
✗ Branch 2 not taken.
 4426 return S.norm();
579 }
580 }
581
582 // UA0, LB0
583
584
4/4
✓ Branch 0 taken 46362 times.
✓ Branch 1 taken 33890 times.
✓ Branch 2 taken 17108 times.
✓ Branch 3 taken 29254 times.
 80252 if ((UA0_ly < 0) && (LB0_uy > a[1])) {
585
3/4
✓ Branch 1 taken 9806 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4232 times.
✓ Branch 4 taken 5574 times.
 9806 if (((UA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1,
586
7/10
✓ Branch 0 taken 9806 times.
✓ Branch 1 taken 7302 times.
✓ Branch 3 taken 9806 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9806 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9806 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 7869 times.
✓ Branch 12 taken 9239 times.
 34216 A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0])) &&
587
2/2
✓ Branch 0 taken 5947 times.
✓ Branch 1 taken 5587 times.
 11534 ((LB0_ly > a[1]) ||
588
4/6
✓ Branch 1 taken 5947 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5947 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2282 times.
✓ Branch 7 taken 3665 times.
 5947 inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], A0_dot_B0, Tab[0],
589
2/4
✓ Branch 1 taken 5947 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5947 times.
✗ Branch 5 not taken.
 5947 Tba[0] - aA1_dot_B0))) {
590
3/6
✓ Branch 1 taken 7869 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7869 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7869 times.
✗ Branch 8 not taken.
 7869 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
591
592
3/6
✓ Branch 1 taken 7869 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7869 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7869 times.
✗ Branch 8 not taken.
 7869 S[0] = Tab[0] + Rab(0, 0) * u - t;
593
3/6
✓ Branch 1 taken 7869 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7869 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7869 times.
✗ Branch 8 not taken.
 7869 S[1] = Tab[1] + Rab(1, 0) * u - a[1];
594
3/6
✓ Branch 1 taken 7869 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7869 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7869 times.
✗ Branch 8 not taken.
 7869 S[2] = Tab[2] + Rab(2, 0) * u;
595
596
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 7869 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 7869 if (P && Q) {
597 ✗ *P << t, a[1], 0;
598 ✗ *Q = S + (*P);
599 }
600
601
1/2
✓ Branch 1 taken 7869 times.
✗ Branch 2 not taken.
 7869 return S.norm();
602 }
603 }
604
605 // LA0, UB0
606
607
4/4
✓ Branch 0 taken 43672 times.
✓ Branch 1 taken 28711 times.
✓ Branch 2 taken 14619 times.
✓ Branch 3 taken 29053 times.
 72383 if ((LA0_uy > b[1]) && (UB0_ly < 0)) {
608 39999 if (((LA0_ly > b[1]) ||
609
4/6
✓ Branch 1 taken 10761 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10761 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4677 times.
✓ Branch 7 taken 6084 times.
 10761 inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
610
8/10
✓ Branch 0 taken 10761 times.
✓ Branch 1 taken 3858 times.
✓ Branch 3 taken 10761 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10761 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6296 times.
✓ Branch 9 taken 2239 times.
✓ Branch 10 taken 4863 times.
✓ Branch 11 taken 9756 times.
 31676 -Tab[0] - bA0_dot_B1)) &&
611
612
3/4
✓ Branch 1 taken 6296 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2624 times.
✓ Branch 4 taken 3672 times.
 6296 ((UB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1,
613
3/6
✓ Branch 1 taken 6296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6296 times.
✗ Branch 8 not taken.
 6296 A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]))) {
614
3/6
✓ Branch 1 taken 4863 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4863 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4863 times.
✗ Branch 8 not taken.
 4863 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
615
616
4/8
✓ Branch 1 taken 4863 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4863 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4863 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4863 times.
✗ Branch 11 not taken.
 4863 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
617
4/8
✓ Branch 1 taken 4863 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4863 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4863 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4863 times.
✗ Branch 11 not taken.
 4863 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u;
618
4/8
✓ Branch 1 taken 4863 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4863 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4863 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4863 times.
✗ Branch 11 not taken.
 4863 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
619
620
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4863 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4863 if (P && Q) {
621 ✗ *P << t, 0, 0;
622 ✗ *Q = S + (*P);
623 }
624
625
1/2
✓ Branch 1 taken 4863 times.
✗ Branch 2 not taken.
 4863 return S.norm();
626 }
627 }
628
629 // LA0, LB0
630
631
4/4
✓ Branch 0 taken 39410 times.
✓ Branch 1 taken 28110 times.
✓ Branch 2 taken 11519 times.
✓ Branch 3 taken 27891 times.
 67520 if ((LA0_ly < 0) && (LB0_ly < 0)) {
632
3/4
✓ Branch 1 taken 8520 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3754 times.
✓ Branch 4 taken 4766 times.
 8520 if (((LA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
633
9/12
✓ Branch 0 taken 8520 times.
✓ Branch 1 taken 2999 times.
✓ Branch 3 taken 8520 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8520 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8520 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4942 times.
✓ Branch 12 taken 1811 times.
✓ Branch 13 taken 3317 times.
✓ Branch 14 taken 8202 times.
 24981 -Tba[0], -Tab[0])) &&
634
3/4
✓ Branch 1 taken 4942 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1506 times.
✓ Branch 4 taken 3436 times.
 4942 ((LB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
635
3/6
✓ Branch 1 taken 4942 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4942 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4942 times.
✗ Branch 8 not taken.
 4942 Tab[0], Tba[0]))) {
636
3/6
✓ Branch 1 taken 3317 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3317 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3317 times.
✗ Branch 8 not taken.
 3317 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
637
638
3/6
✓ Branch 1 taken 3317 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3317 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3317 times.
✗ Branch 8 not taken.
 3317 S[0] = Tab[0] + Rab(0, 0) * u - t;
639
3/6
✓ Branch 1 taken 3317 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3317 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3317 times.
✗ Branch 8 not taken.
 3317 S[1] = Tab[1] + Rab(1, 0) * u;
640
3/6
✓ Branch 1 taken 3317 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3317 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3317 times.
✗ Branch 8 not taken.
 3317 S[2] = Tab[2] + Rab(2, 0) * u;
641
642
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3317 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3317 if (P && Q) {
643 ✗ *P << t, 0, 0;
644 ✗ *Q = S + (*P);
645 }
646
647
1/2
✓ Branch 1 taken 3317 times.
✗ Branch 2 not taken.
 3317 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 64203 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33704 times.
✓ Branch 4 taken 30499 times.
 64203 if (Tab[2] > 0.0) {
656
1/2
✓ Branch 1 taken 33704 times.
✗ Branch 2 not taken.
 33704 sep1 = Tab[2];
657
4/6
✓ Branch 1 taken 33704 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20057 times.
✓ Branch 4 taken 13647 times.
✓ Branch 6 taken 20057 times.
✗ Branch 7 not taken.
 33704 if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
658
4/6
✓ Branch 1 taken 33704 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18519 times.
✓ Branch 4 taken 15185 times.
✓ Branch 6 taken 18519 times.
✗ Branch 7 not taken.
 33704 if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
659 } else {
660
1/2
✓ Branch 1 taken 30499 times.
✗ Branch 2 not taken.
 30499 sep1 = -Tab[2];
661
4/6
✓ Branch 1 taken 30499 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18243 times.
✓ Branch 4 taken 12256 times.
✓ Branch 6 taken 18243 times.
✗ Branch 7 not taken.
 30499 if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
662
4/6
✓ Branch 1 taken 30499 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17180 times.
✓ Branch 4 taken 13319 times.
✓ Branch 6 taken 17180 times.
✗ Branch 7 not taken.
 30499 if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
663 }
664
665
3/4
✓ Branch 1 taken 64203 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33416 times.
✓ Branch 4 taken 30787 times.
 64203 if (Tba[2] < 0) {
666
1/2
✓ Branch 1 taken 33416 times.
✗ Branch 2 not taken.
 33416 sep2 = -Tba[2];
667
4/6
✓ Branch 1 taken 33416 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23287 times.
✓ Branch 4 taken 10129 times.
✓ Branch 6 taken 23287 times.
✗ Branch 7 not taken.
 33416 if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
668
4/6
✓ Branch 1 taken 33416 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 19981 times.
✓ Branch 4 taken 13435 times.
✓ Branch 6 taken 19981 times.
✗ Branch 7 not taken.
 33416 if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
669 } else {
670
1/2
✓ Branch 1 taken 30787 times.
✗ Branch 2 not taken.
 30787 sep2 = Tba[2];
671
4/6
✓ Branch 1 taken 30787 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23151 times.
✓ Branch 4 taken 7636 times.
✓ Branch 6 taken 23151 times.
✗ Branch 7 not taken.
 30787 if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
672
4/6
✓ Branch 1 taken 30787 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18706 times.
✓ Branch 4 taken 12081 times.
✓ Branch 6 taken 18706 times.
✗ Branch 7 not taken.
 30787 if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
673 }
674
675
4/4
✓ Branch 0 taken 44126 times.
✓ Branch 1 taken 20077 times.
✓ Branch 2 taken 34040 times.
✓ Branch 3 taken 10086 times.
 64203 if (sep1 >= sep2 && sep1 >= 0) {
676
3/4
✓ Branch 1 taken 34040 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17438 times.
✓ Branch 4 taken 16602 times.
 34040 if (Tab[2] > 0)
677
3/6
✓ Branch 1 taken 17438 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17438 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17438 times.
✗ Branch 8 not taken.
 17438 S << 0, 0, sep1;
678 else
679
3/6
✓ Branch 1 taken 16602 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16602 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16602 times.
✗ Branch 8 not taken.
 16602 S << 0, 0, -sep1;
680
681
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 34040 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 34040 if (P && Q) {
682 ✗ *Q = S;
683 ✗ P->setZero();
684 }
685 }
686
687
4/4
✓ Branch 0 taken 20242 times.
✓ Branch 1 taken 43961 times.
✓ Branch 2 taken 15156 times.
✓ Branch 3 taken 5086 times.
 64203 if (sep2 >= sep1 && sep2 >= 0) {
688
4/8
✓ Branch 1 taken 15156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15156 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15156 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15156 times.
✗ Branch 11 not taken.
 15156 Vec3s Q_(Tab[0], Tab[1], Tab[2]);
689
1/2
✓ Branch 1 taken 15156 times.
✗ Branch 2 not taken.
 15156 Vec3s P_;
690
3/4
✓ Branch 1 taken 15156 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8579 times.
✓ Branch 4 taken 6577 times.
 15156 if (Tba[2] < 0) {
691
3/6
✓ Branch 1 taken 8579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8579 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8579 times.
✗ Branch 8 not taken.
 8579 P_[0] = Rab(0, 2) * sep2 + Tab[0];
692
3/6
✓ Branch 1 taken 8579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8579 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8579 times.
✗ Branch 8 not taken.
 8579 P_[1] = Rab(1, 2) * sep2 + Tab[1];
693
3/6
✓ Branch 1 taken 8579 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8579 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8579 times.
✗ Branch 8 not taken.
 8579 P_[2] = Rab(2, 2) * sep2 + Tab[2];
694 } else {
695
3/6
✓ Branch 1 taken 6577 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6577 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6577 times.
✗ Branch 8 not taken.
 6577 P_[0] = -Rab(0, 2) * sep2 + Tab[0];
696
3/6
✓ Branch 1 taken 6577 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6577 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6577 times.
✗ Branch 8 not taken.
 6577 P_[1] = -Rab(1, 2) * sep2 + Tab[1];
697
3/6
✓ Branch 1 taken 6577 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6577 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6577 times.
✗ Branch 8 not taken.
 6577 P_[2] = -Rab(2, 2) * sep2 + Tab[2];
698 }
699
700
2/4
✓ Branch 1 taken 15156 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15156 times.
✗ Branch 5 not taken.
 15156 S = Q_ - P_;
701
702
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 15156 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 15156 if (P && Q) {
703 ✗ *P = P_;
704 ✗ *Q = Q_;
705 }
706 }
707
708
2/2
✓ Branch 0 taken 43961 times.
✓ Branch 1 taken 20242 times.
 64203 Scalar sep = (sep1 > sep2 ? sep1 : sep2);
709
2/2
✓ Branch 0 taken 49029 times.
✓ Branch 1 taken 15174 times.
 64203 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.
 1928 return (dist < (Scalar)0.0) ? (Scalar)0.0 : dist;
993 }
994
995 1589986 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 1589986 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1589986 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1589986 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1589986 times.
✗ Branch 11 not taken.
 1589986 Matrix3s R(b1.axes.transpose() * R0 * b2.axes);
998
4/8
✓ Branch 1 taken 1589986 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1589986 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1589986 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1589986 times.
✗ Branch 11 not taken.
 1589986 Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr);
999
1000
3/6
✓ Branch 1 taken 1589986 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1589986 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1589986 times.
✗ Branch 8 not taken.
 1589986 Vec3s T(b1.axes.transpose() * Ttemp);
1001
1002
1/2
✓ Branch 1 taken 1589986 times.
✗ Branch 2 not taken.
 1589986 Scalar dist = rectDistance(R, T, b1.length, b2.length, P, Q);
1003 1589986 dist -= (b1.radius + b2.radius);
1004
2/2
✓ Branch 0 taken 17786 times.
✓ Branch 1 taken 1572200 times.
 3179972 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