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 |