Public Member Functions | Public Attributes | Protected Member Functions | Static Private Attributes | List of all members
coal::GJKSolver Struct Reference

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>

Inheritance diagram for coal::GJKSolver:
Inheritance graph
[legend]

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
 

Detailed Description

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.

Constructor & Destructor Documentation

◆ GJKSolver() [1/4]

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.

◆ GJKSolver() [2/4]

coal::GJKSolver::GJKSolver ( const DistanceRequest request)
inlineexplicit

Constructor from a DistanceRequest.

Parameters
[in]requestDistanceRequest 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.

◆ GJKSolver() [3/4]

coal::GJKSolver::GJKSolver ( const CollisionRequest request)
inlineexplicit

Constructor from a CollisionRequest.

Parameters
[in]requestCollisionRequest 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.

◆ GJKSolver() [4/4]

coal::GJKSolver::GJKSolver ( const GJKSolver other)
default

Copy constructor.

Member Function Documentation

◆ EPAExtractWitnessPointsAndNormal()

void coal::GJKSolver::EPAExtractWitnessPointsAndNormal ( const Transform3s tf1,
CoalScalar distance,
Vec3s p1,
Vec3s p2,
Vec3s normal 
) const
inlineprotected

Definition at line 657 of file coal/narrowphase/narrowphase.h.

◆ EPAFailedExtractWitnessPointsAndNormal()

void coal::GJKSolver::EPAFailedExtractWitnessPointsAndNormal ( const Transform3s tf1,
CoalScalar distance,
Vec3s p1,
Vec3s p2,
Vec3s normal 
) const
inlineprotected

Definition at line 712 of file coal/narrowphase/narrowphase.h.

◆ getDistancePrecision()

CoalScalar coal::GJKSolver::getDistancePrecision ( const bool  compute_penetration) const
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.

◆ getGJKInitialGuess()

template<typename S1 , typename S2 >
void coal::GJKSolver::getGJKInitialGuess ( const S1 &  s1,
const S2 &  s2,
Vec3s guess,
support_func_guess_t support_hint,
const Vec3s default_guess = Vec3s(1, 0, 0) 
) const
inlineprotected

initialize GJK. This method assumes minkowski_difference has been set.

Definition at line 354 of file coal/narrowphase/narrowphase.h.

◆ GJKCollisionExtractWitnessPointsAndNormal()

void coal::GJKSolver::GJKCollisionExtractWitnessPointsAndNormal ( const Transform3s tf1,
CoalScalar distance,
Vec3s p1,
Vec3s p2,
Vec3s normal 
) const
inlineprotected

Definition at line 637 of file coal/narrowphase/narrowphase.h.

◆ GJKEarlyStopExtractWitnessPointsAndNormal()

void coal::GJKSolver::GJKEarlyStopExtractWitnessPointsAndNormal ( const Transform3s tf1,
CoalScalar distance,
Vec3s p1,
Vec3s p2,
Vec3s normal 
) const
inlineprotected

Definition at line 589 of file coal/narrowphase/narrowphase.h.

◆ GJKExtractWitnessPointsAndNormal()

void coal::GJKSolver::GJKExtractWitnessPointsAndNormal ( const Transform3s tf1,
CoalScalar distance,
Vec3s p1,
Vec3s p2,
Vec3s normal 
) const
inlineprotected

Definition at line 610 of file coal/narrowphase/narrowphase.h.

◆ operator!=()

COAL_COMPILER_DIAGNOSTIC_POP bool coal::GJKSolver::operator!= ( const GJKSolver other) const
inline

Definition at line 268 of file coal/narrowphase/narrowphase.h.

◆ operator==()

COAL_COMPILER_DIAGNOSTIC_PUSH COAL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool coal::GJKSolver::operator== ( const GJKSolver other) const
inline

Definition at line 250 of file coal/narrowphase/narrowphase.h.

◆ runGJKAndEPA()

template<typename S1 , typename S2 , int _SupportOptions = details::SupportOptions::NoSweptSphere>
void coal::GJKSolver::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
inlineprotected

Runs the GJK algorithm.

Parameters
<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.
Template Parameters
SupportOptions,seeMinkowskiDiff::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.
Returns
the estimate of the distance between the two shapes.
Note
: The variables 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.

◆ set() [1/2]

void coal::GJKSolver::set ( const CollisionRequest request)
inline

setter from a CollisionRequest

Parameters
[in]requestCollisionRequest input

Definition at line 213 of file coal/narrowphase/narrowphase.h.

◆ set() [2/2]

void coal::GJKSolver::set ( const DistanceRequest request)
inline

setter from a DistanceRequest

Parameters
[in]requestDistanceRequest input

Definition at line 161 of file coal/narrowphase/narrowphase.h.

◆ shapeDistance() [1/3]

template<typename S1 , typename S2 >
CoalScalar coal::GJKSolver::shapeDistance ( const S1 &  s1,
const Transform3s tf1,
const S2 &  s2,
const Transform3s tf2,
const bool  compute_penetration,
Vec3s p1,
Vec3s p2,
Vec3s normal 
) const
inline

Uses GJK and EPA to compute the distance between two shapes.

Parameters
<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.
Returns
the estimate of the distance between the two shapes.
Note
: if 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.
: the variables this->gjk.status and this->epa.status can be used to examine the status of GJK and EPA.
: GJK and EPA give an estimate of the distance between the two shapes. This estimate is precise up to the tolerance of the algorithms:
  • If compute_penetration is false, the distance is precise up to gjk_tolerance.
  • If 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.

◆ shapeDistance() [2/3]

template<typename S1 >
CoalScalar coal::GJKSolver::shapeDistance ( const S1 &  s1,
const Transform3s tf1,
const TriangleP s2,
const Transform3s tf2,
const bool  compute_penetration,
Vec3s p1,
Vec3s p2,
Vec3s normal 
) const
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.

◆ shapeDistance() [3/3]

template<typename S2 >
CoalScalar coal::GJKSolver::shapeDistance ( const TriangleP s1,
const Transform3s tf1,
const S2 &  s2,
const Transform3s tf2,
const bool  compute_penetration,
Vec3s p1,
Vec3s p2,
Vec3s normal 
) const
inline

See other partial template specialization of shapeDistance above.

Definition at line 340 of file coal/narrowphase/narrowphase.h.

Member Data Documentation

◆ cached_guess

Vec3s coal::GJKSolver::cached_guess
mutable

smart guess

Definition at line 79 of file coal/narrowphase/narrowphase.h.

◆ distance_upper_bound

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.

◆ enable_cached_guess

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.

◆ epa

details::EPA coal::GJKSolver::epa
mutable

EPA algorithm.

Definition at line 98 of file coal/narrowphase/narrowphase.h.

◆ epa_max_iterations

size_t coal::GJKSolver::epa_max_iterations

maximum number of iterations of EPA

Definition at line 101 of file coal/narrowphase/narrowphase.h.

◆ epa_tolerance

CoalScalar coal::GJKSolver::epa_tolerance

tolerance of EPA

Definition at line 104 of file coal/narrowphase/narrowphase.h.

◆ gjk

EIGEN_MAKE_ALIGNED_OPERATOR_NEW details::GJK coal::GJKSolver::gjk
mutable

GJK algorithm.

Definition at line 62 of file coal/narrowphase/narrowphase.h.

◆ gjk_convergence_criterion

GJKConvergenceCriterion coal::GJKSolver::gjk_convergence_criterion

Convergence criterion for GJK.

Definition at line 92 of file coal/narrowphase/narrowphase.h.

◆ gjk_convergence_criterion_type

GJKConvergenceCriterionType coal::GJKSolver::gjk_convergence_criterion_type

Absolute or relative convergence criterion for GJK.

Definition at line 95 of file coal/narrowphase/narrowphase.h.

◆ gjk_initial_guess

GJKInitialGuess coal::GJKSolver::gjk_initial_guess

which warm start to use for GJK

Definition at line 71 of file coal/narrowphase/narrowphase.h.

◆ gjk_max_iterations

size_t coal::GJKSolver::gjk_max_iterations

maximum number of iterations of GJK

Definition at line 65 of file coal/narrowphase/narrowphase.h.

◆ gjk_tolerance

CoalScalar coal::GJKSolver::gjk_tolerance

tolerance of GJK

Definition at line 68 of file coal/narrowphase/narrowphase.h.

◆ gjk_variant

GJKVariant coal::GJKSolver::gjk_variant

Variant of the GJK algorithm (Default, Nesterov or Polyak).

Definition at line 89 of file coal/narrowphase/narrowphase.h.

◆ m_dummy_precision

constexpr CoalScalar coal::GJKSolver::m_dummy_precision = 1e-6
staticconstexprprivate

Definition at line 111 of file coal/narrowphase/narrowphase.h.

◆ minkowski_difference

details::MinkowskiDiff coal::GJKSolver::minkowski_difference
mutable

Minkowski difference used by GJK and EPA algorithms.

Definition at line 107 of file coal/narrowphase/narrowphase.h.

◆ support_func_cached_guess

support_func_guess_t coal::GJKSolver::support_func_cached_guess
mutable

smart guess for the support function

Definition at line 82 of file coal/narrowphase/narrowphase.h.


The documentation for this struct was generated from the following file:


hpp-fcl
Author(s):
autogenerated on Sat Nov 23 2024 03:45:00