| Directory: | ./ |
|---|---|
| File: | include/coal/narrowphase/narrowphase.h |
| Date: | 2025-04-13 14:25:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 178 | 217 | 82.0% |
| Branches: | 131 | 351 | 37.3% |
| 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 | 45875 | explicit GJKSolver(const DistanceRequest& request) | |
| 149 | 45875 | : gjk(request.gjk_max_iterations, request.gjk_tolerance), | |
| 150 |
1/2✓ Branch 4 taken 45875 times.
✗ Branch 5 not taken.
|
45875 | epa(0, request.epa_tolerance) { |
| 151 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | this->cached_guess = Vec3s(1, 0, 0); |
| 152 |
2/4✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 45875 times.
✗ Branch 5 not taken.
|
45875 | this->support_func_cached_guess = support_func_guess_t::Zero(); |
| 153 | |||
| 154 |
1/2✓ Branch 1 taken 45875 times.
✗ Branch 2 not taken.
|
45875 | set(request); |
| 155 | 45875 | } | |
| 156 | |||
| 157 | /// @brief setter from a DistanceRequest | ||
| 158 | /// | ||
| 159 | /// \param[in] request DistanceRequest input | ||
| 160 | /// | ||
| 161 | 45875 | void set(const DistanceRequest& request) { | |
| 162 | // --------------------- | ||
| 163 | // GJK settings | ||
| 164 | 45875 | this->gjk_initial_guess = request.gjk_initial_guess; | |
| 165 | 45875 | this->enable_cached_guess = request.enable_cached_gjk_guess; | |
| 166 |
1/2✓ Branch 0 taken 45875 times.
✗ Branch 1 not taken.
|
45875 | if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || |
| 167 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 45875 times.
|
45875 | 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 | 45875 | this->gjk_max_iterations = request.gjk_max_iterations; | |
| 172 | 45875 | this->gjk_tolerance = request.gjk_tolerance; | |
| 173 | // For distance computation, we don't want GJK to early stop | ||
| 174 | 45875 | this->distance_upper_bound = (std::numeric_limits<Scalar>::max)(); | |
| 175 | 45875 | this->gjk_variant = request.gjk_variant; | |
| 176 | 45875 | this->gjk_convergence_criterion = request.gjk_convergence_criterion; | |
| 177 | 45875 | this->gjk_convergence_criterion_type = | |
| 178 | 45875 | request.gjk_convergence_criterion_type; | |
| 179 | |||
| 180 | // --------------------- | ||
| 181 | // EPA settings | ||
| 182 | 45875 | this->epa_max_iterations = request.epa_max_iterations; | |
| 183 | 45875 | this->epa_tolerance = request.epa_tolerance; | |
| 184 | |||
| 185 | // --------------------- | ||
| 186 | // Reset GJK and EPA status | ||
| 187 | 45875 | this->epa.status = details::EPA::Status::DidNotRun; | |
| 188 | 45875 | this->gjk.status = details::GJK::Status::DidNotRun; | |
| 189 | 45875 | } | |
| 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 | 30427878 | explicit GJKSolver(const CollisionRequest& request) | |
| 201 | 30427878 | : gjk(request.gjk_max_iterations, request.gjk_tolerance), | |
| 202 |
1/2✓ Branch 4 taken 30427878 times.
✗ Branch 5 not taken.
|
30427878 | epa(0, request.epa_tolerance) { |
| 203 |
1/2✓ Branch 1 taken 30427878 times.
✗ Branch 2 not taken.
|
30427878 | this->cached_guess = Vec3s(1, 0, 0); |
| 204 |
2/4✓ Branch 1 taken 30427878 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30427878 times.
✗ Branch 5 not taken.
|
30427878 | this->support_func_cached_guess = support_func_guess_t::Zero(); |
| 205 | |||
| 206 |
1/2✓ Branch 1 taken 30427878 times.
✗ Branch 2 not taken.
|
30427878 | set(request); |
| 207 | 30427878 | } | |
| 208 | |||
| 209 | /// @brief setter from a CollisionRequest | ||
| 210 | /// | ||
| 211 | /// \param[in] request CollisionRequest input | ||
| 212 | /// | ||
| 213 | 30428360 | void set(const CollisionRequest& request) { | |
| 214 | // --------------------- | ||
| 215 | // GJK settings | ||
| 216 | 30428360 | this->gjk_initial_guess = request.gjk_initial_guess; | |
| 217 | 30428360 | this->enable_cached_guess = request.enable_cached_gjk_guess; | |
| 218 |
1/2✓ Branch 0 taken 30428360 times.
✗ Branch 1 not taken.
|
30428360 | if (this->gjk_initial_guess == GJKInitialGuess::CachedGuess || |
| 219 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 30428356 times.
|
30428360 | 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 | 30428360 | this->gjk_tolerance = request.gjk_tolerance; | |
| 224 | 30428360 | 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 | 30428360 | this->distance_upper_bound = | |
| 228 | 30428360 | (std::max)(Scalar(0), (std::max)(request.distance_upper_bound, | |
| 229 | 30428360 | request.security_margin)); | |
| 230 | 30428360 | this->gjk_variant = request.gjk_variant; | |
| 231 | 30428360 | this->gjk_convergence_criterion = request.gjk_convergence_criterion; | |
| 232 | 30428360 | this->gjk_convergence_criterion_type = | |
| 233 | 30428360 | request.gjk_convergence_criterion_type; | |
| 234 | |||
| 235 | // --------------------- | ||
| 236 | // EPA settings | ||
| 237 | 30428360 | this->epa_max_iterations = request.epa_max_iterations; | |
| 238 | 30428360 | this->epa_tolerance = request.epa_tolerance; | |
| 239 | |||
| 240 | // --------------------- | ||
| 241 | // Reset GJK and EPA status | ||
| 242 | 30428360 | this->gjk.status = details::GJK::Status::DidNotRun; | |
| 243 | 30428360 | this->epa.status = details::EPA::Status::DidNotRun; | |
| 244 | 30428360 | } | |
| 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 |
1/2✗ Branch 0 not taken.
✓ Branch 1 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 |
3/6✓ Branch 1 taken 781187 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 781187 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 781187 times.
✗ Branch 9 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 |
1/2✓ Branch 1 taken 8545 times.
✗ Branch 2 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 |
1/2✓ Branch 1 taken 4430 times.
✗ Branch 2 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 |
2/4✓ Branch 1 taken 789732 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 789732 times.
✗ Branch 5 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 1 taken 285 times.
✗ Branch 2 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 |