GCC Code Coverage Report


Directory: ./
File: include/coal/narrowphase/narrowphase.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 178 217 82.0%
Branches: 136 367 37.1%

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) 2018-2019, Centre National de la Recherche Scientifique
7 * All rights reserved.
8 * Copyright (c) 2021-2024, INRIA
9 * All rights reserved.
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of Open Source Robotics Foundation nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 */
37
38 /** \author Jia Pan, Florent Lamiraux */
39
40 #ifndef COAL_NARROWPHASE_H
41 #define COAL_NARROWPHASE_H
42
43 #include <limits>
44
45 #include "coal/narrowphase/gjk.h"
46 #include "coal/collision_data.h"
47 #include "coal/narrowphase/narrowphase_defaults.h"
48 #include "coal/logging.h"
49
50 namespace coal {
51
52 /// @brief collision and distance solver based on the GJK and EPA algorithms.
53 /// Originally, GJK and EPA were implemented in fcl which itself took
54 /// inspiration from the code of the GJK in bullet. Since then, both GJK and EPA
55 /// have been largely modified to be faster and more robust to numerical
56 /// accuracy and edge cases.
57 struct COAL_DLLAPI GJKSolver {
58 public:
59 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60
61 /// @brief GJK algorithm
62 mutable details::GJK gjk;
63
64 /// @brief maximum number of iterations of GJK
65 size_t gjk_max_iterations;
66
67 /// @brief tolerance of GJK
68 Scalar gjk_tolerance;
69
70 /// @brief which warm start to use for GJK
71 GJKInitialGuess gjk_initial_guess;
72
73 /// @brief Whether smart guess can be provided
74 /// @Deprecated Use gjk_initial_guess instead
75 COAL_DEPRECATED_MESSAGE(Use gjk_initial_guess instead)
76 bool enable_cached_guess;
77
78 /// @brief smart guess
79 mutable Vec3s cached_guess;
80
81 /// @brief smart guess for the support function
82 mutable support_func_guess_t support_func_cached_guess;
83
84 /// @brief If GJK can guarantee that the distance between the shapes is
85 /// greater than this value, it will early stop.
86 Scalar distance_upper_bound;
87
88 /// @brief Variant of the GJK algorithm (Default, Nesterov or Polyak).
89 GJKVariant gjk_variant;
90
91 /// @brief Convergence criterion for GJK
92 GJKConvergenceCriterion gjk_convergence_criterion;
93
94 /// @brief Absolute or relative convergence criterion for GJK
95 GJKConvergenceCriterionType gjk_convergence_criterion_type;
96
97 /// @brief EPA algorithm
98 mutable details::EPA epa;
99
100 /// @brief maximum number of iterations of EPA
101 size_t epa_max_iterations;
102
103 /// @brief tolerance of EPA
104 Scalar epa_tolerance;
105
106 /// @brief Minkowski difference used by GJK and EPA algorithms
107 mutable details::MinkowskiDiff minkowski_difference;
108
109 private:
110 // Used internally for assertion checks.
111 static constexpr Scalar m_dummy_precision = Scalar(1e-6);
112
113 public:
114 COAL_COMPILER_DIAGNOSTIC_PUSH
115 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
116 /// @brief Default constructor for GJK algorithm
117 /// By default, we don't want EPA to allocate memory because
118 /// certain functions of the `GJKSolver` class have specializations
119 /// which don't use EPA (and/or GJK).
120 /// So we give EPA's constructor a max number of iterations of zero.
121 /// Only the functions that need EPA will reset the algorithm and allocate
122 /// memory if needed.
123 2160 GJKSolver()
124 2160 : gjk(GJK_DEFAULT_MAX_ITERATIONS, GJK_DEFAULT_TOLERANCE),
125 2160 gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS),
126 2160 gjk_tolerance(GJK_DEFAULT_TOLERANCE),
127 2160 gjk_initial_guess(GJKInitialGuess::DefaultGuess),
128 2160 enable_cached_guess(false), // Use gjk_initial_guess instead
129
1/2
✓ Branch 1 taken 2160 times.
✗ Branch 2 not taken.
2160 cached_guess(Vec3s(1, 0, 0)),
130
1/2
✓ Branch 2 taken 2160 times.
✗ Branch 3 not taken.
2160 support_func_cached_guess(support_func_guess_t::Zero()),
131 2160 distance_upper_bound((std::numeric_limits<Scalar>::max)()),
132 2160 gjk_variant(GJKVariant::DefaultGJK),
133 2160 gjk_convergence_criterion(GJKConvergenceCriterion::Default),
134 2160 gjk_convergence_criterion_type(GJKConvergenceCriterionType::Absolute),
135 2160 epa(0, EPA_DEFAULT_TOLERANCE),
136 2160 epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS),
137
1/2
✓ Branch 1 taken 2160 times.
✗ Branch 2 not taken.
2160 epa_tolerance(EPA_DEFAULT_TOLERANCE) {}
138
139 /// @brief Constructor from a DistanceRequest
140 ///
141 /// \param[in] request DistanceRequest input
142 ///
143 /// See the default constructor; by default, we don't want
144 /// EPA to allocate memory so we call EPA's constructor with 0 max
145 /// number of iterations.
146 /// However, the `set` method stores the actual values of the request.
147 /// EPA will thus allocate memory only if needed.
148 45878 explicit GJKSolver(const DistanceRequest& request)
149 45878 : gjk(request.gjk_max_iterations, request.gjk_tolerance),
150
1/2
✓ Branch 4 taken 45878 times.
✗ Branch 5 not taken.
45878 epa(0, request.epa_tolerance) {
151
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 this->cached_guess = Vec3s(1, 0, 0);
152
2/4
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45878 times.
✗ Branch 5 not taken.
45878 this->support_func_cached_guess = support_func_guess_t::Zero();
153
154
1/2
✓ Branch 1 taken 45878 times.
✗ Branch 2 not taken.
45878 set(request);
155 45878 }
156
157 /// @brief setter from a DistanceRequest
158 ///
159 /// \param[in] request DistanceRequest input
160 ///
161 45878 void set(const DistanceRequest& request) {
162 // ---------------------
163 // GJK settings
164 45878 this->gjk_initial_guess = request.gjk_initial_guess;
165 45878 this->enable_cached_guess = request.enable_cached_gjk_guess;
166
1/2
✓ Branch 0 taken 45878 times.
✗ Branch 1 not taken.
45878 if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
167
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 45878 times.
45878 this->enable_cached_guess) {
168 this->cached_guess = request.cached_gjk_guess;
169 this->support_func_cached_guess = request.cached_support_func_guess;
170 }
171 45878 this->gjk_max_iterations = request.gjk_max_iterations;
172 45878 this->gjk_tolerance = request.gjk_tolerance;
173 // For distance computation, we don't want GJK to early stop
174 45878 this->distance_upper_bound = (std::numeric_limits<Scalar>::max)();
175 45878 this->gjk_variant = request.gjk_variant;
176 45878 this->gjk_convergence_criterion = request.gjk_convergence_criterion;
177 45878 this->gjk_convergence_criterion_type =
178 45878 request.gjk_convergence_criterion_type;
179
180 // ---------------------
181 // EPA settings
182 45878 this->epa_max_iterations = request.epa_max_iterations;
183 45878 this->epa_tolerance = request.epa_tolerance;
184
185 // ---------------------
186 // Reset GJK and EPA status
187 45878 this->epa.status = details::EPA::Status::DidNotRun;
188 45878 this->gjk.status = details::GJK::Status::DidNotRun;
189 45878 }
190
191 /// @brief Constructor from a CollisionRequest
192 ///
193 /// \param[in] request CollisionRequest input
194 ///
195 /// See the default constructor; by default, we don't want
196 /// EPA to allocate memory so we call EPA's constructor with 0 max
197 /// number of iterations.
198 /// However, the `set` method stores the actual values of the request.
199 /// EPA will thus allocate memory only if needed.
200 30427876 explicit GJKSolver(const CollisionRequest& request)
201 30427876 : gjk(request.gjk_max_iterations, request.gjk_tolerance),
202
1/2
✓ Branch 4 taken 30427876 times.
✗ Branch 5 not taken.
30427876 epa(0, request.epa_tolerance) {
203
1/2
✓ Branch 1 taken 30427876 times.
✗ Branch 2 not taken.
30427876 this->cached_guess = Vec3s(1, 0, 0);
204
2/4
✓ Branch 1 taken 30427876 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30427876 times.
✗ Branch 5 not taken.
30427876 this->support_func_cached_guess = support_func_guess_t::Zero();
205
206
1/2
✓ Branch 1 taken 30427876 times.
✗ Branch 2 not taken.
30427876 set(request);
207 30427876 }
208
209 /// @brief setter from a CollisionRequest
210 ///
211 /// \param[in] request CollisionRequest input
212 ///
213 30428358 void set(const CollisionRequest& request) {
214 // ---------------------
215 // GJK settings
216 30428358 this->gjk_initial_guess = request.gjk_initial_guess;
217 30428358 this->enable_cached_guess = request.enable_cached_gjk_guess;
218
1/2
✓ Branch 0 taken 30428358 times.
✗ Branch 1 not taken.
30428358 if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess ||
219
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 30428354 times.
30428358 this->enable_cached_guess) {
220 4 this->cached_guess = request.cached_gjk_guess;
221 4 this->support_func_cached_guess = request.cached_support_func_guess;
222 }
223 30428358 this->gjk_tolerance = request.gjk_tolerance;
224 30428358 this->gjk_max_iterations = request.gjk_max_iterations;
225 // The distance upper bound should be at least greater to the requested
226 // security margin. Otherwise, we will likely miss some collisions.
227 91285074 this->distance_upper_bound = (std::max)(
228 30428358 Scalar(0),
229 30428358 (std::max)(request.distance_upper_bound, request.security_margin));
230 30428358 this->gjk_variant = request.gjk_variant;
231 30428358 this->gjk_convergence_criterion = request.gjk_convergence_criterion;
232 30428358 this->gjk_convergence_criterion_type =
233 30428358 request.gjk_convergence_criterion_type;
234
235 // ---------------------
236 // EPA settings
237 30428358 this->epa_max_iterations = request.epa_max_iterations;
238 30428358 this->epa_tolerance = request.epa_tolerance;
239
240 // ---------------------
241 // Reset GJK and EPA status
242 30428358 this->gjk.status = details::GJK::Status::DidNotRun;
243 30428358 this->epa.status = details::EPA::Status::DidNotRun;
244 30428358 }
245
246 /// @brief Copy constructor
247 GJKSolver(const GJKSolver& other) = default;
248
249 COAL_COMPILER_DIAGNOSTIC_PUSH
250 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
251 bool operator==(const GJKSolver& other) const {
252 return this->enable_cached_guess ==
253 other.enable_cached_guess && // use gjk_initial_guess instead
254 this->cached_guess == other.cached_guess &&
255 this->support_func_cached_guess == other.support_func_cached_guess &&
256 this->gjk_max_iterations == other.gjk_max_iterations &&
257 this->gjk_tolerance == other.gjk_tolerance &&
258 this->distance_upper_bound == other.distance_upper_bound &&
259 this->gjk_variant == other.gjk_variant &&
260 this->gjk_convergence_criterion == other.gjk_convergence_criterion &&
261 this->gjk_convergence_criterion_type ==
262 other.gjk_convergence_criterion_type &&
263 this->gjk_initial_guess == other.gjk_initial_guess &&
264 this->epa_max_iterations == other.epa_max_iterations &&
265 this->epa_tolerance == other.epa_tolerance;
266 }
267 COAL_COMPILER_DIAGNOSTIC_POP
268
269 bool operator!=(const GJKSolver& other) const { return !(*this == other); }
270
271 /// @brief Helper to return the precision of the solver on the distance
272 /// estimate, depending on whether or not `compute_penetration` is true.
273 580 Scalar getDistancePrecision(const bool compute_penetration) const {
274 return compute_penetration
275
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 480 times.
580 ? (std::max)(this->gjk_tolerance, this->epa_tolerance)
276 580 : this->gjk_tolerance;
277 }
278
279 /// @brief Uses GJK and EPA to compute the distance between two shapes.
280 /// @param `s1` the first shape.
281 /// @param `tf1` the transformation of the first shape.
282 /// @param `s2` the second shape.
283 /// @param `tf2` the transformation of the second shape.
284 /// @param `compute_penetration` if true and GJK finds the shape in collision,
285 /// the EPA algorithm is also ran to compute penetration information.
286 /// @param[out] `p1` the witness point on the first shape.
287 /// @param[out] `p2` the witness point on the second shape.
288 /// @param[out] `normal` the normal of the collision, pointing from the first
289 /// to the second shape.
290 /// @return the estimate of the distance between the two shapes.
291 ///
292 /// @note: if `this->distance_upper_bound` is set to a positive value, GJK
293 /// will early stop if it finds the distance to be above this value. The
294 /// distance returned by `this->shapeDistance` will be a lower bound on the
295 /// distance between the two shapes.
296 ///
297 /// @note: the variables `this->gjk.status` and `this->epa.status` can be used
298 /// to examine the status of GJK and EPA.
299 ///
300 /// @note: GJK and EPA give an estimate of the distance between the two
301 /// shapes. This estimate is precise up to the tolerance of the algorithms:
302 /// - If `compute_penetration` is false, the distance is precise up to
303 /// `gjk_tolerance`.
304 /// - If `compute_penetration` is true, the distance is precise up to
305 /// `std::max(gjk_tolerance, epa_tolerance)`
306 /// It's up to the user to decide whether the shapes are in collision or not,
307 /// based on that estimate.
308 template <typename S1, typename S2>
309 1576602 Scalar shapeDistance(const S1& s1, const Transform3s& tf1, const S2& s2,
310 const Transform3s& tf2, const bool compute_penetration,
311 Vec3s& p1, Vec3s& p2, Vec3s& normal) const {
312 1576602 constexpr bool relative_transformation_already_computed = false;
313 Scalar distance;
314
1/2
✓ Branch 1 taken 788351 times.
✗ Branch 2 not taken.
1576602 this->runGJKAndEPA(s1, tf1, s2, tf2, compute_penetration, distance, p1, p2,
315 normal, relative_transformation_already_computed);
316 1576602 return distance;
317 }
318
319 /// @brief Partial specialization of `shapeDistance` for the case where the
320 /// second shape is a triangle. It is more efficient to pre-compute the
321 /// relative transformation between the two shapes before calling GJK/EPA.
322 template <typename S1>
323 5822 Scalar shapeDistance(const S1& s1, const Transform3s& tf1,
324 const TriangleP& s2, const Transform3s& tf2,
325 const bool compute_penetration, Vec3s& p1, Vec3s& p2,
326 Vec3s& normal) const {
327
1/2
✓ Branch 1 taken 2911 times.
✗ Branch 2 not taken.
5822 const Transform3s tf_1M2(tf1.inverseTimes(tf2));
328
3/6
✓ Branch 1 taken 2911 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2911 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2911 times.
✗ Branch 8 not taken.
5822 TriangleP tri(tf_1M2.transform(s2.a), tf_1M2.transform(s2.b),
329
1/2
✓ Branch 1 taken 2911 times.
✗ Branch 2 not taken.
5822 tf_1M2.transform(s2.c));
330
331 5822 constexpr bool relative_transformation_already_computed = true;
332 Scalar distance;
333
1/2
✓ Branch 1 taken 2911 times.
✗ Branch 2 not taken.
5822 this->runGJKAndEPA(s1, tf1, tri, tf_1M2, compute_penetration, distance, p1,
334 p2, normal, relative_transformation_already_computed);
335 5822 return distance;
336 }
337
338 /// @brief See other partial template specialization of shapeDistance above.
339 template <typename S2>
340 3784 Scalar shapeDistance(const TriangleP& s1, const Transform3s& tf1,
341 const S2& s2, const Transform3s& tf2,
342 const bool compute_penetration, Vec3s& p1, Vec3s& p2,
343 Vec3s& normal) const {
344 3784 Scalar distance = this->shapeDistance<S2>(
345 s2, tf2, s1, tf1, compute_penetration, p2, p1, normal);
346
1/2
✓ Branch 2 taken 1892 times.
✗ Branch 3 not taken.
3784 normal = -normal;
347 3784 return distance;
348 }
349
350 protected:
351 /// @brief initialize GJK.
352 /// This method assumes `minkowski_difference` has been set.
353 template <typename S1, typename S2>
354 794462 void getGJKInitialGuess(const S1& s1, const S2& s2, Vec3s& guess,
355 support_func_guess_t& support_hint,
356 const Vec3s& default_guess = Vec3s(1, 0, 0)) const {
357 // There is no reason not to warm-start the support function, so we always
358 // do it.
359 794462 support_hint = this->support_func_cached_guess;
360 // The following switch takes care of the GJK warm-start.
361
1/4
✓ Branch 0 taken 794462 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
794462 switch (gjk_initial_guess) {
362 794462 case GJKInitialGuess::DefaultGuess:
363 794462 guess = default_guess;
364 794462 break;
365 case GJKInitialGuess::CachedGuess:
366 guess = this->cached_guess;
367 break;
368 case GJKInitialGuess::BoundingVolumeGuess:
369 if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) {
370 COAL_THROW_PRETTY(
371 "computeLocalAABB must have been called on the shapes before "
372 "using "
373 "GJKInitialGuess::BoundingVolumeGuess.",
374 std::logic_error);
375 }
376 guess.noalias() =
377 s1.aabb_local.center() -
378 (this->minkowski_difference.oR1 * s2.aabb_local.center() +
379 this->minkowski_difference.ot1);
380 break;
381 default:
382 COAL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error);
383 }
384 // TODO: use gjk_initial_guess instead
385 COAL_COMPILER_DIAGNOSTIC_PUSH
386 COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
387
2/2
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 794458 times.
794462 if (this->enable_cached_guess) {
388 4 guess = this->cached_guess;
389 }
390 COAL_COMPILER_DIAGNOSTIC_POP
391 794462 }
392
393 /// @brief Runs the GJK algorithm.
394 /// @param `s1` the first shape.
395 /// @param `tf1` the transformation of the first shape.
396 /// @param `s2` the second shape.
397 /// @param `tf2` the transformation of the second shape.
398 /// @param `compute_penetration` if true and if the shapes are in found in
399 /// collision, the EPA algorithm is also ran to compute penetration
400 /// information.
401 /// @param[out] `distance` the distance between the two shapes.
402 /// @param[out] `p1` the witness point on the first shape.
403 /// @param[out] `p2` the witness point on the second shape.
404 /// @param[out] `normal` the normal of the collision, pointing from the first
405 /// to the second shape.
406 /// @param `relative_transformation_already_computed` whether the relative
407 /// transformation between the two shapes has already been computed.
408 /// @tparam SupportOptions, see `MinkowskiDiff::set`. Whether the support
409 /// computations should take into account the shapes' swept-sphere radii
410 /// during the iterations of GJK and EPA. Please leave this default value to
411 /// `false` unless you know what you are doing. This template parameter is
412 /// only used for debugging/testing purposes. In short, there is no need to
413 /// take into account the swept sphere radius when computing supports in the
414 /// iterations of GJK and EPA. GJK and EPA will correct the solution once they
415 /// have converged.
416 /// @return the estimate of the distance between the two shapes.
417 ///
418 /// @note: The variables `this->gjk.status` and `this->epa.status` can be used
419 /// to examine the status of GJK and EPA.
420 template <typename S1, typename S2,
421 int _SupportOptions = details::SupportOptions::NoSweptSphere>
422 1588824 void runGJKAndEPA(
423 const S1& s1, const Transform3s& tf1, const S2& s2,
424 const Transform3s& tf2, const bool compute_penetration, Scalar& distance,
425 Vec3s& p1, Vec3s& p2, Vec3s& normal,
426 const bool relative_transformation_already_computed = false) const {
427 // Reset internal state of GJK algorithm
428
2/2
✓ Branch 0 taken 2911 times.
✓ Branch 1 taken 791551 times.
1588824 if (relative_transformation_already_computed)
429
1/2
✓ Branch 1 taken 2911 times.
✗ Branch 2 not taken.
5822 this->minkowski_difference.set<_SupportOptions>(&s1, &s2);
430 else
431
1/2
✓ Branch 1 taken 791551 times.
✗ Branch 2 not taken.
1583002 this->minkowski_difference.set<_SupportOptions>(&s1, &s2, tf1, tf2);
432
1/2
✓ Branch 1 taken 794462 times.
✗ Branch 2 not taken.
1588824 this->gjk.reset(this->gjk_max_iterations, this->gjk_tolerance);
433 1588824 this->gjk.setDistanceEarlyBreak(this->distance_upper_bound);
434 1588824 this->gjk.gjk_variant = this->gjk_variant;
435 1588824 this->gjk.convergence_criterion = this->gjk_convergence_criterion;
436 1588824 this->gjk.convergence_criterion_type = this->gjk_convergence_criterion_type;
437 1588824 this->epa.status = details::EPA::Status::DidNotRun;
438
439 // Get initial guess for GJK: default, cached or bounding volume guess
440
1/2
✓ Branch 1 taken 794462 times.
✗ Branch 2 not taken.
1588824 Vec3s guess;
441
1/2
✓ Branch 1 taken 794462 times.
✗ Branch 2 not taken.
1588824 support_func_guess_t support_hint;
442
1/2
✓ Branch 1 taken 794462 times.
✗ Branch 2 not taken.
1588824 getGJKInitialGuess(*(this->minkowski_difference.shapes[0]),
443
1/2
✓ Branch 1 taken 794462 times.
✗ Branch 2 not taken.
1588824 *(this->minkowski_difference.shapes[1]), guess,
444 support_hint);
445
446
2/4
✓ Branch 1 taken 794462 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 794462 times.
✗ Branch 5 not taken.
1588824 this->gjk.evaluate(this->minkowski_difference, guess.cast<SolverScalar>(),
447 support_hint);
448
449
4/7
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 781187 times.
✓ Branch 4 taken 8545 times.
✓ Branch 5 taken 4728 times.
✗ Branch 6 not taken.
1588824 switch (this->gjk.status) {
450 case details::GJK::DidNotRun:
451 COAL_ASSERT(false, "GJK did not run. It should have!",
452 std::logic_error);
453 this->cached_guess = Vec3s(1, 0, 0);
454 this->support_func_cached_guess.setZero();
455 distance = -(std::numeric_limits<Scalar>::max)();
456 p1 = p2 = normal =
457 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
458 break;
459 case details::GJK::Failed:
460 //
461 // GJK ran out of iterations.
462 COAL_LOG_WARNING("GJK ran out of iterations.");
463 GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
464 break;
465 4 case details::GJK::NoCollisionEarlyStopped:
466 //
467 // Case where GJK early stopped because the distance was found to be
468 // above the `distance_upper_bound`.
469 // The two witness points have no meaning.
470
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
4 GJKEarlyStopExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
471 normal);
472
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
4 COAL_ASSERT(distance >= this->gjk.distance_upper_bound -
473 this->m_dummy_precision,
474 "The distance should be bigger than GJK's "
475 "`distance_upper_bound`.",
476 std::logic_error);
477 4 break;
478 1562303 case details::GJK::NoCollision:
479 //
480 // Case where GJK converged and proved that the shapes are not in
481 // collision, i.e their distance is above GJK's tolerance (default
482 // 1e-6).
483
1/2
✓ Branch 1 taken 781187 times.
✗ Branch 2 not taken.
1562303 GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
484
4/8
✓ Branch 1 taken 781187 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 781187 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 781187 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 781187 times.
✗ Branch 13 not taken.
1562303 COAL_ASSERT(std::abs((p1 - p2).norm() - distance) <=
485 this->gjk.getTolerance() + this->m_dummy_precision,
486 "The distance found by GJK should coincide with the "
487 "distance between the closest points.",
488 std::logic_error);
489 1562303 break;
490 //
491 // Next are the cases where GJK found the shapes to be in collision, i.e.
492 // their distance is below GJK's tolerance (default 1e-6).
493 17090 case details::GJK::CollisionWithPenetrationInformation:
494
1/2
✓ Branch 1 taken 8545 times.
✗ Branch 2 not taken.
17090 GJKExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
495
2/4
✓ Branch 1 taken 8545 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8545 times.
✗ Branch 6 not taken.
17090 COAL_ASSERT(
496 distance <= this->gjk.getTolerance() + this->m_dummy_precision,
497 "The distance found by GJK should be negative or at "
498 "least below GJK's tolerance.",
499 std::logic_error);
500 17090 break;
501 9427 case details::GJK::Collision:
502
2/2
✓ Branch 0 taken 285 times.
✓ Branch 1 taken 4443 times.
9427 if (!compute_penetration) {
503 // Skip EPA and set the witness points and the normal to nans.
504
1/2
✓ Branch 1 taken 285 times.
✗ Branch 2 not taken.
570 GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
505 normal);
506 } else {
507 //
508 // GJK was not enough to recover the penetration information.
509 // We need to run the EPA algorithm to find the witness points,
510 // penetration depth and the normal.
511
512 // Reset EPA algorithm. Potentially allocate memory if
513 // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's
514 // current storage.
515
1/2
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
8857 this->epa.reset(this->epa_max_iterations, this->epa_tolerance);
516
517 // TODO: understand why EPA's performance is so bad on cylinders and
518 // cones.
519
3/6
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4443 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4443 times.
✗ Branch 9 not taken.
8857 this->epa.evaluate(this->gjk, (-guess).cast<SolverScalar>());
520
521
2/10
✓ Branch 0 taken 13 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 4430 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
8857 switch (epa.status) {
522 //
523 // In the following switch cases, until the "Valid" case,
524 // EPA either ran out of iterations, of faces or of vertices.
525 // The depth, witness points and the normal are still valid,
526 // simply not at the precision of EPA's tolerance.
527 // The flag `COAL_ENABLE_LOGGING` enables feebdack on these
528 // cases.
529 //
530 // TODO: Remove OutOfFaces and OutOfVertices statuses and simply
531 // compute the upper bound on max faces and max vertices as a
532 // function of the number of iterations.
533 26 case details::EPA::OutOfFaces:
534 COAL_LOG_WARNING("EPA ran out of faces.");
535
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
26 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
536 26 break;
537 case details::EPA::OutOfVertices:
538 COAL_LOG_WARNING("EPA ran out of vertices.");
539 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
540 break;
541 case details::EPA::Failed:
542 COAL_LOG_WARNING("EPA ran out of iterations.");
543 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
544 break;
545 8831 case details::EPA::Valid:
546 case details::EPA::AccuracyReached:
547
2/4
✓ Branch 1 taken 4430 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4430 times.
✗ Branch 6 not taken.
8831 COAL_ASSERT(
548 -epa.depth <= epa.getTolerance() + this->m_dummy_precision,
549 "EPA's penetration distance should be negative (or "
550 "at least below EPA's tolerance).",
551 std::logic_error);
552
1/2
✓ Branch 1 taken 4430 times.
✗ Branch 2 not taken.
8831 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
553 8831 break;
554 case details::EPA::Degenerated:
555 COAL_LOG_WARNING(
556 "EPA warning: created a polytope with a degenerated face.");
557 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
558 break;
559 case details::EPA::NonConvex:
560 COAL_LOG_WARNING(
561 "EPA warning: EPA got called onto non-convex shapes.");
562 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
563 break;
564 case details::EPA::InvalidHull:
565 COAL_LOG_WARNING("EPA warning: created an invalid polytope.");
566 EPAExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal);
567 break;
568 case details::EPA::DidNotRun:
569 COAL_ASSERT(false, "EPA did not run. It should have!",
570 std::logic_error);
571 COAL_LOG_ERROR("EPA error: did not run. It should have.");
572 EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
573 normal);
574 break;
575 case details::EPA::FallBack:
576 COAL_ASSERT(
577 false,
578 "EPA went into fallback mode. It should never do that.",
579 std::logic_error);
580 COAL_LOG_ERROR("EPA error: FallBack.");
581 EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2,
582 normal);
583 break;
584 }
585 }
586 9427 break; // End of case details::GJK::Collision
587 }
588 1588824 }
589
590 2 void GJKEarlyStopExtractWitnessPointsAndNormal(const Transform3s& tf1,
591 Scalar& distance, Vec3s& p1,
592 Vec3s& p2,
593 Vec3s& normal) const {
594 COAL_UNUSED_VARIABLE(tf1);
595 // Cache gjk result for potential future call to this GJKSolver.
596 2 this->cached_guess = this->gjk.ray.cast<Scalar>();
597 2 this->support_func_cached_guess = this->gjk.support_hint;
598
599 2 distance = Scalar(this->gjk.distance);
600 p1 = p2 = normal =
601
4/8
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
2 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
602 // If we absolutely want to return some witness points, we could use
603 // the following code (or simply merge the early stopped case with the
604 // valid case below):
605 // gjk.getWitnessPointsAndNormal(minkowski_difference, p1, p2, normal);
606 // p1 = tf1.transform(p1);
607 // p2 = tf1.transform(p2);
608 // normal = tf1.getRotation() * normal;
609 2 }
610
611 789732 void GJKExtractWitnessPointsAndNormal(const Transform3s& tf1,
612 Scalar& distance, Vec3s& p1, Vec3s& p2,
613 Vec3s& normal) const {
614 // Apart from early stopping, there are two cases where GJK says there is no
615 // collision:
616 // 1. GJK proved the distance is above its tolerance (default 1e-6).
617 // 2. GJK ran out of iterations.
618 // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus
619 // it can safely be normalized.
620
3/6
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 789732 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 789732 times.
✗ Branch 9 not taken.
789732 COAL_ASSERT(this->gjk.ray.norm() >
621 this->gjk.getTolerance() - this->m_dummy_precision,
622 "The norm of GJK's ray should be bigger than GJK's tolerance.",
623 std::logic_error);
624 // Cache gjk result for potential future call to this GJKSolver.
625
2/4
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 not taken.
789732 this->cached_guess = this->gjk.ray.cast<Scalar>();
626
1/2
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
789732 this->support_func_cached_guess = this->gjk.support_hint;
627
628 789732 distance = Scalar(this->gjk.distance);
629 // TODO: On degenerated case, the closest points may be non-unique.
630 // (i.e. an object face normal is colinear to `gjk.ray`)
631
3/6
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 789732 times.
✗ Branch 8 not taken.
789732 Vec3ps p1_, p2_, normal_;
632
1/2
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
789732 gjk.getWitnessPointsAndNormal(this->minkowski_difference, p1_, p2_,
633 normal_);
634
2/4
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 not taken.
789732 p1 = p1_.cast<Scalar>();
635
2/4
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 not taken.
789732 p2 = p2_.cast<Scalar>();
636
2/4
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 not taken.
789732 normal = normal_.cast<Scalar>();
637
3/6
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 789732 times.
✗ Branch 8 not taken.
789732 Vec3s p = tf1.transform(0.5 * (p1 + p2));
638
2/4
✓ Branch 2 taken 789732 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 789732 times.
✗ Branch 6 not taken.
789732 normal = tf1.getRotation() * normal;
639
4/8
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 789732 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 789732 times.
✗ Branch 11 not taken.
789732 p1.noalias() = p - 0.5 * distance * normal;
640
4/8
✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 789732 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 789732 times.
✗ Branch 11 not taken.
789732 p2.noalias() = p + 0.5 * distance * normal;
641 789732 }
642
643 285 void GJKCollisionExtractWitnessPointsAndNormal(const Transform3s& tf1,
644 Scalar& distance, Vec3s& p1,
645 Vec3s& p2,
646 Vec3s& normal) const {
647 COAL_UNUSED_VARIABLE(tf1);
648
1/2
✓ Branch 3 taken 285 times.
✗ Branch 4 not taken.
285 COAL_ASSERT(this->gjk.distance <=
649 this->gjk.getTolerance() + this->m_dummy_precision,
650 "The distance should be lower than GJK's tolerance.",
651 std::logic_error);
652 // Because GJK has returned the `Collision` status and EPA has not run,
653 // we purposefully do not cache the result of GJK, because ray is zero.
654 // However, we can cache the support function hint.
655 // this->cached_guess = this->gjk.ray;
656 285 this->support_func_cached_guess = this->gjk.support_hint;
657
658 285 distance = Scalar(this->gjk.distance);
659 p1 = p2 = normal =
660
4/8
✓ Branch 2 taken 285 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 285 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 285 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 285 times.
✗ Branch 12 not taken.
285 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
661 285 }
662
663 4443 void EPAExtractWitnessPointsAndNormal(const Transform3s& tf1,
664 Scalar& distance, Vec3s& p1, Vec3s& p2,
665 Vec3s& normal) const {
666 // Cache EPA result for potential future call to this GJKSolver.
667 // This caching allows to warm-start the next GJK call.
668
4/8
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4443 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4443 times.
✗ Branch 11 not taken.
4443 this->cached_guess = -(this->epa.depth * this->epa.normal).cast<Scalar>();
669
1/2
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
4443 this->support_func_cached_guess = this->epa.support_hint;
670 4443 distance = (std::min)(Scalar(0), -Scalar(this->epa.depth));
671
3/6
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4443 times.
✗ Branch 8 not taken.
4443 Vec3ps p1_, p2_, normal_;
672
1/2
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
4443 this->epa.getWitnessPointsAndNormal(this->minkowski_difference, p1_, p2_,
673 normal_);
674
2/4
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4443 times.
✗ Branch 5 not taken.
4443 p1 = p1_.cast<Scalar>();
675
2/4
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4443 times.
✗ Branch 5 not taken.
4443 p2 = p2_.cast<Scalar>();
676
2/4
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4443 times.
✗ Branch 5 not taken.
4443 normal = normal_.cast<Scalar>();
677 // The following is very important to understand why EPA can sometimes
678 // return a normal that is not colinear to the vector $p_1 - p_2$ when
679 // working with tolerances like $\epsilon = 10^{-3}$.
680 // It can be resumed with a simple idea:
681 // EPA is an algorithm meant to find the penetration depth and the
682 // normal. It is not meant to find the closest points.
683 // Again, the issue here is **not** the normal, it's $p_1$ and $p_2$.
684 //
685 // More details:
686 // We'll denote $S_1$ and $S_2$ the two shapes, $n$ the normal and $p_1$ and
687 // $p_2$ the witness points. In theory, when EPA converges to $\epsilon =
688 // 0$, the normal and witness points verify the following property (P):
689 // - $p_1 \in \partial \sigma_{S_1}(n)$,
690 // - $p_2 \in \partial \sigma_{S_2}(-n),
691 // where $\sigma_{S_1}$ and $\sigma_{S_2}$ are the support functions of
692 // $S_1$ and $S_2$. The $\partial \sigma(n)$ simply denotes the support set
693 // of the support function in the direction $n$. (Note: I am leaving out the
694 // details of frame choice for the support function, to avoid making the
695 // mathematical notation too heavy.)
696 // --> In practice, EPA converges to $\epsilon > 0$.
697 // On polytopes and the likes, this does not change much and the property
698 // given above is still valid.
699 // --> However, this is very different on curved surfaces, such as
700 // ellipsoids, cylinders, cones, capsules etc. For these shapes, converging
701 // at $\epsilon = 10^{-6}$ or to $\epsilon = 10^{-3}$ does not change the
702 // normal much, but the property (P) given above is no longer valid, which
703 // means that the points $p_1$ and $p_2$ do not necessarily belong to the
704 // support sets in the direction of $n$ and thus $n$ and $p_1 - p_2$ are not
705 // colinear.
706 //
707 // Do not panic! This is fine.
708 // Although the property above is not verified, it's almost verified,
709 // meaning that $p_1$ and $p_2$ belong to support sets in directions that
710 // are very close to $n$.
711 //
712 // Solution to compute better $p_1$ and $p_2$:
713 // We compute the middle points of the current $p_1$ and $p_2$ and we use
714 // the normal and the distance given by EPA to compute the new $p_1$ and
715 // $p_2$.
716
3/6
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4443 times.
✗ Branch 8 not taken.
4443 Vec3s p = tf1.transform(0.5 * (p1 + p2));
717
2/4
✓ Branch 2 taken 4443 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4443 times.
✗ Branch 6 not taken.
4443 normal = tf1.getRotation() * normal;
718
4/8
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4443 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4443 times.
✗ Branch 11 not taken.
4443 p1.noalias() = p - 0.5 * distance * normal;
719
4/8
✓ Branch 1 taken 4443 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4443 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4443 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4443 times.
✗ Branch 11 not taken.
4443 p2.noalias() = p + 0.5 * distance * normal;
720 4443 }
721
722 void EPAFailedExtractWitnessPointsAndNormal(const Transform3s& tf1,
723 Scalar& distance, Vec3s& p1,
724 Vec3s& p2, Vec3s& normal) const {
725 this->cached_guess = Vec3s(1, 0, 0);
726 this->support_func_cached_guess.setZero();
727
728 COAL_UNUSED_VARIABLE(tf1);
729 distance = -(std::numeric_limits<Scalar>::max)();
730 p1 = p2 = normal =
731 Vec3s::Constant(std::numeric_limits<Scalar>::quiet_NaN());
732 }
733 };
734
735 } // namespace coal
736
737 #endif
738