collision and distance solver based on the GJK and EPA algorithms. Originally, GJK and EPA were implemented in fcl which itself took inspiration from the code of the GJK in bullet. Since then, both GJK and EPA have been largely modified to be faster and more robust to numerical accuracy and edge cases. More...
#include <narrowphase.h>
Public Member Functions | |
CoalScalar | getDistancePrecision (const bool compute_penetration) const |
Helper to return the precision of the solver on the distance estimate, depending on whether or not compute_penetration is true. More... | |
COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | GJKSolver () |
Default constructor for GJK algorithm By default, we don't want EPA to allocate memory because certain functions of the GJKSolver class have specializations which don't use EPA (and/or GJK). So we give EPA's constructor a max number of iterations of zero. Only the functions that need EPA will reset the algorithm and allocate memory if needed. More... | |
GJKSolver (const CollisionRequest &request) | |
Constructor from a CollisionRequest. More... | |
GJKSolver (const DistanceRequest &request) | |
Constructor from a DistanceRequest. More... | |
GJKSolver (const GJKSolver &other)=default | |
Copy constructor. More... | |
COAL_COMPILER_DIAGNOSTIC_POP bool | operator!= (const GJKSolver &other) const |
COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool | operator== (const GJKSolver &other) const |
void | set (const CollisionRequest &request) |
setter from a CollisionRequest More... | |
void | set (const DistanceRequest &request) |
setter from a DistanceRequest More... | |
template<typename S1 , typename S2 > | |
CoalScalar | shapeDistance (const S1 &s1, const Transform3s &tf1, const S2 &s2, const Transform3s &tf2, const bool compute_penetration, Vec3s &p1, Vec3s &p2, Vec3s &normal) const |
Uses GJK and EPA to compute the distance between two shapes. More... | |
template<typename S1 > | |
CoalScalar | shapeDistance (const S1 &s1, const Transform3s &tf1, const TriangleP &s2, const Transform3s &tf2, const bool compute_penetration, Vec3s &p1, Vec3s &p2, Vec3s &normal) const |
Partial specialization of shapeDistance for the case where the second shape is a triangle. It is more efficient to pre-compute the relative transformation between the two shapes before calling GJK/EPA. More... | |
template<typename S2 > | |
CoalScalar | shapeDistance (const TriangleP &s1, const Transform3s &tf1, const S2 &s2, const Transform3s &tf2, const bool compute_penetration, Vec3s &p1, Vec3s &p2, Vec3s &normal) const |
See other partial template specialization of shapeDistance above. More... | |
Public Attributes | |
Vec3s | cached_guess |
smart guess More... | |
CoalScalar | distance_upper_bound |
If GJK can guarantee that the distance between the shapes is greater than this value, it will early stop. More... | |
bool | enable_cached_guess |
Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead. More... | |
details::EPA | epa |
EPA algorithm. More... | |
size_t | epa_max_iterations |
maximum number of iterations of EPA More... | |
CoalScalar | epa_tolerance |
tolerance of EPA More... | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW details::GJK | gjk |
GJK algorithm. More... | |
GJKConvergenceCriterion | gjk_convergence_criterion |
Convergence criterion for GJK. More... | |
GJKConvergenceCriterionType | gjk_convergence_criterion_type |
Absolute or relative convergence criterion for GJK. More... | |
GJKInitialGuess | gjk_initial_guess |
which warm start to use for GJK More... | |
size_t | gjk_max_iterations |
maximum number of iterations of GJK More... | |
CoalScalar | gjk_tolerance |
tolerance of GJK More... | |
GJKVariant | gjk_variant |
Variant of the GJK algorithm (Default, Nesterov or Polyak). More... | |
details::MinkowskiDiff | minkowski_difference |
Minkowski difference used by GJK and EPA algorithms. More... | |
support_func_guess_t | support_func_cached_guess |
smart guess for the support function More... | |
Protected Member Functions | |
void | EPAExtractWitnessPointsAndNormal (const Transform3s &tf1, CoalScalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const |
void | EPAFailedExtractWitnessPointsAndNormal (const Transform3s &tf1, CoalScalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const |
template<typename S1 , typename S2 > | |
void | getGJKInitialGuess (const S1 &s1, const S2 &s2, Vec3s &guess, support_func_guess_t &support_hint, const Vec3s &default_guess=Vec3s(1, 0, 0)) const |
initialize GJK. This method assumes minkowski_difference has been set. More... | |
void | GJKCollisionExtractWitnessPointsAndNormal (const Transform3s &tf1, CoalScalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const |
void | GJKEarlyStopExtractWitnessPointsAndNormal (const Transform3s &tf1, CoalScalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const |
void | GJKExtractWitnessPointsAndNormal (const Transform3s &tf1, CoalScalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const |
template<typename S1 , typename S2 , int _SupportOptions = details::SupportOptions::NoSweptSphere> | |
void | runGJKAndEPA (const S1 &s1, const Transform3s &tf1, const S2 &s2, const Transform3s &tf2, const bool compute_penetration, CoalScalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal, const bool relative_transformation_already_computed=false) const |
Runs the GJK algorithm. More... | |
Static Private Attributes | |
static constexpr CoalScalar | m_dummy_precision = 1e-6 |
collision and distance solver based on the GJK and EPA algorithms. Originally, GJK and EPA were implemented in fcl which itself took inspiration from the code of the GJK in bullet. Since then, both GJK and EPA have been largely modified to be faster and more robust to numerical accuracy and edge cases.
Definition at line 57 of file coal/narrowphase/narrowphase.h.
|
inline |
Default constructor for GJK algorithm By default, we don't want EPA to allocate memory because certain functions of the GJKSolver
class have specializations which don't use EPA (and/or GJK). So we give EPA's constructor a max number of iterations of zero. Only the functions that need EPA will reset the algorithm and allocate memory if needed.
Definition at line 123 of file coal/narrowphase/narrowphase.h.
|
inlineexplicit |
Constructor from a DistanceRequest.
[in] | request | DistanceRequest input |
See the default constructor; by default, we don't want EPA to allocate memory so we call EPA's constructor with 0 max number of iterations. However, the set
method stores the actual values of the request. EPA will thus allocate memory only if needed.
Definition at line 148 of file coal/narrowphase/narrowphase.h.
|
inlineexplicit |
Constructor from a CollisionRequest.
[in] | request | CollisionRequest input |
See the default constructor; by default, we don't want EPA to allocate memory so we call EPA's constructor with 0 max number of iterations. However, the set
method stores the actual values of the request. EPA will thus allocate memory only if needed.
Definition at line 200 of file coal/narrowphase/narrowphase.h.
|
default |
Copy constructor.
|
inlineprotected |
Definition at line 657 of file coal/narrowphase/narrowphase.h.
|
inlineprotected |
Definition at line 712 of file coal/narrowphase/narrowphase.h.
|
inline |
Helper to return the precision of the solver on the distance estimate, depending on whether or not compute_penetration
is true.
Definition at line 272 of file coal/narrowphase/narrowphase.h.
|
inlineprotected |
initialize GJK. This method assumes minkowski_difference
has been set.
Definition at line 354 of file coal/narrowphase/narrowphase.h.
|
inlineprotected |
Definition at line 637 of file coal/narrowphase/narrowphase.h.
|
inlineprotected |
Definition at line 589 of file coal/narrowphase/narrowphase.h.
|
inlineprotected |
Definition at line 610 of file coal/narrowphase/narrowphase.h.
|
inline |
Definition at line 268 of file coal/narrowphase/narrowphase.h.
|
inline |
Definition at line 250 of file coal/narrowphase/narrowphase.h.
|
inlineprotected |
Runs the GJK algorithm.
<tt>s1</tt> | the first shape. | |
<tt>tf1</tt> | the transformation of the first shape. | |
<tt>s2</tt> | the second shape. | |
<tt>tf2</tt> | the transformation of the second shape. | |
<tt>compute_penetration</tt> | if true and if the shapes are in found in collision, the EPA algorithm is also ran to compute penetration information. | |
[out] | <tt>distance</tt> | the distance between the two shapes. |
[out] | <tt>p1</tt> | the witness point on the first shape. |
[out] | <tt>p2</tt> | the witness point on the second shape. |
[out] | <tt>normal</tt> | the normal of the collision, pointing from the first to the second shape. |
<tt>relative_transformation_already_computed</tt> | whether the relative transformation between the two shapes has already been computed. |
SupportOptions,see | MinkowskiDiff::set . Whether the support computations should take into account the shapes' swept-sphere radii during the iterations of GJK and EPA. Please leave this default value to false unless you know what you are doing. This template parameter is only used for debugging/testing purposes. In short, there is no need to take into account the swept sphere radius when computing supports in the iterations of GJK and EPA. GJK and EPA will correct the solution once they have converged. |
this->gjk.status
and this->epa.status
can be used to examine the status of GJK and EPA. Definition at line 422 of file coal/narrowphase/narrowphase.h.
|
inline |
setter from a CollisionRequest
[in] | request | CollisionRequest input |
Definition at line 213 of file coal/narrowphase/narrowphase.h.
|
inline |
setter from a DistanceRequest
[in] | request | DistanceRequest input |
Definition at line 161 of file coal/narrowphase/narrowphase.h.
|
inline |
Uses GJK and EPA to compute the distance between two shapes.
<tt>s1</tt> | the first shape. | |
<tt>tf1</tt> | the transformation of the first shape. | |
<tt>s2</tt> | the second shape. | |
<tt>tf2</tt> | the transformation of the second shape. | |
<tt>compute_penetration</tt> | if true and GJK finds the shape in collision, the EPA algorithm is also ran to compute penetration information. | |
[out] | <tt>p1</tt> | the witness point on the first shape. |
[out] | <tt>p2</tt> | the witness point on the second shape. |
[out] | <tt>normal</tt> | the normal of the collision, pointing from the first to the second shape. |
this->distance_upper_bound
is set to a positive value, GJK will early stop if it finds the distance to be above this value. The distance returned by this->shapeDistance
will be a lower bound on the distance between the two shapes.this->gjk.status
and this->epa.status
can be used to examine the status of GJK and EPA.compute_penetration
is false, the distance is precise up to gjk_tolerance
.compute_penetration
is true, the distance is precise up to std::max(gjk_tolerance, epa_tolerance)
It's up to the user to decide whether the shapes are in collision or not, based on that estimate. Definition at line 308 of file coal/narrowphase/narrowphase.h.
|
inline |
Partial specialization of shapeDistance
for the case where the second shape is a triangle. It is more efficient to pre-compute the relative transformation between the two shapes before calling GJK/EPA.
Definition at line 323 of file coal/narrowphase/narrowphase.h.
|
inline |
See other partial template specialization of shapeDistance above.
Definition at line 340 of file coal/narrowphase/narrowphase.h.
|
mutable |
smart guess
Definition at line 79 of file coal/narrowphase/narrowphase.h.
CoalScalar coal::GJKSolver::distance_upper_bound |
If GJK can guarantee that the distance between the shapes is greater than this value, it will early stop.
Definition at line 86 of file coal/narrowphase/narrowphase.h.
bool coal::GJKSolver::enable_cached_guess |
Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.
Definition at line 76 of file coal/narrowphase/narrowphase.h.
|
mutable |
EPA algorithm.
Definition at line 98 of file coal/narrowphase/narrowphase.h.
size_t coal::GJKSolver::epa_max_iterations |
maximum number of iterations of EPA
Definition at line 101 of file coal/narrowphase/narrowphase.h.
CoalScalar coal::GJKSolver::epa_tolerance |
tolerance of EPA
Definition at line 104 of file coal/narrowphase/narrowphase.h.
|
mutable |
GJK algorithm.
Definition at line 62 of file coal/narrowphase/narrowphase.h.
GJKConvergenceCriterion coal::GJKSolver::gjk_convergence_criterion |
Convergence criterion for GJK.
Definition at line 92 of file coal/narrowphase/narrowphase.h.
GJKConvergenceCriterionType coal::GJKSolver::gjk_convergence_criterion_type |
Absolute or relative convergence criterion for GJK.
Definition at line 95 of file coal/narrowphase/narrowphase.h.
GJKInitialGuess coal::GJKSolver::gjk_initial_guess |
which warm start to use for GJK
Definition at line 71 of file coal/narrowphase/narrowphase.h.
size_t coal::GJKSolver::gjk_max_iterations |
maximum number of iterations of GJK
Definition at line 65 of file coal/narrowphase/narrowphase.h.
CoalScalar coal::GJKSolver::gjk_tolerance |
tolerance of GJK
Definition at line 68 of file coal/narrowphase/narrowphase.h.
GJKVariant coal::GJKSolver::gjk_variant |
Variant of the GJK algorithm (Default, Nesterov or Polyak).
Definition at line 89 of file coal/narrowphase/narrowphase.h.
|
staticconstexprprivate |
Definition at line 111 of file coal/narrowphase/narrowphase.h.
|
mutable |
Minkowski difference used by GJK and EPA algorithms.
Definition at line 107 of file coal/narrowphase/narrowphase.h.
|
mutable |
smart guess for the support function
Definition at line 82 of file coal/narrowphase/narrowphase.h.