GCC Code Coverage Report


Directory: ./
File: include/coal/narrowphase/gjk.h
Date: 2025-04-01 09:23:31
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 91428122 Simplex() {}
78
79 30480359 void reset() {
80 30480359 rank = 0;
81
2/2
✓ Branch 0 taken 121921436 times.
✓ Branch 1 taken 30480359 times.
152401795 for (size_t i = 0; i < 4; ++i) vertex[i] = nullptr;
82 30480359 }
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 30476103 GJK(size_t max_iterations_, SolverScalar tolerance_)
144
4/4
✓ Branch 3 taken 121904412 times.
✓ Branch 4 taken 30476103 times.
✓ Branch 6 taken 60952206 times.
✓ Branch 7 taken 30476103 times.
213332721 : max_iterations(max_iterations_), tolerance(tolerance_) {
145
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 30476103 times.
30476103 COAL_ASSERT(tolerance_ > 0, "Tolerance must be positive.",
146 std::invalid_argument);
147 30476103 initialize();
148 30476103 }
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 72451398 inline void getSupport(const Vec3ps& d, SimplexV& sv,
163 support_func_guess_t& hint) const {
164
2/4
✓ Branch 1 taken 72451398 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451398 times.
✗ Branch 5 not taken.
72451398 Vec3s w0, w1;
165
2/4
✓ Branch 1 taken 72451398 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451398 times.
✗ Branch 5 not taken.
72451398 shape->support(d.cast<Scalar>(), w0, w1, hint);
166
2/4
✓ Branch 1 taken 72451398 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451398 times.
✗ Branch 5 not taken.
72451398 sv.w0 = w0.cast<SolverScalar>();
167
2/4
✓ Branch 1 taken 72451398 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451398 times.
✗ Branch 5 not taken.
72451398 sv.w1 = w1.cast<SolverScalar>();
168
2/4
✓ Branch 1 taken 72451398 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 72451398 times.
✗ Branch 5 not taken.
72451398 sv.w = sv.w0 - sv.w1;
169 72451398 }
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 122686176 times.
✗ Branch 3 not taken.
122686176 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 60951832 SimplexFaceList() : root(nullptr), count(0) {}
288
289 60960718 void reset() {
290 60960718 root = nullptr;
291 60960718 count = 0;
292 60960718 }
293
294 125289024 void append(SimplexFace* face) {
295 125289024 face->prev_face = nullptr;
296 125289024 face->next_face = root;
297
2/2
✓ Branch 0 taken 94804208 times.
✓ Branch 1 taken 30484816 times.
125289024 if (root != nullptr) root->prev_face = face;
298 125289024 root = face;
299 125289024 ++count;
300 125289024 }
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 30475916 EPA(size_t max_iterations_, SolverScalar tolerance_)
367 30475916 : max_iterations(max_iterations_), tolerance(tolerance_) {
368
1/2
✓ Branch 1 taken 30475916 times.
✗ Branch 2 not taken.
30475916 initialize();
369 30475916 }
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