GCC Code Coverage Report

Directory: ./
File: test/obb.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 513 666 77.0%
Branches: 1015 2112 48.1%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014-2016, CNRS-LAAS
5 * Copyright (c) 2023, Inria
6 * Author: Florent Lamiraux
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of CNRS nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 */
36
37 #include <iostream>
38 #include <fstream>
39 #include <sstream>
40
41 #include <chrono>
42
43 #include "coal/narrowphase/narrowphase.h"
44
45 #include "../src/BV/OBB.h"
46 #include "coal/internal/shape_shape_func.h"
47 #include "utility.h"
48
49 using namespace coal;
50
51 10 void randomOBBs(Vec3s& a, Vec3s& b, Scalar extentNorm) {
52 // Extent norm is between 0 and extentNorm on each axis
53 // a = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3));
54 // b = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3));
55
56
4/8
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
 10 a = extentNorm * Vec3s::Random().cwiseAbs().normalized();
57
4/8
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
 10 b = extentNorm * Vec3s::Random().cwiseAbs().normalized();
58 10 }
59
60 100 void randomTransform(Matrix3s& B, Vec3s& T, const Vec3s& a, const Vec3s& b,
61 const Scalar extentNorm) {
62 // TODO Should we scale T to a and b norm ?
63 (void)a;
64 (void)b;
65 (void)extentNorm;
66
67
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 Scalar N = a.norm() + b.norm();
68 // A translation of norm N ensures there is no collision.
69 // Set translation to be between 0 and 2 * N;
70
5/10
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 100 times.
✗ Branch 15 not taken.
 100 T = (Vec3s::Random() / sqrt(3)) * 1.5 * N;
71 // T.setZero();
72
73
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 Quats q;
74
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 100 q.coeffs().setRandom();
75
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 q.normalize();
76
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 B = q;
77 100 }
78
79 #define NB_METHODS 7
80 #define MANUAL_PRODUCT 1
81
82 #if MANUAL_PRODUCT
83 #define PRODUCT(M33, v3) \
84 (M33.col(0) * v3[0] + M33.col(1) * v3[1] + M33.col(2) * v3[2])
85 #else
86 #define PRODUCT(M33, v3) (M33 * v3)
87 #endif
88
89 typedef std::chrono::high_resolution_clock clock_type;
90 typedef clock_type::duration duration_type;
91
92 const char* sep = ",\t";
93 const Scalar eps = Scalar(1.5e-7);
94
95 const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0,
96 ", ", // Coeff separator
97 ",\n", // Row separator
98 "(", // row prefix
99 ",)", // row suffix
100 "( ", // mat prefix
101 ", )" // mat suffix
102 );
103
104 namespace obbDisjoint_impls {
105 /// @return true if OBB are disjoint.
106 100 bool distance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b,
107 Scalar& distance) {
108
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 GJKSolver gjk;
109
6/12
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
 100 Box ba(2 * a), bb(2 * b);
110
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 Transform3s tfa, tfb(B, T);
111
112
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 100 Vec3s p1, p2, normal;
113 100 bool compute_penetration = true;
114 100 distance =
115
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 gjk.shapeDistance(ba, tfa, bb, tfb, compute_penetration, p1, p2, normal);
116
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 return (distance > gjk.getDistancePrecision(compute_penetration));
117 100 }
118
119 4100 inline Scalar _computeDistanceForCase1(const Vec3s& T, const Vec3s& a,
120 const Vec3s& b, const Matrix3s& Bf) {
121
1/2
✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
 4100 Vec3s AABB_corner;
122 /* This seems to be slower
123 AABB_corner.noalias() = T.cwiseAbs () - a;
124 AABB_corner.noalias() -= PRODUCT(Bf,b);
125 /*/
126 #if MANUAL_PRODUCT
127
4/8
✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
 4100 AABB_corner.noalias() = T.cwiseAbs() - a;
128
5/10
✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4100 times.
✗ Branch 14 not taken.
 4100 AABB_corner.noalias() -= Bf.col(0) * b[0];
129
5/10
✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4100 times.
✗ Branch 14 not taken.
 4100 AABB_corner.noalias() -= Bf.col(1) * b[1];
130
5/10
✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4100 times.
✗ Branch 14 not taken.
 4100 AABB_corner.noalias() -= Bf.col(2) * b[2];
131 #else
132 AABB_corner.noalias() = T.cwiseAbs() - Bf * b - a;
133 #endif
134 // */
135
4/8
✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
 4100 return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm();
136 }
137
138 1435 inline Scalar _computeDistanceForCase2(const Matrix3s& B, const Vec3s& T,
139 const Vec3s& a, const Vec3s& b,
140 const Matrix3s& Bf) {
141 /*
142 Vec3s AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b);
143 AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a);
144 return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm ();
145 /*/
146 #if MANUAL_PRODUCT
147 1435 Scalar s, t = 0;
148
4/8
✓ Branch 2 taken 1435 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1435 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1435 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1435 times.
✗ Branch 13 not taken.
 1435 s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
149
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1435 times.
 1435 if (s > 0) t += s * s;
150
4/8
✓ Branch 2 taken 1435 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1435 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1435 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1435 times.
✗ Branch 13 not taken.
 1435 s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
151
2/2
✓ Branch 0 taken 82 times.
✓ Branch 1 taken 1353 times.
 1435 if (s > 0) t += s * s;
152
4/8
✓ Branch 2 taken 1435 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1435 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1435 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1435 times.
✗ Branch 13 not taken.
 1435 s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2];
153
2/2
✓ Branch 0 taken 82 times.
✓ Branch 1 taken 1353 times.
 1435 if (s > 0) t += s * s;
154 1435 return t;
155 #else
156 Vec3s AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b);
157 return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm();
158 #endif
159 // */
160 }
161
162 100 int separatingAxisId(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
163 const Vec3s& b, const Scalar& breakDistance2,
164 Scalar& squaredLowerBoundDistance) {
165 100 int id = 0;
166
167
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 100 Matrix3s Bf(B.cwiseAbs());
168
169 // Corner of b axis aligned bounding box the closest to the origin
170
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
171
2/2
✓ Branch 0 taken 65 times.
✓ Branch 1 taken 35 times.
 100 if (squaredLowerBoundDistance > breakDistance2) return id;
172 35 ++id;
173
174 // | B^T T| - b - Bf^T a
175
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
176
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 31 times.
 35 if (squaredLowerBoundDistance > breakDistance2) return id;
177 31 ++id;
178
179 31 int ja = 1, ka = 2, jb = 1, kb = 2;
180
2/2
✓ Branch 0 taken 91 times.
✓ Branch 1 taken 29 times.
 120 for (int ia = 0; ia < 3; ++ia) {
181
2/2
✓ Branch 0 taken 273 times.
✓ Branch 1 taken 89 times.
 362 for (int ib = 0; ib < 3; ++ib) {
182
4/8
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 273 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 273 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 273 times.
✗ Branch 11 not taken.
 273 const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
183
184
4/8
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 273 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 273 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 273 times.
✗ Branch 11 not taken.
 273 const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
185
4/8
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 273 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 273 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 273 times.
✗ Branch 11 not taken.
 273 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
186 // We need to divide by the norm || Aia x Bib ||
187 // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
188
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 271 times.
 273 if (diff > 0) {
189
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
190
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (sinus2 > 1e-6) {
191 2 squaredLowerBoundDistance = diff * diff / sinus2;
192
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 if (squaredLowerBoundDistance > breakDistance2) {
193 2 return id;
194 }
195 }
196 /* // or
197 Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
198 squaredLowerBoundDistance = diff * diff;
199 if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
200 squaredLowerBoundDistance /= sinus2;
201 return true;
202 }
203 // */
204 }
205 271 ++id;
206
207 271 jb = kb;
208 271 kb = ib;
209 }
210 89 ja = ka;
211 89 ka = ia;
212 }
213
214 29 return id;
215 }
216
217 // ------------------------ 0 --------------------------------------
218 1000 bool withRuntimeLoop(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
219 const Vec3s& b, const Scalar& breakDistance2,
220 Scalar& squaredLowerBoundDistance) {
221
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 Matrix3s Bf(B.cwiseAbs());
222
223 // Corner of b axis aligned bounding box the closest to the origin
224
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
225
2/2
✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
 1000 if (squaredLowerBoundDistance > breakDistance2) return true;
226
227 // | B^T T| - b - Bf^T a
228
1/2
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
 350 squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
229
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
 350 if (squaredLowerBoundDistance > breakDistance2) return true;
230
231 310 int ja = 1, ka = 2, jb = 1, kb = 2;
232
2/2
✓ Branch 0 taken 910 times.
✓ Branch 1 taken 290 times.
 1200 for (int ia = 0; ia < 3; ++ia) {
233
2/2
✓ Branch 0 taken 2730 times.
✓ Branch 1 taken 890 times.
 3620 for (int ib = 0; ib < 3; ++ib) {
234
4/8
✓ Branch 1 taken 2730 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2730 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2730 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2730 times.
✗ Branch 11 not taken.
 2730 const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
235
236
4/8
✓ Branch 1 taken 2730 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2730 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2730 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2730 times.
✗ Branch 11 not taken.
 2730 const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
237
4/8
✓ Branch 1 taken 2730 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2730 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2730 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2730 times.
✗ Branch 11 not taken.
 2730 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
238 // We need to divide by the norm || Aia x Bib ||
239 // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
240
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 2710 times.
 2730 if (diff > 0) {
241
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 20 Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
242
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 20 if (sinus2 > 1e-6) {
243 20 squaredLowerBoundDistance = diff * diff / sinus2;
244
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 20 if (squaredLowerBoundDistance > breakDistance2) {
245 20 return true;
246 }
247 }
248 /* // or
249 Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
250 squaredLowerBoundDistance = diff * diff;
251 if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
252 squaredLowerBoundDistance /= sinus2;
253 return true;
254 }
255 // */
256 }
257
258 2710 jb = kb;
259 2710 kb = ib;
260 }
261 890 ja = ka;
262 890 ka = ia;
263 }
264
265 290 return false;
266 }
267
268 // ------------------------ 1 --------------------------------------
269 1000 bool withManualLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
270 const Vec3s& a, const Vec3s& b,
271 const Scalar& breakDistance2,
272 Scalar& squaredLowerBoundDistance) {
273 Scalar t, s;
274 Scalar diff;
275
276 // Matrix3s Bf = abs(B);
277 // Bf += reps;
278
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 Matrix3s Bf(B.cwiseAbs());
279
280 // Corner of b axis aligned bounding box the closest to the origin
281
4/8
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1000 times.
✗ Branch 11 not taken.
 1000 Vec3s AABB_corner(T.cwiseAbs() - Bf * b);
282
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 Vec3s diff3(AABB_corner - a);
283
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 diff3 = diff3.cwiseMax(Vec3s::Zero());
284
285 // for (Vec3s::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]);
286
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 squaredLowerBoundDistance = diff3.squaredNorm();
287
2/2
✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
 1000 if (squaredLowerBoundDistance > breakDistance2) return true;
288
289
7/14
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 350 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 350 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 350 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 350 times.
✗ Branch 20 not taken.
 350 AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a;
290 // diff3 = | B^T T| - b - Bf^T a
291
2/4
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
 350 diff3 = AABB_corner - b;
292
3/6
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
 350 diff3 = diff3.cwiseMax(Vec3s::Zero());
293
1/2
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
 350 squaredLowerBoundDistance = diff3.squaredNorm();
294
295
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
 350 if (squaredLowerBoundDistance > breakDistance2) return true;
296
297 // A0 x B0
298
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 0) - T[1] * B(2, 0);
299
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 130 times.
 310 t = ((s < 0.0) ? -s : s);
300
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 assert(t == fabs(s));
301
302 Scalar sinus2;
303
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
304
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[2] * Bf(0, 1));
305 // We need to divide by the norm || A0 x B0 ||
306 // As ||A0|| = ||B0|| = 1,
307 // 2 2
308 // || A0 x B0 || + (A0 | B0) = 1
309
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 if (diff > 0) {
310 ✗ sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
311 ✗ if (sinus2 > 1e-6) {
312 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
313 ✗ if (squaredLowerBoundDistance > breakDistance2) {
314 ✗ return true;
315 }
316 }
317 }
318
319 // A0 x B1
320
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 1) - T[1] * B(2, 1);
321
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 160 times.
 310 t = ((s < 0.0) ? -s : s);
322
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 assert(t == fabs(s));
323
324
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
325
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[2] * Bf(0, 0));
326
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 if (diff > 0) {
327 ✗ sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
328 ✗ if (sinus2 > 1e-6) {
329 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
330 ✗ if (squaredLowerBoundDistance > breakDistance2) {
331 ✗ return true;
332 }
333 }
334 }
335
336 // A0 x B2
337
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 2) - T[1] * B(2, 2);
338
2/2
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 110 times.
 310 t = ((s < 0.0) ? -s : s);
339
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 assert(t == fabs(s));
340
341
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
342
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[1] * Bf(0, 0));
343
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 300 times.
 310 if (diff > 0) {
344
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
345
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (sinus2 > 1e-6) {
346 10 squaredLowerBoundDistance = diff * diff / sinus2;
347
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (squaredLowerBoundDistance > breakDistance2) {
348 10 return true;
349 }
350 }
351 }
352
353 // A1 x B0
354
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 0) - T[2] * B(0, 0);
355
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 150 times.
 300 t = ((s < 0.0) ? -s : s);
356
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
357
358
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
359
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(1, 1));
360
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
361 ✗ sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
362 ✗ if (sinus2 > 1e-6) {
363 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
364 ✗ if (squaredLowerBoundDistance > breakDistance2) {
365 ✗ return true;
366 }
367 }
368 }
369
370 // A1 x B1
371
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 1) - T[2] * B(0, 1);
372
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 110 times.
 300 t = ((s < 0.0) ? -s : s);
373
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
374
375
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
376
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(1, 0));
377
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
378 ✗ sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
379 ✗ if (sinus2 > 1e-6) {
380 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
381 ✗ if (squaredLowerBoundDistance > breakDistance2) {
382 ✗ return true;
383 }
384 }
385 }
386
387 // A1 x B2
388
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 2) - T[2] * B(0, 2);
389
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
 300 t = ((s < 0.0) ? -s : s);
390
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
391
392
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
393
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[1] * Bf(1, 0));
394
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
395 ✗ sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
396 ✗ if (sinus2 > 1e-6) {
397 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
398 ✗ if (squaredLowerBoundDistance > breakDistance2) {
399 ✗ return true;
400 }
401 }
402 }
403
404 // A2 x B0
405
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 0) - T[0] * B(1, 0);
406
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 120 times.
 300 t = ((s < 0.0) ? -s : s);
407
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
408
409
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
410
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(2, 1));
411
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
412 ✗ sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
413 ✗ if (sinus2 > 1e-6) {
414 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
415 ✗ if (squaredLowerBoundDistance > breakDistance2) {
416 ✗ return true;
417 }
418 }
419 }
420
421 // A2 x B1
422
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 1) - T[0] * B(1, 1);
423
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
 300 t = ((s < 0.0) ? -s : s);
424
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
425
426
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
427
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(2, 0));
428
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
429 ✗ sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
430 ✗ if (sinus2 > 1e-6) {
431 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
432 ✗ if (squaredLowerBoundDistance > breakDistance2) {
433 ✗ return true;
434 }
435 }
436 }
437
438 // A2 x B2
439
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 2) - T[0] * B(1, 2);
440
2/2
✓ Branch 0 taken 170 times.
✓ Branch 1 taken 130 times.
 300 t = ((s < 0.0) ? -s : s);
441
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
442
443
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
444
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[1] * Bf(2, 0));
445
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 290 times.
 300 if (diff > 0) {
446
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
447
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (sinus2 > 1e-6) {
448 10 squaredLowerBoundDistance = diff * diff / sinus2;
449
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (squaredLowerBoundDistance > breakDistance2) {
450 10 return true;
451 }
452 }
453 }
454
455 290 return false;
456 }
457
458 // ------------------------ 2 --------------------------------------
459 1000 bool withManualLoopUnrolling_2(const Matrix3s& B, const Vec3s& T,
460 const Vec3s& a, const Vec3s& b,
461 const Scalar& breakDistance2,
462 Scalar& squaredLowerBoundDistance) {
463
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 Matrix3s Bf(B.cwiseAbs());
464
465 // Corner of b axis aligned bounding box the closest to the origin
466
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
467
2/2
✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
 1000 if (squaredLowerBoundDistance > breakDistance2) return true;
468
469
1/2
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
 350 squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
470
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
 350 if (squaredLowerBoundDistance > breakDistance2) return true;
471
472 // A0 x B0
473 Scalar t, s;
474
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 0) - T[1] * B(2, 0);
475
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 130 times.
 310 t = ((s < 0.0) ? -s : s);
476
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 assert(t == fabs(s));
477
478 Scalar sinus2;
479 Scalar diff;
480
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
481
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[2] * Bf(0, 1));
482 // We need to divide by the norm || A0 x B0 ||
483 // As ||A0|| = ||B0|| = 1,
484 // 2 2
485 // || A0 x B0 || + (A0 | B0) = 1
486
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 if (diff > 0) {
487 ✗ sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
488 ✗ if (sinus2 > 1e-6) {
489 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
490 ✗ if (squaredLowerBoundDistance > breakDistance2) {
491 ✗ return true;
492 }
493 }
494 }
495
496 // A0 x B1
497
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 1) - T[1] * B(2, 1);
498
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 160 times.
 310 t = ((s < 0.0) ? -s : s);
499
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 assert(t == fabs(s));
500
501
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
502
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[2] * Bf(0, 0));
503
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 if (diff > 0) {
504 ✗ sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
505 ✗ if (sinus2 > 1e-6) {
506 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
507 ✗ if (squaredLowerBoundDistance > breakDistance2) {
508 ✗ return true;
509 }
510 }
511 }
512
513 // A0 x B2
514
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 2) - T[1] * B(2, 2);
515
2/2
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 110 times.
 310 t = ((s < 0.0) ? -s : s);
516
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 assert(t == fabs(s));
517
518
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
519
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[1] * Bf(0, 0));
520
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 300 times.
 310 if (diff > 0) {
521
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
522
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (sinus2 > 1e-6) {
523 10 squaredLowerBoundDistance = diff * diff / sinus2;
524
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (squaredLowerBoundDistance > breakDistance2) {
525 10 return true;
526 }
527 }
528 }
529
530 // A1 x B0
531
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 0) - T[2] * B(0, 0);
532
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 150 times.
 300 t = ((s < 0.0) ? -s : s);
533
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
534
535
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
536
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(1, 1));
537
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
538 ✗ sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
539 ✗ if (sinus2 > 1e-6) {
540 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
541 ✗ if (squaredLowerBoundDistance > breakDistance2) {
542 ✗ return true;
543 }
544 }
545 }
546
547 // A1 x B1
548
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 1) - T[2] * B(0, 1);
549
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 110 times.
 300 t = ((s < 0.0) ? -s : s);
550
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
551
552
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
553
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(1, 0));
554
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
555 ✗ sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
556 ✗ if (sinus2 > 1e-6) {
557 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
558 ✗ if (squaredLowerBoundDistance > breakDistance2) {
559 ✗ return true;
560 }
561 }
562 }
563
564 // A1 x B2
565
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 2) - T[2] * B(0, 2);
566
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
 300 t = ((s < 0.0) ? -s : s);
567
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
568
569
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
570
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[1] * Bf(1, 0));
571
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
572 ✗ sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
573 ✗ if (sinus2 > 1e-6) {
574 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
575 ✗ if (squaredLowerBoundDistance > breakDistance2) {
576 ✗ return true;
577 }
578 }
579 }
580
581 // A2 x B0
582
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 0) - T[0] * B(1, 0);
583
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 120 times.
 300 t = ((s < 0.0) ? -s : s);
584
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
585
586
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
587
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(2, 1));
588
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
589 ✗ sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
590 ✗ if (sinus2 > 1e-6) {
591 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
592 ✗ if (squaredLowerBoundDistance > breakDistance2) {
593 ✗ return true;
594 }
595 }
596 }
597
598 // A2 x B1
599
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 1) - T[0] * B(1, 1);
600
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
 300 t = ((s < 0.0) ? -s : s);
601
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
602
603
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
604
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(2, 0));
605
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
606 ✗ sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
607 ✗ if (sinus2 > 1e-6) {
608 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
609 ✗ if (squaredLowerBoundDistance > breakDistance2) {
610 ✗ return true;
611 }
612 }
613 }
614
615 // A2 x B2
616
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 2) - T[0] * B(1, 2);
617
2/2
✓ Branch 0 taken 170 times.
✓ Branch 1 taken 130 times.
 300 t = ((s < 0.0) ? -s : s);
618
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 assert(t == fabs(s));
619
620
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
621
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[1] * Bf(2, 0));
622
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 290 times.
 300 if (diff > 0) {
623
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
624
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (sinus2 > 1e-6) {
625 10 squaredLowerBoundDistance = diff * diff / sinus2;
626
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (squaredLowerBoundDistance > breakDistance2) {
627 10 return true;
628 }
629 }
630 }
631
632 290 return false;
633 }
634
635 // ------------------------ 3 --------------------------------------
636 template <int ia, int ib, int ja = (ia + 1) % 3, int ka = (ia + 2) % 3,
637 int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
638 struct loop_case_1 {
639 5460 static inline bool run(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
640 const Vec3s& b, const Matrix3s& Bf,
641 const Scalar& breakDistance2,
642 Scalar& squaredLowerBoundDistance) {
643 5460 const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
644
645 5460 const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
646 5460 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
647 // We need to divide by the norm || Aia x Bib ||
648 // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
649
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 2710 times.
 5460 if (diff > 0) {
650 40 Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
651
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 40 if (sinus2 > 1e-6) {
652 40 squaredLowerBoundDistance = diff * diff / sinus2;
653
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 40 if (squaredLowerBoundDistance > breakDistance2) {
654 40 return true;
655 }
656 }
657 /* // or
658 Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
659 squaredLowerBoundDistance = diff * diff;
660 if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
661 squaredLowerBoundDistance /= sinus2;
662 return true;
663 }
664 // */
665 }
666 5420 return false;
667 }
668 };
669
670 1000 bool withTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
671 const Vec3s& a, const Vec3s& b,
672 const Scalar& breakDistance2,
673 Scalar& squaredLowerBoundDistance) {
674
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 Matrix3s Bf(B.cwiseAbs());
675
676 // Corner of b axis aligned bounding box the closest to the origin
677
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
678
2/2
✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
 1000 if (squaredLowerBoundDistance > breakDistance2) return true;
679
680
1/2
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
 350 squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
681
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
 350 if (squaredLowerBoundDistance > breakDistance2) return true;
682
683 // Ai x Bj
684
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 310 times.
 310 if (loop_case_1<0, 0>::run(B, T, a, b, Bf, breakDistance2,
685 squaredLowerBoundDistance))
686 ✗ return true;
687
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 310 times.
 310 if (loop_case_1<0, 1>::run(B, T, a, b, Bf, breakDistance2,
688 squaredLowerBoundDistance))
689 ✗ return true;
690
3/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✓ Branch 4 taken 300 times.
 310 if (loop_case_1<0, 2>::run(B, T, a, b, Bf, breakDistance2,
691 squaredLowerBoundDistance))
692 10 return true;
693
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
 300 if (loop_case_1<1, 0>::run(B, T, a, b, Bf, breakDistance2,
694 squaredLowerBoundDistance))
695 ✗ return true;
696
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
 300 if (loop_case_1<1, 1>::run(B, T, a, b, Bf, breakDistance2,
697 squaredLowerBoundDistance))
698 ✗ return true;
699
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
 300 if (loop_case_1<1, 2>::run(B, T, a, b, Bf, breakDistance2,
700 squaredLowerBoundDistance))
701 ✗ return true;
702
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
 300 if (loop_case_1<2, 0>::run(B, T, a, b, Bf, breakDistance2,
703 squaredLowerBoundDistance))
704 ✗ return true;
705
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
 300 if (loop_case_1<2, 1>::run(B, T, a, b, Bf, breakDistance2,
706 squaredLowerBoundDistance))
707 ✗ return true;
708
3/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✓ Branch 4 taken 290 times.
 300 if (loop_case_1<2, 2>::run(B, T, a, b, Bf, breakDistance2,
709 squaredLowerBoundDistance))
710 10 return true;
711
712 290 return false;
713 }
714
715 // ------------------------ 4 --------------------------------------
716
717 template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
718 struct loop_case_2 {
719 5460 static inline bool run(int ia, int ja, int ka, const Matrix3s& B,
720 const Vec3s& T, const Vec3s& a, const Vec3s& b,
721 const Matrix3s& Bf, const Scalar& breakDistance2,
722 Scalar& squaredLowerBoundDistance) {
723 5460 const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
724
725 5460 const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
726 5460 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
727 // We need to divide by the norm || Aia x Bib ||
728 // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
729
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 2710 times.
 5460 if (diff > 0) {
730 40 Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib);
731
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 40 if (sinus2 > 1e-6) {
732 40 squaredLowerBoundDistance = diff * diff / sinus2;
733
1/2
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
 40 if (squaredLowerBoundDistance > breakDistance2) {
734 40 return true;
735 }
736 }
737 /* // or
738 Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
739 squaredLowerBoundDistance = diff * diff;
740 if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
741 squaredLowerBoundDistance /= sinus2;
742 return true;
743 }
744 // */
745 }
746 5420 return false;
747 }
748 };
749
750 1000 bool withPartialTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T,
751 const Vec3s& a, const Vec3s& b,
752 const Scalar& breakDistance2,
753 Scalar& squaredLowerBoundDistance) {
754
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 Matrix3s Bf(B.cwiseAbs());
755
756 // Corner of b axis aligned bounding box the closest to the origin
757
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf);
758
2/2
✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
 1000 if (squaredLowerBoundDistance > breakDistance2) return true;
759
760
1/2
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
 350 squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf);
761
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
 350 if (squaredLowerBoundDistance > breakDistance2) return true;
762
763 // Ai x Bj
764 310 int ja = 1, ka = 2;
765
2/2
✓ Branch 0 taken 910 times.
✓ Branch 1 taken 290 times.
 1200 for (int ia = 0; ia < 3; ++ia) {
766
2/4
✓ Branch 1 taken 910 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 910 times.
 910 if (loop_case_2<0>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2,
767 squaredLowerBoundDistance))
768 ✗ return true;
769
2/4
✓ Branch 1 taken 910 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 910 times.
 910 if (loop_case_2<1>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2,
770 squaredLowerBoundDistance))
771 ✗ return true;
772
3/4
✓ Branch 1 taken 910 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
✓ Branch 4 taken 890 times.
 910 if (loop_case_2<2>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2,
773 squaredLowerBoundDistance))
774 20 return true;
775 890 ja = ka;
776 890 ka = ia;
777 }
778
779 290 return false;
780 }
781
782 // ------------------------ 5 --------------------------------------
783 1000 bool originalWithLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
784 const Vec3s& b, const Scalar& breakDistance2,
785 Scalar& squaredLowerBoundDistance) {
786 Scalar t, s;
787 Scalar diff;
788
789
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 1000 Matrix3s Bf(B.cwiseAbs());
790 1000 squaredLowerBoundDistance = 0;
791
792 // if any of these tests are one-sided, then the polyhedra are disjoint
793
794 // A1 x A2 = A0
795
5/8
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 520 times.
✓ Branch 4 taken 480 times.
✓ Branch 6 taken 520 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 480 times.
✗ Branch 10 not taken.
 1000 t = ((T[0] < 0.0) ? -T[0] : T[0]);
796
797
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 diff = t - (a[0] + Bf.row(0).dot(b));
798
2/2
✓ Branch 0 taken 290 times.
✓ Branch 1 taken 710 times.
 1000 if (diff > 0) {
799 290 squaredLowerBoundDistance = diff * diff;
800 }
801
802 // A2 x A0 = A1
803
5/8
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 480 times.
✓ Branch 4 taken 520 times.
✓ Branch 6 taken 480 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 520 times.
✗ Branch 10 not taken.
 1000 t = ((T[1] < 0.0) ? -T[1] : T[1]);
804
805
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 diff = t - (a[1] + Bf.row(1).dot(b));
806
2/2
✓ Branch 0 taken 350 times.
✓ Branch 1 taken 650 times.
 1000 if (diff > 0) {
807 350 squaredLowerBoundDistance += diff * diff;
808 }
809
810 // A0 x A1 = A2
811
5/8
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 500 times.
✓ Branch 4 taken 500 times.
✓ Branch 6 taken 500 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 500 times.
✗ Branch 10 not taken.
 1000 t = ((T[2] < 0.0) ? -T[2] : T[2]);
812
813
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 1000 diff = t - (a[2] + Bf.row(2).dot(b));
814
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 820 times.
 1000 if (diff > 0) {
815 180 squaredLowerBoundDistance += diff * diff;
816 }
817
818
2/2
✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
 1000 if (squaredLowerBoundDistance > breakDistance2) return true;
819
820 350 squaredLowerBoundDistance = 0;
821
822 // B1 x B2 = B0
823
2/4
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
 350 s = B.col(0).dot(T);
824
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 170 times.
 350 t = ((s < 0.0) ? -s : s);
825
826
3/6
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
 350 diff = t - (b[0] + Bf.col(0).dot(a));
827
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 350 times.
 350 if (diff > 0) {
828 ✗ squaredLowerBoundDistance += diff * diff;
829 }
830
831 // B2 x B0 = B1
832
2/4
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
 350 s = B.col(1).dot(T);
833
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 170 times.
 350 t = ((s < 0.0) ? -s : s);
834
835
3/6
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
 350 diff = t - (b[1] + Bf.col(1).dot(a));
836
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 330 times.
 350 if (diff > 0) {
837 20 squaredLowerBoundDistance += diff * diff;
838 }
839
840 // B0 x B1 = B2
841
2/4
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
 350 s = B.col(2).dot(T);
842
2/2
✓ Branch 0 taken 220 times.
✓ Branch 1 taken 130 times.
 350 t = ((s < 0.0) ? -s : s);
843
844
3/6
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
 350 diff = t - (b[2] + Bf.col(2).dot(a));
845
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 330 times.
 350 if (diff > 0) {
846 20 squaredLowerBoundDistance += diff * diff;
847 }
848
849
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
 350 if (squaredLowerBoundDistance > breakDistance2) return true;
850
851 // A0 x B0
852
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 0) - T[1] * B(2, 0);
853
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 130 times.
 310 t = ((s < 0.0) ? -s : s);
854
855 Scalar sinus2;
856
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) +
857
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[2] * Bf(0, 1));
858 // We need to divide by the norm || A0 x B0 ||
859 // As ||A0|| = ||B0|| = 1,
860 // 2 2
861 // || A0 x B0 || + (A0 | B0) = 1
862
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 if (diff > 0) {
863 ✗ sinus2 = 1 - Bf(0, 0) * Bf(0, 0);
864 ✗ if (sinus2 > 1e-6) {
865 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
866 ✗ if (squaredLowerBoundDistance > breakDistance2) {
867 ✗ return true;
868 }
869 }
870 }
871
872 // A0 x B1
873
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 1) - T[1] * B(2, 1);
874
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 160 times.
 310 t = ((s < 0.0) ? -s : s);
875
876
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) +
877
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[2] * Bf(0, 0));
878
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
 310 if (diff > 0) {
879 ✗ sinus2 = 1 - Bf(0, 1) * Bf(0, 1);
880 ✗ if (sinus2 > 1e-6) {
881 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
882 ✗ if (squaredLowerBoundDistance > breakDistance2) {
883 ✗ return true;
884 }
885 }
886 }
887
888 // A0 x B2
889
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 2) - T[1] * B(2, 2);
890
2/2
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 110 times.
 310 t = ((s < 0.0) ? -s : s);
891
892
6/12
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
 310 diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) +
893
2/4
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
 310 b[1] * Bf(0, 0));
894
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 300 times.
 310 if (diff > 0) {
895
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 sinus2 = 1 - Bf(0, 2) * Bf(0, 2);
896
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (sinus2 > 1e-6) {
897 10 squaredLowerBoundDistance = diff * diff / sinus2;
898
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (squaredLowerBoundDistance > breakDistance2) {
899 10 return true;
900 }
901 }
902 }
903
904 // A1 x B0
905
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 0) - T[2] * B(0, 0);
906
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 150 times.
 300 t = ((s < 0.0) ? -s : s);
907
908
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) +
909
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(1, 1));
910
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
911 ✗ sinus2 = 1 - Bf(1, 0) * Bf(1, 0);
912 ✗ if (sinus2 > 1e-6) {
913 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
914 ✗ if (squaredLowerBoundDistance > breakDistance2) {
915 ✗ return true;
916 }
917 }
918 }
919
920 // A1 x B1
921
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 1) - T[2] * B(0, 1);
922
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 110 times.
 300 t = ((s < 0.0) ? -s : s);
923
924
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) +
925
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(1, 0));
926
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
927 ✗ sinus2 = 1 - Bf(1, 1) * Bf(1, 1);
928 ✗ if (sinus2 > 1e-6) {
929 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
930 ✗ if (squaredLowerBoundDistance > breakDistance2) {
931 ✗ return true;
932 }
933 }
934 }
935
936 // A1 x B2
937
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 2) - T[2] * B(0, 2);
938
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
 300 t = ((s < 0.0) ? -s : s);
939
940
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) +
941
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[1] * Bf(1, 0));
942
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
943 ✗ sinus2 = 1 - Bf(1, 2) * Bf(1, 2);
944 ✗ if (sinus2 > 1e-6) {
945 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
946 ✗ if (squaredLowerBoundDistance > breakDistance2) {
947 ✗ return true;
948 }
949 }
950 }
951
952 // A2 x B0
953
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 0) - T[0] * B(1, 0);
954
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 120 times.
 300 t = ((s < 0.0) ? -s : s);
955
956
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) +
957
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(2, 1));
958
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
959 ✗ sinus2 = 1 - Bf(2, 0) * Bf(2, 0);
960 ✗ if (sinus2 > 1e-6) {
961 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
962 ✗ if (squaredLowerBoundDistance > breakDistance2) {
963 ✗ return true;
964 }
965 }
966 }
967
968 // A2 x B1
969
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 1) - T[0] * B(1, 1);
970
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
 300 t = ((s < 0.0) ? -s : s);
971
972
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) +
973
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[2] * Bf(2, 0));
974
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
 300 if (diff > 0) {
975 ✗ sinus2 = 1 - Bf(2, 1) * Bf(2, 1);
976 ✗ if (sinus2 > 1e-6) {
977 ✗ squaredLowerBoundDistance = diff * diff / sinus2;
978 ✗ if (squaredLowerBoundDistance > breakDistance2) {
979 ✗ return true;
980 }
981 }
982 }
983
984 // A2 x B2
985
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 2) - T[0] * B(1, 2);
986
2/2
✓ Branch 0 taken 170 times.
✓ Branch 1 taken 130 times.
 300 t = ((s < 0.0) ? -s : s);
987
988
6/12
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
 300 diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) +
989
2/4
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
 300 b[1] * Bf(2, 0));
990
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 290 times.
 300 if (diff > 0) {
991
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 10 sinus2 = 1 - Bf(2, 2) * Bf(2, 2);
992
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (sinus2 > 1e-6) {
993 10 squaredLowerBoundDistance = diff * diff / sinus2;
994
1/2
✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
 10 if (squaredLowerBoundDistance > breakDistance2) {
995 10 return true;
996 }
997 }
998 }
999
1000 290 return false;
1001 }
1002
1003 // ------------------------ 6 --------------------------------------
1004 1000 bool originalWithNoLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
1005 const Vec3s& b, const Scalar&,
1006 Scalar& squaredLowerBoundDistance) {
1007 Scalar t, s;
1008 1000 const Scalar reps = Scalar(1e-6);
1009
1010 1000 squaredLowerBoundDistance = 0;
1011
1012
4/8
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1000 times.
✗ Branch 11 not taken.
 1000 Matrix3s Bf(B.array().abs() + reps);
1013 // Bf += reps;
1014
1015 // if any of these tests are one-sided, then the polyhedra are disjoint
1016
1017 // A1 x A2 = A0
1018
5/8
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 520 times.
✓ Branch 4 taken 480 times.
✓ Branch 6 taken 520 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 480 times.
✗ Branch 10 not taken.
 1000 t = ((T[0] < 0.0) ? -T[0] : T[0]);
1019
1020 // if(t > (a[0] + Bf.dotX(b)))
1021
5/8
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 290 times.
✓ Branch 10 taken 710 times.
 1000 if (t > (a[0] + Bf.row(0).dot(b))) return true;
1022
1023 // B1 x B2 = B0
1024 // s = B.transposeDotX(T);
1025
2/4
✓ Branch 1 taken 710 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 710 times.
✗ Branch 5 not taken.
 710 s = B.col(0).dot(T);
1026
2/2
✓ Branch 0 taken 410 times.
✓ Branch 1 taken 300 times.
 710 t = ((s < 0.0) ? -s : s);
1027
1028 // if(t > (b[0] + Bf.transposeDotX(a)))
1029
5/8
✓ Branch 1 taken 710 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 710 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 710 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 140 times.
✓ Branch 10 taken 570 times.
 710 if (t > (b[0] + Bf.col(0).dot(a))) return true;
1030
1031 // A2 x A0 = A1
1032
5/8
✓ Branch 1 taken 570 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 290 times.
✓ Branch 4 taken 280 times.
✓ Branch 6 taken 290 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 280 times.
✗ Branch 10 not taken.
 570 t = ((T[1] < 0.0) ? -T[1] : T[1]);
1033
1034 // if(t > (a[1] + Bf.dotY(b)))
1035
5/8
✓ Branch 1 taken 570 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 570 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 570 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 160 times.
✓ Branch 10 taken 410 times.
 570 if (t > (a[1] + Bf.row(1).dot(b))) return true;
1036
1037 // A0 x A1 = A2
1038
5/8
✓ Branch 1 taken 410 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 190 times.
✓ Branch 4 taken 220 times.
✓ Branch 6 taken 190 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 220 times.
✗ Branch 10 not taken.
 410 t = ((T[2] < 0.0) ? -T[2] : T[2]);
1039
1040 // if(t > (a[2] + Bf.dotZ(b)))
1041
5/8
✓ Branch 1 taken 410 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 410 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 410 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 60 times.
✓ Branch 10 taken 350 times.
 410 if (t > (a[2] + Bf.row(2).dot(b))) return true;
1042
1043 // B2 x B0 = B1
1044 // s = B.transposeDotY(T);
1045
2/4
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
 350 s = B.col(1).dot(T);
1046
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 170 times.
 350 t = ((s < 0.0) ? -s : s);
1047
1048 // if(t > (b[1] + Bf.transposeDotY(a)))
1049
5/8
✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 20 times.
✓ Branch 10 taken 330 times.
 350 if (t > (b[1] + Bf.col(1).dot(a))) return true;
1050
1051 // B0 x B1 = B2
1052 // s = B.transposeDotZ(T);
1053
2/4
✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 330 times.
✗ Branch 5 not taken.
 330 s = B.col(2).dot(T);
1054
2/2
✓ Branch 0 taken 210 times.
✓ Branch 1 taken 120 times.
 330 t = ((s < 0.0) ? -s : s);
1055
1056 // if(t > (b[2] + Bf.transposeDotZ(a)))
1057
5/8
✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 330 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 330 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 20 times.
✓ Branch 10 taken 310 times.
 330 if (t > (b[2] + Bf.col(2).dot(a))) return true;
1058
1059 // A0 x B0
1060
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 0) - T[1] * B(2, 0);
1061
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 130 times.
 310 t = ((s < 0.0) ? -s : s);
1062
1063 310 if (t >
1064
9/18
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 310 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 310 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 310 times.
 310 (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
1065 ✗ return true;
1066
1067 // A0 x B1
1068
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 1) - T[1] * B(2, 1);
1069
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 160 times.
 310 t = ((s < 0.0) ? -s : s);
1070
1071 310 if (t >
1072
9/18
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 310 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 310 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 310 times.
 310 (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
1073 ✗ return true;
1074
1075 // A0 x B2
1076
4/8
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
 310 s = T[2] * B(1, 2) - T[1] * B(2, 2);
1077
2/2
✓ Branch 0 taken 200 times.
✓ Branch 1 taken 110 times.
 310 t = ((s < 0.0) ? -s : s);
1078
1079 310 if (t >
1080
10/18
✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 310 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 310 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 10 times.
✓ Branch 25 taken 300 times.
 310 (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
1081 10 return true;
1082
1083 // A1 x B0
1084
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 0) - T[2] * B(0, 0);
1085
2/2
✓ Branch 0 taken 150 times.
✓ Branch 1 taken 150 times.
 300 t = ((s < 0.0) ? -s : s);
1086
1087 300 if (t >
1088
9/18
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
 300 (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
1089 ✗ return true;
1090
1091 // A1 x B1
1092
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 1) - T[2] * B(0, 1);
1093
2/2
✓ Branch 0 taken 190 times.
✓ Branch 1 taken 110 times.
 300 t = ((s < 0.0) ? -s : s);
1094
1095 300 if (t >
1096
9/18
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
 300 (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
1097 ✗ return true;
1098
1099 // A1 x B2
1100
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[0] * B(2, 2) - T[2] * B(0, 2);
1101
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
 300 t = ((s < 0.0) ? -s : s);
1102
1103 300 if (t >
1104
9/18
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
 300 (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
1105 ✗ return true;
1106
1107 // A2 x B0
1108
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 0) - T[0] * B(1, 0);
1109
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 120 times.
 300 t = ((s < 0.0) ? -s : s);
1110
1111 300 if (t >
1112
9/18
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
 300 (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
1113 ✗ return true;
1114
1115 // A2 x B1
1116
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 1) - T[0] * B(1, 1);
1117
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
 300 t = ((s < 0.0) ? -s : s);
1118
1119 300 if (t >
1120
9/18
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
 300 (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
1121 ✗ return true;
1122
1123 // A2 x B2
1124
4/8
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
 300 s = T[1] * B(0, 2) - T[0] * B(1, 2);
1125
2/2
✓ Branch 0 taken 170 times.
✓ Branch 1 taken 130 times.
 300 t = ((s < 0.0) ? -s : s);
1126
1127 300 if (t >
1128
10/18
✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 10 times.
✓ Branch 25 taken 290 times.
 300 (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
1129 10 return true;
1130
1131 290 return false;
1132 }
1133 } // namespace obbDisjoint_impls
1134
1135 struct BenchmarkResult {
1136 /// The test ID:
1137 /// - 0-10 identifies a separating axes.
1138 /// - 11 means no separatins axes could be found. (distance should be <= 0)
1139 int ifId;
1140 Scalar distance;
1141 Scalar squaredLowerBoundDistance;
1142 duration_type duration[NB_METHODS];
1143 bool failure;
1144
1145 ✗ static std::ostream& headers(std::ostream& os) {
1146 ✗ const std::string unit = " (us)";
1147 os << "separating axis" << sep << "distance lower bound" << sep
1148 << "distance" << sep << "failure" << sep << "Runtime Loop" << unit << sep
1149 << "Manual Loop Unrolling 1" << unit << sep << "Manual Loop Unrolling 2"
1150 << unit << sep << "Template Unrolling" << unit << sep
1151 << "Partial Template Unrolling" << unit << sep << "Original (LowerBound)"
1152 ✗ << unit << sep << "Original (NoLowerBound)" << unit;
1153 ✗ return os;
1154 }
1155
1156 ✗ std::ostream& print(std::ostream& os) const {
1157 ✗ os << ifId << sep << std::sqrt(squaredLowerBoundDistance) << sep << distance
1158 ✗ << sep << failure;
1159 ✗ for (int i = 0; i < NB_METHODS; ++i)
1160 ✗ os << sep
1161 ✗ << static_cast<double>(
1162 ✗ std::chrono::duration_cast<std::chrono::nanoseconds>(
1163 ✗ duration[i])
1164 ✗ .count()) *
1165 ✗ 1e-3;
1166 ✗ return os;
1167 }
1168 };
1169
1170 ✗ std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) {
1171 ✗ return br.print(os);
1172 }
1173
1174 100 BenchmarkResult benchmark_obb_case(const Matrix3s& B, const Vec3s& T,
1175 const Vec3s& a, const Vec3s& b,
1176 const CollisionRequest& request,
1177 std::size_t N) {
1178 100 const Scalar breakDistance(request.break_distance + request.security_margin);
1179 100 const Scalar breakDistance2 = breakDistance * breakDistance;
1180
1181 BenchmarkResult result;
1182 // First determine which axis provide the answer
1183
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 result.ifId = obbDisjoint_impls::separatingAxisId(
1184 B, T, a, b, breakDistance2, result.squaredLowerBoundDistance);
1185
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 bool disjoint = obbDisjoint_impls::distance(B, T, a, b, result.distance);
1186
2/4
✓ Branch 0 taken 100 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 100 assert(0 <= result.ifId && result.ifId <= 11);
1187
1188 // Sanity check
1189 100 result.failure = true;
1190 100 bool overlap = (result.ifId == 11);
1191 100 Scalar dist_thr = request.break_distance + request.security_margin;
1192
3/4
✓ Branch 0 taken 71 times.
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 71 times.
 100 if (!overlap && result.distance <= 0) {
1193 ✗ std::cerr << "Failure: negative distance for disjoint OBBs.";
1194
3/4
✓ Branch 0 taken 71 times.
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 71 times.
 100 } else if (!overlap && result.squaredLowerBoundDistance < 0) {
1195 ✗ std::cerr << "Failure: negative distance lower bound.";
1196
3/4
✓ Branch 0 taken 71 times.
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 100 times.
 171 } else if (!overlap && eps < std::sqrt(result.squaredLowerBoundDistance) -
1197
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 71 times.
 71 result.distance) {
1198 ✗ std::cerr << "Failure: distance is inferior to its lower bound (diff = "
1199 ✗ << std::sqrt(result.squaredLowerBoundDistance) - result.distance
1200 ✗ << ").";
1201
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 100 } else if (overlap != !disjoint && result.distance >= dist_thr - eps) {
1202 ✗ std::cerr << "Failure: overlapping test and distance query mismatch.";
1203
3/4
✓ Branch 0 taken 29 times.
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29 times.
 100 } else if (overlap && result.distance >= dist_thr - eps) {
1204 ✗ std::cerr << "Failure: positive distance for overlapping OBBs.";
1205 } else {
1206 100 result.failure = false;
1207 }
1208
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
 100 if (result.failure) {
1209 ✗ std::cerr << "\nR = " << Quats(B).coeffs().transpose().format(py_fmt)
1210 ✗ << "\nT = " << T.transpose().format(py_fmt)
1211 ✗ << "\na = " << a.transpose().format(py_fmt)
1212 ✗ << "\nb = " << b.transpose().format(py_fmt)
1213 ✗ << "\nresult = " << result << '\n'
1214 ✗ << std::endl;
1215 }
1216
1217 // Compute time
1218 Scalar tmp;
1219 100 clock_type::time_point start, end;
1220
1221 // ------------------------ 0 --------------------------------------
1222 100 start = clock_type::now();
1223
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
 1100 for (std::size_t i = 0; i < N; ++i)
1224
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 obbDisjoint_impls::withRuntimeLoop(B, T, a, b, breakDistance2, tmp);
1225 100 end = clock_type::now();
1226
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 100 result.duration[0] = (end - start) / N;
1227
1228 // ------------------------ 1 --------------------------------------
1229 100 start = clock_type::now();
1230
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
 1100 for (std::size_t i = 0; i < N; ++i)
1231
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 obbDisjoint_impls::withManualLoopUnrolling_1(B, T, a, b, breakDistance2,
1232 tmp);
1233 100 end = clock_type::now();
1234
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 100 result.duration[1] = (end - start) / N;
1235
1236 // ------------------------ 2 --------------------------------------
1237 100 start = clock_type::now();
1238
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
 1100 for (std::size_t i = 0; i < N; ++i)
1239
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 obbDisjoint_impls::withManualLoopUnrolling_2(B, T, a, b, breakDistance2,
1240 tmp);
1241 100 end = clock_type::now();
1242
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 100 result.duration[2] = (end - start) / N;
1243
1244 // ------------------------ 3 --------------------------------------
1245 100 start = clock_type::now();
1246
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
 1100 for (std::size_t i = 0; i < N; ++i)
1247
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 obbDisjoint_impls::withTemplateLoopUnrolling_1(B, T, a, b, breakDistance2,
1248 tmp);
1249 100 end = clock_type::now();
1250
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 100 result.duration[3] = (end - start) / N;
1251
1252 // ------------------------ 4 --------------------------------------
1253 100 start = clock_type::now();
1254
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
 1100 for (std::size_t i = 0; i < N; ++i)
1255
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 obbDisjoint_impls::withPartialTemplateLoopUnrolling_1(B, T, a, b,
1256 breakDistance2, tmp);
1257 100 end = clock_type::now();
1258
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 100 result.duration[4] = (end - start) / N;
1259
1260 // ------------------------ 5 --------------------------------------
1261 100 start = clock_type::now();
1262
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
 1100 for (std::size_t i = 0; i < N; ++i)
1263
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 obbDisjoint_impls::originalWithLowerBound(B, T, a, b, breakDistance2, tmp);
1264 100 end = clock_type::now();
1265
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 100 result.duration[5] = (end - start) / N;
1266
1267 // ------------------------ 6 --------------------------------------
1268 100 start = clock_type::now();
1269 // The 2 last arguments are unused.
1270
2/2
✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
 1100 for (std::size_t i = 0; i < N; ++i)
1271
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 1000 obbDisjoint_impls::originalWithNoLowerBound(B, T, a, b, breakDistance2,
1272 tmp);
1273 100 end = clock_type::now();
1274
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 100 result.duration[6] = (end - start) / N;
1275
1276 200 return result;
1277 }
1278
1279 1 std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) {
1280 1 std::size_t nbFailure = 0;
1281
1282 // Create two OBBs axis
1283
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Vec3s a, b;
1284
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Matrix3s B;
1285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Vec3s T;
1286
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CollisionRequest request;
1287
1288 #ifndef NDEBUG // if debug mode
1289 static const size_t nbRandomOBB = 10;
1290 static const size_t nbTransformPerOBB = 10;
1291 static const size_t nbRunForTimeMeas = 10;
1292 #else
1293 static const size_t nbRandomOBB = 100;
1294 static const size_t nbTransformPerOBB = 100;
1295 static const size_t nbRunForTimeMeas = 1000;
1296 #endif
1297 static const Scalar extentNorm = 1.;
1298
1299
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 if (output != NULL) *output << BenchmarkResult::headers << '\n';
1300
1301 BenchmarkResult result;
1302
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
 11 for (std::size_t iobb = 0; iobb < nbRandomOBB; ++iobb) {
1303
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 randomOBBs(a, b, extentNorm);
1304
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 10 times.
 110 for (std::size_t itf = 0; itf < nbTransformPerOBB; ++itf) {
1305
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 randomTransform(B, T, a, b, extentNorm);
1306
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 100 result = benchmark_obb_case(B, T, a, b, request, nbRunForTimeMeas);
1307
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 100 if (output != NULL) *output << result << '\n';
1308
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
 100 if (result.failure) nbFailure++;
1309 }
1310 }
1311 1 return nbFailure;
1312 }
1313
1314 1 int main(int argc, char** argv) {
1315 1 std::ostream* output = NULL;
1316
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 1 if (argc > 1 && strcmp(argv[1], "--generate-output") == 0) {
1317 ✗ output = &std::cout;
1318 }
1319
1320 std::cout << "The benchmark real time measurements may be noisy and "
1321 "will incur extra overhead."
1322 "\nUse the following commands to turn ON:"
1323 "\n\tsudo cpufreq-set --governor performance"
1324 "\nor OFF:"
1325 "\n\tsudo cpufreq-set --governor powersave"
1326 1 "\n";
1327
1328 1 std::size_t nbFailure = obb_overlap_and_lower_bound_distance(output);
1329
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (nbFailure > INT_MAX) return INT_MAX;
1330 1 return (int)nbFailure;
1331 }
1332