GCC Code Coverage Report

Directory: ./
File: src/narrowphase/gjk.cpp
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 684 795 86.0%
Functions: 26 28 92.9%
Branches: 673 1509 44.6%
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 * Copyright (c) 2021-2024, INRIA
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 Open Source Robotics Foundation 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 /** \author Jia Pan */
38
39 #include "coal/shape/geometric_shapes.h"
40 #include "coal/narrowphase/gjk.h"
41 #include "coal/internal/intersect.h"
42 #include "coal/internal/tools.h"
43 #include "coal/shape/geometric_shapes_traits.h"
44 #include "coal/narrowphase/narrowphase_defaults.h"
45
46 #include "coal/tracy.hh"
47
48 namespace coal {
49
50 namespace details {
51
52 30476115 void GJK::initialize() {
53 30476115 distance_upper_bound = (std::numeric_limits<SolverScalar>::max)();
54 30476115 gjk_variant = GJKVariant::DefaultGJK;
55 30476115 convergence_criterion = GJKConvergenceCriterion::Default;
56 30476115 convergence_criterion_type = GJKConvergenceCriterionType::Relative;
57 30476115 reset(max_iterations, tolerance);
58 30476115 }
59
60 60476183 void GJK::reset(size_t max_iterations_, SolverScalar tolerance_) {
61 60476183 max_iterations = max_iterations_;
62 60476183 tolerance = tolerance_;
63
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 60476183 times.
 60476183 COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.",
64 std::invalid_argument);
65 60476183 status = DidNotRun;
66 60476183 nfree = 0;
67 60476183 simplex = nullptr;
68 60476183 iterations = 0;
69 60476183 iterations_momentum_stop = 0;
70 60476183 }
71
72 29205606 Vec3ps GJK::getGuessFromSimplex() const { return ray; }
73
74 namespace details {
75
76 // This function computes the weights associated with projecting the origin
77 // onto the final simplex of GJK or EPA.
78 // The origin is always a convex combination of the supports of the simplex.
79 // The weights are then linearly distributed among the shapes' supports, thanks
80 // to the following property:
81 // if s is a support of the Minkowski difference, then:
82 // w = w.w0 - w.w1, with w.w0 a support of shape0 and w.w1 a support of
83 // shape1.
84 // clang-format off
85 // Suppose the final simplex is of rank 2:
86 // We have 0 = alpha * w[0] + (1 - alpha) * w[1], with alpha the weight of the convex
87 // decomposition, then:
88 // 0 = alpha * (w[0].w0 - w[0].w1) + (1 - alpha) * (w[1].w0 - w[1].w1)
89 // => 0 = alpha * w[0].w0 + (1 - alpha) * w[1].w0
90 // - alpha * w[0].w1 - (1 - alpha) * w[1].w1
91 // Therefore we have two witness points:
92 // w0 = alpha * w[0].w0 + (1 - alpha) * w[1].w0
93 // w1 = alpha * w[0].w1 + (1 - alpha) * w[1].w1
94 // clang-format on
95 // TODO
96 29999867 void getClosestPoints(const GJK::Simplex& simplex, Vec3ps& w0, Vec3ps& w1) {
97 29999867 GJK::SimplexV* const* vs = simplex.vertex;
98
99
2/2
✓ Branch 0 taken 38306369 times.
✓ Branch 1 taken 29999867 times.
 68306236 for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) {
100
3/6
✓ Branch 2 taken 38306369 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38306369 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 38306369 times.
 38306369 assert(vs[i]->w.isApprox(vs[i]->w0 - vs[i]->w1));
101 }
102
103 29999867 Project<SolverScalar>::ProjectResult projection;
104
4/5
✓ Branch 0 taken 22111879 times.
✓ Branch 1 taken 7490583 times.
✓ Branch 2 taken 376296 times.
✓ Branch 3 taken 21109 times.
✗ Branch 4 not taken.
 29999867 switch (simplex.rank) {
105 22111879 case 1:
106
1/2
✓ Branch 1 taken 22111879 times.
✗ Branch 2 not taken.
 22111879 w0 = vs[0]->w0;
107
1/2
✓ Branch 1 taken 22111879 times.
✗ Branch 2 not taken.
 22111879 w1 = vs[0]->w1;
108 22111879 return;
109 7490583 case 2: {
110
3/6
✓ Branch 1 taken 7490583 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490583 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7490583 times.
✗ Branch 8 not taken.
 7490583 const Vec3ps &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w,
111
2/4
✓ Branch 1 taken 7490583 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490583 times.
✗ Branch 5 not taken.
 7490583 b0 = vs[1]->w0, b1 = vs[1]->w1;
112 SolverScalar la, lb;
113
2/4
✓ Branch 1 taken 7490583 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490583 times.
✗ Branch 5 not taken.
 7490583 Vec3ps N(b - a);
114
2/4
✓ Branch 1 taken 7490583 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490583 times.
✗ Branch 5 not taken.
 7490583 la = N.dot(-a);
115
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7490583 times.
 7490583 if (la <= 0) {
116 ✗ assert(false);
117 w0 = a0;
118 w1 = a1;
119 } else {
120
1/2
✓ Branch 1 taken 7490583 times.
✗ Branch 2 not taken.
 7490583 lb = N.squaredNorm();
121
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7490583 times.
 7490583 if (la > lb) {
122 ✗ assert(false);
123 w0 = b0;
124 w1 = b1;
125 } else {
126 7490583 lb = la / lb;
127 7490583 la = 1 - lb;
128
4/8
✓ Branch 1 taken 7490583 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490583 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7490583 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7490583 times.
✗ Branch 11 not taken.
 7490583 w0 = la * a0 + lb * b0;
129
4/8
✓ Branch 1 taken 7490583 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490583 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7490583 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7490583 times.
✗ Branch 11 not taken.
 7490583 w1 = la * a1 + lb * b1;
130 }
131 }
132 }
133 7490583 return;
134 376296 case 3:
135 // TODO avoid the reprojection
136 ✗ projection = Project<SolverScalar>::projectTriangleOrigin(
137
1/2
✓ Branch 1 taken 376296 times.
✗ Branch 2 not taken.
 376296 vs[0]->w, vs[1]->w, vs[2]->w);
138 376296 break;
139 21109 case 4: // We are in collision.
140 ✗ projection = Project<SolverScalar>::projectTetrahedraOrigin(
141
1/2
✓ Branch 1 taken 21109 times.
✗ Branch 2 not taken.
 21109 vs[0]->w, vs[1]->w, vs[2]->w, vs[3]->w);
142 21109 break;
143 ✗ default:
144 ✗ COAL_THROW_PRETTY("The simplex rank must be in [ 1, 4 ]",
145 std::logic_error);
146 }
147
1/2
✓ Branch 1 taken 397405 times.
✗ Branch 2 not taken.
 397405 w0.setZero();
148
1/2
✓ Branch 1 taken 397405 times.
✗ Branch 2 not taken.
 397405 w1.setZero();
149
2/2
✓ Branch 0 taken 1213324 times.
✓ Branch 1 taken 397405 times.
 1610729 for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) {
150
2/4
✓ Branch 1 taken 1213324 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1213324 times.
✗ Branch 5 not taken.
 1213324 w0 += projection.parameterization[i] * vs[i]->w0;
151
2/4
✓ Branch 1 taken 1213324 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1213324 times.
✗ Branch 5 not taken.
 1213324 w1 += projection.parameterization[i] * vs[i]->w1;
152 }
153 397405 return;
154 }
155
156 /// Inflate the points along a normal.
157 /// The normal is typically the normal of the separating plane found by GJK
158 /// or the normal found by EPA.
159 /// The normal should follow coal convention: it points from shape0 to
160 /// shape1.
161 template <bool Separated>
162 59999734 void inflate(const MinkowskiDiff& shape, const Vec3ps& normal, Vec3ps& w0,
163 Vec3ps& w1) {
164 #ifndef NDEBUG
165 59999734 const SolverScalar dummy_precision =
166 Eigen::NumTraits<SolverScalar>::dummy_precision();
167
2/4
✓ Branch 1 taken 29999867 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 29999867 times.
 59999734 assert((normal.norm() - 1) < dummy_precision);
168 #endif
169
170 const Eigen::Array<SolverScalar, 1, 2>& I(
171
1/2
✓ Branch 1 taken 29999867 times.
✗ Branch 2 not taken.
 59999734 shape.swept_sphere_radius.cast<SolverScalar>());
172
2/4
✓ Branch 1 taken 29999867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29999867 times.
✗ Branch 5 not taken.
 59999734 Eigen::Array<bool, 1, 2> inflate(I > 0);
173
3/4
✓ Branch 1 taken 29999867 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29935168 times.
✓ Branch 4 taken 64699 times.
 59999734 if (!inflate.any()) return;
174
175
6/10
✓ Branch 1 taken 64699 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2102 times.
✓ Branch 4 taken 62597 times.
✓ Branch 6 taken 2102 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2102 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2102 times.
✗ Branch 13 not taken.
 129398 if (inflate[0]) w0 += I[0] * normal;
176
6/10
✓ Branch 1 taken 64699 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 64110 times.
✓ Branch 4 taken 589 times.
✓ Branch 6 taken 64110 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 64110 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 64110 times.
✗ Branch 13 not taken.
 129398 if (inflate[1]) w1 -= I[1] * normal;
177 }
178
179 } // namespace details
180
181 29995422 void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0,
182 Vec3ps& w1, Vec3ps& normal) const {
183 29995422 details::getClosestPoints(*simplex, w0, w1);
184
4/6
✓ Branch 1 taken 29995422 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29995422 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29977737 times.
✓ Branch 8 taken 17685 times.
 29995422 if ((w1 - w0).norm() > Eigen::NumTraits<SolverScalar>::dummy_precision()) {
185
3/6
✓ Branch 1 taken 29977737 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29977737 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29977737 times.
✗ Branch 8 not taken.
 29977737 normal = (w1 - w0).normalized();
186 } else {
187
3/6
✓ Branch 1 taken 17685 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17685 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17685 times.
✗ Branch 8 not taken.
 17685 normal = -this->ray.normalized();
188 }
189 29995422 details::inflate<true>(shape, normal, w0, w1);
190 29995422 }
191
192 30198156 GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3ps& guess,
193 const support_func_guess_t& supportHint) {
194 COAL_TRACY_ZONE_SCOPED_N("coal::details::GJK::evaluate");
195 30198156 SolverScalar alpha = 0;
196 30198156 iterations = 0;
197
1/2
✓ Branch 1 taken 30198156 times.
✗ Branch 2 not taken.
 30198156 const SolverScalar swept_sphere_radius = shape_.swept_sphere_radius.sum();
198 30198156 const SolverScalar upper_bound = distance_upper_bound + swept_sphere_radius;
199
200 30198156 free_v[0] = &store_v[0];
201 30198156 free_v[1] = &store_v[1];
202 30198156 free_v[2] = &store_v[2];
203 30198156 free_v[3] = &store_v[3];
204
205 30198156 nfree = 4;
206 30198156 status = NoCollision;
207 30198156 shape = &shape_;
208 30198156 distance = 0.0;
209 30198156 current = 0;
210 30198156 simplices[current].rank = 0;
211
1/2
✓ Branch 1 taken 30198156 times.
✗ Branch 2 not taken.
 30198156 support_hint = supportHint;
212
213
1/2
✓ Branch 1 taken 30198156 times.
✗ Branch 2 not taken.
 30198156 SolverScalar rl = guess.norm();
214
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 30198150 times.
 30198156 if (rl < tolerance) {
215
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 ray = Vec3ps(-1, 0, 0);
216 6 rl = 1;
217 } else
218
1/2
✓ Branch 1 taken 30198150 times.
✗ Branch 2 not taken.
 30198150 ray = guess;
219
220 // Momentum
221 30198156 GJKVariant current_gjk_variant = gjk_variant;
222
1/2
✓ Branch 1 taken 30198156 times.
✗ Branch 2 not taken.
 30198156 Vec3ps w = ray;
223
1/2
✓ Branch 1 taken 30198156 times.
✗ Branch 2 not taken.
 30198156 Vec3ps dir = ray;
224
1/2
✓ Branch 1 taken 30198156 times.
✗ Branch 2 not taken.
 30198156 Vec3ps y;
225 SolverScalar momentum;
226 30198156 bool normalize_support_direction = shape->normalize_support_direction;
227 do {
228 72327390 vertex_id_t next = (vertex_id_t)(1 - current);
229 72327390 Simplex& curr_simplex = simplices[current];
230 72327390 Simplex& next_simplex = simplices[next];
231
232 // check A: when origin is near the existing simplex, stop
233
2/2
✓ Branch 0 taken 188 times.
✓ Branch 1 taken 72327202 times.
 72327390 if (rl < tolerance) // mean origin is near the face of original simplex,
234 // return touch
235 {
236 // At this point, GJK has converged but we don't know if GJK is enough to
237 // recover penetration information.
238 // EPA needs to be run.
239 // Unless the Minkowski difference is degenerated, EPA will run fine even
240 // if the final simplex of GJK is not a tetrahedron.
241
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 188 times.
 188 assert(rl > 0);
242 188 status = Collision;
243 // GJK is not enough to recover the penetration depth, hence we ignore the
244 // swept-sphere radius for now.
245 // EPA needs to be run to recover the penetration depth.
246 188 distance = rl;
247 30198156 break;
248 }
249
250 // Compute direction for support call
251
3/4
✓ Branch 0 taken 71571908 times.
✓ Branch 1 taken 426502 times.
✓ Branch 2 taken 328792 times.
✗ Branch 3 not taken.
 72327202 switch (current_gjk_variant) {
252 71571908 case DefaultGJK:
253
1/2
✓ Branch 1 taken 71571908 times.
✗ Branch 2 not taken.
 71571908 dir = ray;
254 71571908 break;
255
256 426502 case NesterovAcceleration:
257 // Normalize heuristic for collision pairs involving convex but not
258 // strictly-convex shapes This corresponds to most use cases.
259
2/2
✓ Branch 0 taken 19470 times.
✓ Branch 1 taken 407032 times.
 426502 if (normalize_support_direction) {
260 19470 momentum =
261 19470 (SolverScalar(iterations) + 2) / (SolverScalar(iterations) + 3);
262
4/8
✓ Branch 1 taken 19470 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19470 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19470 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19470 times.
✗ Branch 11 not taken.
 19470 y = momentum * ray + (1 - momentum) * w;
263
1/2
✓ Branch 1 taken 19470 times.
✗ Branch 2 not taken.
 19470 SolverScalar y_norm = y.norm();
264 // ray is the point of the Minkowski difference which currently the
265 // closest to the origin. Therefore, y.norm() > ray.norm() Hence, if
266 // check A above has not stopped the algorithm, we necessarily have
267 // y.norm() > tolerance. The following assert is just a safety check.
268
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 19470 times.
 19470 assert(y_norm > tolerance);
269
7/14
✓ Branch 1 taken 19470 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19470 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19470 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19470 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 19470 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 19470 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 19470 times.
✗ Branch 20 not taken.
 19470 dir = momentum * dir / dir.norm() + (1 - momentum) * y / y_norm;
270 } else {
271 407032 momentum =
272 407032 (SolverScalar(iterations) + 1) / (SolverScalar(iterations) + 3);
273
4/8
✓ Branch 1 taken 407032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 407032 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 407032 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 407032 times.
✗ Branch 11 not taken.
 407032 y = momentum * ray + (1 - momentum) * w;
274
4/8
✓ Branch 1 taken 407032 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 407032 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 407032 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 407032 times.
✗ Branch 11 not taken.
 407032 dir = momentum * dir + (1 - momentum) * y;
275 }
276 426502 break;
277
278 328792 case PolyakAcceleration:
279 328792 momentum = 1 / (SolverScalar(iterations) + 1);
280
4/8
✓ Branch 1 taken 328792 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 328792 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 328792 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 328792 times.
✗ Branch 11 not taken.
 328792 dir = momentum * dir + (1 - momentum) * ray;
281 328792 break;
282
283 ✗ default:
284 ✗ COAL_THROW_PRETTY("Invalid momentum variant.", std::logic_error);
285 }
286
287 // see below, ray points away from origin
288
3/6
✓ Branch 1 taken 72327202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72327202 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72327202 times.
✗ Branch 8 not taken.
 72327202 appendVertex(curr_simplex, -dir, support_hint);
289
290 // check removed (by ?): when the new support point is close to previous
291 // support points, stop (as the new simplex is degenerated)
292
1/2
✓ Branch 1 taken 72327202 times.
✗ Branch 2 not taken.
 72327202 w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
293
294 // check B: no collision if omega > 0
295
2/4
✓ Branch 1 taken 72327202 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72327202 times.
✗ Branch 5 not taken.
 72327202 SolverScalar omega = dir.dot(w) / dir.norm();
296
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 72327200 times.
 72327202 if (omega > upper_bound) {
297 2 distance = omega - swept_sphere_radius;
298 2 status = NoCollisionEarlyStopped;
299 2 break;
300 }
301
302 // Check to remove acceleration
303
2/2
✓ Branch 0 taken 755294 times.
✓ Branch 1 taken 71571906 times.
 72327200 if (current_gjk_variant != DefaultGJK) {
304
2/4
✓ Branch 1 taken 755294 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 755294 times.
✗ Branch 5 not taken.
 755294 SolverScalar frank_wolfe_duality_gap = 2 * ray.dot(ray - w);
305
2/2
✓ Branch 0 taken 108694 times.
✓ Branch 1 taken 646600 times.
 755294 if (frank_wolfe_duality_gap - tolerance <= 0) {
306 108694 removeVertex(simplices[current]);
307 108694 current_gjk_variant = DefaultGJK; // move back to classic GJK
308 108694 iterations_momentum_stop = iterations;
309 130298 continue; // continue to next iteration
310 }
311 }
312
313 // check C: when the new support point is close to the sub-simplex where the
314 // ray point lies, stop (as the new simplex again is degenerated)
315
1/2
✓ Branch 1 taken 72218506 times.
✗ Branch 2 not taken.
 72218506 bool cv_check_passed = checkConvergence(w, rl, alpha, omega);
316
4/4
✓ Branch 0 taken 42020351 times.
✓ Branch 1 taken 30198155 times.
✓ Branch 2 taken 30181399 times.
✓ Branch 3 taken 11838952 times.
 72218506 if (iterations > 0 && cv_check_passed) {
317
1/2
✓ Branch 0 taken 30181399 times.
✗ Branch 1 not taken.
 30181399 if (iterations > 0) removeVertex(simplices[current]);
318
2/2
✓ Branch 0 taken 21604 times.
✓ Branch 1 taken 30159795 times.
 30181399 if (current_gjk_variant != DefaultGJK) {
319 21604 current_gjk_variant = DefaultGJK; // move back to classic GJK
320 21604 iterations_momentum_stop = iterations;
321 21604 continue;
322 }
323 // At this point, GJK has converged and we know that rl > tolerance (see
324 // check above). Therefore, penetration information can always be
325 // recovered without running EPA.
326 30159795 distance = rl - swept_sphere_radius;
327
2/2
✓ Branch 0 taken 16565 times.
✓ Branch 1 taken 30143230 times.
 30159795 if (distance < tolerance) {
328 16565 status = CollisionWithPenetrationInformation;
329 } else {
330 30143230 status = NoCollision;
331 }
332 30159795 break;
333 }
334
335 // This has been rewritten thanks to the excellent video:
336 // https://youtu.be/Qupqu1xe7Io
337 bool inside;
338
4/5
✓ Branch 0 taken 30198155 times.
✓ Branch 1 taken 10002060 times.
✓ Branch 2 taken 1547928 times.
✓ Branch 3 taken 288964 times.
✗ Branch 4 not taken.
 42037107 switch (curr_simplex.rank) {
339 30198155 case 1: // Only at the first iteration
340
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 30198155 times.
 30198155 assert(iterations == 0);
341
1/2
✓ Branch 1 taken 30198155 times.
✗ Branch 2 not taken.
 30198155 ray = w;
342 30198155 inside = false;
343 30198155 next_simplex.rank = 1;
344 30198155 next_simplex.vertex[0] = curr_simplex.vertex[0];
345 30198155 break;
346 10002060 case 2:
347
1/2
✓ Branch 1 taken 10002060 times.
✗ Branch 2 not taken.
 10002060 inside = projectLineOrigin(curr_simplex, next_simplex);
348 10002060 break;
349 1547928 case 3:
350
1/2
✓ Branch 1 taken 1547928 times.
✗ Branch 2 not taken.
 1547928 inside = projectTriangleOrigin(curr_simplex, next_simplex);
351 1547928 break;
352 288964 case 4:
353
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 inside = projectTetrahedraOrigin(curr_simplex, next_simplex);
354 288964 break;
355 ✗ default:
356 ✗ COAL_THROW_PRETTY("Invalid simplex rank", std::logic_error);
357 }
358
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 42037107 times.
 42037107 assert(nfree + next_simplex.rank == 4);
359 42037107 current = next;
360
1/2
✓ Branch 1 taken 42037107 times.
✗ Branch 2 not taken.
 42037107 rl = ray.norm();
361
4/4
✓ Branch 0 taken 41999034 times.
✓ Branch 1 taken 38073 times.
✓ Branch 2 taken 98 times.
✓ Branch 3 taken 41998936 times.
 42037107 if (inside || rl == 0) {
362 38171 status = Collision;
363 // GJK is not enough to recover the penetration depth, hence we ignore the
364 // swept-sphere radius for now.
365 // EPA needs to be run to recover the penetration depth.
366 38171 distance = rl;
367 38171 break;
368 }
369
370
1/2
✓ Branch 0 taken 41998936 times.
✗ Branch 1 not taken.
 41998936 status = ((++iterations) < max_iterations) ? status : Failed;
371
372
1/2
✓ Branch 0 taken 42129234 times.
✗ Branch 1 not taken.
 42129234 } while (status == NoCollision);
373
374 30198156 simplex = &simplices[current];
375
2/4
✓ Branch 0 taken 30198156 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 30198156 times.
✗ Branch 3 not taken.
 30198156 assert(simplex->rank > 0 && simplex->rank < 5);
376 30198156 return status;
377 }
378
379 72218506 bool GJK::checkConvergence(const Vec3ps& w, const SolverScalar& rl,
380 SolverScalar& alpha,
381 const SolverScalar& omega) const {
382 // x^* is the optimal solution (projection of origin onto the Minkowski
383 // difference).
384 // x^k is the current iterate (x^k = `ray` in the code).
385 // Each criterion provides a different guarantee on the distance to the
386 // optimal solution.
387
3/4
✓ Branch 0 taken 72184502 times.
✓ Branch 1 taken 17002 times.
✓ Branch 2 taken 17002 times.
✗ Branch 3 not taken.
 72218506 switch (convergence_criterion) {
388 72184502 case Default: {
389 // alpha is the distance to the best separating hyperplane found so far
390 72184502 alpha = std::max(alpha, omega);
391 // ||x^*|| - ||x^k|| <= diff
392 72184502 const SolverScalar diff = rl - alpha;
393 72184502 return ((diff - (tolerance + tolerance * rl)) <= 0);
394 } break;
395
396 17002 case DualityGap: {
397 // ||x^* - x^k||^2 <= diff
398
2/4
✓ Branch 1 taken 17002 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17002 times.
✗ Branch 5 not taken.
 17002 const SolverScalar diff = 2 * ray.dot(ray - w);
399
2/3
✓ Branch 0 taken 8490 times.
✓ Branch 1 taken 8512 times.
✗ Branch 2 not taken.
 17002 switch (convergence_criterion_type) {
400 8490 case Absolute:
401 8490 return ((diff - tolerance) <= 0);
402 break;
403 8512 case Relative:
404 8512 return (((diff / tolerance * rl) - tolerance * rl) <= 0);
405 break;
406 ✗ default:
407 ✗ COAL_THROW_PRETTY("Invalid convergence criterion type.",
408 std::logic_error);
409 }
410 } break;
411
412 17002 case Hybrid: {
413 // alpha is the distance to the best separating hyperplane found so far
414 17002 alpha = std::max(alpha, omega);
415 // ||x^* - x^k||^2 <= diff
416 17002 const SolverScalar diff = rl * rl - alpha * alpha;
417
2/3
✓ Branch 0 taken 8490 times.
✓ Branch 1 taken 8512 times.
✗ Branch 2 not taken.
 17002 switch (convergence_criterion_type) {
418 8490 case Absolute:
419 8490 return ((diff - tolerance) <= 0);
420 break;
421 8512 case Relative:
422 8512 return (((diff / tolerance * rl) - tolerance * rl) <= 0);
423 break;
424 ✗ default:
425 ✗ COAL_THROW_PRETTY("Invalid convergence criterion type.",
426 std::logic_error);
427 }
428 } break;
429
430 ✗ default:
431 ✗ COAL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error);
432 }
433 }
434
435 30290103 inline void GJK::removeVertex(Simplex& simplex) {
436 30290103 free_v[nfree++] = simplex.vertex[--simplex.rank];
437 30290103 }
438
439 72327410 inline void GJK::appendVertex(Simplex& simplex, const Vec3ps& v,
440 support_func_guess_t& hint) {
441 72327410 simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
442 72327410 getSupport(v, *simplex.vertex[simplex.rank++], hint);
443 72327410 }
444
445 4653 bool GJK::encloseOrigin() {
446
2/4
✓ Branch 1 taken 4653 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4653 times.
✗ Branch 5 not taken.
 4653 Vec3ps axis(Vec3ps::Zero());
447
2/4
✓ Branch 1 taken 4653 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4653 times.
✗ Branch 5 not taken.
 4653 support_func_guess_t hint = support_func_guess_t::Zero();
448
4/5
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 58 times.
✓ Branch 2 taken 144 times.
✓ Branch 3 taken 4447 times.
✗ Branch 4 not taken.
 4653 switch (simplex->rank) {
449 4 case 1:
450
1/2
✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
 7 for (int i = 0; i < 3; ++i) {
451
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 axis[i] = 1;
452
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 appendVertex(*simplex, axis, hint);
453
3/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 3 times.
 7 if (encloseOrigin()) return true;
454 3 removeVertex(*simplex);
455
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 axis[i] = -1;
456
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 appendVertex(*simplex, -axis, hint);
457
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
 3 if (encloseOrigin()) return true;
458 3 removeVertex(*simplex);
459
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 axis[i] = 0;
460 }
461 ✗ break;
462 58 case 2: {
463
2/4
✓ Branch 1 taken 58 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
 58 Vec3ps d = simplex->vertex[1]->w - simplex->vertex[0]->w;
464
2/2
✓ Branch 0 taken 113 times.
✓ Branch 1 taken 6 times.
 119 for (int i = 0; i < 3; ++i) {
465
1/2
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
 113 axis[i] = 1;
466
1/2
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
 113 Vec3ps p = d.cross(axis);
467
3/4
✓ Branch 2 taken 113 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 52 times.
✓ Branch 5 taken 61 times.
 113 if (!p.isZero()) {
468
1/2
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
 52 appendVertex(*simplex, p, hint);
469
3/4
✓ Branch 1 taken 52 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 50 times.
✓ Branch 4 taken 2 times.
 54 if (encloseOrigin()) return true;
470 2 removeVertex(*simplex);
471
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 appendVertex(*simplex, -p, hint);
472
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 if (encloseOrigin()) return true;
473 ✗ removeVertex(*simplex);
474 }
475
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 61 axis[i] = 0;
476 }
477 6 } break;
478 144 case 3:
479
1/2
✓ Branch 1 taken 144 times.
✗ Branch 2 not taken.
 144 axis.noalias() =
480
1/2
✓ Branch 1 taken 144 times.
✗ Branch 2 not taken.
 144 (simplex->vertex[1]->w - simplex->vertex[0]->w)
481
3/6
✓ Branch 1 taken 144 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 144 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 144 times.
✗ Branch 8 not taken.
 288 .cross(simplex->vertex[2]->w - simplex->vertex[0]->w);
482
3/4
✓ Branch 2 taken 144 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 142 times.
✓ Branch 5 taken 2 times.
 144 if (!axis.isZero()) {
483
1/2
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
 142 appendVertex(*simplex, axis, hint);
484
3/4
✓ Branch 1 taken 142 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 140 times.
✓ Branch 4 taken 2 times.
 142 if (encloseOrigin()) return true;
485 2 removeVertex(*simplex);
486
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 appendVertex(*simplex, -axis, hint);
487
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 if (encloseOrigin()) return true;
488 ✗ removeVertex(*simplex);
489 }
490 2 break;
491 4447 case 4:
492
2/4
✓ Branch 1 taken 4447 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4447 times.
✗ Branch 5 not taken.
 4447 if (std::abs(triple(simplex->vertex[0]->w - simplex->vertex[3]->w,
493
1/2
✓ Branch 1 taken 4447 times.
✗ Branch 2 not taken.
 4447 simplex->vertex[1]->w - simplex->vertex[3]->w,
494
3/4
✓ Branch 1 taken 4447 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4445 times.
✓ Branch 4 taken 2 times.
 8894 simplex->vertex[2]->w - simplex->vertex[3]->w)) > 0)
495 4445 return true;
496 2 break;
497 }
498
499 10 return false;
500 }
501
502 2009728 inline void originToPoint(const GJK::Simplex& current, GJK::vertex_id_t a,
503 const Vec3ps& A, GJK::Simplex& next, Vec3ps& ray) {
504 // A is the closest to the origin
505 2009728 ray = A;
506 2009728 next.vertex[0] = current.vertex[a];
507 2009728 next.rank = 1;
508 2009728 }
509
510 9102615 inline void originToSegment(const GJK::Simplex& current, GJK::vertex_id_t a,
511 GJK::vertex_id_t b, const Vec3ps& A,
512 const Vec3ps& B, const Vec3ps& AB,
513 const SolverScalar& ABdotAO, GJK::Simplex& next,
514 Vec3ps& ray) {
515 // ray = - ( AB ^ AO ) ^ AB = (AB.B) A + (-AB.A) B
516
5/10
✓ Branch 1 taken 9102615 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9102615 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9102615 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9102615 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9102615 times.
✗ Branch 14 not taken.
 9102615 ray = AB.dot(B) * A + ABdotAO * B;
517
518 9102615 next.vertex[0] = current.vertex[b];
519 9102615 next.vertex[1] = current.vertex[a];
520 9102615 next.rank = 2;
521
522 // To ensure backward compatibility
523
2/4
✓ Branch 1 taken 9102615 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9102615 times.
✗ Branch 5 not taken.
 9102615 ray /= AB.squaredNorm();
524 9102615 }
525
526 688659 inline bool originToTriangle(const GJK::Simplex& current, GJK::vertex_id_t a,
527 GJK::vertex_id_t b, GJK::vertex_id_t c,
528 const Vec3ps& ABC, const SolverScalar& ABCdotAO,
529 GJK::Simplex& next, Vec3ps& ray) {
530 688659 next.rank = 3;
531 688659 next.vertex[2] = current.vertex[a];
532
533
2/2
✓ Branch 0 taken 136 times.
✓ Branch 1 taken 688523 times.
 688659 if (ABCdotAO == 0) {
534 136 next.vertex[0] = current.vertex[c];
535 136 next.vertex[1] = current.vertex[b];
536 136 ray.setZero();
537 136 return true;
538 }
539
2/2
✓ Branch 0 taken 453684 times.
✓ Branch 1 taken 234839 times.
 688523 if (ABCdotAO > 0) { // Above triangle
540 453684 next.vertex[0] = current.vertex[c];
541 453684 next.vertex[1] = current.vertex[b];
542 } else {
543 234839 next.vertex[0] = current.vertex[b];
544 234839 next.vertex[1] = current.vertex[c];
545 }
546
547 // To ensure backward compatibility
548
3/6
✓ Branch 1 taken 688523 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 688523 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 688523 times.
✗ Branch 8 not taken.
 688523 ray = -ABCdotAO / ABC.squaredNorm() * ABC;
549 688523 return false;
550 }
551
552 10002060 bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) {
553 10002060 const vertex_id_t a = 1, b = 0;
554 // A is the last point we added.
555 10002060 const Vec3ps& A = current.vertex[a]->w;
556 10002060 const Vec3ps& B = current.vertex[b]->w;
557
558
2/4
✓ Branch 1 taken 10002060 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10002060 times.
✗ Branch 5 not taken.
 10002060 const Vec3ps AB = B - A;
559
2/4
✓ Branch 1 taken 10002060 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10002060 times.
✗ Branch 5 not taken.
 10002060 const SolverScalar d = AB.dot(-A);
560
2/4
✓ Branch 1 taken 10002060 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 10002060 times.
 10002060 assert(d <= AB.squaredNorm());
561
562
2/2
✓ Branch 0 taken 5509 times.
✓ Branch 1 taken 9996551 times.
 10002060 if (d == 0) {
563 // Two extremely unlikely cases:
564 // - AB is orthogonal to A: should never happen because it means the support
565 // function did not do any progress and GJK should have stopped.
566 // - A == origin
567 // In any case, A is the closest to the origin
568
1/2
✓ Branch 1 taken 5509 times.
✗ Branch 2 not taken.
 5509 originToPoint(current, a, A, next, ray);
569 5509 free_v[nfree++] = current.vertex[b];
570
1/2
✓ Branch 2 taken 5509 times.
✗ Branch 3 not taken.
 5509 return A.isZero();
571
2/2
✓ Branch 0 taken 1864230 times.
✓ Branch 1 taken 8132321 times.
 9996551 } else if (d < 0) {
572 // A is the closest to the origin
573
1/2
✓ Branch 1 taken 1864230 times.
✗ Branch 2 not taken.
 1864230 originToPoint(current, a, A, next, ray);
574 1864230 free_v[nfree++] = current.vertex[b];
575 } else
576
1/2
✓ Branch 1 taken 8132321 times.
✗ Branch 2 not taken.
 8132321 originToSegment(current, a, b, A, B, AB, d, next, ray);
577 9996551 return false;
578 }
579
580 1547928 bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) {
581 1547928 const vertex_id_t a = 2, b = 1, c = 0;
582 // A is the last point we added.
583
1/2
✓ Branch 1 taken 1547928 times.
✗ Branch 2 not taken.
 1547928 const Vec3ps &A = current.vertex[a]->w, B = current.vertex[b]->w,
584
1/2
✓ Branch 1 taken 1547928 times.
✗ Branch 2 not taken.
 1547928 C = current.vertex[c]->w;
585
586
5/10
✓ Branch 1 taken 1547928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1547928 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1547928 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1547928 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1547928 times.
✗ Branch 14 not taken.
 1547928 const Vec3ps AB = B - A, AC = C - A, ABC = AB.cross(AC);
587
588
3/6
✓ Branch 1 taken 1547928 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1547928 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1547928 times.
✗ Branch 8 not taken.
 1547928 SolverScalar edgeAC2o = ABC.cross(AC).dot(-A);
589
2/2
✓ Branch 0 taken 822215 times.
✓ Branch 1 taken 725713 times.
 1547928 if (edgeAC2o >= 0) {
590
2/4
✓ Branch 1 taken 822215 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 822215 times.
✗ Branch 5 not taken.
 822215 SolverScalar towardsC = AC.dot(-A);
591
2/2
✓ Branch 0 taken 510275 times.
✓ Branch 1 taken 311940 times.
 822215 if (towardsC >= 0) { // Region 1
592
1/2
✓ Branch 1 taken 510275 times.
✗ Branch 2 not taken.
 510275 originToSegment(current, a, c, A, C, AC, towardsC, next, ray);
593 510275 free_v[nfree++] = current.vertex[b];
594 } else { // Region 4 or 5
595
2/4
✓ Branch 1 taken 311940 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 311940 times.
✗ Branch 5 not taken.
 311940 SolverScalar towardsB = AB.dot(-A);
596
2/2
✓ Branch 0 taken 138875 times.
✓ Branch 1 taken 173065 times.
 311940 if (towardsB < 0) { // Region 5
597 // A is the closest to the origin
598
1/2
✓ Branch 1 taken 138875 times.
✗ Branch 2 not taken.
 138875 originToPoint(current, a, A, next, ray);
599 138875 free_v[nfree++] = current.vertex[b];
600 } else // Region 4
601
1/2
✓ Branch 1 taken 173065 times.
✗ Branch 2 not taken.
 173065 originToSegment(current, a, b, A, B, AB, towardsB, next, ray);
602 311940 free_v[nfree++] = current.vertex[c];
603 }
604 } else {
605
3/6
✓ Branch 1 taken 725713 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 725713 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 725713 times.
✗ Branch 8 not taken.
 725713 SolverScalar edgeAB2o = AB.cross(ABC).dot(-A);
606
2/2
✓ Branch 0 taken 257374 times.
✓ Branch 1 taken 468339 times.
 725713 if (edgeAB2o >= 0) { // Region 4 or 5
607
2/4
✓ Branch 1 taken 257374 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 257374 times.
✗ Branch 5 not taken.
 257374 SolverScalar towardsB = AB.dot(-A);
608
2/2
✓ Branch 0 taken 152 times.
✓ Branch 1 taken 257222 times.
 257374 if (towardsB < 0) { // Region 5
609 // A is the closest to the origin
610
1/2
✓ Branch 1 taken 152 times.
✗ Branch 2 not taken.
 152 originToPoint(current, a, A, next, ray);
611 152 free_v[nfree++] = current.vertex[b];
612 } else // Region 4
613
1/2
✓ Branch 1 taken 257222 times.
✗ Branch 2 not taken.
 257222 originToSegment(current, a, b, A, B, AB, towardsB, next, ray);
614 257374 free_v[nfree++] = current.vertex[c];
615 } else {
616
3/6
✓ Branch 1 taken 468339 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 468339 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 468339 times.
✗ Branch 8 not taken.
 468339 return originToTriangle(current, a, b, c, ABC, ABC.dot(-A), next, ray);
617 }
618 }
619 1079589 return false;
620 }
621
622 288964 bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
623 // The code of this function was generated using doc/gjk.py
624 288964 const vertex_id_t a = 3, b = 2, c = 1, d = 0;
625 288964 const Vec3ps& A(current.vertex[a]->w);
626 288964 const Vec3ps& B(current.vertex[b]->w);
627 288964 const Vec3ps& C(current.vertex[c]->w);
628 288964 const Vec3ps& D(current.vertex[d]->w);
629
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar aa = A.squaredNorm();
630
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar da = D.dot(A);
631
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar db = D.dot(B);
632
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar dc = D.dot(C);
633
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar dd = D.dot(D);
634 288964 const SolverScalar da_aa = da - aa;
635
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar ca = C.dot(A);
636
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar cb = C.dot(B);
637
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar cc = C.dot(C);
638 288964 const SolverScalar& cd = dc;
639 288964 const SolverScalar ca_aa = ca - aa;
640
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar ba = B.dot(A);
641
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const SolverScalar bb = B.dot(B);
642 288964 const SolverScalar& bc = cb;
643 288964 const SolverScalar& bd = db;
644 288964 const SolverScalar ba_aa = ba - aa;
645 288964 const SolverScalar ba_ca = ba - ca;
646 288964 const SolverScalar ca_da = ca - da;
647 288964 const SolverScalar da_ba = da - ba;
648
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const Vec3ps a_cross_b = A.cross(B);
649
1/2
✓ Branch 1 taken 288964 times.
✗ Branch 2 not taken.
 288964 const Vec3ps a_cross_c = A.cross(C);
650
651 288964 const SolverScalar dummy_precision(
652 3 * std::sqrt(std::numeric_limits<SolverScalar>::epsilon()));
653 COAL_UNUSED_VARIABLE(dummy_precision);
654
655 #define REGION_INSIDE() \
656 ray.setZero(); \
657 next.vertex[0] = current.vertex[d]; \
658 next.vertex[1] = current.vertex[c]; \
659 next.vertex[2] = current.vertex[b]; \
660 next.vertex[3] = current.vertex[a]; \
661 next.rank = 4; \
662 return true;
663
664 // clang-format off
665
2/2
✓ Branch 0 taken 230862 times.
✓ Branch 1 taken 58102 times.
 288964 if (ba_aa <= 0) { // if AB.AO >= 0 / a10
666
3/4
✓ Branch 1 taken 230862 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 160417 times.
✓ Branch 4 taken 70445 times.
 230862 if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / a10.a3
667
2/2
✓ Branch 0 taken 70878 times.
✓ Branch 1 taken 89539 times.
 160417 if (ba * da_ba + bd * ba_aa - bb * da_aa <=
668 0) { // if (ADB ^ AB).AO >= 0 / a10.a3.a9
669
2/2
✓ Branch 0 taken 22361 times.
✓ Branch 1 taken 48517 times.
 70878 if (da_aa <= 0) { // if AD.AO >= 0 / a10.a3.a9.a12
670
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 22361 times.
 22361 assert(da * da_ba + dd * ba_aa - db * da_aa <=
671 dummy_precision); // (ADB ^ AD).AO >= 0 / a10.a3.a9.a12.a8
672
2/2
✓ Branch 0 taken 16304 times.
✓ Branch 1 taken 6057 times.
 22361 if (ba * ba_ca + bb * ca_aa - bc * ba_aa <=
673 0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.a4
674 // Region ABC
675
3/6
✓ Branch 1 taken 16304 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16304 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16304 times.
✗ Branch 8 not taken.
 16304 originToTriangle(current, a, b, c, (B - A).cross(C - A),
676
2/4
✓ Branch 1 taken 16304 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16304 times.
✗ Branch 5 not taken.
 16304 -C.dot(a_cross_b), next, ray);
677 16304 free_v[nfree++] = current.vertex[d];
678 } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.a12.a8.!a4
679 // Region AB
680
3/6
✓ Branch 1 taken 6057 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6057 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6057 times.
✗ Branch 8 not taken.
 6057 originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray);
681 6057 free_v[nfree++] = current.vertex[c];
682 6057 free_v[nfree++] = current.vertex[d];
683 } // end of (ABC ^ AB).AO >= 0
684 } else { // not AD.AO >= 0 / a10.a3.a9.!a12
685
2/2
✓ Branch 0 taken 41291 times.
✓ Branch 1 taken 7226 times.
 48517 if (ba * ba_ca + bb * ca_aa - bc * ba_aa <=
686 0) { // if (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.a4
687
2/2
✓ Branch 0 taken 1373 times.
✓ Branch 1 taken 39918 times.
 41291 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
688 0) { // if (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5
689
2/2
✓ Branch 0 taken 411 times.
✓ Branch 1 taken 962 times.
 1373 if (ca * ca_da + cc * da_aa - cd * ca_aa <=
690 0) { // if (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.a6
691 // Region ACD
692
3/6
✓ Branch 1 taken 411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 411 times.
✗ Branch 8 not taken.
 411 originToTriangle(current, a, c, d, (C - A).cross(D - A),
693
2/4
✓ Branch 1 taken 411 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 411 times.
✗ Branch 5 not taken.
 411 -D.dot(a_cross_c), next, ray);
694 411 free_v[nfree++] = current.vertex[b];
695 } else { // not (ACD ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.a5.!a6
696 // Region AC
697
3/6
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 962 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 962 times.
✗ Branch 8 not taken.
 962 originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
698 962 free_v[nfree++] = current.vertex[b];
699 962 free_v[nfree++] = current.vertex[d];
700 } // end of (ACD ^ AC).AO >= 0
701 } else { // not (ABC ^ AC).AO >= 0 / a10.a3.a9.!a12.a4.!a5
702 // Region ABC
703
3/6
✓ Branch 1 taken 39918 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39918 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39918 times.
✗ Branch 8 not taken.
 39918 originToTriangle(current, a, b, c, (B - A).cross(C - A),
704
2/4
✓ Branch 1 taken 39918 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39918 times.
✗ Branch 5 not taken.
 39918 -C.dot(a_cross_b), next, ray);
705 39918 free_v[nfree++] = current.vertex[d];
706 } // end of (ABC ^ AC).AO >= 0
707 } else { // not (ABC ^ AB).AO >= 0 / a10.a3.a9.!a12.!a4
708 // Region AB
709
3/6
✓ Branch 1 taken 7226 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7226 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7226 times.
✗ Branch 8 not taken.
 7226 originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray);
710 7226 free_v[nfree++] = current.vertex[c];
711 7226 free_v[nfree++] = current.vertex[d];
712 } // end of (ABC ^ AB).AO >= 0
713 } // end of AD.AO >= 0
714 } else { // not (ADB ^ AB).AO >= 0 / a10.a3.!a9
715
2/2
✓ Branch 0 taken 84918 times.
✓ Branch 1 taken 4621 times.
 89539 if (da * da_ba + dd * ba_aa - db * da_aa <=
716 0) { // if (ADB ^ AD).AO >= 0 / a10.a3.!a9.a8
717 // Region ADB
718
3/6
✓ Branch 1 taken 84918 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 84918 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 84918 times.
✗ Branch 8 not taken.
 84918 originToTriangle(current, a, d, b, (D - A).cross(B - A),
719
2/4
✓ Branch 1 taken 84918 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 84918 times.
✗ Branch 5 not taken.
 84918 D.dot(a_cross_b), next, ray);
720 84918 free_v[nfree++] = current.vertex[c];
721 } else { // not (ADB ^ AD).AO >= 0 / a10.a3.!a9.!a8
722
2/2
✓ Branch 0 taken 4537 times.
✓ Branch 1 taken 84 times.
 4621 if (ca * ca_da + cc * da_aa - cd * ca_aa <=
723 0) { // if (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.a6
724
2/2
✓ Branch 0 taken 2384 times.
✓ Branch 1 taken 2153 times.
 4537 if (da * ca_da + dc * da_aa - dd * ca_aa <=
725 0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.a7
726 // Region AD
727
3/6
✓ Branch 1 taken 2384 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2384 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2384 times.
✗ Branch 8 not taken.
 2384 originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
728 2384 free_v[nfree++] = current.vertex[b];
729 2384 free_v[nfree++] = current.vertex[c];
730 } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.a6.!a7
731 // Region ACD
732
3/6
✓ Branch 1 taken 2153 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2153 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2153 times.
✗ Branch 8 not taken.
 2153 originToTriangle(current, a, c, d, (C - A).cross(D - A),
733
2/4
✓ Branch 1 taken 2153 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2153 times.
✗ Branch 5 not taken.
 2153 -D.dot(a_cross_c), next, ray);
734 2153 free_v[nfree++] = current.vertex[b];
735 } // end of (ACD ^ AD).AO >= 0
736 } else { // not (ACD ^ AC).AO >= 0 / a10.a3.!a9.!a8.!a6
737
1/2
✓ Branch 0 taken 84 times.
✗ Branch 1 not taken.
 84 if (da * ca_da + dc * da_aa - dd * ca_aa <=
738 0) { // if (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.a7
739 // Region AD
740
3/6
✓ Branch 1 taken 84 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 84 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 84 times.
✗ Branch 8 not taken.
 84 originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
741 84 free_v[nfree++] = current.vertex[b];
742 84 free_v[nfree++] = current.vertex[c];
743 } else { // not (ACD ^ AD).AO >= 0 / a10.a3.!a9.!a8.!a6.!a7
744 // Region AC
745 ✗ originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
746 ✗ free_v[nfree++] = current.vertex[b];
747 ✗ free_v[nfree++] = current.vertex[d];
748 } // end of (ACD ^ AD).AO >= 0
749 } // end of (ACD ^ AC).AO >= 0
750 } // end of (ADB ^ AD).AO >= 0
751 } // end of (ADB ^ AB).AO >= 0
752 } else { // not ADB.AO >= 0 / a10.!a3
753
3/4
✓ Branch 1 taken 70445 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 35077 times.
✓ Branch 4 taken 35368 times.
 70445 if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / a10.!a3.a1
754
2/2
✓ Branch 0 taken 35065 times.
✓ Branch 1 taken 12 times.
 35077 if (ba * ba_ca + bb * ca_aa - bc * ba_aa <=
755 0) { // if (ABC ^ AB).AO >= 0 / a10.!a3.a1.a4
756
2/2
✓ Branch 0 taken 2248 times.
✓ Branch 1 taken 32817 times.
 35065 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
757 0) { // if (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.a5
758
2/2
✓ Branch 0 taken 1305 times.
✓ Branch 1 taken 943 times.
 2248 if (ca * ca_da + cc * da_aa - cd * ca_aa <=
759 0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.a6
760 // Region ACD
761
3/6
✓ Branch 1 taken 1305 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1305 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1305 times.
✗ Branch 8 not taken.
 1305 originToTriangle(current, a, c, d, (C - A).cross(D - A),
762
2/4
✓ Branch 1 taken 1305 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1305 times.
✗ Branch 5 not taken.
 1305 -D.dot(a_cross_c), next, ray);
763 1305 free_v[nfree++] = current.vertex[b];
764 } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.a1.a4.a5.!a6
765 // Region AC
766
3/6
✓ Branch 1 taken 943 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 943 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 943 times.
✗ Branch 8 not taken.
 943 originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
767 943 free_v[nfree++] = current.vertex[b];
768 943 free_v[nfree++] = current.vertex[d];
769 } // end of (ACD ^ AC).AO >= 0
770 } else { // not (ABC ^ AC).AO >= 0 / a10.!a3.a1.a4.!a5
771 // Region ABC
772
3/6
✓ Branch 1 taken 32817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32817 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32817 times.
✗ Branch 8 not taken.
 32817 originToTriangle(current, a, b, c, (B - A).cross(C - A),
773
2/4
✓ Branch 1 taken 32817 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32817 times.
✗ Branch 5 not taken.
 32817 -C.dot(a_cross_b), next, ray);
774 32817 free_v[nfree++] = current.vertex[d];
775 } // end of (ABC ^ AC).AO >= 0
776 } else { // not (ABC ^ AB).AO >= 0 / a10.!a3.a1.!a4
777 // Region AB
778
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 originToSegment(current, a, b, A, B, B - A, -ba_aa, next, ray);
779 12 free_v[nfree++] = current.vertex[c];
780 12 free_v[nfree++] = current.vertex[d];
781 } // end of (ABC ^ AB).AO >= 0
782 } else { // not ABC.AO >= 0 / a10.!a3.!a1
783
3/4
✓ Branch 1 taken 35368 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2128 times.
✓ Branch 4 taken 33240 times.
 35368 if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / a10.!a3.!a1.a2
784
1/2
✓ Branch 0 taken 2128 times.
✗ Branch 1 not taken.
 2128 if (ca * ca_da + cc * da_aa - cd * ca_aa <=
785 0) { // if (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.a6
786
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2128 times.
 2128 if (da * ca_da + dc * da_aa - dd * ca_aa <=
787 0) { // if (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.a7
788 // Region AD
789 ✗ originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
790 ✗ free_v[nfree++] = current.vertex[b];
791 ✗ free_v[nfree++] = current.vertex[c];
792 } else { // not (ACD ^ AD).AO >= 0 / a10.!a3.!a1.a2.a6.!a7
793 // Region ACD
794
3/6
✓ Branch 1 taken 2128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2128 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2128 times.
✗ Branch 8 not taken.
 2128 originToTriangle(current, a, c, d, (C - A).cross(D - A),
795
2/4
✓ Branch 1 taken 2128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2128 times.
✗ Branch 5 not taken.
 2128 -D.dot(a_cross_c), next, ray);
796 2128 free_v[nfree++] = current.vertex[b];
797 } // end of (ACD ^ AD).AO >= 0
798 } else { // not (ACD ^ AC).AO >= 0 / a10.!a3.!a1.a2.!a6
799 ✗ if (ca_aa <= 0) { // if AC.AO >= 0 / a10.!a3.!a1.a2.!a6.a11
800 // Region AC
801 ✗ originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
802 ✗ free_v[nfree++] = current.vertex[b];
803 ✗ free_v[nfree++] = current.vertex[d];
804 } else { // not AC.AO >= 0 / a10.!a3.!a1.a2.!a6.!a11
805 // Region AD
806 ✗ originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
807 ✗ free_v[nfree++] = current.vertex[b];
808 ✗ free_v[nfree++] = current.vertex[c];
809 } // end of AC.AO >= 0
810 } // end of (ACD ^ AC).AO >= 0
811 } else { // not ACD.AO >= 0 / a10.!a3.!a1.!a2
812 // Region Inside
813
1/2
✓ Branch 1 taken 33240 times.
✗ Branch 2 not taken.
 33240 REGION_INSIDE()
814 } // end of ACD.AO >= 0
815 } // end of ABC.AO >= 0
816 } // end of ADB.AO >= 0
817 } else { // not AB.AO >= 0 / !a10
818
2/2
✓ Branch 0 taken 41447 times.
✓ Branch 1 taken 16655 times.
 58102 if (ca_aa <= 0) { // if AC.AO >= 0 / !a10.a11
819
3/4
✓ Branch 1 taken 41447 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 37138 times.
✓ Branch 4 taken 4309 times.
 41447 if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.a11.a2
820
2/2
✓ Branch 0 taken 23914 times.
✓ Branch 1 taken 13224 times.
 37138 if (da_aa <= 0) { // if AD.AO >= 0 / !a10.a11.a2.a12
821
2/2
✓ Branch 0 taken 22658 times.
✓ Branch 1 taken 1256 times.
 23914 if (ca * ca_da + cc * da_aa - cd * ca_aa <=
822 0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.a6
823
2/2
✓ Branch 0 taken 1338 times.
✓ Branch 1 taken 21320 times.
 22658 if (da * ca_da + dc * da_aa - dd * ca_aa <=
824 0) { // if (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7
825
2/2
✓ Branch 0 taken 514 times.
✓ Branch 1 taken 824 times.
 1338 if (da * da_ba + dd * ba_aa - db * da_aa <=
826 0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.a8
827 // Region ADB
828
3/6
✓ Branch 1 taken 514 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 514 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 514 times.
✗ Branch 8 not taken.
 514 originToTriangle(current, a, d, b, (D - A).cross(B - A),
829
2/4
✓ Branch 1 taken 514 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 514 times.
✗ Branch 5 not taken.
 514 D.dot(a_cross_b), next, ray);
830 514 free_v[nfree++] = current.vertex[c];
831 } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.a7.!a8
832 // Region AD
833
3/6
✓ Branch 1 taken 824 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 824 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 824 times.
✗ Branch 8 not taken.
 824 originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
834 824 free_v[nfree++] = current.vertex[b];
835 824 free_v[nfree++] = current.vertex[c];
836 } // end of (ADB ^ AD).AO >= 0
837 } else { // not (ACD ^ AD).AO >= 0 / !a10.a11.a2.a12.a6.!a7
838 // Region ACD
839
3/6
✓ Branch 1 taken 21320 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21320 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21320 times.
✗ Branch 8 not taken.
 21320 originToTriangle(current, a, c, d, (C - A).cross(D - A),
840
2/4
✓ Branch 1 taken 21320 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21320 times.
✗ Branch 5 not taken.
 21320 -D.dot(a_cross_c), next, ray);
841 21320 free_v[nfree++] = current.vertex[b];
842 } // end of (ACD ^ AD).AO >= 0
843 } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6
844
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1256 times.
 1256 assert(!(da * ca_da + dc * da_aa - dd * ca_aa <=
845 -dummy_precision)); // Not (ACD ^ AD).AO >= 0 /
846 // !a10.a11.a2.a12.!a6.!a7
847
2/2
✓ Branch 0 taken 815 times.
✓ Branch 1 taken 441 times.
 1256 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
848 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.a5
849 // Region AC
850
3/6
✓ Branch 1 taken 815 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 815 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 815 times.
✗ Branch 8 not taken.
 815 originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
851 815 free_v[nfree++] = current.vertex[b];
852 815 free_v[nfree++] = current.vertex[d];
853 } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.a12.!a6.!a7.!a5
854 // Region ABC
855
3/6
✓ Branch 1 taken 441 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 441 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 441 times.
✗ Branch 8 not taken.
 441 originToTriangle(current, a, b, c, (B - A).cross(C - A),
856
2/4
✓ Branch 1 taken 441 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 441 times.
✗ Branch 5 not taken.
 441 -C.dot(a_cross_b), next, ray);
857 441 free_v[nfree++] = current.vertex[d];
858 } // end of (ABC ^ AC).AO >= 0
859 } // end of (ACD ^ AC).AO >= 0
860 } else { // not AD.AO >= 0 / !a10.a11.a2.!a12
861
2/2
✓ Branch 0 taken 9717 times.
✓ Branch 1 taken 3507 times.
 13224 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
862 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5
863
2/2
✓ Branch 0 taken 4798 times.
✓ Branch 1 taken 4919 times.
 9717 if (ca * ca_da + cc * da_aa - cd * ca_aa <=
864 0) { // if (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.a6
865
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4798 times.
 4798 assert(!(da * ca_da + dc * da_aa - dd * ca_aa <=
866 -dummy_precision)); // Not (ACD ^ AD).AO >= 0 /
867 // !a10.a11.a2.!a12.a5.a6.!a7
868 // Region ACD
869
3/6
✓ Branch 1 taken 4798 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4798 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4798 times.
✗ Branch 8 not taken.
 4798 originToTriangle(current, a, c, d, (C - A).cross(D - A),
870
2/4
✓ Branch 1 taken 4798 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4798 times.
✗ Branch 5 not taken.
 4798 -D.dot(a_cross_c), next, ray);
871 4798 free_v[nfree++] = current.vertex[b];
872 } else { // not (ACD ^ AC).AO >= 0 / !a10.a11.a2.!a12.a5.!a6
873 // Region AC
874
3/6
✓ Branch 1 taken 4919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4919 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4919 times.
✗ Branch 8 not taken.
 4919 originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
875 4919 free_v[nfree++] = current.vertex[b];
876 4919 free_v[nfree++] = current.vertex[d];
877 } // end of (ACD ^ AC).AO >= 0
878 } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.a2.!a12.!a5
879
3/4
✓ Branch 1 taken 3507 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2855 times.
✓ Branch 4 taken 652 times.
 3507 if (C.dot(a_cross_b) <=
880 0) { // if ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.a1
881
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2855 times.
 2855 assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <=
882 dummy_precision); // (ABC ^ AB).AO >= 0 /
883 // !a10.a11.a2.!a12.!a5.a1.a4
884 // Region ABC
885
3/6
✓ Branch 1 taken 2855 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2855 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2855 times.
✗ Branch 8 not taken.
 2855 originToTriangle(current, a, b, c, (B - A).cross(C - A),
886
2/4
✓ Branch 1 taken 2855 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2855 times.
✗ Branch 5 not taken.
 2855 -C.dot(a_cross_b), next, ray);
887 2855 free_v[nfree++] = current.vertex[d];
888 } else { // not ABC.AO >= 0 / !a10.a11.a2.!a12.!a5.!a1
889
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 652 times.
 652 assert(!(da * ca_da + dc * da_aa - dd * ca_aa <=
890 -dummy_precision)); // Not (ACD ^ AD).AO >= 0 /
891 // !a10.a11.a2.!a12.!a5.!a1.!a7
892 // Region ACD
893
3/6
✓ Branch 1 taken 652 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 652 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 652 times.
✗ Branch 8 not taken.
 652 originToTriangle(current, a, c, d, (C - A).cross(D - A),
894
2/4
✓ Branch 1 taken 652 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 652 times.
✗ Branch 5 not taken.
 652 -D.dot(a_cross_c), next, ray);
895 652 free_v[nfree++] = current.vertex[b];
896 } // end of ABC.AO >= 0
897 } // end of (ABC ^ AC).AO >= 0
898 } // end of AD.AO >= 0
899 } else { // not ACD.AO >= 0 / !a10.a11.!a2
900
3/4
✓ Branch 1 taken 4309 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 910 times.
✓ Branch 4 taken 3399 times.
 4309 if (C.dot(a_cross_b) <= 0) { // if ABC.AO >= 0 / !a10.a11.!a2.a1
901
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 903 times.
 910 if (ca * ba_ca + cb * ca_aa - cc * ba_aa <=
902 0) { // if (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.a5
903 // Region AC
904
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 originToSegment(current, a, c, A, C, C - A, -ca_aa, next, ray);
905 7 free_v[nfree++] = current.vertex[b];
906 7 free_v[nfree++] = current.vertex[d];
907 } else { // not (ABC ^ AC).AO >= 0 / !a10.a11.!a2.a1.!a5
908
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 903 times.
 903 assert(ba * ba_ca + bb * ca_aa - bc * ba_aa <=
909 dummy_precision); // (ABC ^ AB).AO >= 0 /
910 // !a10.a11.!a2.a1.!a5.a4
911 // Region ABC
912
3/6
✓ Branch 1 taken 903 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 903 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 903 times.
✗ Branch 8 not taken.
 903 originToTriangle(current, a, b, c, (B - A).cross(C - A),
913
2/4
✓ Branch 1 taken 903 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 903 times.
✗ Branch 5 not taken.
 903 -C.dot(a_cross_b), next, ray);
914 903 free_v[nfree++] = current.vertex[d];
915 } // end of (ABC ^ AC).AO >= 0
916 } else { // not ABC.AO >= 0 / !a10.a11.!a2.!a1
917
3/4
✓ Branch 1 taken 3399 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 27 times.
✓ Branch 4 taken 3372 times.
 3399 if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.a11.!a2.!a1.a3
918
1/2
✓ Branch 0 taken 27 times.
✗ Branch 1 not taken.
 27 if (da * da_ba + dd * ba_aa - db * da_aa <=
919 0) { // if (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.a8
920 // Region ADB
921
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 27 originToTriangle(current, a, d, b, (D - A).cross(B - A),
922
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 D.dot(a_cross_b), next, ray);
923 27 free_v[nfree++] = current.vertex[c];
924 } else { // not (ADB ^ AD).AO >= 0 / !a10.a11.!a2.!a1.a3.!a8
925 // Region AD
926 ✗ originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
927 ✗ free_v[nfree++] = current.vertex[b];
928 ✗ free_v[nfree++] = current.vertex[c];
929 } // end of (ADB ^ AD).AO >= 0
930 } else { // not ADB.AO >= 0 / !a10.a11.!a2.!a1.!a3
931 // Region Inside
932
1/2
✓ Branch 1 taken 3372 times.
✗ Branch 2 not taken.
 3372 REGION_INSIDE()
933 } // end of ADB.AO >= 0
934 } // end of ABC.AO >= 0
935 } // end of ACD.AO >= 0
936 } else { // not AC.AO >= 0 / !a10.!a11
937
2/2
✓ Branch 0 taken 15693 times.
✓ Branch 1 taken 962 times.
 16655 if (da_aa <= 0) { // if AD.AO >= 0 / !a10.!a11.a12
938
3/4
✓ Branch 1 taken 15693 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12794 times.
✓ Branch 4 taken 2899 times.
 15693 if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / !a10.!a11.a12.a3
939
2/2
✓ Branch 0 taken 9035 times.
✓ Branch 1 taken 3759 times.
 12794 if (da * ca_da + dc * da_aa - dd * ca_aa <=
940 0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7
941
2/2
✓ Branch 0 taken 3567 times.
✓ Branch 1 taken 5468 times.
 9035 if (da * da_ba + dd * ba_aa - db * da_aa <=
942 0) { // if (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.a8
943
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3567 times.
 3567 assert(!(ba * da_ba + bd * ba_aa - bb * da_aa <=
944 -dummy_precision)); // Not (ADB ^ AB).AO >= 0 /
945 // !a10.!a11.a12.a3.a7.a8.!a9
946 // Region ADB
947
3/6
✓ Branch 1 taken 3567 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3567 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3567 times.
✗ Branch 8 not taken.
 3567 originToTriangle(current, a, d, b, (D - A).cross(B - A),
948
2/4
✓ Branch 1 taken 3567 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3567 times.
✗ Branch 5 not taken.
 3567 D.dot(a_cross_b), next, ray);
949 3567 free_v[nfree++] = current.vertex[c];
950 } else { // not (ADB ^ AD).AO >= 0 / !a10.!a11.a12.a3.a7.!a8
951 // Region AD
952
3/6
✓ Branch 1 taken 5468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5468 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5468 times.
✗ Branch 8 not taken.
 5468 originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
953 5468 free_v[nfree++] = current.vertex[b];
954 5468 free_v[nfree++] = current.vertex[c];
955 } // end of (ADB ^ AD).AO >= 0
956 } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.a3.!a7
957
3/4
✓ Branch 1 taken 3759 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3610 times.
✓ Branch 4 taken 149 times.
 3759 if (D.dot(a_cross_c) <=
958 0) { // if ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.a2
959
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3610 times.
 3610 assert(ca * ca_da + cc * da_aa - cd * ca_aa <=
960 dummy_precision); // (ACD ^ AC).AO >= 0 /
961 // !a10.!a11.a12.a3.!a7.a2.a6
962 // Region ACD
963
3/6
✓ Branch 1 taken 3610 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3610 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3610 times.
✗ Branch 8 not taken.
 3610 originToTriangle(current, a, c, d, (C - A).cross(D - A),
964
2/4
✓ Branch 1 taken 3610 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3610 times.
✗ Branch 5 not taken.
 3610 -D.dot(a_cross_c), next, ray);
965 3610 free_v[nfree++] = current.vertex[b];
966 } else { // not ACD.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2
967
3/4
✓ Branch 1 taken 149 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 94 times.
✓ Branch 4 taken 55 times.
 149 if (C.dot(a_cross_b) <=
968 0) { // if ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.a1
969
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 94 times.
 94 assert(!(ba * ba_ca + bb * ca_aa - bc * ba_aa <=
970 -dummy_precision)); // Not (ABC ^ AB).AO >= 0 /
971 // !a10.!a11.a12.a3.!a7.!a2.a1.!a4
972 // Region ADB
973
3/6
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 94 times.
✗ Branch 8 not taken.
 94 originToTriangle(current, a, d, b, (D - A).cross(B - A),
974
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 D.dot(a_cross_b), next, ray);
975 94 free_v[nfree++] = current.vertex[c];
976 } else { // not ABC.AO >= 0 / !a10.!a11.a12.a3.!a7.!a2.!a1
977 // Region ADB
978
3/6
✓ Branch 1 taken 55 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 55 times.
✗ Branch 8 not taken.
 55 originToTriangle(current, a, d, b, (D - A).cross(B - A),
979
2/4
✓ Branch 1 taken 55 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55 times.
✗ Branch 5 not taken.
 55 D.dot(a_cross_b), next, ray);
980 55 free_v[nfree++] = current.vertex[c];
981 } // end of ABC.AO >= 0
982 } // end of ACD.AO >= 0
983 } // end of (ACD ^ AD).AO >= 0
984 } else { // not ADB.AO >= 0 / !a10.!a11.a12.!a3
985
3/4
✓ Branch 1 taken 2899 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1561 times.
✓ Branch 4 taken 1338 times.
 2899 if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.!a11.a12.!a3.a2
986
2/2
✓ Branch 0 taken 31 times.
✓ Branch 1 taken 1530 times.
 1561 if (da * ca_da + dc * da_aa - dd * ca_aa <=
987 0) { // if (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.a7
988 // Region AD
989
3/6
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31 times.
✗ Branch 8 not taken.
 31 originToSegment(current, a, d, A, D, D - A, -da_aa, next, ray);
990 31 free_v[nfree++] = current.vertex[b];
991 31 free_v[nfree++] = current.vertex[c];
992 } else { // not (ACD ^ AD).AO >= 0 / !a10.!a11.a12.!a3.a2.!a7
993
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1530 times.
 1530 assert(ca * ca_da + cc * da_aa - cd * ca_aa <=
994 dummy_precision); // (ACD ^ AC).AO >= 0 /
995 // !a10.!a11.a12.!a3.a2.!a7.a6
996 // Region ACD
997
3/6
✓ Branch 1 taken 1530 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1530 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1530 times.
✗ Branch 8 not taken.
 1530 originToTriangle(current, a, c, d, (C - A).cross(D - A),
998
2/4
✓ Branch 1 taken 1530 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1530 times.
✗ Branch 5 not taken.
 1530 -D.dot(a_cross_c), next, ray);
999 1530 free_v[nfree++] = current.vertex[b];
1000 } // end of (ACD ^ AD).AO >= 0
1001 } else { // not ACD.AO >= 0 / !a10.!a11.a12.!a3.!a2
1002 // Region Inside
1003
1/2
✓ Branch 1 taken 1338 times.
✗ Branch 2 not taken.
 1338 REGION_INSIDE()
1004 } // end of ACD.AO >= 0
1005 } // end of ADB.AO >= 0
1006 } else { // not AD.AO >= 0 / !a10.!a11.!a12
1007 // Region A
1008
1/2
✓ Branch 1 taken 962 times.
✗ Branch 2 not taken.
 962 originToPoint(current, a, A, next, ray);
1009 962 free_v[nfree++] = current.vertex[b];
1010 962 free_v[nfree++] = current.vertex[c];
1011 962 free_v[nfree++] = current.vertex[d];
1012 } // end of AD.AO >= 0
1013 } // end of AC.AO >= 0
1014 } // end of AB.AO >= 0
1015 // clang-format on
1016
1017 #undef REGION_INSIDE
1018 251014 return false;
1019 }
1020
1021 30475928 void EPA::initialize() { reset(max_iterations, tolerance); }
1022
1023 30480371 void EPA::reset(size_t max_iterations_, SolverScalar tolerance_) {
1024 30480371 max_iterations = max_iterations_;
1025 30480371 tolerance = tolerance_;
1026 // EPA creates only 2 faces and 1 vertex per iteration.
1027 // (+ the 4 (or 8 in the future) faces at the beginning
1028 // + the 4 vertices (or 6 in the future) at the beginning)
1029 30480371 sv_store.resize(max_iterations + 4);
1030 30480371 fc_store.resize(2 * max_iterations + 4);
1031 30480371 status = DidNotRun;
1032 30480371 normal.setZero();
1033 30480371 support_hint.setZero();
1034 30480371 depth = 0;
1035 30480371 closest_face = nullptr;
1036 30480371 result.reset();
1037 30480371 hull.reset();
1038 30480371 num_vertices = 0;
1039 30480371 stock.reset();
1040 // The stock is initialized with the faces in reverse order so that the
1041 // hull and the stock do not overlap (the stock will shring as the hull will
1042 // grow).
1043
2/2
✓ Branch 1 taken 124414332 times.
✓ Branch 2 taken 30480371 times.
 154894703 for (size_t i = 0; i < fc_store.size(); ++i)
1044 124414332 stock.append(&fc_store[fc_store.size() - i - 1]);
1045 30480371 iterations = 0;
1046 30480371 }
1047
1048 ✗ bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a,
1049 const SimplexVertex& b, SolverScalar& dist) {
1050 ✗ Vec3ps ab = b.w - a.w;
1051 ✗ Vec3ps n_ab = ab.cross(face->n);
1052 ✗ SolverScalar a_dot_nab = a.w.dot(n_ab);
1053
1054 ✗ if (a_dot_nab < 0) // the origin is on the outside part of ab
1055 {
1056 // following is similar to projectOrigin for two points
1057 // however, as we dont need to compute the parameterization, dont need to
1058 // compute 0 or 1
1059 ✗ SolverScalar a_dot_ab = a.w.dot(ab);
1060 ✗ SolverScalar b_dot_ab = b.w.dot(ab);
1061
1062 ✗ if (a_dot_ab > 0)
1063 ✗ dist = a.w.norm();
1064 ✗ else if (b_dot_ab < 0)
1065 ✗ dist = b.w.norm();
1066 else {
1067 ✗ dist = std::sqrt(
1068 ✗ std::max(a.w.squaredNorm() - a_dot_ab * a_dot_ab / ab.squaredNorm(),
1069 ✗ SolverScalar(0)));
1070 }
1071
1072 ✗ return true;
1073 }
1074
1075 ✗ return false;
1076 }
1077
1078 565848 EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c,
1079 bool force) {
1080
2/2
✓ Branch 0 taken 565835 times.
✓ Branch 1 taken 13 times.
 565848 if (stock.root != nullptr) {
1081 565835 SimplexFace* face = stock.root;
1082 565835 stock.remove(face);
1083 565835 hull.append(face);
1084 565835 face->pass = 0;
1085 565835 face->vertex_id[0] = id_a;
1086 565835 face->vertex_id[1] = id_b;
1087 565835 face->vertex_id[2] = id_c;
1088 565835 const SimplexVertex& a = sv_store[id_a];
1089 565835 const SimplexVertex& b = sv_store[id_b];
1090 565835 const SimplexVertex& c = sv_store[id_c];
1091
3/6
✓ Branch 1 taken 565835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 565835 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 565835 times.
✗ Branch 8 not taken.
 565835 face->n = (b.w - a.w).cross(c.w - a.w);
1092
1093
1/2
✓ Branch 2 taken 565835 times.
✗ Branch 3 not taken.
 565835 if (face->n.norm() > Eigen::NumTraits<SolverScalar>::epsilon()) {
1094 565835 face->n.normalize();
1095
1096 // If the origin projects outside the face, skip it in the
1097 // `findClosestFace` method.
1098 // The origin always projects inside the closest face.
1099
3/6
✓ Branch 1 taken 565835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 565835 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 565835 times.
✗ Branch 8 not taken.
 565835 SolverScalar a_dot_nab = a.w.dot((b.w - a.w).cross(face->n));
1100
3/6
✓ Branch 1 taken 565835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 565835 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 565835 times.
✗ Branch 8 not taken.
 565835 SolverScalar b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n));
1101
3/6
✓ Branch 1 taken 565835 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 565835 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 565835 times.
✗ Branch 8 not taken.
 565835 SolverScalar c_dot_nca = c.w.dot((a.w - c.w).cross(face->n));
1102
2/2
✓ Branch 0 taken 490967 times.
✓ Branch 1 taken 74868 times.
 565835 if (a_dot_nab >= -tolerance && //
1103
2/2
✓ Branch 0 taken 425791 times.
✓ Branch 1 taken 65176 times.
 490967 b_dot_nbc >= -tolerance && //
1104
2/2
✓ Branch 0 taken 371997 times.
✓ Branch 1 taken 53794 times.
 425791 c_dot_nca >= -tolerance) {
1105 371997 face->d = a.w.dot(face->n);
1106 371997 face->ignore = false;
1107 } else {
1108 // We will never check this face, so we don't care about
1109 // its true distance to the origin.
1110 193838 face->d = std::numeric_limits<SolverScalar>::max();
1111 193838 face->ignore = true;
1112 }
1113
1114 // For the initialization of EPA, we need to force the addition of the
1115 // face. This is because the origin can lie outside the initial
1116 // tetrahedron. This is expected. GJK can converge in two different ways:
1117 // either by enclosing the origin with a simplex, or by converging
1118 // sufficiently close to the origin.
1119 // The thing is, "sufficiently close to the origin" depends on the
1120 // tolerance of GJK. So in this case, GJK **cannot** guarantee that the
1121 // shapes are indeed in collision. If it turns out they are not in
1122 // collision, the origin will lie outside of the Minkowski difference and
1123 // thus outside the initial tetrahedron. But EPA is ultimately a
1124 // projection algorithm, so it will work fine!
1125 //
1126 // Actually, the `NonConvex` status is badly named. There should not be
1127 // such a status! Again, if the origin lies outside the Minkowski
1128 // difference, EPA will work fine, and will find the right (signed)
1129 // distance between the shapes as well as the right normal. This is a
1130 // very nice mathematical property, making GJK + EPA a **very** robust set
1131 // of algorithms. :)
1132 // TODO(louis): remove the `NonConvex` status.
1133
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 565835 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 565835 if (face->d >= -tolerance || force)
1134 565835 return face;
1135 else
1136 ✗ status = NonConvex;
1137 } else
1138 ✗ status = Degenerated;
1139
1140 ✗ hull.remove(face);
1141 ✗ stock.append(face);
1142 ✗ return nullptr;
1143 }
1144
1145
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 assert(hull.count >= fc_store.size() && "EPA: should not be out of faces.");
1146 13 status = OutOfFaces;
1147 13 return nullptr;
1148 }
1149
1150 /** @brief Find the best polytope face to split */
1151 124007 EPA::SimplexFace* EPA::findClosestFace() {
1152 124007 SimplexFace* minf = hull.root;
1153 124007 SolverScalar mind = std::numeric_limits<SolverScalar>::max();
1154
2/2
✓ Branch 0 taken 16581538 times.
✓ Branch 1 taken 124007 times.
 16705545 for (SimplexFace* f = minf; f; f = f->next_face) {
1155
2/2
✓ Branch 0 taken 3469136 times.
✓ Branch 1 taken 13112402 times.
 16581538 if (f->ignore) continue;
1156 13112402 SolverScalar sqd = f->d * f->d;
1157
2/2
✓ Branch 0 taken 608580 times.
✓ Branch 1 taken 12503822 times.
 13112402 if (sqd < mind) {
1158 608580 minf = f;
1159 608580 mind = sqd;
1160 }
1161 }
1162
2/4
✓ Branch 0 taken 124007 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 124007 times.
✗ Branch 3 not taken.
 124007 assert(minf && !(minf->ignore) && "EPA: minf should not be flagged ignored.");
1163 124007 return minf;
1164 }
1165
1166 4445 EPA::Status EPA::evaluate(GJK& gjk, const Vec3ps& guess) {
1167 COAL_TRACY_ZONE_SCOPED_N("coal::details::EPA::evaluate");
1168 4445 GJK::Simplex& simplex = *gjk.getSimplex();
1169
1/2
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
 4445 support_hint = gjk.support_hint;
1170
1171 // TODO(louis): we might want to start with a hexahedron if the
1172 // simplex given by GJK is of rank <= 3.
1173
1/2
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
 4445 bool enclosed_origin = gjk.encloseOrigin();
1174
2/4
✓ Branch 0 taken 4445 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 4445 times.
✗ Branch 3 not taken.
 4445 if ((simplex.rank > 1) && enclosed_origin) {
1175
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4445 times.
 4445 assert(simplex.rank == 4 &&
1176 "When starting EPA, simplex should be of rank 4.");
1177
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4445 times.
 4445 while (hull.root) {
1178 ✗ SimplexFace* f = hull.root;
1179 ✗ hull.remove(f);
1180 ✗ stock.append(f);
1181 }
1182
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4445 times.
 4445 assert(hull.count == 0);
1183
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4445 times.
 4445 assert(stock.count == fc_store.size());
1184
1185 4445 status = Valid;
1186 4445 num_vertices = 0;
1187
1188 // Make sure the tetrahedron has its normals pointing outside.
1189
1/2
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
 4445 if ((simplex.vertex[0]->w - simplex.vertex[3]->w)
1190
2/4
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4445 times.
✗ Branch 5 not taken.
 8890 .dot((simplex.vertex[1]->w - simplex.vertex[3]->w)
1191
4/6
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4445 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 140 times.
✓ Branch 7 taken 4305 times.
 8890 .cross(simplex.vertex[2]->w - simplex.vertex[3]->w)) < 0) {
1192 140 SimplexVertex* tmp = simplex.vertex[0];
1193 140 simplex.vertex[0] = simplex.vertex[1];
1194 140 simplex.vertex[1] = tmp;
1195 }
1196
1197 // Add the 4 vertices to sv_store
1198
2/2
✓ Branch 0 taken 17780 times.
✓ Branch 1 taken 4445 times.
 22225 for (size_t i = 0; i < 4; ++i) {
1199
1/2
✓ Branch 2 taken 17780 times.
✗ Branch 3 not taken.
 17780 sv_store[num_vertices++] = *simplex.vertex[i];
1200 }
1201
1202
1/2
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
 4445 SimplexFace* tetrahedron[] = {newFace(0, 1, 2, true), //
1203 8890 newFace(1, 0, 3, true), //
1204 8890 newFace(2, 1, 3, true), //
1205
3/6
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4445 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4445 times.
✗ Branch 8 not taken.
 4445 newFace(0, 2, 3, true)};
1206
1207
1/2
✓ Branch 0 taken 4445 times.
✗ Branch 1 not taken.
 4445 if (hull.count == 4) {
1208 // set the face connectivity
1209 4445 bind(tetrahedron[0], 0, tetrahedron[1], 0);
1210 4445 bind(tetrahedron[0], 1, tetrahedron[2], 0);
1211 4445 bind(tetrahedron[0], 2, tetrahedron[3], 0);
1212 4445 bind(tetrahedron[1], 1, tetrahedron[3], 2);
1213 4445 bind(tetrahedron[1], 2, tetrahedron[2], 1);
1214 4445 bind(tetrahedron[2], 2, tetrahedron[3], 1);
1215
1216 4445 closest_face =
1217
1/2
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
 4445 findClosestFace(); // find the best face (the face with the
1218 // minimum distance to origin) to split
1219
1/2
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
 4445 SimplexFace outer = *closest_face;
1220
1221 4445 status = Valid;
1222 4445 iterations = 0;
1223 4445 size_t pass = 0;
1224
1/2
✓ Branch 0 taken 124007 times.
✗ Branch 1 not taken.
 124007 for (; iterations < max_iterations; ++iterations) {
1225
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 124007 times.
 124007 if (num_vertices >= sv_store.size()) {
1226 ✗ status = OutOfVertices;
1227 4445 break;
1228 }
1229
1230 // Step 1: find the support point in the direction of the closest_face
1231 // normal.
1232 // --------------------------------------------------------------------------
1233 124007 SimplexHorizon horizon;
1234 124007 SimplexVertex& w = sv_store[num_vertices++];
1235 124007 bool valid = true;
1236 124007 closest_face->pass = ++pass;
1237 // At the moment, SimplexF.n is always normalized. This could be revised
1238 // in the future...
1239
1/2
✓ Branch 1 taken 124007 times.
✗ Branch 2 not taken.
 124007 gjk.getSupport(closest_face->n, w, support_hint);
1240
1241 // Step 2: check for convergence.
1242 // ------------------------------
1243 // Preambule to understand the convergence criterion of EPA:
1244 // the support we just added is in the direction of the normal of
1245 // the closest_face. Therefore, the support point will **always**
1246 // lie "after" the closest_face, i.e closest_face.n.dot(w.w) > 0.
1247
2/2
✓ Branch 0 taken 119562 times.
✓ Branch 1 taken 4445 times.
 124007 if (iterations > 0) {
1248
2/4
✓ Branch 1 taken 119562 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 119562 times.
✗ Branch 4 not taken.
 119562 assert(closest_face->n.dot(w.w) > -tolerance &&
1249 "The support is not in the right direction.");
1250 }
1251 //
1252 // 1) First check: `fdist` (see below) is an upper bound of how much
1253 // more penetration depth we can expect to "gain" by adding `w` to EPA's
1254 // polytope. This first check, as any convergence check, should be both
1255 // absolute and relative. This allows to adapt the tolerance to the
1256 // scale of the objects.
1257 124007 const SimplexVertex& vf1 = sv_store[closest_face->vertex_id[0]];
1258 124007 const SimplexVertex& vf2 = sv_store[closest_face->vertex_id[1]];
1259 124007 const SimplexVertex& vf3 = sv_store[closest_face->vertex_id[2]];
1260
2/4
✓ Branch 1 taken 124007 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 124007 times.
✗ Branch 5 not taken.
 124007 SolverScalar fdist = closest_face->n.dot(w.w - vf1.w);
1261
1/2
✓ Branch 1 taken 124007 times.
✗ Branch 2 not taken.
 124007 SolverScalar wnorm = w.w.norm();
1262 // TODO(louis): we might want to use tol_abs and tol_rel; this might
1263 // obfuscate the code for the user though.
1264
2/2
✓ Branch 0 taken 4432 times.
✓ Branch 1 taken 119575 times.
 124007 if (fdist <= tolerance + tolerance * wnorm) {
1265 4432 status = AccuracyReached;
1266 4432 break;
1267 }
1268 // 2) Second check: the expand function **assumes** that the support we
1269 // just computed is not a vertex of the face. We make sure that this
1270 // is the case:
1271 // TODO(louis): should we use squaredNorm everywhere instead of norm?
1272
2/4
✓ Branch 1 taken 119575 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 119575 times.
✗ Branch 5 not taken.
 119575 if ((w.w - vf1.w).norm() <= tolerance + tolerance * wnorm ||
1273
4/8
✓ Branch 0 taken 119575 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 119575 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 119575 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 119575 times.
✗ Branch 9 not taken.
 239150 (w.w - vf2.w).norm() <= tolerance + tolerance * wnorm ||
1274
4/8
✓ Branch 1 taken 119575 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 119575 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 119575 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 119575 times.
 239150 (w.w - vf3.w).norm() <= tolerance + tolerance * wnorm) {
1275 ✗ status = AccuracyReached;
1276 ✗ break;
1277 }
1278
1279 // Step 3: expand the polytope
1280 // ---------------------------
1281
4/4
✓ Branch 0 taken 358725 times.
✓ Branch 1 taken 119566 times.
✓ Branch 2 taken 358716 times.
✓ Branch 3 taken 9 times.
 478291 for (size_t j = 0; (j < 3) && valid; ++j)
1282 358716 valid &= expand(pass, w, closest_face->adjacent_faces[j],
1283
1/2
✓ Branch 1 taken 358716 times.
✗ Branch 2 not taken.
 358716 closest_face->adjacent_edge[j], horizon);
1284
1285
3/4
✓ Branch 0 taken 119562 times.
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 119562 times.
 119575 if (!valid || horizon.num_faces < 3) {
1286 // The status has already been set by the expand function.
1287
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 13 times.
 13 assert(!(status & Valid));
1288 13 break;
1289 }
1290 // need to add the edge connectivity between first and last faces
1291 119562 bind(horizon.first_face, 2, horizon.current_face, 1);
1292 119562 hull.remove(closest_face);
1293 119562 stock.append(closest_face);
1294
1/2
✓ Branch 1 taken 119562 times.
✗ Branch 2 not taken.
 119562 closest_face = findClosestFace();
1295
1/2
✓ Branch 1 taken 119562 times.
✗ Branch 2 not taken.
 119562 outer = *closest_face;
1296 }
1297
1298
1/2
✓ Branch 0 taken 4445 times.
✗ Branch 1 not taken.
 4445 status = ((iterations) < max_iterations) ? status : Failed;
1299
1/2
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
 4445 normal = outer.n;
1300
1/2
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
 4445 depth = outer.d + gjk.shape->swept_sphere_radius.sum();
1301 4445 result.rank = 3;
1302 4445 result.vertex[0] = &sv_store[outer.vertex_id[0]];
1303 4445 result.vertex[1] = &sv_store[outer.vertex_id[1]];
1304 4445 result.vertex[2] = &sv_store[outer.vertex_id[2]];
1305 4445 return status;
1306 }
1307 ✗ assert(false && "The tetrahedron with which EPA started is degenerated.");
1308 }
1309
1310 // FallBack when the simplex given by GJK is of rank 1.
1311 // Since the simplex only contains support points which convex
1312 // combination describe the origin, the point in the simplex is actually
1313 // the origin.
1314 ✗ status = FallBack;
1315 // TODO: define a better normal
1316 ✗ assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance()));
1317 ✗ normal = -guess;
1318 ✗ SolverScalar nl = normal.norm();
1319 ✗ if (nl > 0)
1320 ✗ normal /= nl;
1321 else
1322 ✗ normal = Vec3ps(1, 0, 0);
1323 ✗ depth = 0;
1324 ✗ result.rank = 1;
1325 ✗ result.vertex[0] = simplex.vertex[0];
1326 ✗ return status;
1327 }
1328
1329 // Use this function to debug `EPA::expand` if needed.
1330 // void EPA::PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w) {
1331 // std::cout << "Vertices:\n";
1332 // for (size_t i = 0; i < num_vertices; ++i) {
1333 // std::cout << "[";
1334 // std::cout << sv_store[i].w(0) << ", ";
1335 // std::cout << sv_store[i].w(1) << ", ";
1336 // std::cout << sv_store[i].w(2) << "]\n";
1337 // }
1338 // //
1339 // std::cout << "\nTriangles:\n";
1340 // SimplexFace* face = hull.root;
1341 // for (size_t i = 0; i < hull.count; ++i) {
1342 // std::cout << "[";
1343 // std::cout << face->vertex_id[0] << ", ";
1344 // std::cout << face->vertex_id[1] << ", ";
1345 // std::cout << face->vertex_id[2] << "]\n";
1346 // face = face->next_face;
1347 // }
1348 // //
1349 // std::cout << "\nNormals:\n";
1350 // face = hull.root;
1351 // for (size_t i = 0; i < hull.count; ++i) {
1352 // std::cout << "[";
1353 // std::cout << face->n(0) << ", ";
1354 // std::cout << face->n(1) << ", ";
1355 // std::cout << face->n(2) << "]\n";
1356 // face = face->next_face;
1357 // }
1358 // //
1359 // std::cout << "\nClosest face:\n";
1360 // face = hull.root;
1361 // for (size_t i = 0; i < hull.count; ++i) {
1362 // if (face == closest_face) {
1363 // std::cout << i << "\n";
1364 // }
1365 // face = face->next_face;
1366 // }
1367 // std::cout << "\nSupport point:\n";
1368 // std::cout << "[" << w.w(0) << ", " << w.w(1) << ", " << w.w(2) << "]\n";
1369 // }
1370
1371 /** @brief the goal is to add a face connecting vertex w and face edge f[e] */
1372 737422 bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
1373 SimplexHorizon& horizon) {
1374 static const size_t nexti[] = {1, 2, 0};
1375 static const size_t previ[] = {2, 0, 1};
1376 737422 const size_t id_w =
1377 737422 num_vertices - 1; // w is always the last vertex added to sv_store
1378
1379 // Check if we loop through expand indefinitely.
1380
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 737422 times.
 737422 if (f->pass == pass) {
1381 // Uncomment the following line and the associated EPA method
1382 // to debug the infinite loop if needed.
1383 // EPAPrintExpandLooping(this, f);
1384 ✗ assert(f != closest_face && "EPA is looping indefinitely.");
1385 ✗ status = InvalidHull;
1386 ✗ return false;
1387 }
1388
1389 737422 const size_t e1 = nexti[e];
1390
1391 // Preambule: when expanding the polytope, the `closest_face` is always
1392 // deleted. This is handled in EPA::evaluate after calling the expand
1393 // function. This function handles how the neighboring face `f` of the
1394 // `closest_face` is connected to the new support point. (Note: because
1395 // `expand` is recursive, `f` can also denote a face of a face of the
1396 // `closest_face`, and so on. But the reasoning is the same.)
1397 //
1398 // EPA can handle `f` in two ways, depending on where the new support point
1399 // is located:
1400 // 1) If it is "below" `f`, then `f` is preserved. A new face is created
1401 // and connects to the edge `e` of `f`. This new face is made of the
1402 // two points of the edge `e` of `f` and the new support point `w`.
1403 // Geometrically, this corresponds to the case where the projection of
1404 // the origin on the `closest_face` is **inside** the triangle defined by
1405 // the `closest_face`.
1406 // 2) If it is "above" `f`, then `f` has to be deleted, simply because the
1407 // edge `e` of `f` is not part of the convex hull anymore.
1408 // The two faces adjacent to `f` are thus expanded following
1409 // either 1) or 2).
1410 // Geometrically, this corresponds to the case where the projection of
1411 // the origin on the `closest_face` is on an edge of the triangle defined
1412 // by the `closest_face`. The projection of the origin cannot lie on a
1413 // vertex of the `closest_face` because EPA should have exited before
1414 // reaching this point.
1415 //
1416 // The following checks for these two cases.
1417 // This check is however subject to numerical precision and due to the
1418 // recursive nature of `expand`, it is safer to go through the first case.
1419 // This is because `expand` can potentially loop indefinitly if the
1420 // Minkowski difference is very flat (hence the check above).
1421 737422 const SolverScalar dummy_precision(
1422 3 * std::sqrt(std::numeric_limits<SolverScalar>::epsilon()));
1423 737422 const SimplexVertex& vf = sv_store[f->vertex_id[e]];
1424
4/6
✓ Branch 1 taken 737422 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 737422 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 548068 times.
✓ Branch 7 taken 189354 times.
 737422 if (f->n.dot(w.w - vf.w) < dummy_precision) {
1425 // case 1: the support point is "below" `f`.
1426 548068 SimplexFace* new_face = newFace(f->vertex_id[e1], f->vertex_id[e], id_w);
1427
2/2
✓ Branch 0 taken 548055 times.
✓ Branch 1 taken 13 times.
 548068 if (new_face != nullptr) {
1428 // add face-face connectivity
1429 548055 bind(new_face, 0, f, e);
1430
1431 // if there is last face in the horizon, then need to add another
1432 // connectivity, i.e. the edge connecting the current new add edge and the
1433 // last new add edge. This does not finish all the connectivities because
1434 // the final face need to connect with the first face, this will be
1435 // handled in the evaluate function. Notice the face is anti-clockwise, so
1436 // the edges are 0 (bottom), 1 (right), 2 (left)
1437
2/2
✓ Branch 0 taken 428480 times.
✓ Branch 1 taken 119575 times.
 548055 if (horizon.current_face != nullptr) {
1438 428480 bind(new_face, 2, horizon.current_face, 1);
1439 } else {
1440 119575 horizon.first_face = new_face;
1441 }
1442
1443 548055 horizon.current_face = new_face;
1444 548055 ++horizon.num_faces;
1445 548055 return true;
1446 }
1447 13 return false;
1448 }
1449
1450 // case 2: the support point is "above" `f`.
1451 189354 const size_t e2 = previ[e];
1452 189354 f->pass = pass;
1453
4/4
✓ Branch 1 taken 189352 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 189343 times.
✓ Branch 4 taken 11 times.
 378706 if (expand(pass, w, f->adjacent_faces[e1], f->adjacent_edge[e1], horizon) &&
1454
2/2
✓ Branch 1 taken 189343 times.
✓ Branch 2 taken 9 times.
 189352 expand(pass, w, f->adjacent_faces[e2], f->adjacent_edge[e2], horizon)) {
1455 189343 hull.remove(f);
1456 189343 stock.append(f);
1457 189343 return true;
1458 }
1459 11 return false;
1460 }
1461
1462 4445 void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0,
1463 Vec3ps& w1, Vec3ps& normal) const {
1464 4445 details::getClosestPoints(result, w0, w1);
1465
4/6
✓ Branch 1 taken 4445 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4445 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4435 times.
✓ Branch 8 taken 10 times.
 4445 if ((w0 - w1).norm() > Eigen::NumTraits<SolverScalar>::dummy_precision()) {
1466
1/2
✓ Branch 0 taken 4435 times.
✗ Branch 1 not taken.
 4435 if (this->depth >= 0) {
1467 // The shapes are in collision.
1468
3/6
✓ Branch 1 taken 4435 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4435 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4435 times.
✗ Branch 8 not taken.
 4435 normal = (w0 - w1).normalized();
1469 } else {
1470 // The shapes are not in collision.
1471 ✗ normal = (w1 - w0).normalized();
1472 }
1473 } else {
1474 10 normal = this->normal;
1475 }
1476 4445 details::inflate<false>(shape, normal, w0, w1);
1477 4445 }
1478
1479 } // namespace details
1480
1481 template <typename IndexType>
1482 122634 void ConvexBaseTpl<IndexType>::buildSupportWarmStart() {
1483 typedef ConvexBaseTpl<IndexType> ConvexBaseType;
1484
1/2
✓ Branch 2 taken 61317 times.
✗ Branch 3 not taken.
 122634 if (this->points->size() <
1485 ConvexBaseType::num_vertices_large_convex_threshold) {
1486 122634 return;
1487 }
1488
1489 ✗ this->support_warm_starts.points.reserve(
1490 ConvexBaseType::num_support_warm_starts);
1491 ✗ this->support_warm_starts.indices.reserve(
1492 ConvexBaseType::num_support_warm_starts);
1493
1494 ✗ Vec3s axiis(0, 0, 0);
1495 ✗ details::ShapeSupportData support_data;
1496 ✗ int support_hint = 0;
1497 ✗ for (int i = 0; i < 3; ++i) {
1498 ✗ axiis(i) = 1;
1499 {
1500 ✗ Vec3s support;
1501 ✗ coal::details::getShapeSupport<false, IndexType>(
1502 this, axiis, support, support_hint, support_data);
1503 ✗ this->support_warm_starts.points.emplace_back(support);
1504 ✗ this->support_warm_starts.indices.emplace_back(support_hint);
1505 }
1506
1507 ✗ axiis(i) = -1;
1508 {
1509 ✗ Vec3s support;
1510 ✗ coal::details::getShapeSupport<false, IndexType>(
1511 this, axiis, support, support_hint, support_data);
1512 ✗ this->support_warm_starts.points.emplace_back(support);
1513 ✗ this->support_warm_starts.indices.emplace_back(support_hint);
1514 }
1515
1516 ✗ axiis(i) = 0;
1517 }
1518
1519 ✗ std::array<Vec3s, 4> eis = {Vec3s(1, 1, 1), //
1520 Vec3s(-1, 1, 1), //
1521 Vec3s(-1, -1, 1), //
1522 Vec3s(1, -1, 1)};
1523
1524 ✗ for (size_t ei_index = 0; ei_index < 4; ++ei_index) {
1525 {
1526 ✗ Vec3s support;
1527 ✗ coal::details::getShapeSupport<false, IndexType>(
1528 this, eis[ei_index], support, support_hint, support_data);
1529 ✗ this->support_warm_starts.points.emplace_back(support);
1530 ✗ this->support_warm_starts.indices.emplace_back(support_hint);
1531 }
1532
1533 {
1534 ✗ Vec3s support;
1535 ✗ coal::details::getShapeSupport<false, IndexType>(
1536 ✗ this, -eis[ei_index], support, support_hint, support_data);
1537 ✗ this->support_warm_starts.points.emplace_back(support);
1538 ✗ this->support_warm_starts.indices.emplace_back(support_hint);
1539 }
1540 }
1541
1542 ✗ if (this->support_warm_starts.points.size() !=
1543 ✗ ConvexBaseType::num_support_warm_starts ||
1544 ✗ this->support_warm_starts.indices.size() !=
1545 ConvexBaseType::num_support_warm_starts) {
1546 ✗ COAL_THROW_PRETTY("Wrong number of support warm starts.",
1547 std::runtime_error);
1548 }
1549 ✗ }
1550
1551 template void COAL_DLLAPI
1552 ConvexBaseTpl<Triangle16::IndexType>::buildSupportWarmStart();
1553 template void COAL_DLLAPI
1554 ConvexBaseTpl<Triangle32::IndexType>::buildSupportWarmStart();
1555
1556 } // namespace coal
1557