| Directory: | ./ |
|---|---|
| File: | include/coal/narrowphase/gjk.h |
| Date: | 2025-04-13 14:25:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 59 | 69 | 85.5% |
| Branches: | 37 | 56 | 66.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) 2021-2024, INRIA | ||
| 7 | * All rights reserved. | ||
| 8 | * | ||
| 9 | * Redistribution and use in source and binary forms, with or without | ||
| 10 | * modification, are permitted provided that the following conditions | ||
| 11 | * are met: | ||
| 12 | * | ||
| 13 | * * Redistributions of source code must retain the above copyright | ||
| 14 | * notice, this list of conditions and the following disclaimer. | ||
| 15 | * * Redistributions in binary form must reproduce the above | ||
| 16 | * copyright notice, this list of conditions and the following | ||
| 17 | * disclaimer in the documentation and/or other materials provided | ||
| 18 | * with the distribution. | ||
| 19 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
| 20 | * contributors may be used to endorse or promote products derived | ||
| 21 | * from this software without specific prior written permission. | ||
| 22 | * | ||
| 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 34 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 35 | */ | ||
| 36 | |||
| 37 | /** \author Jia Pan */ | ||
| 38 | |||
| 39 | #ifndef COAL_GJK_H | ||
| 40 | #define COAL_GJK_H | ||
| 41 | |||
| 42 | #include <vector> | ||
| 43 | |||
| 44 | #include "coal/narrowphase/minkowski_difference.h" | ||
| 45 | |||
| 46 | namespace coal { | ||
| 47 | |||
| 48 | namespace details { | ||
| 49 | |||
| 50 | /// @brief class for GJK algorithm | ||
| 51 | /// | ||
| 52 | /// @note The computations are performed in the frame of the first shape. | ||
| 53 | struct COAL_DLLAPI GJK { | ||
| 54 | struct COAL_DLLAPI SimplexV { | ||
| 55 | /// @brief support vector for shape 0 and 1. | ||
| 56 | Vec3ps w0, w1; | ||
| 57 | /// @brief support vector (i.e., the furthest point on the shape along the | ||
| 58 | /// support direction) | ||
| 59 | Vec3ps w; | ||
| 60 | }; | ||
| 61 | |||
| 62 | typedef unsigned char vertex_id_t; | ||
| 63 | |||
| 64 | /// @brief A simplex is a set of up to 4 vertices. | ||
| 65 | /// Its rank is the number of vertices it contains. | ||
| 66 | /// @note This data structure does **not** own the vertices it refers to. | ||
| 67 | /// To be efficient, the constructor of `GJK` creates storage for 4 vertices. | ||
| 68 | /// Since GJK does not need any more storage, it reuses these vertices | ||
| 69 | /// throughout the algorithm by using multiple instance of this `Simplex` | ||
| 70 | /// class. | ||
| 71 | struct COAL_DLLAPI Simplex { | ||
| 72 | /// @brief simplex vertex | ||
| 73 | SimplexV* vertex[4]; | ||
| 74 | /// @brief size of simplex (number of vertices) | ||
| 75 | vertex_id_t rank; | ||
| 76 | |||
| 77 | 91428119 | Simplex() {} | |
| 78 | |||
| 79 | 30480358 | void reset() { | |
| 80 | 30480358 | rank = 0; | |
| 81 |
2/2✓ Branch 0 taken 121921432 times.
✓ Branch 1 taken 30480358 times.
|
152401790 | for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr; |
| 82 | 30480358 | } | |
| 83 | }; | ||
| 84 | |||
| 85 | /// @brief Status of the GJK algorithm: | ||
| 86 | /// DidNotRun: GJK has not been run. | ||
| 87 | /// Failed: GJK did not converge (it exceeded the maximum number of | ||
| 88 | /// iterations). | ||
| 89 | /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited | ||
| 90 | /// before converting. The shapes are not in collision. | ||
| 91 | /// NoCollision: GJK converged and the shapes are not in collision. | ||
| 92 | /// Collision: GJK converged and the shapes are in collision. | ||
| 93 | /// Failed: GJK did not converge. | ||
| 94 | enum Status { | ||
| 95 | DidNotRun, | ||
| 96 | Failed, | ||
| 97 | NoCollisionEarlyStopped, | ||
| 98 | NoCollision, | ||
| 99 | CollisionWithPenetrationInformation, | ||
| 100 | Collision | ||
| 101 | }; | ||
| 102 | |||
| 103 | public: | ||
| 104 | SolverScalar distance_upper_bound; | ||
| 105 | Status status; | ||
| 106 | GJKVariant gjk_variant; | ||
| 107 | GJKConvergenceCriterion convergence_criterion; | ||
| 108 | GJKConvergenceCriterionType convergence_criterion_type; | ||
| 109 | |||
| 110 | MinkowskiDiff const* shape; | ||
| 111 | Vec3ps ray; | ||
| 112 | support_func_guess_t support_hint; | ||
| 113 | /// @brief The distance between the two shapes, computed by GJK. | ||
| 114 | /// If the distance is below GJK's threshold, the shapes are in collision in | ||
| 115 | /// the eyes of GJK. If `distance_upper_bound` is set to a value lower than | ||
| 116 | /// infinity, GJK will early stop as soon as it finds `distance` to be greater | ||
| 117 | /// than `distance_upper_bound`. | ||
| 118 | SolverScalar distance; | ||
| 119 | Simplex* simplex; // Pointer to the result of the last run of GJK. | ||
| 120 | |||
| 121 | private: | ||
| 122 | // max_iteration and tolerance are made private | ||
| 123 | // because they are meant to be set by the `reset` function. | ||
| 124 | size_t max_iterations; | ||
| 125 | SolverScalar tolerance; | ||
| 126 | |||
| 127 | SimplexV store_v[4]; | ||
| 128 | SimplexV* free_v[4]; | ||
| 129 | vertex_id_t nfree; | ||
| 130 | vertex_id_t current; | ||
| 131 | Simplex simplices[2]; | ||
| 132 | size_t iterations; | ||
| 133 | size_t iterations_momentum_stop; | ||
| 134 | |||
| 135 | public: | ||
| 136 | /// \param max_iterations_ number of iteration before GJK returns failure. | ||
| 137 | /// \param tolerance_ precision of the algorithm. | ||
| 138 | /// | ||
| 139 | /// The tolerance argument is useful for continuous shapes and for polyhedron | ||
| 140 | /// with some vertices closer than this threshold. | ||
| 141 | /// | ||
| 142 | /// Suggested values are 100 iterations and a tolerance of 1e-6. | ||
| 143 | 30476102 | GJK(size_t max_iterations_, SolverScalar tolerance_) | |
| 144 |
4/4✓ Branch 3 taken 121904408 times.
✓ Branch 4 taken 30476102 times.
✓ Branch 6 taken 60952204 times.
✓ Branch 7 taken 30476102 times.
|
213332714 | : max_iterations(max_iterations_), tolerance(tolerance_) { |
| 145 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 30476102 times.
|
30476102 | COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.", |
| 146 | std::invalid_argument); | ||
| 147 | 30476102 | initialize(); | |
| 148 | 30476102 | } | |
| 149 | |||
| 150 | /// @brief resets the GJK algorithm, preparing it for a new run. | ||
| 151 | /// Other than the maximum number of iterations and the tolerance, | ||
| 152 | /// this function does **not** modify the parameters of the GJK algorithm. | ||
| 153 | void reset(size_t max_iterations_, SolverScalar tolerance_); | ||
| 154 | |||
| 155 | /// @brief GJK algorithm, given the initial value guess | ||
| 156 | Status evaluate( | ||
| 157 | const MinkowskiDiff& shape, const Vec3ps& guess, | ||
| 158 | const support_func_guess_t& supportHint = support_func_guess_t::Zero()); | ||
| 159 | |||
| 160 | /// @brief apply the support function along a direction, the result is return | ||
| 161 | /// in sv | ||
| 162 | 72451393 | inline void getSupport(const Vec3ps& d, SimplexV& sv, | |
| 163 | support_func_guess_t& hint) const { | ||
| 164 |
2/4✓ Branch 1 taken 72451393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451393 times.
✗ Branch 5 not taken.
|
72451393 | Vec3s w0, w1; |
| 165 |
2/4✓ Branch 1 taken 72451393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451393 times.
✗ Branch 5 not taken.
|
72451393 | shape->support(d.cast<Scalar>(), w0, w1, hint); |
| 166 |
2/4✓ Branch 1 taken 72451393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451393 times.
✗ Branch 5 not taken.
|
72451393 | sv.w0 = w0.cast<SolverScalar>(); |
| 167 |
2/4✓ Branch 1 taken 72451393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451393 times.
✗ Branch 5 not taken.
|
72451393 | sv.w1 = w1.cast<SolverScalar>(); |
| 168 |
2/4✓ Branch 1 taken 72451393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451393 times.
✗ Branch 5 not taken.
|
72451393 | sv.w = sv.w0 - sv.w1; |
| 169 | 72451393 | } | |
| 170 | |||
| 171 | /// @brief whether the simplex enclose the origin | ||
| 172 | bool encloseOrigin(); | ||
| 173 | |||
| 174 | /// @brief get the underlying simplex using in GJK, can be used for cache in | ||
| 175 | /// next iteration | ||
| 176 | 4445 | inline Simplex* getSimplex() const { return simplex; } | |
| 177 | |||
| 178 | /// Tells whether the closest points are available. | ||
| 179 | ✗ | bool hasClosestPoints() const { return distance < distance_upper_bound; } | |
| 180 | |||
| 181 | /// Get the witness points on each object, and the corresponding normal. | ||
| 182 | /// @param[in] shape is the Minkowski difference of the two shapes. | ||
| 183 | /// @param[out] w0 is the witness point on shape0. | ||
| 184 | /// @param[out] w1 is the witness point on shape1. | ||
| 185 | /// @param[out] normal is the normal of the separating plane found by | ||
| 186 | /// GJK. It points from shape0 to shape1. | ||
| 187 | void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0, | ||
| 188 | Vec3ps& w1, Vec3ps& normal) const; | ||
| 189 | |||
| 190 | /// @brief get the guess from current simplex | ||
| 191 | Vec3ps getGuessFromSimplex() const; | ||
| 192 | |||
| 193 | /// @brief Distance threshold for early break. | ||
| 194 | /// GJK stops when it proved the distance is more than this threshold. | ||
| 195 | /// @note The closest points will be erroneous in this case. | ||
| 196 | /// If you want the closest points, set this to infinity (the default). | ||
| 197 | 794502 | void setDistanceEarlyBreak(const SolverScalar& dup) { | |
| 198 | 794502 | distance_upper_bound = dup; | |
| 199 | 794502 | } | |
| 200 | |||
| 201 | /// @brief Convergence check used to stop GJK when shapes are not in | ||
| 202 | /// collision. | ||
| 203 | bool checkConvergence(const Vec3ps& w, const SolverScalar& rl, | ||
| 204 | SolverScalar& alpha, const SolverScalar& omega) const; | ||
| 205 | |||
| 206 | /// @brief Get the max number of iterations of GJK. | ||
| 207 | ✗ | size_t getNumMaxIterations() const { return max_iterations; } | |
| 208 | |||
| 209 | /// @brief Get the tolerance of GJK. | ||
| 210 | 30764194 | SolverScalar getTolerance() const { return tolerance; } | |
| 211 | |||
| 212 | /// @brief Get the number of iterations of the last run of GJK. | ||
| 213 | 130000 | size_t getNumIterations() const { return iterations; } | |
| 214 | |||
| 215 | /// @brief Get GJK number of iterations before momentum stops. | ||
| 216 | /// Only usefull if the Nesterov or Polyak acceleration activated. | ||
| 217 | ✗ | size_t getNumIterationsMomentumStopped() const { | |
| 218 | ✗ | return iterations_momentum_stop; | |
| 219 | } | ||
| 220 | |||
| 221 | private: | ||
| 222 | /// @brief Initializes the GJK algorithm. | ||
| 223 | /// This function should only be called by the constructor. | ||
| 224 | /// Otherwise use \ref reset. | ||
| 225 | void initialize(); | ||
| 226 | |||
| 227 | /// @brief discard one vertex from the simplex | ||
| 228 | inline void removeVertex(Simplex& simplex); | ||
| 229 | |||
| 230 | /// @brief append one vertex to the simplex | ||
| 231 | inline void appendVertex(Simplex& simplex, const Vec3ps& v, | ||
| 232 | support_func_guess_t& hint); | ||
| 233 | |||
| 234 | /// @brief Project origin (0) onto line a-b | ||
| 235 | /// For a detailed explanation of how to efficiently project onto a simplex, | ||
| 236 | /// check out Ericson's book, page 403: | ||
| 237 | /// https://realtimecollisiondetection.net/ To sum up, a simplex has a voronoi | ||
| 238 | /// region for each feature it has (vertex, edge, face). We find the voronoi | ||
| 239 | /// region in which the origin lies and stop as soon as we find it; we then | ||
| 240 | /// project onto it and return the result. We start by voronoi regions | ||
| 241 | /// generated by vertices then move on to edges then faces. Checking voronoi | ||
| 242 | /// regions is done using simple dot products. Moreover, edges voronoi checks | ||
| 243 | /// reuse computations of vertices voronoi checks. The same goes for faces | ||
| 244 | /// which reuse checks from edges. | ||
| 245 | /// Finally, in addition to the voronoi procedure, checks relying on the order | ||
| 246 | /// of construction | ||
| 247 | /// of the simplex are added. To know more about these, visit | ||
| 248 | /// https://caseymuratori.com/blog_0003. | ||
| 249 | bool projectLineOrigin(const Simplex& current, Simplex& next); | ||
| 250 | |||
| 251 | /// @brief Project origin (0) onto triangle a-b-c | ||
| 252 | /// See \ref projectLineOrigin for an explanation on simplex projections. | ||
| 253 | bool projectTriangleOrigin(const Simplex& current, Simplex& next); | ||
| 254 | |||
| 255 | /// @brief Project origin (0) onto tetrahedron a-b-c-d | ||
| 256 | /// See \ref projectLineOrigin for an explanation on simplex projections. | ||
| 257 | bool projectTetrahedraOrigin(const Simplex& current, Simplex& next); | ||
| 258 | }; | ||
| 259 | |||
| 260 | /// @brief class for EPA algorithm | ||
| 261 | struct COAL_DLLAPI EPA { | ||
| 262 | typedef GJK::SimplexV SimplexVertex; | ||
| 263 | struct COAL_DLLAPI SimplexFace { | ||
| 264 | Vec3ps n; | ||
| 265 | SolverScalar d; | ||
| 266 | bool ignore; // If the origin does not project inside the face, we | ||
| 267 | // ignore this face. | ||
| 268 | size_t vertex_id[3]; // Index of vertex in sv_store. | ||
| 269 | SimplexFace* adjacent_faces[3]; // A face has three adjacent faces. | ||
| 270 | SimplexFace* prev_face; // The previous face in the list. | ||
| 271 | SimplexFace* next_face; // The next face in the list. | ||
| 272 | size_t adjacent_edge[3]; // Each face has 3 edges: `0`, `1` and `2`. | ||
| 273 | // The i-th adjacent face is bound (to this face) | ||
| 274 | // along its `adjacent_edge[i]`-th edge | ||
| 275 | // (with 0 <= i <= 2). | ||
| 276 | size_t pass; | ||
| 277 | |||
| 278 |
1/2✓ Branch 2 taken 122686172 times.
✗ Branch 3 not taken.
|
122686172 | SimplexFace() : n(Vec3ps::Zero()), ignore(false) {}; |
| 279 | }; | ||
| 280 | |||
| 281 | /// @brief The simplex list of EPA is a linked list of faces. | ||
| 282 | /// Note: EPA's linked list does **not** own any memory. | ||
| 283 | /// The memory it refers to is contiguous and owned by a std::vector. | ||
| 284 | struct COAL_DLLAPI SimplexFaceList { | ||
| 285 | SimplexFace* root; | ||
| 286 | size_t count; | ||
| 287 | 60951830 | SimplexFaceList() : root(nullptr), count(0) {} | |
| 288 | |||
| 289 | 60960716 | void reset() { | |
| 290 | 60960716 | root = nullptr; | |
| 291 | 60960716 | count = 0; | |
| 292 | 60960716 | } | |
| 293 | |||
| 294 | 125289020 | void append(SimplexFace* face) { | |
| 295 | 125289020 | face->prev_face = nullptr; | |
| 296 | 125289020 | face->next_face = root; | |
| 297 |
2/2✓ Branch 0 taken 94804205 times.
✓ Branch 1 taken 30484815 times.
|
125289020 | if (root != nullptr) root->prev_face = face; |
| 298 | 125289020 | root = face; | |
| 299 | 125289020 | ++count; | |
| 300 | 125289020 | } | |
| 301 | |||
| 302 | 874740 | void remove(SimplexFace* face) { | |
| 303 |
2/2✓ Branch 0 taken 861772 times.
✓ Branch 1 taken 12968 times.
|
874740 | if (face->next_face != nullptr) |
| 304 | 861772 | face->next_face->prev_face = face->prev_face; | |
| 305 |
2/2✓ Branch 0 taken 308905 times.
✓ Branch 1 taken 565835 times.
|
874740 | if (face->prev_face != nullptr) |
| 306 | 308905 | face->prev_face->next_face = face->next_face; | |
| 307 |
2/2✓ Branch 0 taken 565835 times.
✓ Branch 1 taken 308905 times.
|
874740 | if (face == root) root = face->next_face; |
| 308 | 874740 | --count; | |
| 309 | 874740 | } | |
| 310 | }; | ||
| 311 | |||
| 312 | /// @brief We bind the face `fa` along its edge `ea` to the face `fb` along | ||
| 313 | /// its edge `fb`. | ||
| 314 | 1122767 | static inline void bind(SimplexFace* fa, size_t ea, SimplexFace* fb, | |
| 315 | size_t eb) { | ||
| 316 |
5/6✓ Branch 0 taken 570267 times.
✓ Branch 1 taken 552500 times.
✓ Branch 2 taken 561377 times.
✓ Branch 3 taken 8890 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 561377 times.
|
1122767 | assert(ea == 0 || ea == 1 || ea == 2); |
| 317 |
5/6✓ Branch 0 taken 936102 times.
✓ Branch 1 taken 186665 times.
✓ Branch 2 taken 191947 times.
✓ Branch 3 taken 744155 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 191947 times.
|
1122767 | assert(eb == 0 || eb == 1 || eb == 2); |
| 318 | 1122767 | fa->adjacent_edge[ea] = eb; | |
| 319 | 1122767 | fa->adjacent_faces[ea] = fb; | |
| 320 | 1122767 | fb->adjacent_edge[eb] = ea; | |
| 321 | 1122767 | fb->adjacent_faces[eb] = fa; | |
| 322 | 1122767 | } | |
| 323 | |||
| 324 | struct COAL_DLLAPI SimplexHorizon { | ||
| 325 | SimplexFace* current_face; // current face in the horizon | ||
| 326 | SimplexFace* first_face; // first face in the horizon | ||
| 327 | size_t num_faces; // number of faces in the horizon | ||
| 328 | 124007 | SimplexHorizon() | |
| 329 | 124007 | : current_face(nullptr), first_face(nullptr), num_faces(0) {} | |
| 330 | }; | ||
| 331 | |||
| 332 | enum Status { | ||
| 333 | DidNotRun = -1, | ||
| 334 | Failed = 0, | ||
| 335 | Valid = 1, | ||
| 336 | AccuracyReached = 1 << 1 | Valid, | ||
| 337 | Degenerated = 1 << 1 | Failed, | ||
| 338 | NonConvex = 2 << 1 | Failed, | ||
| 339 | InvalidHull = 3 << 1 | Failed, | ||
| 340 | OutOfFaces = 4 << 1 | Failed, | ||
| 341 | OutOfVertices = 5 << 1 | Failed, | ||
| 342 | FallBack = 6 << 1 | Failed | ||
| 343 | }; | ||
| 344 | |||
| 345 | public: | ||
| 346 | Status status; | ||
| 347 | GJK::Simplex result; | ||
| 348 | Vec3ps normal; | ||
| 349 | support_func_guess_t support_hint; | ||
| 350 | SolverScalar depth; | ||
| 351 | SimplexFace* closest_face; | ||
| 352 | |||
| 353 | private: | ||
| 354 | // max_iteration and tolerance are made private | ||
| 355 | // because they are meant to be set by the `reset` function. | ||
| 356 | size_t max_iterations; | ||
| 357 | SolverScalar tolerance; | ||
| 358 | |||
| 359 | std::vector<SimplexVertex> sv_store; | ||
| 360 | std::vector<SimplexFace> fc_store; | ||
| 361 | SimplexFaceList hull, stock; | ||
| 362 | size_t num_vertices; // number of vertices in polytpoe constructed by EPA | ||
| 363 | size_t iterations; | ||
| 364 | |||
| 365 | public: | ||
| 366 | 30475915 | EPA(size_t max_iterations_, SolverScalar tolerance_) | |
| 367 | 30475915 | : max_iterations(max_iterations_), tolerance(tolerance_) { | |
| 368 |
1/2✓ Branch 1 taken 30475915 times.
✗ Branch 2 not taken.
|
30475915 | initialize(); |
| 369 | 30475915 | } | |
| 370 | |||
| 371 | /// @brief Copy constructor of EPA. | ||
| 372 | /// Mostly needed for the copy constructor of `GJKSolver`. | ||
| 373 | ✗ | EPA(const EPA& other) | |
| 374 | ✗ | : max_iterations(other.max_iterations), | |
| 375 | ✗ | tolerance(other.tolerance), | |
| 376 | ✗ | sv_store(other.sv_store), | |
| 377 | ✗ | fc_store(other.fc_store) { | |
| 378 | ✗ | initialize(); | |
| 379 | } | ||
| 380 | |||
| 381 | /// @brief Get the max number of iterations of EPA. | ||
| 382 | size_t getNumMaxIterations() const { return max_iterations; } | ||
| 383 | |||
| 384 | /// @brief Get the max number of vertices of EPA. | ||
| 385 | size_t getNumMaxVertices() const { return sv_store.size(); } | ||
| 386 | |||
| 387 | /// @brief Get the max number of faces of EPA. | ||
| 388 | size_t getNumMaxFaces() const { return fc_store.size(); } | ||
| 389 | |||
| 390 | /// @brief Get the tolerance of EPA. | ||
| 391 | 4430 | SolverScalar getTolerance() const { return tolerance; } | |
| 392 | |||
| 393 | /// @brief Get the number of iterations of the last run of EPA. | ||
| 394 | size_t getNumIterations() const { return iterations; } | ||
| 395 | |||
| 396 | /// @brief Get the number of vertices in the polytope of the last run of EPA. | ||
| 397 | size_t getNumVertices() const { return num_vertices; } | ||
| 398 | |||
| 399 | /// @brief Get the number of faces in the polytope of the last run of EPA. | ||
| 400 | size_t getNumFaces() const { return hull.count; } | ||
| 401 | |||
| 402 | /// @brief resets the EPA algorithm, preparing it for a new run. | ||
| 403 | /// It potentially reallocates memory for the vertices and faces | ||
| 404 | /// if the passed parameters are bigger than the previous ones. | ||
| 405 | /// This function does **not** modify the parameters of the EPA algorithm, | ||
| 406 | /// i.e. the maximum number of iterations and the tolerance. | ||
| 407 | /// @note calling this function destroys the previous state of EPA. | ||
| 408 | /// In the future, we may want to copy it instead, i.e. when EPA will | ||
| 409 | /// be (properly) warm-startable. | ||
| 410 | void reset(size_t max_iterations, SolverScalar tolerance); | ||
| 411 | |||
| 412 | /// \return a Status which can be demangled using (status & Valid) or | ||
| 413 | /// (status & Failed). The other values provide a more detailled | ||
| 414 | /// status | ||
| 415 | Status evaluate(GJK& gjk, const Vec3ps& guess); | ||
| 416 | |||
| 417 | /// Get the witness points on each object, and the corresponding normal. | ||
| 418 | /// @param[in] shape is the Minkowski difference of the two shapes. | ||
| 419 | /// @param[out] w0 is the witness point on shape0. | ||
| 420 | /// @param[out] w1 is the witness point on shape1. | ||
| 421 | /// @param[in] normal is the normal found by EPA. It points from shape0 to | ||
| 422 | /// shape1. The normal is used to correct the witness points on the shapes if | ||
| 423 | /// the shapes have a non-zero swept-sphere radius. | ||
| 424 | void getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3ps& w0, | ||
| 425 | Vec3ps& w1, Vec3ps& normal) const; | ||
| 426 | |||
| 427 | private: | ||
| 428 | /// @brief Allocates memory for the EPA algorithm. | ||
| 429 | /// This function should only be called by the constructor. | ||
| 430 | /// Otherwise use \ref reset. | ||
| 431 | void initialize(); | ||
| 432 | |||
| 433 | bool getEdgeDist(SimplexFace* face, const SimplexVertex& a, | ||
| 434 | const SimplexVertex& b, SolverScalar& dist); | ||
| 435 | |||
| 436 | /// @brief Add a new face to the polytope. | ||
| 437 | /// This function sets the `ignore` flag to `true` if the origin does not | ||
| 438 | /// project inside the face. | ||
| 439 | SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex, | ||
| 440 | bool force = false); | ||
| 441 | |||
| 442 | /// @brief Find the best polytope face to split | ||
| 443 | SimplexFace* findClosestFace(); | ||
| 444 | |||
| 445 | /// @brief the goal is to add a face connecting vertex w and face edge f[e] | ||
| 446 | bool expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, | ||
| 447 | SimplexHorizon& horizon); | ||
| 448 | |||
| 449 | // @brief Use this function to debug expand if needed. | ||
| 450 | // void PrintExpandLooping(const SimplexFace* f, const SimplexVertex& w); | ||
| 451 | }; | ||
| 452 | |||
| 453 | } // namespace details | ||
| 454 | |||
| 455 | } // namespace coal | ||
| 456 | |||
| 457 | #endif | ||
| 458 |