GCC Code Coverage Report

Directory: ./
File: src/BV/RSS.cpp
Date: 2025-04-13 14:25:19
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 3708196 void clipToRange(Scalar& val, Scalar a, Scalar b) {
49
2/2
✓ Branch 0 taken 1360175 times.
✓ Branch 1 taken 2348021 times.
 3708196 if (val < a)
50 1360175 val = a;
51
2/2
✓ Branch 0 taken 1402057 times.
✓ Branch 1 taken 945964 times.
 2348021 else if (val > b)
52 1402057 val = b;
53 3708196 }
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 1552340 void segCoords(Scalar& t, Scalar& u, Scalar a, Scalar b, Scalar A_dot_B,
67 Scalar A_dot_T, Scalar B_dot_T) {
68 1552340 Scalar denom = 1 - A_dot_B * A_dot_B;
69
70
2/2
✓ Branch 0 taken 91932 times.
✓ Branch 1 taken 1460408 times.
 1552340 if (denom == 0)
71 91932 t = 0;
72 else {
73 1460408 t = (A_dot_T - B_dot_T * A_dot_B) / denom;
74 1460408 clipToRange(t, 0, a);
75 }
76
77 1552340 u = t * A_dot_B - B_dot_T;
78
2/2
✓ Branch 0 taken 648337 times.
✓ Branch 1 taken 904003 times.
 1552340 if (u < 0) {
79 648337 u = 0;
80 648337 t = A_dot_T;
81 648337 clipToRange(t, 0, a);
82
2/2
✓ Branch 0 taken 683585 times.
✓ Branch 1 taken 220418 times.
 904003 } else if (u > b) {
83 683585 u = b;
84 683585 t = u * A_dot_B + A_dot_T;
85 683585 clipToRange(t, 0, a);
86 }
87 1552340 }
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 457963 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 457933 times.
 457963 if (fabs(Anorm_dot_B) < 1e-7) return false;
97
98 Scalar t, u, v;
99
100 457933 u = -Anorm_dot_T / Anorm_dot_B;
101
1/2
✓ Branch 1 taken 457933 times.
✗ Branch 2 not taken.
 457933 clipToRange(u, 0, b);
102
103 457933 t = u * A_dot_B + A_dot_T;
104
1/2
✓ Branch 1 taken 457933 times.
✗ Branch 2 not taken.
 457933 clipToRange(t, 0, a);
105
106 457933 v = t * A_dot_B - B_dot_T;
107
108
2/2
✓ Branch 0 taken 227377 times.
✓ Branch 1 taken 230556 times.
 457933 if (Anorm_dot_B > 0) {
109
2/2
✓ Branch 0 taken 98630 times.
✓ Branch 1 taken 128747 times.
 227377 if (v > (u + 1e-7)) return true;
110 } else {
111
2/2
✓ Branch 0 taken 86060 times.
✓ Branch 1 taken 144496 times.
 230556 if (v < (u - 1e-7)) return true;
112 }
113 273243 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 1616513 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 1616513 times.
✗ Branch 2 not taken.
 1616513 A0_dot_B0 = Rab(0, 0);
124
1/2
✓ Branch 1 taken 1616513 times.
✗ Branch 2 not taken.
 1616513 A0_dot_B1 = Rab(0, 1);
125
1/2
✓ Branch 1 taken 1616513 times.
✗ Branch 2 not taken.
 1616513 A1_dot_B0 = Rab(1, 0);
126
1/2
✓ Branch 1 taken 1616513 times.
✗ Branch 2 not taken.
 1616513 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 1616513 aA0_dot_B0 = a[0] * A0_dot_B0;
132 1616513 aA0_dot_B1 = a[0] * A0_dot_B1;
133 1616513 aA1_dot_B0 = a[1] * A1_dot_B0;
134 1616513 aA1_dot_B1 = a[1] * A1_dot_B1;
135 1616513 bA0_dot_B0 = b[0] * A0_dot_B0;
136 1616513 bA1_dot_B0 = b[0] * A1_dot_B0;
137 1616513 bA0_dot_B1 = b[1] * A0_dot_B1;
138 1616513 bA1_dot_B1 = b[1] * A1_dot_B1;
139
140
3/6
✓ Branch 1 taken 1616513 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1616513 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1616513 times.
✗ Branch 8 not taken.
 1616513 Vec3s Tba(Rab.transpose() * Tab);
141
142
1/2
✓ Branch 1 taken 1616513 times.
✗ Branch 2 not taken.
 1616513 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 1616513 times.
✗ Branch 2 not taken.
 1616513 ALL_x = -Tba[0];
152 1616513 ALU_x = ALL_x + aA1_dot_B0;
153 1616513 AUL_x = ALL_x + aA0_dot_B0;
154 1616513 AUU_x = ALU_x + aA0_dot_B0;
155
156
2/2
✓ Branch 0 taken 855360 times.
✓ Branch 1 taken 761153 times.
 1616513 if (ALL_x < ALU_x) {
157 855360 LA1_lx = ALL_x;
158 855360 LA1_ux = ALU_x;
159 855360 UA1_lx = AUL_x;
160 855360 UA1_ux = AUU_x;
161 } else {
162 761153 LA1_lx = ALU_x;
163 761153 LA1_ux = ALL_x;
164 761153 UA1_lx = AUU_x;
165 761153 UA1_ux = AUL_x;
166 }
167
168
1/2
✓ Branch 1 taken 1616513 times.
✗ Branch 2 not taken.
 1616513 BLL_x = Tab[0];
169 1616513 BLU_x = BLL_x + bA0_dot_B1;
170 1616513 BUL_x = BLL_x + bA0_dot_B0;
171 1616513 BUU_x = BLU_x + bA0_dot_B0;
172
173
2/2
✓ Branch 0 taken 863763 times.
✓ Branch 1 taken 752750 times.
 1616513 if (BLL_x < BLU_x) {
174 863763 LB1_lx = BLL_x;
175 863763 LB1_ux = BLU_x;
176 863763 UB1_lx = BUL_x;
177 863763 UB1_ux = BUU_x;
178 } else {
179 752750 LB1_lx = BLU_x;
180 752750 LB1_ux = BLL_x;
181 752750 UB1_lx = BUU_x;
182 752750 UB1_ux = BUL_x;
183 }
184
185 // UA1, UB1
186
187
4/4
✓ Branch 0 taken 821856 times.
✓ Branch 1 taken 794657 times.
✓ Branch 2 taken 393207 times.
✓ Branch 3 taken 428649 times.
 1616513 if ((UA1_ux > b[0]) && (UB1_ux > a[0])) {
188 806822 if (((UA1_lx > b[0]) ||
189
3/4
✓ Branch 1 taken 20408 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9603 times.
✓ Branch 4 taken 10805 times.
 20408 inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0], A1_dot_B1,
190
7/10
✓ Branch 0 taken 20408 times.
✓ Branch 1 taken 372799 times.
✓ Branch 3 taken 20408 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 20408 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 20408 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 366544 times.
✓ Branch 12 taken 26663 times.
 786414 aA0_dot_B1 - Tba[1], -Tab[1] - bA1_dot_B0)) &&
191
2/2
✓ Branch 0 taken 26552 times.
✓ Branch 1 taken 355850 times.
 382402 ((UB1_lx > a[0]) ||
192
3/4
✓ Branch 1 taken 26552 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10694 times.
✓ Branch 4 taken 15858 times.
 26552 inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0], A1_dot_B1,
193
3/6
✓ Branch 1 taken 26552 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26552 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26552 times.
✗ Branch 8 not taken.
 26552 Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1))) {
194
1/2
✓ Branch 1 taken 366544 times.
✗ Branch 2 not taken.
 366544 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
195
2/4
✓ Branch 1 taken 366544 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 366544 times.
✗ Branch 5 not taken.
 366544 Tba[1] - aA0_dot_B1);
196
197
4/8
✓ Branch 1 taken 366544 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 366544 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 366544 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 366544 times.
✗ Branch 11 not taken.
 366544 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0];
198
4/8
✓ Branch 1 taken 366544 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 366544 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 366544 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 366544 times.
✗ Branch 11 not taken.
 366544 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
199
4/8
✓ Branch 1 taken 366544 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 366544 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 366544 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 366544 times.
✗ Branch 11 not taken.
 366544 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
200
201
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 366544 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 366544 if (P && Q) {
202 ✗ *P << a[0], t, 0;
203 ✗ *Q = S + (*P);
204 }
205
206
1/2
✓ Branch 1 taken 366544 times.
✗ Branch 2 not taken.
 366544 return S.norm();
207 }
208 }
209
210 // UA1, LB1
211
212
4/4
✓ Branch 0 taken 690317 times.
✓ Branch 1 taken 559652 times.
✓ Branch 2 taken 318037 times.
✓ Branch 3 taken 372280 times.
 1249969 if ((UA1_lx < 0) && (LB1_ux > a[0])) {
213
3/4
✓ Branch 1 taken 19957 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7712 times.
✓ Branch 4 taken 12245 times.
 19957 if (((UA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
214
7/10
✓ Branch 0 taken 19957 times.
✓ Branch 1 taken 298080 times.
✓ Branch 3 taken 19957 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19957 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19957 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 295651 times.
✓ Branch 12 taken 22386 times.
 636074 A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1])) &&
215
2/2
✓ Branch 0 taken 23058 times.
✓ Branch 1 taken 282734 times.
 305792 ((LB1_lx > a[0]) ||
216
4/6
✓ Branch 1 taken 23058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23058 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 12917 times.
✓ Branch 7 taken 10141 times.
 23058 inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0], A1_dot_B1, Tab[1],
217
2/4
✓ Branch 1 taken 23058 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23058 times.
✗ Branch 5 not taken.
 23058 Tba[1] - aA0_dot_B1))) {
218
3/6
✓ Branch 1 taken 295651 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 295651 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 295651 times.
✗ Branch 8 not taken.
 295651 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
219
220
3/6
✓ Branch 1 taken 295651 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 295651 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 295651 times.
✗ Branch 8 not taken.
 295651 S[0] = Tab[0] + Rab(0, 1) * u - a[0];
221
3/6
✓ Branch 1 taken 295651 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 295651 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 295651 times.
✗ Branch 8 not taken.
 295651 S[1] = Tab[1] + Rab(1, 1) * u - t;
222
3/6
✓ Branch 1 taken 295651 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 295651 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 295651 times.
✗ Branch 8 not taken.
 295651 S[2] = Tab[2] + Rab(2, 1) * u;
223
224
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 295651 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 295651 if (P && Q) {
225 ✗ *P << a[0], t, 0;
226 ✗ *Q = S + (*P);
227 }
228
229
1/2
✓ Branch 1 taken 295651 times.
✗ Branch 2 not taken.
 295651 return S.norm();
230 }
231 }
232
233 // LA1, UB1
234
235
4/4
✓ Branch 0 taken 442011 times.
✓ Branch 1 taken 512307 times.
✓ Branch 2 taken 327837 times.
✓ Branch 3 taken 114174 times.
 954318 if ((LA1_ux > b[0]) && (UB1_lx < 0)) {
236 674649 if (((LA1_lx > b[0]) ||
237
4/6
✓ Branch 1 taken 18975 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18975 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 9749 times.
✓ Branch 7 taken 9226 times.
 18975 inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0], A1_dot_B1, -Tba[1],
238
8/10
✓ Branch 0 taken 18975 times.
✓ Branch 1 taken 308862 times.
✓ Branch 3 taken 18975 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18975 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 26173 times.
✓ Branch 9 taken 292438 times.
✓ Branch 10 taken 306421 times.
✓ Branch 11 taken 21416 times.
 372985 -Tab[1] - bA1_dot_B0)) &&
239
3/4
✓ Branch 1 taken 26173 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 13983 times.
✓ Branch 4 taken 12190 times.
 26173 ((UB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
240
3/6
✓ Branch 1 taken 26173 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26173 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26173 times.
✗ Branch 8 not taken.
 26173 A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]))) {
241
3/6
✓ Branch 1 taken 306421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 306421 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 306421 times.
✗ Branch 8 not taken.
 306421 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
242
243
4/8
✓ Branch 1 taken 306421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 306421 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 306421 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 306421 times.
✗ Branch 11 not taken.
 306421 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u;
244
4/8
✓ Branch 1 taken 306421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 306421 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 306421 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 306421 times.
✗ Branch 11 not taken.
 306421 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
245
4/8
✓ Branch 1 taken 306421 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 306421 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 306421 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 306421 times.
✗ Branch 11 not taken.
 306421 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
246
247
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 306421 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 306421 if (P && Q) {
248 ✗ *P << 0, t, 0;
249 ✗ *Q = S + (*P);
250 }
251
252
1/2
✓ Branch 1 taken 306421 times.
✗ Branch 2 not taken.
 306421 return S.norm();
253 }
254 }
255
256 // LA1, LB1
257
258
4/4
✓ Branch 0 taken 413573 times.
✓ Branch 1 taken 234324 times.
✓ Branch 2 taken 304303 times.
✓ Branch 3 taken 109270 times.
 647897 if ((LA1_lx < 0) && (LB1_lx < 0)) {
259
3/4
✓ Branch 1 taken 19049 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6138 times.
✓ Branch 4 taken 12911 times.
 19049 if (((LA1_ux < 0) || inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0], A1_dot_B1,
260
9/12
✓ Branch 0 taken 19049 times.
✓ Branch 1 taken 285254 times.
✓ Branch 3 taken 19049 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19049 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19049 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 17337 times.
✓ Branch 12 taken 274055 times.
✓ Branch 13 taken 281252 times.
✓ Branch 14 taken 23051 times.
 340689 -Tba[1], -Tab[1])) &&
261
3/4
✓ Branch 1 taken 17337 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7197 times.
✓ Branch 4 taken 10140 times.
 17337 ((LB1_ux < 0) || inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0], A1_dot_B1,
262
3/6
✓ Branch 1 taken 17337 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17337 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17337 times.
✗ Branch 8 not taken.
 17337 Tab[1], Tba[1]))) {
263
3/6
✓ Branch 1 taken 281252 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281252 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281252 times.
✗ Branch 8 not taken.
 281252 segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1]);
264
265
3/6
✓ Branch 1 taken 281252 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281252 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281252 times.
✗ Branch 8 not taken.
 281252 S[0] = Tab[0] + Rab(0, 1) * u;
266
3/6
✓ Branch 1 taken 281252 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281252 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281252 times.
✗ Branch 8 not taken.
 281252 S[1] = Tab[1] + Rab(1, 1) * u - t;
267
3/6
✓ Branch 1 taken 281252 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 281252 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 281252 times.
✗ Branch 8 not taken.
 281252 S[2] = Tab[2] + Rab(2, 1) * u;
268
269
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 281252 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 281252 if (P && Q) {
270 ✗ *P << 0, t, 0;
271 ✗ *Q = S + (*P);
272 }
273
274
1/2
✓ Branch 1 taken 281252 times.
✗ Branch 2 not taken.
 281252 return S.norm();
275 }
276 }
277
278 Scalar ALL_y, ALU_y, AUL_y, AUU_y;
279
280
1/2
✓ Branch 1 taken 366645 times.
✗ Branch 2 not taken.
 366645 ALL_y = -Tba[1];
281 366645 ALU_y = ALL_y + aA1_dot_B1;
282 366645 AUL_y = ALL_y + aA0_dot_B1;
283 366645 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 195930 times.
✓ Branch 1 taken 170715 times.
 366645 if (ALL_y < ALU_y) {
288 195930 LA1_ly = ALL_y;
289 195930 LA1_uy = ALU_y;
290 195930 UA1_ly = AUL_y;
291 195930 UA1_uy = AUU_y;
292 } else {
293 170715 LA1_ly = ALU_y;
294 170715 LA1_uy = ALL_y;
295 170715 UA1_ly = AUU_y;
296 170715 UA1_uy = AUL_y;
297 }
298
299
2/2
✓ Branch 0 taken 190103 times.
✓ Branch 1 taken 176542 times.
 366645 if (BLL_x < BUL_x) {
300 190103 LB0_lx = BLL_x;
301 190103 LB0_ux = BUL_x;
302 190103 UB0_lx = BLU_x;
303 190103 UB0_ux = BUU_x;
304 } else {
305 176542 LB0_lx = BUL_x;
306 176542 LB0_ux = BLL_x;
307 176542 UB0_lx = BUU_x;
308 176542 UB0_ux = BLU_x;
309 }
310
311 // UA1, UB0
312
313
4/4
✓ Branch 0 taken 162845 times.
✓ Branch 1 taken 203800 times.
✓ Branch 2 taken 46940 times.
✓ Branch 3 taken 115905 times.
 366645 if ((UA1_uy > b[1]) && (UB0_ux > a[0])) {
314 100839 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 39981 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 18848 times.
✓ Branch 12 taken 28092 times.
 93880 aA0_dot_B0 - Tba[0], -Tab[1] - bA1_dot_B1)) &&
317
2/2
✓ Branch 0 taken 28627 times.
✓ Branch 1 taken 14844 times.
 43471 ((UB0_lx > a[0]) ||
318
3/4
✓ Branch 1 taken 28627 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4004 times.
✓ Branch 4 taken 24623 times.
 28627 inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0] + bA0_dot_B1, A1_dot_B0,
319
3/6
✓ Branch 1 taken 28627 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28627 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28627 times.
✗ Branch 8 not taken.
 28627 Tab[1] + bA1_dot_B1, Tba[0] - aA0_dot_B0))) {
320
1/2
✓ Branch 1 taken 18848 times.
✗ Branch 2 not taken.
 18848 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1,
321
2/4
✓ Branch 1 taken 18848 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18848 times.
✗ Branch 5 not taken.
 18848 Tba[0] - aA0_dot_B0);
322
323
4/8
✓ Branch 1 taken 18848 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18848 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18848 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18848 times.
✗ Branch 11 not taken.
 18848 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - a[0];
324
4/8
✓ Branch 1 taken 18848 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18848 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18848 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18848 times.
✗ Branch 11 not taken.
 18848 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
325
4/8
✓ Branch 1 taken 18848 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18848 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18848 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18848 times.
✗ Branch 11 not taken.
 18848 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
326
327
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 18848 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 18848 if (P && Q) {
328 ✗ *P << a[0], t, 0;
329 ✗ *Q = S + (*P);
330 }
331
332
1/2
✓ Branch 1 taken 18848 times.
✗ Branch 2 not taken.
 18848 return S.norm();
333 }
334 }
335
336 // UA1, LB0
337
338
4/4
✓ Branch 0 taken 196582 times.
✓ Branch 1 taken 151215 times.
✓ Branch 2 taken 58531 times.
✓ Branch 3 taken 138051 times.
 347797 if ((UA1_ly < 0) && (LB0_ux > a[0])) {
339
3/4
✓ Branch 1 taken 7882 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4636 times.
✓ Branch 4 taken 3246 times.
 7882 if (((UA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1] - aA0_dot_B1,
340
7/10
✓ Branch 0 taken 7882 times.
✓ Branch 1 taken 50649 times.
✓ Branch 3 taken 7882 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7882 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7882 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 37037 times.
✓ Branch 12 taken 21494 times.
 117062 A1_dot_B0, aA0_dot_B0 - Tba[0], -Tab[1])) &&
341
2/2
✓ Branch 0 taken 23937 times.
✓ Branch 1 taken 31348 times.
 55285 ((LB0_lx > a[0]) ||
342
4/6
✓ Branch 1 taken 23937 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23937 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5689 times.
✓ Branch 7 taken 18248 times.
 23937 inVoronoi(a[1], b[0], A0_dot_B0, Tab[0] - a[0], A1_dot_B0, Tab[1],
343
2/4
✓ Branch 1 taken 23937 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23937 times.
✗ Branch 5 not taken.
 23937 Tba[0] - aA0_dot_B0))) {
344
3/6
✓ Branch 1 taken 37037 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37037 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37037 times.
✗ Branch 8 not taken.
 37037 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0] - aA0_dot_B0);
345
346
3/6
✓ Branch 1 taken 37037 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37037 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37037 times.
✗ Branch 8 not taken.
 37037 S[0] = Tab[0] + Rab(0, 0) * u - a[0];
347
3/6
✓ Branch 1 taken 37037 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37037 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37037 times.
✗ Branch 8 not taken.
 37037 S[1] = Tab[1] + Rab(1, 0) * u - t;
348
3/6
✓ Branch 1 taken 37037 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37037 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37037 times.
✗ Branch 8 not taken.
 37037 S[2] = Tab[2] + Rab(2, 0) * u;
349
350
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 37037 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 37037 if (P && Q) {
351 ✗ *P << a[0], t, 0;
352 ✗ *Q = S + (*P);
353 }
354
355
1/2
✓ Branch 1 taken 37037 times.
✗ Branch 2 not taken.
 37037 return S.norm();
356 }
357 }
358
359 // LA1, UB0
360
361
4/4
✓ Branch 0 taken 133636 times.
✓ Branch 1 taken 177124 times.
✓ Branch 2 taken 39910 times.
✓ Branch 3 taken 93726 times.
 310760 if ((LA1_uy > b[1]) && (UB0_lx < 0)) {
362 87316 if (((LA1_ly > b[1]) ||
363
4/6
✓ Branch 1 taken 7496 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7496 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4121 times.
✓ Branch 7 taken 3375 times.
 7496 inVoronoi(b[0], a[1], A1_dot_B1, -Tba[1] - b[1], A1_dot_B0, -Tba[0],
364
8/10
✓ Branch 0 taken 7496 times.
✓ Branch 1 taken 32414 times.
✓ Branch 3 taken 7496 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7496 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 22585 times.
✓ Branch 9 taken 13950 times.
✓ Branch 10 taken 19842 times.
✓ Branch 11 taken 20068 times.
 69991 -Tab[1] - bA1_dot_B1)) &&
365
366
3/4
✓ Branch 1 taken 22585 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5892 times.
✓ Branch 4 taken 16693 times.
 22585 ((UB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0] - bA0_dot_B1,
367
3/6
✓ Branch 1 taken 22585 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22585 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22585 times.
✗ Branch 8 not taken.
 22585 A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]))) {
368
3/6
✓ Branch 1 taken 19842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19842 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19842 times.
✗ Branch 8 not taken.
 19842 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1] + bA1_dot_B1, Tba[0]);
369
370
4/8
✓ Branch 1 taken 19842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19842 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19842 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19842 times.
✗ Branch 11 not taken.
 19842 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u;
371
4/8
✓ Branch 1 taken 19842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19842 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19842 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19842 times.
✗ Branch 11 not taken.
 19842 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - t;
372
4/8
✓ Branch 1 taken 19842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19842 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19842 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19842 times.
✗ Branch 11 not taken.
 19842 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
373
374
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 19842 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 19842 if (P && Q) {
375 ✗ *P << 0, t, 0;
376 ✗ *Q = S + (*P);
377 }
378
379
1/2
✓ Branch 1 taken 19842 times.
✗ Branch 2 not taken.
 19842 return S.norm();
380 }
381 }
382
383 // LA1, LB0
384
385
4/4
✓ Branch 0 taken 170268 times.
✓ Branch 1 taken 120650 times.
✓ Branch 2 taken 76671 times.
✓ Branch 3 taken 93597 times.
 290918 if ((LA1_ly < 0) && (LB0_lx < 0)) {
386
3/4
✓ Branch 1 taken 7298 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3073 times.
✓ Branch 4 taken 4225 times.
 7298 if (((LA1_uy < 0) || inVoronoi(b[0], a[1], -A1_dot_B1, Tba[1], A1_dot_B0,
387
9/12
✓ Branch 0 taken 7298 times.
✓ Branch 1 taken 69373 times.
✓ Branch 3 taken 7298 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7298 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7298 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 28553 times.
✓ Branch 12 taken 43893 times.
✓ Branch 13 taken 50502 times.
✓ Branch 14 taken 26169 times.
 112522 -Tba[0], -Tab[1])) &&
388
3/4
✓ Branch 1 taken 28553 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6609 times.
✓ Branch 4 taken 21944 times.
 28553 ((LB0_ux < 0) || inVoronoi(a[1], b[0], -A0_dot_B0, -Tab[0], A1_dot_B0,
389
3/6
✓ Branch 1 taken 28553 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28553 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28553 times.
✗ Branch 8 not taken.
 28553 Tab[1], Tba[0]))) {
390
3/6
✓ Branch 1 taken 50502 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50502 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50502 times.
✗ Branch 8 not taken.
 50502 segCoords(t, u, a[1], b[0], A1_dot_B0, Tab[1], Tba[0]);
391
392
3/6
✓ Branch 1 taken 50502 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50502 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50502 times.
✗ Branch 8 not taken.
 50502 S[0] = Tab[0] + Rab(0, 0) * u;
393
3/6
✓ Branch 1 taken 50502 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50502 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50502 times.
✗ Branch 8 not taken.
 50502 S[1] = Tab[1] + Rab(1, 0) * u - t;
394
3/6
✓ Branch 1 taken 50502 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50502 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50502 times.
✗ Branch 8 not taken.
 50502 S[2] = Tab[2] + Rab(2, 0) * u;
395
396
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 50502 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 50502 if (P && Q) {
397 ✗ *P << 0, t, 0;
398 ✗ *Q = S + (*P);
399 }
400
401
1/2
✓ Branch 1 taken 50502 times.
✗ Branch 2 not taken.
 50502 return S.norm();
402 }
403 }
404
405 Scalar BLL_y, BLU_y, BUL_y, BUU_y;
406
407
1/2
✓ Branch 1 taken 240416 times.
✗ Branch 2 not taken.
 240416 BLL_y = Tab[1];
408 240416 BLU_y = BLL_y + bA1_dot_B1;
409 240416 BUL_y = BLL_y + bA1_dot_B0;
410 240416 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 124019 times.
✓ Branch 1 taken 116397 times.
 240416 if (ALL_x < AUL_x) {
415 124019 LA0_lx = ALL_x;
416 124019 LA0_ux = AUL_x;
417 124019 UA0_lx = ALU_x;
418 124019 UA0_ux = AUU_x;
419 } else {
420 116397 LA0_lx = AUL_x;
421 116397 LA0_ux = ALL_x;
422 116397 UA0_lx = AUU_x;
423 116397 UA0_ux = ALU_x;
424 }
425
426
2/2
✓ Branch 0 taken 130569 times.
✓ Branch 1 taken 109847 times.
 240416 if (BLL_y < BLU_y) {
427 130569 LB1_ly = BLL_y;
428 130569 LB1_uy = BLU_y;
429 130569 UB1_ly = BUL_y;
430 130569 UB1_uy = BUU_y;
431 } else {
432 109847 LB1_ly = BLU_y;
433 109847 LB1_uy = BLL_y;
434 109847 UB1_ly = BUU_y;
435 109847 UB1_uy = BUL_y;
436 }
437
438 // UA0, UB1
439
440
4/4
✓ Branch 0 taken 131946 times.
✓ Branch 1 taken 108470 times.
✓ Branch 2 taken 47394 times.
✓ Branch 3 taken 84552 times.
 240416 if ((UA0_ux > b[0]) && (UB1_uy > a[1])) {
441 111310 if (((UA0_lx > b[0]) ||
442
3/4
✓ Branch 1 taken 16522 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7097 times.
✓ Branch 4 taken 9425 times.
 16522 inVoronoi(b[1], a[0], A0_dot_B0, aA1_dot_B0 - Tba[0] - b[0], A0_dot_B1,
443
7/10
✓ Branch 0 taken 16522 times.
✓ Branch 1 taken 30872 times.
✓ Branch 3 taken 16522 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16522 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16522 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 35707 times.
✓ Branch 12 taken 11687 times.
 94788 aA1_dot_B1 - Tba[1], -Tab[0] - bA0_dot_B0)) &&
444
2/2
✓ Branch 0 taken 4466 times.
✓ Branch 1 taken 33503 times.
 37969 ((UB1_ly > a[1]) ||
445
3/4
✓ Branch 1 taken 4466 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2204 times.
✓ Branch 4 taken 2262 times.
 4466 inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1] + bA1_dot_B0, A0_dot_B1,
446
3/6
✓ Branch 1 taken 4466 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4466 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4466 times.
✗ Branch 8 not taken.
 4466 Tab[0] + bA0_dot_B0, Tba[1] - aA1_dot_B1))) {
447
1/2
✓ Branch 1 taken 35707 times.
✗ Branch 2 not taken.
 35707 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0,
448
2/4
✓ Branch 1 taken 35707 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35707 times.
✗ Branch 5 not taken.
 35707 Tba[1] - aA1_dot_B1);
449
450
4/8
✓ Branch 1 taken 35707 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35707 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35707 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35707 times.
✗ Branch 11 not taken.
 35707 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
451
4/8
✓ Branch 1 taken 35707 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35707 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35707 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35707 times.
✗ Branch 11 not taken.
 35707 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - a[1];
452
4/8
✓ Branch 1 taken 35707 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35707 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35707 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 35707 times.
✗ Branch 11 not taken.
 35707 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
453
454
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 35707 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 35707 if (P && Q) {
455 ✗ *P << t, a[1], 0;
456 ✗ *Q = S + (*P);
457 }
458
459
1/2
✓ Branch 1 taken 35707 times.
✗ Branch 2 not taken.
 35707 return S.norm();
460 }
461 }
462
463 // UA0, LB1
464
465
4/4
✓ Branch 0 taken 110414 times.
✓ Branch 1 taken 94295 times.
✓ Branch 2 taken 45176 times.
✓ Branch 3 taken 65238 times.
 204709 if ((UA0_lx < 0) && (LB1_uy > a[1])) {
466
3/4
✓ Branch 1 taken 16255 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6942 times.
✓ Branch 4 taken 9313 times.
 16255 if (((UA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0] - aA1_dot_B0,
467
7/10
✓ Branch 0 taken 16255 times.
✓ Branch 1 taken 28921 times.
✓ Branch 3 taken 16255 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16255 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 16255 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 34304 times.
✓ Branch 12 taken 10872 times.
 90352 A0_dot_B1, aA1_dot_B1 - Tba[1], -Tab[0])) &&
468
2/2
✓ Branch 0 taken 5151 times.
✓ Branch 1 taken 30712 times.
 35863 ((LB1_ly > a[1]) ||
469
4/6
✓ Branch 1 taken 5151 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5151 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3592 times.
✓ Branch 7 taken 1559 times.
 5151 inVoronoi(a[0], b[1], A1_dot_B1, Tab[1] - a[1], A0_dot_B1, Tab[0],
470
2/4
✓ Branch 1 taken 5151 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5151 times.
✗ Branch 5 not taken.
 5151 Tba[1] - aA1_dot_B1))) {
471
3/6
✓ Branch 1 taken 34304 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34304 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34304 times.
✗ Branch 8 not taken.
 34304 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1] - aA1_dot_B1);
472
473
3/6
✓ Branch 1 taken 34304 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34304 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34304 times.
✗ Branch 8 not taken.
 34304 S[0] = Tab[0] + Rab(0, 1) * u - t;
474
3/6
✓ Branch 1 taken 34304 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34304 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34304 times.
✗ Branch 8 not taken.
 34304 S[1] = Tab[1] + Rab(1, 1) * u - a[1];
475
3/6
✓ Branch 1 taken 34304 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34304 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34304 times.
✗ Branch 8 not taken.
 34304 S[2] = Tab[2] + Rab(2, 1) * u;
476
477
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 34304 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 34304 if (P && Q) {
478 ✗ *P << t, a[1], 0;
479 ✗ *Q = S + (*P);
480 }
481
482
1/2
✓ Branch 1 taken 34304 times.
✗ Branch 2 not taken.
 34304 return S.norm();
483 }
484 }
485
486 // LA0, UB1
487
488
4/4
✓ Branch 0 taken 94085 times.
✓ Branch 1 taken 76320 times.
✓ Branch 2 taken 63555 times.
✓ Branch 3 taken 30530 times.
 170405 if ((LA0_ux > b[0]) && (UB1_ly < 0)) {
489 145140 if (((LA0_lx > b[0]) ||
490
4/6
✓ Branch 1 taken 18030 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18030 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 8746 times.
✓ Branch 7 taken 9284 times.
 18030 inVoronoi(b[1], a[0], A0_dot_B0, -b[0] - Tba[0], A0_dot_B1, -Tba[1],
491
8/10
✓ Branch 0 taken 18030 times.
✓ Branch 1 taken 45525 times.
✓ Branch 3 taken 18030 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18030 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 5616 times.
✓ Branch 9 taken 48655 times.
✓ Branch 10 taken 52312 times.
✓ Branch 11 taken 11243 times.
 87201 -bA0_dot_B0 - Tab[0])) &&
492
3/4
✓ Branch 1 taken 5616 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3657 times.
✓ Branch 4 taken 1959 times.
 5616 ((UB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1] - bA1_dot_B0,
493
3/6
✓ Branch 1 taken 5616 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5616 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5616 times.
✗ Branch 8 not taken.
 5616 A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]))) {
494
3/6
✓ Branch 1 taken 52312 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52312 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52312 times.
✗ Branch 8 not taken.
 52312 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0, Tba[1]);
495
496
4/8
✓ Branch 1 taken 52312 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52312 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52312 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52312 times.
✗ Branch 11 not taken.
 52312 S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - t;
497
4/8
✓ Branch 1 taken 52312 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52312 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52312 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52312 times.
✗ Branch 11 not taken.
 52312 S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u;
498
4/8
✓ Branch 1 taken 52312 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 52312 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52312 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52312 times.
✗ Branch 11 not taken.
 52312 S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
499
500
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 52312 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 52312 if (P && Q) {
501 ✗ *P << t, 0, 0;
502 ✗ *Q = S + (*P);
503 }
504
505
1/2
✓ Branch 1 taken 52312 times.
✗ Branch 2 not taken.
 52312 return S.norm();
506 }
507 }
508
509 // LA0, LB1
510
511
4/4
✓ Branch 0 taken 78110 times.
✓ Branch 1 taken 39983 times.
✓ Branch 2 taken 42546 times.
✓ Branch 3 taken 35564 times.
 118093 if ((LA0_lx < 0) && (LB1_ly < 0)) {
512
3/4
✓ Branch 1 taken 15365 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8419 times.
✓ Branch 4 taken 6946 times.
 15365 if (((LA0_ux < 0) || inVoronoi(b[1], a[0], -A0_dot_B0, Tba[0], A0_dot_B1,
513
9/12
✓ Branch 0 taken 15365 times.
✓ Branch 1 taken 27181 times.
✓ Branch 3 taken 15365 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15365 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 15365 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 3692 times.
✓ Branch 12 taken 31908 times.
✓ Branch 13 taken 33490 times.
✓ Branch 14 taken 9056 times.
 61603 -Tba[1], -Tab[0])) &&
514
3/4
✓ Branch 1 taken 3692 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1582 times.
✓ Branch 4 taken 2110 times.
 3692 ((LB1_uy < 0) || inVoronoi(a[0], b[1], -A1_dot_B1, -Tab[1], A0_dot_B1,
515
3/6
✓ Branch 1 taken 3692 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3692 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3692 times.
✗ Branch 8 not taken.
 3692 Tab[0], Tba[1]))) {
516
3/6
✓ Branch 1 taken 33490 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33490 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33490 times.
✗ Branch 8 not taken.
 33490 segCoords(t, u, a[0], b[1], A0_dot_B1, Tab[0], Tba[1]);
517
518
3/6
✓ Branch 1 taken 33490 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33490 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33490 times.
✗ Branch 8 not taken.
 33490 S[0] = Tab[0] + Rab(0, 1) * u - t;
519
3/6
✓ Branch 1 taken 33490 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33490 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33490 times.
✗ Branch 8 not taken.
 33490 S[1] = Tab[1] + Rab(1, 1) * u;
520
3/6
✓ Branch 1 taken 33490 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 33490 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 33490 times.
✗ Branch 8 not taken.
 33490 S[2] = Tab[2] + Rab(2, 1) * u;
521
522
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 33490 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 33490 if (P && Q) {
523 ✗ *P << t, 0, 0;
524 ✗ *Q = S + (*P);
525 }
526
527
1/2
✓ Branch 1 taken 33490 times.
✗ Branch 2 not taken.
 33490 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 42707 times.
✓ Branch 1 taken 41896 times.
 84603 if (ALL_y < AUL_y) {
534 42707 LA0_ly = ALL_y;
535 42707 LA0_uy = AUL_y;
536 42707 UA0_ly = ALU_y;
537 42707 UA0_uy = AUU_y;
538 } else {
539 41896 LA0_ly = AUL_y;
540 41896 LA0_uy = ALL_y;
541 41896 UA0_ly = AUU_y;
542 41896 UA0_uy = ALU_y;
543 }
544
545
2/2
✓ Branch 0 taken 43332 times.
✓ Branch 1 taken 41271 times.
 84603 if (BLL_y < BUL_y) {
546 43332 LB0_ly = BLL_y;
547 43332 LB0_uy = BUL_y;
548 43332 UB0_ly = BLU_y;
549 43332 UB0_uy = BUU_y;
550 } else {
551 41271 LB0_ly = BUL_y;
552 41271 LB0_uy = BLL_y;
553 41271 UB0_ly = BUU_y;
554 41271 UB0_uy = BLU_y;
555 }
556
557 // UA0, UB0
558
559
4/4
✓ Branch 0 taken 50990 times.
✓ Branch 1 taken 33613 times.
✓ Branch 2 taken 18296 times.
✓ Branch 3 taken 32694 times.
 84603 if ((UA0_uy > b[1]) && (UB0_uy > a[1])) {
560 50042 if (((UA0_ly > b[1]) ||
561
3/4
✓ Branch 1 taken 13450 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5687 times.
✓ Branch 4 taken 7763 times.
 13450 inVoronoi(b[0], a[0], A0_dot_B1, aA1_dot_B1 - Tba[1] - b[1], A0_dot_B0,
562
7/10
✓ Branch 0 taken 13450 times.
✓ Branch 1 taken 4846 times.
✓ Branch 3 taken 13450 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13450 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13450 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4407 times.
✓ Branch 12 taken 13889 times.
 36592 aA1_dot_B0 - Tba[0], -Tab[0] - bA0_dot_B1)) &&
563
2/2
✓ Branch 0 taken 8334 times.
✓ Branch 1 taken 2199 times.
 10533 ((UB0_ly > a[1]) ||
564
3/4
✓ Branch 1 taken 8334 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2208 times.
✓ Branch 4 taken 6126 times.
 8334 inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1] + bA1_dot_B1, A0_dot_B0,
565
3/6
✓ Branch 1 taken 8334 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8334 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8334 times.
✗ Branch 8 not taken.
 8334 Tab[0] + bA0_dot_B1, Tba[0] - aA1_dot_B0))) {
566
1/2
✓ Branch 1 taken 4407 times.
✗ Branch 2 not taken.
 4407 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1,
567
2/4
✓ Branch 1 taken 4407 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4407 times.
✗ Branch 5 not taken.
 4407 Tba[0] - aA1_dot_B0);
568
569
4/8
✓ Branch 1 taken 4407 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4407 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4407 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4407 times.
✗ Branch 11 not taken.
 4407 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
570
4/8
✓ Branch 1 taken 4407 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4407 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4407 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4407 times.
✗ Branch 11 not taken.
 4407 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u - a[1];
571
4/8
✓ Branch 1 taken 4407 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4407 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4407 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4407 times.
✗ Branch 11 not taken.
 4407 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
572
573
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4407 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4407 if (P && Q) {
574 ✗ *P << t, a[1], 0;
575 ✗ *Q = S + (*P);
576 }
577
578
1/2
✓ Branch 1 taken 4407 times.
✗ Branch 2 not taken.
 4407 return S.norm();
579 }
580 }
581
582 // UA0, LB0
583
584
4/4
✓ Branch 0 taken 46326 times.
✓ Branch 1 taken 33870 times.
✓ Branch 2 taken 17064 times.
✓ Branch 3 taken 29262 times.
 80196 if ((UA0_ly < 0) && (LB0_uy > a[1])) {
585
3/4
✓ Branch 1 taken 9777 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4210 times.
✓ Branch 4 taken 5567 times.
 9777 if (((UA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1] - aA1_dot_B1,
586
7/10
✓ Branch 0 taken 9777 times.
✓ Branch 1 taken 7287 times.
✓ Branch 3 taken 9777 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9777 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9777 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 7842 times.
✓ Branch 12 taken 9222 times.
 34128 A0_dot_B0, aA1_dot_B0 - Tba[0], -Tab[0])) &&
587
2/2
✓ Branch 0 taken 5937 times.
✓ Branch 1 taken 5560 times.
 11497 ((LB0_ly > a[1]) ||
588
4/6
✓ Branch 1 taken 5937 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5937 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2282 times.
✓ Branch 7 taken 3655 times.
 5937 inVoronoi(a[0], b[0], A1_dot_B0, Tab[1] - a[1], A0_dot_B0, Tab[0],
589
2/4
✓ Branch 1 taken 5937 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5937 times.
✗ Branch 5 not taken.
 5937 Tba[0] - aA1_dot_B0))) {
590
3/6
✓ Branch 1 taken 7842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7842 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7842 times.
✗ Branch 8 not taken.
 7842 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0] - aA1_dot_B0);
591
592
3/6
✓ Branch 1 taken 7842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7842 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7842 times.
✗ Branch 8 not taken.
 7842 S[0] = Tab[0] + Rab(0, 0) * u - t;
593
3/6
✓ Branch 1 taken 7842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7842 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7842 times.
✗ Branch 8 not taken.
 7842 S[1] = Tab[1] + Rab(1, 0) * u - a[1];
594
3/6
✓ Branch 1 taken 7842 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7842 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7842 times.
✗ Branch 8 not taken.
 7842 S[2] = Tab[2] + Rab(2, 0) * u;
595
596
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 7842 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 7842 if (P && Q) {
597 ✗ *P << t, a[1], 0;
598 ✗ *Q = S + (*P);
599 }
600
601
1/2
✓ Branch 1 taken 7842 times.
✗ Branch 2 not taken.
 7842 return S.norm();
602 }
603 }
604
605 // LA0, UB0
606
607
4/4
✓ Branch 0 taken 43660 times.
✓ Branch 1 taken 28694 times.
✓ Branch 2 taken 14628 times.
✓ Branch 3 taken 29032 times.
 72354 if ((LA0_uy > b[1]) && (UB0_ly < 0)) {
608 40027 if (((LA0_ly > b[1]) ||
609
4/6
✓ Branch 1 taken 10771 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10771 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4673 times.
✓ Branch 7 taken 6098 times.
 10771 inVoronoi(b[0], a[0], A0_dot_B1, -Tba[1] - b[1], A0_dot_B0, -Tba[0],
610
8/10
✓ Branch 0 taken 10771 times.
✓ Branch 1 taken 3857 times.
✓ Branch 3 taken 10771 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10771 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6292 times.
✓ Branch 9 taken 2238 times.
✓ Branch 10 taken 4860 times.
✓ Branch 11 taken 9768 times.
 31691 -Tab[0] - bA0_dot_B1)) &&
611
612
3/4
✓ Branch 1 taken 6292 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2622 times.
✓ Branch 4 taken 3670 times.
 6292 ((UB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1] - bA1_dot_B1,
613
3/6
✓ Branch 1 taken 6292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6292 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6292 times.
✗ Branch 8 not taken.
 6292 A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]))) {
614
3/6
✓ Branch 1 taken 4860 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4860 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4860 times.
✗ Branch 8 not taken.
 4860 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0] + bA0_dot_B1, Tba[0]);
615
616
4/8
✓ Branch 1 taken 4860 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4860 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4860 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4860 times.
✗ Branch 11 not taken.
 4860 S[0] = Tab[0] + Rab(0, 1) * b[1] + Rab(0, 0) * u - t;
617
4/8
✓ Branch 1 taken 4860 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4860 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4860 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4860 times.
✗ Branch 11 not taken.
 4860 S[1] = Tab[1] + Rab(1, 1) * b[1] + Rab(1, 0) * u;
618
4/8
✓ Branch 1 taken 4860 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4860 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4860 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4860 times.
✗ Branch 11 not taken.
 4860 S[2] = Tab[2] + Rab(2, 1) * b[1] + Rab(2, 0) * u;
619
620
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 4860 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4860 if (P && Q) {
621 ✗ *P << t, 0, 0;
622 ✗ *Q = S + (*P);
623 }
624
625
1/2
✓ Branch 1 taken 4860 times.
✗ Branch 2 not taken.
 4860 return S.norm();
626 }
627 }
628
629 // LA0, LB0
630
631
4/4
✓ Branch 0 taken 39381 times.
✓ Branch 1 taken 28113 times.
✓ Branch 2 taken 11518 times.
✓ Branch 3 taken 27863 times.
 67494 if ((LA0_ly < 0) && (LB0_ly < 0)) {
632
3/4
✓ Branch 1 taken 8534 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3756 times.
✓ Branch 4 taken 4778 times.
 8534 if (((LA0_uy < 0) || inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
633
9/12
✓ Branch 0 taken 8534 times.
✓ Branch 1 taken 2984 times.
✓ Branch 3 taken 8534 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8534 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8534 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 4925 times.
✓ Branch 12 taken 1815 times.
✓ Branch 13 taken 3321 times.
✓ Branch 14 taken 8197 times.
 24977 -Tba[0], -Tab[0])) &&
634
3/4
✓ Branch 1 taken 4925 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1506 times.
✓ Branch 4 taken 3419 times.
 4925 ((LB0_uy < 0) || inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
635
3/6
✓ Branch 1 taken 4925 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4925 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4925 times.
✗ Branch 8 not taken.
 4925 Tab[0], Tba[0]))) {
636
3/6
✓ Branch 1 taken 3321 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3321 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3321 times.
✗ Branch 8 not taken.
 3321 segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
637
638
3/6
✓ Branch 1 taken 3321 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3321 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3321 times.
✗ Branch 8 not taken.
 3321 S[0] = Tab[0] + Rab(0, 0) * u - t;
639
3/6
✓ Branch 1 taken 3321 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3321 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3321 times.
✗ Branch 8 not taken.
 3321 S[1] = Tab[1] + Rab(1, 0) * u;
640
3/6
✓ Branch 1 taken 3321 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3321 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3321 times.
✗ Branch 8 not taken.
 3321 S[2] = Tab[2] + Rab(2, 0) * u;
641
642
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3321 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 3321 if (P && Q) {
643 ✗ *P << t, 0, 0;
644 ✗ *Q = S + (*P);
645 }
646
647
1/2
✓ Branch 1 taken 3321 times.
✗ Branch 2 not taken.
 3321 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 64173 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33655 times.
✓ Branch 4 taken 30518 times.
 64173 if (Tab[2] > 0.0) {
656
1/2
✓ Branch 1 taken 33655 times.
✗ Branch 2 not taken.
 33655 sep1 = Tab[2];
657
4/6
✓ Branch 1 taken 33655 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20055 times.
✓ Branch 4 taken 13600 times.
✓ Branch 6 taken 20055 times.
✗ Branch 7 not taken.
 33655 if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
658
4/6
✓ Branch 1 taken 33655 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18491 times.
✓ Branch 4 taken 15164 times.
✓ Branch 6 taken 18491 times.
✗ Branch 7 not taken.
 33655 if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
659 } else {
660
1/2
✓ Branch 1 taken 30518 times.
✗ Branch 2 not taken.
 30518 sep1 = -Tab[2];
661
4/6
✓ Branch 1 taken 30518 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18245 times.
✓ Branch 4 taken 12273 times.
✓ Branch 6 taken 18245 times.
✗ Branch 7 not taken.
 30518 if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
662
4/6
✓ Branch 1 taken 30518 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17185 times.
✓ Branch 4 taken 13333 times.
✓ Branch 6 taken 17185 times.
✗ Branch 7 not taken.
 30518 if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
663 }
664
665
3/4
✓ Branch 1 taken 64173 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33408 times.
✓ Branch 4 taken 30765 times.
 64173 if (Tba[2] < 0) {
666
1/2
✓ Branch 1 taken 33408 times.
✗ Branch 2 not taken.
 33408 sep2 = -Tba[2];
667
4/6
✓ Branch 1 taken 33408 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23267 times.
✓ Branch 4 taken 10141 times.
✓ Branch 6 taken 23267 times.
✗ Branch 7 not taken.
 33408 if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
668
4/6
✓ Branch 1 taken 33408 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 19960 times.
✓ Branch 4 taken 13448 times.
✓ Branch 6 taken 19960 times.
✗ Branch 7 not taken.
 33408 if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
669 } else {
670
1/2
✓ Branch 1 taken 30765 times.
✗ Branch 2 not taken.
 30765 sep2 = Tba[2];
671
4/6
✓ Branch 1 taken 30765 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23132 times.
✓ Branch 4 taken 7633 times.
✓ Branch 6 taken 23132 times.
✗ Branch 7 not taken.
 30765 if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
672
4/6
✓ Branch 1 taken 30765 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18689 times.
✓ Branch 4 taken 12076 times.
✓ Branch 6 taken 18689 times.
✗ Branch 7 not taken.
 30765 if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
673 }
674
675
4/4
✓ Branch 0 taken 44082 times.
✓ Branch 1 taken 20091 times.
✓ Branch 2 taken 33996 times.
✓ Branch 3 taken 10086 times.
 64173 if (sep1 >= sep2 && sep1 >= 0) {
676
3/4
✓ Branch 1 taken 33996 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17379 times.
✓ Branch 4 taken 16617 times.
 33996 if (Tab[2] > 0)
677
3/6
✓ Branch 1 taken 17379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17379 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17379 times.
✗ Branch 8 not taken.
 17379 S << 0, 0, sep1;
678 else
679
3/6
✓ Branch 1 taken 16617 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16617 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16617 times.
✗ Branch 8 not taken.
 16617 S << 0, 0, -sep1;
680
681
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 33996 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 33996 if (P && Q) {
682 ✗ *Q = S;
683 ✗ P->setZero();
684 }
685 }
686
687
4/4
✓ Branch 0 taken 20256 times.
✓ Branch 1 taken 43917 times.
✓ Branch 2 taken 15170 times.
✓ Branch 3 taken 5086 times.
 64173 if (sep2 >= sep1 && sep2 >= 0) {
688
4/8
✓ Branch 1 taken 15170 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15170 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15170 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15170 times.
✗ Branch 11 not taken.
 15170 Vec3s Q_(Tab[0], Tab[1], Tab[2]);
689
1/2
✓ Branch 1 taken 15170 times.
✗ Branch 2 not taken.
 15170 Vec3s P_;
690
3/4
✓ Branch 1 taken 15170 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 8597 times.
✓ Branch 4 taken 6573 times.
 15170 if (Tba[2] < 0) {
691
3/6
✓ Branch 1 taken 8597 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8597 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8597 times.
✗ Branch 8 not taken.
 8597 P_[0] = Rab(0, 2) * sep2 + Tab[0];
692
3/6
✓ Branch 1 taken 8597 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8597 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8597 times.
✗ Branch 8 not taken.
 8597 P_[1] = Rab(1, 2) * sep2 + Tab[1];
693
3/6
✓ Branch 1 taken 8597 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8597 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8597 times.
✗ Branch 8 not taken.
 8597 P_[2] = Rab(2, 2) * sep2 + Tab[2];
694 } else {
695
3/6
✓ Branch 1 taken 6573 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6573 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6573 times.
✗ Branch 8 not taken.
 6573 P_[0] = -Rab(0, 2) * sep2 + Tab[0];
696
3/6
✓ Branch 1 taken 6573 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6573 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6573 times.
✗ Branch 8 not taken.
 6573 P_[1] = -Rab(1, 2) * sep2 + Tab[1];
697
3/6
✓ Branch 1 taken 6573 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6573 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6573 times.
✗ Branch 8 not taken.
 6573 P_[2] = -Rab(2, 2) * sep2 + Tab[2];
698 }
699
700
2/4
✓ Branch 1 taken 15170 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15170 times.
✗ Branch 5 not taken.
 15170 S = Q_ - P_;
701
702
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 15170 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 15170 if (P && Q) {
703 ✗ *P = P_;
704 ✗ *Q = Q_;
705 }
706 }
707
708
2/2
✓ Branch 0 taken 43917 times.
✓ Branch 1 taken 20256 times.
 64173 Scalar sep = (sep1 > sep2 ? sep1 : sep2);
709
2/2
✓ Branch 0 taken 48999 times.
✓ Branch 1 taken 15174 times.
 64173 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 1590856 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 1590856 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1590856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1590856 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1590856 times.
✗ Branch 11 not taken.
 1590856 Matrix3s R(b1.axes.transpose() * R0 * b2.axes);
998
4/8
✓ Branch 1 taken 1590856 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1590856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1590856 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1590856 times.
✗ Branch 11 not taken.
 1590856 Vec3s Ttemp(R0 * b2.Tr + T0 - b1.Tr);
999
1000
3/6
✓ Branch 1 taken 1590856 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1590856 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1590856 times.
✗ Branch 8 not taken.
 1590856 Vec3s T(b1.axes.transpose() * Ttemp);
1001
1002
1/2
✓ Branch 1 taken 1590856 times.
✗ Branch 2 not taken.
 1590856 Scalar dist = rectDistance(R, T, b1.length, b2.length, P, Q);
1003 1590856 dist -= (b1.radius + b2.radius);
1004
2/2
✓ Branch 0 taken 17785 times.
✓ Branch 1 taken 1573071 times.
 1590856 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