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 |