Directory: | ./ |
---|---|
File: | src/narrowphase/gjk.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 686 | 793 | 86.5% |
Branches: | 659 | 1479 | 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 | 30476103 | void GJK::initialize() { | |
53 | 30476103 | distance_upper_bound = (std::numeric_limits<SolverScalar>::max)(); | |
54 | 30476103 | gjk_variant = GJKVariant::DefaultGJK; | |
55 | 30476103 | convergence_criterion = GJKConvergenceCriterion::Default; | |
56 | 30476103 | convergence_criterion_type = GJKConvergenceCriterionType::Relative; | |
57 | 30476103 | reset(max_iterations, tolerance); | |
58 | 30476103 | } | |
59 | |||
60 | 60476169 | void GJK::reset(size_t max_iterations_, SolverScalar tolerance_) { | |
61 | 60476169 | max_iterations = max_iterations_; | |
62 | 60476169 | tolerance = tolerance_; | |
63 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 60476169 times.
|
60476169 | COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", |
64 | std::invalid_argument); | ||
65 | 60476169 | status = DidNotRun; | |
66 | 60476169 | nfree = 0; | |
67 | 60476169 | simplex = nullptr; | |
68 | 60476169 | iterations = 0; | |
69 | 60476169 | iterations_momentum_stop = 0; | |
70 | 60476169 | } | |
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 | 29999865 | void getClosestPoints(const GJK::Simplex& simplex, Vec3ps& w0, Vec3ps& w1) { | |
97 | 29999865 | GJK::SimplexV* const* vs = simplex.vertex; | |
98 | |||
99 |
2/2✓ Branch 0 taken 38306365 times.
✓ Branch 1 taken 29999865 times.
|
68306230 | for (GJK::vertex_id_t i = 0; i < simplex.rank; ++i) { |
100 |
3/6✓ Branch 2 taken 38306365 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38306365 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 38306365 times.
|
38306365 | assert(vs[i]->w.isApprox(vs[i]->w0 - vs[i]->w1)); |
101 | } | ||
102 | |||
103 | 29999865 | Project<SolverScalar>::ProjectResult projection; | |
104 |
4/5✓ Branch 0 taken 22111879 times.
✓ Branch 1 taken 7490581 times.
✓ Branch 2 taken 376296 times.
✓ Branch 3 taken 21109 times.
✗ Branch 4 not taken.
|
29999865 | 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 | 7490581 | case 2: { | |
110 |
3/6✓ Branch 1 taken 7490581 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490581 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7490581 times.
✗ Branch 8 not taken.
|
7490581 | const Vec3ps &a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1, b = vs[1]->w, |
111 |
2/4✓ Branch 1 taken 7490581 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490581 times.
✗ Branch 5 not taken.
|
7490581 | b0 = vs[1]->w0, b1 = vs[1]->w1; |
112 | SolverScalar la, lb; | ||
113 |
2/4✓ Branch 1 taken 7490581 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490581 times.
✗ Branch 5 not taken.
|
7490581 | Vec3ps N(b - a); |
114 |
2/4✓ Branch 1 taken 7490581 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490581 times.
✗ Branch 5 not taken.
|
7490581 | la = N.dot(-a); |
115 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7490581 times.
|
7490581 | if (la <= 0) { |
116 | ✗ | assert(false); | |
117 | w0 = a0; | ||
118 | w1 = a1; | ||
119 | } else { | ||
120 |
1/2✓ Branch 1 taken 7490581 times.
✗ Branch 2 not taken.
|
7490581 | lb = N.squaredNorm(); |
121 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7490581 times.
|
7490581 | if (la > lb) { |
122 | ✗ | assert(false); | |
123 | w0 = b0; | ||
124 | w1 = b1; | ||
125 | } else { | ||
126 | 7490581 | lb = la / lb; | |
127 | 7490581 | la = 1 - lb; | |
128 |
4/8✓ Branch 1 taken 7490581 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490581 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7490581 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7490581 times.
✗ Branch 11 not taken.
|
7490581 | w0 = la * a0 + lb * b0; |
129 |
4/8✓ Branch 1 taken 7490581 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7490581 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7490581 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7490581 times.
✗ Branch 11 not taken.
|
7490581 | w1 = la * a1 + lb * b1; |
130 | } | ||
131 | } | ||
132 | } | ||
133 | 7490581 | return; | |
134 | 376296 | case 3: | |
135 | // TODO avoid the reprojection | ||
136 | 752592 | 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 | 42218 | 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 | 59999730 | void inflate(const MinkowskiDiff& shape, const Vec3ps& normal, Vec3ps& w0, | |
163 | Vec3ps& w1) { | ||
164 | #ifndef NDEBUG | ||
165 | 59999730 | const SolverScalar dummy_precision = | |
166 | Eigen::NumTraits<SolverScalar>::dummy_precision(); | ||
167 |
2/4✓ Branch 1 taken 29999865 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 29999865 times.
|
59999730 | assert((normal.norm() - 1) < dummy_precision); |
168 | #endif | ||
169 | |||
170 | const Eigen::Array<SolverScalar, 1, 2>& I( | ||
171 |
1/2✓ Branch 1 taken 29999865 times.
✗ Branch 2 not taken.
|
59999730 | shape.swept_sphere_radius.cast<SolverScalar>()); |
172 |
2/4✓ Branch 1 taken 29999865 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29999865 times.
✗ Branch 5 not taken.
|
59999730 | Eigen::Array<bool, 1, 2> inflate(I > 0); |
173 |
3/4✓ Branch 1 taken 29999865 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29935166 times.
✓ Branch 4 taken 64699 times.
|
59999730 | 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 | 29995420 | void GJK::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0, | |
182 | Vec3ps& w1, Vec3ps& normal) const { | ||
183 | 29995420 | details::getClosestPoints(*simplex, w0, w1); | |
184 |
3/4✓ Branch 2 taken 29995420 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29977737 times.
✓ Branch 6 taken 17683 times.
|
29995420 | if ((w1 - w0).norm() > Eigen::NumTraits<SolverScalar>::dummy_precision()) { |
185 |
2/4✓ Branch 2 taken 29977737 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29977737 times.
✗ Branch 6 not taken.
|
29977737 | normal = (w1 - w0).normalized(); |
186 | } else { | ||
187 |
2/4✓ Branch 2 taken 17683 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 17683 times.
✗ Branch 6 not taken.
|
17683 | normal = -this->ray.normalized(); |
188 | } | ||
189 | 29995420 | details::inflate<true>(shape, normal, w0, w1); | |
190 | 29995420 | } | |
191 | |||
192 | 30198154 | 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 | 30198154 | SolverScalar alpha = 0; | |
196 | 30198154 | iterations = 0; | |
197 |
1/2✓ Branch 1 taken 30198154 times.
✗ Branch 2 not taken.
|
30198154 | const SolverScalar swept_sphere_radius = shape_.swept_sphere_radius.sum(); |
198 | 30198154 | const SolverScalar upper_bound = distance_upper_bound + swept_sphere_radius; | |
199 | |||
200 | 30198154 | free_v[0] = &store_v[0]; | |
201 | 30198154 | free_v[1] = &store_v[1]; | |
202 | 30198154 | free_v[2] = &store_v[2]; | |
203 | 30198154 | free_v[3] = &store_v[3]; | |
204 | |||
205 | 30198154 | nfree = 4; | |
206 | 30198154 | status = NoCollision; | |
207 | 30198154 | shape = &shape_; | |
208 | 30198154 | distance = 0.0; | |
209 | 30198154 | current = 0; | |
210 | 30198154 | simplices[current].rank = 0; | |
211 |
1/2✓ Branch 1 taken 30198154 times.
✗ Branch 2 not taken.
|
30198154 | support_hint = supportHint; |
212 | |||
213 |
1/2✓ Branch 1 taken 30198154 times.
✗ Branch 2 not taken.
|
30198154 | SolverScalar rl = guess.norm(); |
214 |
2/2✓ Branch 0 taken 6 times.
✓ Branch 1 taken 30198148 times.
|
30198154 | 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 30198148 times.
✗ Branch 2 not taken.
|
30198148 | ray = guess; |
219 | |||
220 | // Momentum | ||
221 | 30198154 | GJKVariant current_gjk_variant = gjk_variant; | |
222 |
1/2✓ Branch 1 taken 30198154 times.
✗ Branch 2 not taken.
|
30198154 | Vec3ps w = ray; |
223 |
1/2✓ Branch 1 taken 30198154 times.
✗ Branch 2 not taken.
|
30198154 | Vec3ps dir = ray; |
224 |
1/2✓ Branch 1 taken 30198154 times.
✗ Branch 2 not taken.
|
30198154 | Vec3ps y; |
225 | SolverScalar momentum; | ||
226 | 30198154 | bool normalize_support_direction = shape->normalize_support_direction; | |
227 | do { | ||
228 | 72327371 | vertex_id_t next = (vertex_id_t)(1 - current); | |
229 | 72327371 | Simplex& curr_simplex = simplices[current]; | |
230 | 72327371 | 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 72327183 times.
|
72327371 | 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 | 30198154 | break; | |
248 | } | ||
249 | |||
250 | // Compute direction for support call | ||
251 |
3/4✓ Branch 0 taken 71571889 times.
✓ Branch 1 taken 426502 times.
✓ Branch 2 taken 328792 times.
✗ Branch 3 not taken.
|
72327183 | switch (current_gjk_variant) { |
252 | 71571889 | case DefaultGJK: | |
253 |
1/2✓ Branch 1 taken 71571889 times.
✗ Branch 2 not taken.
|
71571889 | dir = ray; |
254 | 71571889 | 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 72327183 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72327183 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 72327183 times.
✗ Branch 8 not taken.
|
72327183 | 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 72327183 times.
✗ Branch 2 not taken.
|
72327183 | w = curr_simplex.vertex[curr_simplex.rank - 1]->w; |
293 | |||
294 | // check B: no collision if omega > 0 | ||
295 |
2/4✓ Branch 1 taken 72327183 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72327183 times.
✗ Branch 5 not taken.
|
72327183 | SolverScalar omega = dir.dot(w) / dir.norm(); |
296 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 72327181 times.
|
72327183 | 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 71571887 times.
|
72327181 | 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 72218487 times.
✗ Branch 2 not taken.
|
72218487 | bool cv_check_passed = checkConvergence(w, rl, alpha, omega); |
316 |
4/4✓ Branch 0 taken 42020334 times.
✓ Branch 1 taken 30198153 times.
✓ Branch 2 taken 30181397 times.
✓ Branch 3 taken 11838937 times.
|
72218487 | if (iterations > 0 && cv_check_passed) { |
317 |
1/2✓ Branch 0 taken 30181397 times.
✗ Branch 1 not taken.
|
30181397 | if (iterations > 0) removeVertex(simplices[current]); |
318 |
2/2✓ Branch 0 taken 21604 times.
✓ Branch 1 taken 30159793 times.
|
30181397 | 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 | 30159793 | distance = rl - swept_sphere_radius; | |
327 |
2/2✓ Branch 0 taken 16565 times.
✓ Branch 1 taken 30143228 times.
|
30159793 | if (distance < tolerance) { |
328 | 16565 | status = CollisionWithPenetrationInformation; | |
329 | } else { | ||
330 | 30143228 | status = NoCollision; | |
331 | } | ||
332 | 30159793 | 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 30198153 times.
✓ Branch 1 taken 10002056 times.
✓ Branch 2 taken 1547917 times.
✓ Branch 3 taken 288964 times.
✗ Branch 4 not taken.
|
42037090 | switch (curr_simplex.rank) { |
339 | 30198153 | case 1: // Only at the first iteration | |
340 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 30198153 times.
|
30198153 | assert(iterations == 0); |
341 |
1/2✓ Branch 1 taken 30198153 times.
✗ Branch 2 not taken.
|
30198153 | ray = w; |
342 | 30198153 | inside = false; | |
343 | 30198153 | next_simplex.rank = 1; | |
344 | 30198153 | next_simplex.vertex[0] = curr_simplex.vertex[0]; | |
345 | 30198153 | break; | |
346 | 10002056 | case 2: | |
347 |
1/2✓ Branch 1 taken 10002056 times.
✗ Branch 2 not taken.
|
10002056 | inside = projectLineOrigin(curr_simplex, next_simplex); |
348 | 10002056 | break; | |
349 | 1547917 | case 3: | |
350 |
1/2✓ Branch 1 taken 1547917 times.
✗ Branch 2 not taken.
|
1547917 | inside = projectTriangleOrigin(curr_simplex, next_simplex); |
351 | 1547917 | 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 42037090 times.
|
42037090 | assert(nfree + next_simplex.rank == 4); |
359 | 42037090 | current = next; | |
360 |
1/2✓ Branch 1 taken 42037090 times.
✗ Branch 2 not taken.
|
42037090 | rl = ray.norm(); |
361 |
4/4✓ Branch 0 taken 41999017 times.
✓ Branch 1 taken 38073 times.
✓ Branch 2 taken 98 times.
✓ Branch 3 taken 41998919 times.
|
42037090 | 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 41998919 times.
✗ Branch 1 not taken.
|
41998919 | status = ((++iterations) < max_iterations) ? status : Failed; |
371 | |||
372 |
1/2✓ Branch 0 taken 42129217 times.
✗ Branch 1 not taken.
|
42129217 | } while (status == NoCollision); |
373 | |||
374 | 30198154 | simplex = &simplices[current]; | |
375 |
2/4✓ Branch 0 taken 30198154 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 30198154 times.
✗ Branch 3 not taken.
|
30198154 | assert(simplex->rank > 0 && simplex->rank < 5); |
376 | 30198154 | return status; | |
377 | } | ||
378 | |||
379 | 72218487 | 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 72184483 times.
✓ Branch 1 taken 17002 times.
✓ Branch 2 taken 17002 times.
✗ Branch 3 not taken.
|
72218487 | switch (convergence_criterion) { |
388 | 72184483 | case Default: { | |
389 | // alpha is the distance to the best separating hyperplane found so far | ||
390 | 72184483 | alpha = std::max(alpha, omega); | |
391 | // ||x^*|| - ||x^k|| <= diff | ||
392 | 72184483 | const SolverScalar diff = rl - alpha; | |
393 | 72184483 | return ((diff - (tolerance + tolerance * rl)) <= 0); | |
394 | } break; | ||
395 | |||
396 | 17002 | case DualityGap: { | |
397 | // ||x^* - x^k||^2 <= diff | ||
398 |
1/2✓ Branch 2 taken 17002 times.
✗ Branch 3 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 | 30290101 | inline void GJK::removeVertex(Simplex& simplex) { | |
436 | 30290101 | free_v[nfree++] = simplex.vertex[--simplex.rank]; | |
437 | 30290101 | } | |
438 | |||
439 | 72327391 | inline void GJK::appendVertex(Simplex& simplex, const Vec3ps& v, | |
440 | support_func_guess_t& hint) { | ||
441 | 72327391 | simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory | |
442 | 72327391 | getSupport(v, *simplex.vertex[simplex.rank++], hint); | |
443 | 72327391 | } | |
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 | 2009726 | 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 | 2009726 | ray = A; | |
506 | 2009726 | next.vertex[0] = current.vertex[a]; | |
507 | 2009726 | next.rank = 1; | |
508 | 2009726 | } | |
509 | |||
510 | 9102602 | 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 |
4/8✓ Branch 2 taken 9102602 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9102602 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9102602 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9102602 times.
✗ Branch 12 not taken.
|
9102602 | ray = AB.dot(B) * A + ABdotAO * B; |
517 | |||
518 | 9102602 | next.vertex[0] = current.vertex[b]; | |
519 | 9102602 | next.vertex[1] = current.vertex[a]; | |
520 | 9102602 | next.rank = 2; | |
521 | |||
522 | // To ensure backward compatibility | ||
523 |
1/2✓ Branch 2 taken 9102602 times.
✗ Branch 3 not taken.
|
9102602 | ray /= AB.squaredNorm(); |
524 | 9102602 | } | |
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 453683 times.
✓ Branch 1 taken 234840 times.
|
688523 | if (ABCdotAO > 0) { // Above triangle |
540 | 453683 | next.vertex[0] = current.vertex[c]; | |
541 | 453683 | next.vertex[1] = current.vertex[b]; | |
542 | } else { | ||
543 | 234840 | next.vertex[0] = current.vertex[b]; | |
544 | 234840 | next.vertex[1] = current.vertex[c]; | |
545 | } | ||
546 | |||
547 | // To ensure backward compatibility | ||
548 |
2/4✓ Branch 2 taken 688523 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 688523 times.
✗ Branch 6 not taken.
|
688523 | ray = -ABCdotAO / ABC.squaredNorm() * ABC; |
549 | 688523 | return false; | |
550 | } | ||
551 | |||
552 | 10002056 | bool GJK::projectLineOrigin(const Simplex& current, Simplex& next) { | |
553 | 10002056 | const vertex_id_t a = 1, b = 0; | |
554 | // A is the last point we added. | ||
555 | 10002056 | const Vec3ps& A = current.vertex[a]->w; | |
556 | 10002056 | const Vec3ps& B = current.vertex[b]->w; | |
557 | |||
558 |
2/4✓ Branch 1 taken 10002056 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10002056 times.
✗ Branch 5 not taken.
|
10002056 | const Vec3ps AB = B - A; |
559 |
2/4✓ Branch 1 taken 10002056 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10002056 times.
✗ Branch 5 not taken.
|
10002056 | const SolverScalar d = AB.dot(-A); |
560 |
2/4✓ Branch 1 taken 10002056 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 10002056 times.
|
10002056 | assert(d <= AB.squaredNorm()); |
561 | |||
562 |
2/2✓ Branch 0 taken 5509 times.
✓ Branch 1 taken 9996547 times.
|
10002056 | 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 1864228 times.
✓ Branch 1 taken 8132319 times.
|
9996547 | } else if (d < 0) { |
572 | // A is the closest to the origin | ||
573 |
1/2✓ Branch 1 taken 1864228 times.
✗ Branch 2 not taken.
|
1864228 | originToPoint(current, a, A, next, ray); |
574 | 1864228 | free_v[nfree++] = current.vertex[b]; | |
575 | } else | ||
576 |
1/2✓ Branch 1 taken 8132319 times.
✗ Branch 2 not taken.
|
8132319 | originToSegment(current, a, b, A, B, AB, d, next, ray); |
577 | 9996547 | return false; | |
578 | } | ||
579 | |||
580 | 1547917 | bool GJK::projectTriangleOrigin(const Simplex& current, Simplex& next) { | |
581 | 1547917 | const vertex_id_t a = 2, b = 1, c = 0; | |
582 | // A is the last point we added. | ||
583 |
1/2✓ Branch 1 taken 1547917 times.
✗ Branch 2 not taken.
|
1547917 | const Vec3ps &A = current.vertex[a]->w, B = current.vertex[b]->w, |
584 |
1/2✓ Branch 1 taken 1547917 times.
✗ Branch 2 not taken.
|
1547917 | C = current.vertex[c]->w; |
585 | |||
586 |
5/10✓ Branch 1 taken 1547917 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1547917 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1547917 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1547917 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1547917 times.
✗ Branch 14 not taken.
|
1547917 | const Vec3ps AB = B - A, AC = C - A, ABC = AB.cross(AC); |
587 | |||
588 |
3/6✓ Branch 1 taken 1547917 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1547917 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1547917 times.
✗ Branch 8 not taken.
|
1547917 | SolverScalar edgeAC2o = ABC.cross(AC).dot(-A); |
589 |
2/2✓ Branch 0 taken 822213 times.
✓ Branch 1 taken 725704 times.
|
1547917 | if (edgeAC2o >= 0) { |
590 |
2/4✓ Branch 1 taken 822213 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 822213 times.
✗ Branch 5 not taken.
|
822213 | SolverScalar towardsC = AC.dot(-A); |
591 |
2/2✓ Branch 0 taken 510274 times.
✓ Branch 1 taken 311939 times.
|
822213 | if (towardsC >= 0) { // Region 1 |
592 |
1/2✓ Branch 1 taken 510274 times.
✗ Branch 2 not taken.
|
510274 | originToSegment(current, a, c, A, C, AC, towardsC, next, ray); |
593 | 510274 | free_v[nfree++] = current.vertex[b]; | |
594 | } else { // Region 4 or 5 | ||
595 |
2/4✓ Branch 1 taken 311939 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 311939 times.
✗ Branch 5 not taken.
|
311939 | SolverScalar towardsB = AB.dot(-A); |
596 |
2/2✓ Branch 0 taken 138875 times.
✓ Branch 1 taken 173064 times.
|
311939 | 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 173064 times.
✗ Branch 2 not taken.
|
173064 | originToSegment(current, a, b, A, B, AB, towardsB, next, ray); |
602 | 311939 | free_v[nfree++] = current.vertex[c]; | |
603 | } | ||
604 | } else { | ||
605 |
3/6✓ Branch 1 taken 725704 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 725704 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 725704 times.
✗ Branch 8 not taken.
|
725704 | SolverScalar edgeAB2o = AB.cross(ABC).dot(-A); |
606 |
2/2✓ Branch 0 taken 257365 times.
✓ Branch 1 taken 468339 times.
|
725704 | if (edgeAB2o >= 0) { // Region 4 or 5 |
607 |
2/4✓ Branch 1 taken 257365 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 257365 times.
✗ Branch 5 not taken.
|
257365 | SolverScalar towardsB = AB.dot(-A); |
608 |
2/2✓ Branch 0 taken 152 times.
✓ Branch 1 taken 257213 times.
|
257365 | 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 257213 times.
✗ Branch 2 not taken.
|
257213 | originToSegment(current, a, b, A, B, AB, towardsB, next, ray); |
614 | 257365 | 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 | 1079578 | 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 230863 times.
✓ Branch 1 taken 58101 times.
|
288964 | if (ba_aa <= 0) { // if AB.AO >= 0 / a10 |
666 |
3/4✓ Branch 1 taken 230863 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 160418 times.
✓ Branch 4 taken 70445 times.
|
230863 | if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / a10.a3 |
667 |
2/2✓ Branch 0 taken 70878 times.
✓ Branch 1 taken 89540 times.
|
160418 | 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 84919 times.
✓ Branch 1 taken 4621 times.
|
89540 | 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 84919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 84919 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 84919 times.
✗ Branch 8 not taken.
|
84919 | originToTriangle(current, a, d, b, (D - A).cross(B - A), |
719 |
2/4✓ Branch 1 taken 84919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 84919 times.
✗ Branch 5 not taken.
|
84919 | D.dot(a_cross_b), next, ray); |
720 | 84919 | 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 41446 times.
✓ Branch 1 taken 16655 times.
|
58101 | if (ca_aa <= 0) { // if AC.AO >= 0 / !a10.a11 |
819 |
3/4✓ Branch 1 taken 41446 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 37137 times.
✓ Branch 4 taken 4309 times.
|
41446 | if (D.dot(a_cross_c) <= 0) { // if ACD.AO >= 0 / !a10.a11.a2 |
820 |
2/2✓ Branch 0 taken 23913 times.
✓ Branch 1 taken 13224 times.
|
37137 | if (da_aa <= 0) { // if AD.AO >= 0 / !a10.a11.a2.a12 |
821 |
2/2✓ Branch 0 taken 22657 times.
✓ Branch 1 taken 1256 times.
|
23913 | 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 21319 times.
|
22657 | 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 21319 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21319 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21319 times.
✗ Branch 8 not taken.
|
21319 | originToTriangle(current, a, c, d, (C - A).cross(D - A), |
840 |
2/4✓ Branch 1 taken 21319 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21319 times.
✗ Branch 5 not taken.
|
21319 | -D.dot(a_cross_c), next, ray); |
841 | 21319 | 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 | 30475916 | void EPA::initialize() { reset(max_iterations, tolerance); } | |
1022 | |||
1023 | 30480359 | void EPA::reset(size_t max_iterations_, SolverScalar tolerance_) { | |
1024 | 30480359 | max_iterations = max_iterations_; | |
1025 | 30480359 | 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 | 30480359 | sv_store.resize(max_iterations + 4); | |
1030 | 30480359 | fc_store.resize(2 * max_iterations + 4); | |
1031 | 30480359 | status = DidNotRun; | |
1032 | 30480359 | normal.setZero(); | |
1033 | 30480359 | support_hint.setZero(); | |
1034 | 30480359 | depth = 0; | |
1035 | 30480359 | closest_face = nullptr; | |
1036 | 30480359 | result.reset(); | |
1037 | 30480359 | hull.reset(); | |
1038 | 30480359 | num_vertices = 0; | |
1039 | 30480359 | 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 124414284 times.
✓ Branch 2 taken 30480359 times.
|
154894643 | for (size_t i = 0; i < fc_store.size(); ++i) |
1044 | 124414284 | stock.append(&fc_store[fc_store.size() - i - 1]); | |
1045 | 30480359 | iterations = 0; | |
1046 | 30480359 | } | |
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 |
2/4✓ Branch 2 taken 565835 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 565835 times.
✗ Branch 6 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 |
2/4✓ Branch 2 taken 565835 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 565835 times.
✗ Branch 6 not taken.
|
565835 | SolverScalar a_dot_nab = a.w.dot((b.w - a.w).cross(face->n)); |
1100 |
2/4✓ Branch 2 taken 565835 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 565835 times.
✗ Branch 6 not taken.
|
565835 | SolverScalar b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n)); |
1101 |
2/4✓ Branch 2 taken 565835 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 565835 times.
✗ Branch 6 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 |
3/4✓ Branch 2 taken 737422 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 548068 times.
✓ Branch 5 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 |
3/4✓ Branch 2 taken 4445 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4435 times.
✓ Branch 6 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 |
2/4✓ Branch 2 taken 4435 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4435 times.
✗ Branch 6 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 | 61317 | void ConvexBase::buildSupportWarmStart() { | |
1482 |
1/2✓ Branch 2 taken 61317 times.
✗ Branch 3 not taken.
|
61317 | if (this->points->size() < ConvexBase::num_vertices_large_convex_threshold) { |
1483 | 61317 | return; | |
1484 | } | ||
1485 | |||
1486 | ✗ | this->support_warm_starts.points.reserve(ConvexBase::num_support_warm_starts); | |
1487 | ✗ | this->support_warm_starts.indices.reserve( | |
1488 | ConvexBase::num_support_warm_starts); | ||
1489 | |||
1490 | ✗ | Vec3s axiis(0, 0, 0); | |
1491 | ✗ | details::ShapeSupportData support_data; | |
1492 | ✗ | int support_hint = 0; | |
1493 | ✗ | for (int i = 0; i < 3; ++i) { | |
1494 | ✗ | axiis(i) = 1; | |
1495 | { | ||
1496 | ✗ | Vec3s support; | |
1497 | ✗ | coal::details::getShapeSupport<false>(this, axiis, support, support_hint, | |
1498 | support_data); | ||
1499 | ✗ | this->support_warm_starts.points.emplace_back(support); | |
1500 | ✗ | this->support_warm_starts.indices.emplace_back(support_hint); | |
1501 | } | ||
1502 | |||
1503 | ✗ | axiis(i) = -1; | |
1504 | { | ||
1505 | ✗ | Vec3s support; | |
1506 | ✗ | coal::details::getShapeSupport<false>(this, axiis, support, support_hint, | |
1507 | support_data); | ||
1508 | ✗ | this->support_warm_starts.points.emplace_back(support); | |
1509 | ✗ | this->support_warm_starts.indices.emplace_back(support_hint); | |
1510 | } | ||
1511 | |||
1512 | ✗ | axiis(i) = 0; | |
1513 | } | ||
1514 | |||
1515 | std::array<Vec3s, 4> eis = {Vec3s(1, 1, 1), // | ||
1516 | Vec3s(-1, 1, 1), // | ||
1517 | Vec3s(-1, -1, 1), // | ||
1518 | ✗ | Vec3s(1, -1, 1)}; | |
1519 | |||
1520 | ✗ | for (size_t ei_index = 0; ei_index < 4; ++ei_index) { | |
1521 | { | ||
1522 | ✗ | Vec3s support; | |
1523 | ✗ | coal::details::getShapeSupport<false>(this, eis[ei_index], support, | |
1524 | support_hint, support_data); | ||
1525 | ✗ | this->support_warm_starts.points.emplace_back(support); | |
1526 | ✗ | this->support_warm_starts.indices.emplace_back(support_hint); | |
1527 | } | ||
1528 | |||
1529 | { | ||
1530 | ✗ | Vec3s support; | |
1531 | ✗ | coal::details::getShapeSupport<false>(this, -eis[ei_index], support, | |
1532 | support_hint, support_data); | ||
1533 | ✗ | this->support_warm_starts.points.emplace_back(support); | |
1534 | ✗ | this->support_warm_starts.indices.emplace_back(support_hint); | |
1535 | } | ||
1536 | } | ||
1537 | |||
1538 | ✗ | if (this->support_warm_starts.points.size() != | |
1539 | ✗ | ConvexBase::num_support_warm_starts || | |
1540 | ✗ | this->support_warm_starts.indices.size() != | |
1541 | ConvexBase::num_support_warm_starts) { | ||
1542 | ✗ | COAL_THROW_PRETTY("Wrong number of support warm starts.", | |
1543 | std::runtime_error); | ||
1544 | } | ||
1545 | } | ||
1546 | |||
1547 | } // namespace coal | ||
1548 |