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 <coal/narrowphase/narrowphase.h>

Collaboration diagram for coal::GJKSolver:

[legend]

Public Member Functions
	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.

	GJKSolver (const DistanceRequest &request)
	Constructor from a DistanceRequest.

void	set (const DistanceRequest &request)
	setter from a DistanceRequest

	GJKSolver (const CollisionRequest &request)
	Constructor from a CollisionRequest.

void	set (const CollisionRequest &request)
	setter from a CollisionRequest

	GJKSolver (const GJKSolver &other)=default
	Copy constructor.

bool	operator== (const GJKSolver &other) const

bool	operator!= (const GJKSolver &other) const

Scalar	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.

template<typename S1 , typename S2 >
Scalar	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.

template<typename S1 >
Scalar	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.

template<typename S2 >
Scalar	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.

Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW details::GJK	gjk
	GJK algorithm.

size_t	gjk_max_iterations
	maximum number of iterations of GJK

Scalar	gjk_tolerance
	tolerance of GJK

GJKInitialGuess	gjk_initial_guess
	which warm start to use for GJK

bool	enable_cached_guess
	Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.

Vec3s	cached_guess
	smart guess

support_func_guess_t	support_func_cached_guess
	smart guess for the support function

Scalar	distance_upper_bound
	If GJK can guarantee that the distance between the shapes is greater than this value, it will early stop.

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

GJKConvergenceCriterion	gjk_convergence_criterion
	Convergence criterion for GJK.

GJKConvergenceCriterionType	gjk_convergence_criterion_type
	Absolute or relative convergence criterion for GJK.

details::EPA	epa
	EPA algorithm.

size_t	epa_max_iterations
	maximum number of iterations of EPA

Scalar	epa_tolerance
	tolerance of EPA

details::MinkowskiDiff	minkowski_difference
	Minkowski difference used by GJK and EPA algorithms.

Protected Member Functions
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.

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, Scalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal, const bool relative_transformation_already_computed=false) const
	Runs the GJK algorithm.

void	GJKEarlyStopExtractWitnessPointsAndNormal (const Transform3s &tf1, Scalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const

void	GJKExtractWitnessPointsAndNormal (const Transform3s &tf1, Scalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const

void	GJKCollisionExtractWitnessPointsAndNormal (const Transform3s &tf1, Scalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const

void	EPAExtractWitnessPointsAndNormal (const Transform3s &tf1, Scalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const

void	EPAFailedExtractWitnessPointsAndNormal (const Transform3s &tf1, Scalar &distance, Vec3s &p1, Vec3s &p2, Vec3s &normal) const

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.

Constructor & Destructor Documentation

◆ GJKSolver() [1/4]

coal::GJKSolver::GJKSolver ( )

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.

◆ GJKSolver() [2/4]

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

inlineexplicit

Constructor from a DistanceRequest.

Parameters

[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.

◆ GJKSolver() [3/4]

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

inlineexplicit

Constructor from a CollisionRequest.

Parameters

[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.

◆ GJKSolver() [4/4]

coal::GJKSolver::GJKSolver ( const GJKSolver & other )

default

Copy constructor.

Member Function Documentation

◆ EPAExtractWitnessPointsAndNormal()

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

inlineprotected

◆ EPAFailedExtractWitnessPointsAndNormal()

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

inlineprotected

◆ getDistancePrecision()

Scalar 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.

◆ 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.

◆ GJKCollisionExtractWitnessPointsAndNormal()

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

inlineprotected

◆ GJKEarlyStopExtractWitnessPointsAndNormal()

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

inlineprotected

◆ GJKExtractWitnessPointsAndNormal()

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

inlineprotected

◆ operator!=()

bool coal::GJKSolver::operator!= ( const GJKSolver & other ) const

inline

◆ operator==()

bool coal::GJKSolver::operator== ( const GJKSolver & other ) const

inline

◆ 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,
		Scalar &	distance,
		Vec3s &	p1,
		Vec3s &	p2,
		Vec3s &	normal,
		const bool	relative_transformation_already_computed = `false`
	)		const

inlineprotected

Runs the GJK algorithm.

Parameters

	`s1`	the first shape.
	`tf1`	the transformation of the first shape.
	`s2`	the second shape.
	`tf2`	the transformation of the second shape.
	`compute_penetration`	if true and if the shapes are in found in collision, the EPA algorithm is also ran to compute penetration information.
[out]	`distance`	the distance between the two shapes.
[out]	`p1`	the witness point on the first shape.
[out]	`p2`	the witness point on the second shape.
[out]	`normal`	the normal of the collision, pointing from the first to the second shape.
	`relative_transformation_already_computed`	whether the relative transformation between the two shapes has already been computed.

Template Parameters

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.

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.

◆ set() [1/2]

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

inline

setter from a CollisionRequest

Parameters

[in] request CollisionRequest input

◆ set() [2/2]

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

inline

setter from a DistanceRequest

Parameters

[in] request DistanceRequest input

◆ shapeDistance() [1/3]

template<typename S1 , typename S2 >

Scalar 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

	`s1`	the first shape.
	`tf1`	the transformation of the first shape.
	`s2`	the second shape.
	`tf2`	the transformation of the second shape.
	`compute_penetration`	if true and GJK finds the shape in collision, the EPA algorithm is also ran to compute penetration information.
[out]	`p1`	the witness point on the first shape.
[out]	`p2`	the witness point on the second shape.
[out]	`normal`	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.

◆ shapeDistance() [2/3]

template<typename S1 >

Scalar 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.

◆ shapeDistance() [3/3]

template<typename S2 >

Scalar 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.

Member Data Documentation

◆ cached_guess

Vec3s coal::GJKSolver::cached_guess

mutable

smart guess

◆ distance_upper_bound

Scalar coal::GJKSolver::distance_upper_bound

If GJK can guarantee that the distance between the shapes is greater than this value, it will early stop.

◆ enable_cached_guess

bool coal::GJKSolver::enable_cached_guess

Whether smart guess can be provided @Deprecated Use gjk_initial_guess instead.

◆ epa

details::EPA coal::GJKSolver::epa

mutable

EPA algorithm.

◆ epa_max_iterations

size_t coal::GJKSolver::epa_max_iterations

maximum number of iterations of EPA

◆ epa_tolerance

Scalar coal::GJKSolver::epa_tolerance

tolerance of EPA

◆ gjk

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

mutable

GJK algorithm.

◆ gjk_convergence_criterion

GJKConvergenceCriterion coal::GJKSolver::gjk_convergence_criterion

Convergence criterion for GJK.

◆ gjk_convergence_criterion_type

GJKConvergenceCriterionType coal::GJKSolver::gjk_convergence_criterion_type

Absolute or relative convergence criterion for GJK.

◆ gjk_initial_guess

GJKInitialGuess coal::GJKSolver::gjk_initial_guess

which warm start to use for GJK

◆ gjk_max_iterations

size_t coal::GJKSolver::gjk_max_iterations

maximum number of iterations of GJK

◆ gjk_tolerance

Scalar coal::GJKSolver::gjk_tolerance

tolerance of GJK

◆ gjk_variant

GJKVariant coal::GJKSolver::gjk_variant

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

◆ minkowski_difference

details::MinkowskiDiff coal::GJKSolver::minkowski_difference

mutable

Minkowski difference used by GJK and EPA algorithms.

◆ support_func_cached_guess

support_func_guess_t coal::GJKSolver::support_func_cached_guess

mutable

smart guess for the support function

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

include/coal/narrowphase/narrowphase.h

Public Member Functions

Public Attributes

Protected Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ GJKSolver() [1/4]

◆ GJKSolver() [2/4]

◆ GJKSolver() [3/4]

◆ GJKSolver() [4/4]

Member Function Documentation

◆ EPAExtractWitnessPointsAndNormal()

◆ EPAFailedExtractWitnessPointsAndNormal()

◆ getDistancePrecision()

◆ getGJKInitialGuess()

◆ GJKCollisionExtractWitnessPointsAndNormal()

◆ GJKEarlyStopExtractWitnessPointsAndNormal()

◆ GJKExtractWitnessPointsAndNormal()

◆ operator!=()

◆ operator==()

◆ runGJKAndEPA()

◆ set() [1/2]

◆ set() [2/2]

◆ shapeDistance() [1/3]

◆ shapeDistance() [2/3]

◆ shapeDistance() [3/3]

Member Data Documentation

◆ cached_guess

◆ distance_upper_bound

◆ enable_cached_guess

◆ epa

◆ epa_max_iterations

◆ epa_tolerance

◆ gjk

◆ gjk_convergence_criterion

◆ gjk_convergence_criterion_type

◆ gjk_initial_guess

◆ gjk_max_iterations

◆ gjk_tolerance

◆ gjk_variant

◆ minkowski_difference

◆ support_func_cached_guess