Simox  2.3.74.0
VirtualRobot::CollisionChecker Class Reference
Inheritance diagram for VirtualRobot::CollisionChecker:

Public Member Functions

 CollisionChecker ()
 
virtual ~CollisionChecker ()
 
virtual float calculateDistance (const SceneObjectSetPtr &model1, const SceneObjectSetPtr &model2)
 
virtual float calculateDistance (const CollisionModelPtr &model1, const SceneObjectSetPtr &model2)
 
virtual float calculateDistance (const CollisionModelPtr &model1, const CollisionModelPtr &model2)
 
virtual float calculateDistance (const SceneObjectSetPtr &model1, const SceneObjectSetPtr &model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int *trID1=NULL, int *trID2=NULL)
 
virtual float calculateDistance (const CollisionModelPtr &model1, const SceneObjectSetPtr &model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int *trID1=NULL, int *trID2=NULL)
 
virtual float calculateDistance (const CollisionModelPtr &model1, const CollisionModelPtr &model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int *trID1=NULL, int *trID2=NULL)
 
virtual bool checkCollision (const SceneObjectSetPtr &model1, const SceneObjectSetPtr &model2)
 
virtual bool checkCollision (const std::vector< CollisionModelPtr > &model1, const CollisionModelPtr &model2)
 
virtual bool checkCollision (const CollisionModelPtr &model1, const SceneObjectSetPtr &model2)
 
virtual bool checkCollision (const CollisionModelPtr &model1, const CollisionModelPtr &model2)
 
virtual void getContacts (const MathTools::Plane &p, const CollisionModelPtr &colModel, std::vector< MathTools::ContactPoint > &storeContatcs, float maxDist=1.0f)
 
MultiCollisionResult checkMultipleCollisions (CollisionModelPtr const &model1, CollisionModelPtr const &model2)
 
bool isInitialized ()
 
void setAutomaticSizeCheck (bool checkSizeOnColModelCreation)
 
void enableDebugOutput (bool e)
 
virtual float calculateDistance (const SceneObjectPtr &model1, const SceneObjectSetPtr &model2)
 
virtual float calculateDistance (const SceneObjectPtr &model1, const SceneObjectPtr &model2)
 
virtual float calculateDistance (const SceneObjectPtr &model1, const SceneObjectSetPtr &model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int *trID1=NULL, int *trID2=NULL)
 
virtual float calculateDistance (const SceneObjectPtr &model1, const SceneObjectPtr &model2, Eigen::Vector3f &P1, Eigen::Vector3f &P2, int *trID1=NULL, int *trID2=NULL)
 
virtual bool checkCollision (const SceneObjectPtr &model1, const SceneObjectSetPtr &model2)
 
virtual bool checkCollision (const SceneObjectPtr &model1, const SceneObjectPtr &model2)
 
virtual bool checkCollision (const CollisionModelPtr &model, const Eigen::Vector3f &point, float tolerance)
 checks collision between a sphere (modeled by point + tolerance). The check only performs a collision check of triangles, not if the sphere is inside the given model! More...
 
virtual bool checkCollision (const SceneObjectSetPtr &modelSet, const Eigen::Vector3f &point, float tolerance)
 
std::shared_ptr< CollisionCheckerPQPgetCollisionCheckerImplementation ()
 

Static Public Member Functions

static CollisionCheckerPtr getGlobalCollisionChecker ()
 This is the global collision checker singleton. More...
 
static bool IsSupported_CollisionDetection ()
 
static bool IsSupported_ContinuousCollisionDetection ()
 
static bool IsSupported_DistanceCalculations ()
 
static bool IsSupported_Multithreading_Threadsafe ()
 
static bool IsSupported_Multithreading_MultipleColCheckers ()
 

Data Fields

bool debugOutput
 

Protected Member Functions

SceneObjectSetPtr getRobotModels (const RobotPtr &r)
 

Friends

class Cleanup
 

Detailed Description

A CollisionChecker is an instance that handles collision and distance queries. Internally the requests are passed to the underlying engine (e.g. the PQP library). All objects that should be considered for collision detection (called CollisionModels) must be registered here. Usually the objects take care of registering on their own.

When collision detection should not be performed in parallel, the global CollisionChecker singleton can be used. It can be retrieved with CollisionChecker::getGlobalCollisionChecker().

Constructor & Destructor Documentation

◆ CollisionChecker()

VirtualRobot::CollisionChecker::CollisionChecker ( )

◆ ~CollisionChecker()

VirtualRobot::CollisionChecker::~CollisionChecker ( )
virtualdefault

Member Function Documentation

◆ calculateDistance() [1/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const SceneObjectSetPtr model1,
const SceneObjectSetPtr model2 
)
virtual

Returns distance of the collision models. Returns -1.0 if no distance calculation lib was specified (-> Dummy Col Checker)

◆ calculateDistance() [2/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const CollisionModelPtr model1,
const SceneObjectSetPtr model2 
)
virtual

◆ calculateDistance() [3/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const CollisionModelPtr model1,
const CollisionModelPtr model2 
)
virtual

◆ calculateDistance() [4/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const SceneObjectSetPtr model1,
const SceneObjectSetPtr model2,
Eigen::Vector3f &  P1,
Eigen::Vector3f &  P2,
int *  trID1 = NULL,
int *  trID2 = NULL 
)
virtual

◆ calculateDistance() [5/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const CollisionModelPtr model1,
const SceneObjectSetPtr model2,
Eigen::Vector3f &  P1,
Eigen::Vector3f &  P2,
int *  trID1 = NULL,
int *  trID2 = NULL 
)
virtual

◆ calculateDistance() [6/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const CollisionModelPtr model1,
const CollisionModelPtr model2,
Eigen::Vector3f &  P1,
Eigen::Vector3f &  P2,
int *  trID1 = NULL,
int *  trID2 = NULL 
)
virtual

◆ calculateDistance() [7/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const SceneObjectPtr model1,
const SceneObjectSetPtr model2 
)
virtual

Convenient methods

◆ calculateDistance() [8/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const SceneObjectPtr model1,
const SceneObjectPtr model2 
)
virtual

◆ calculateDistance() [9/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const SceneObjectPtr model1,
const SceneObjectSetPtr model2,
Eigen::Vector3f &  P1,
Eigen::Vector3f &  P2,
int *  trID1 = NULL,
int *  trID2 = NULL 
)
virtual

◆ calculateDistance() [10/10]

float VirtualRobot::CollisionChecker::calculateDistance ( const SceneObjectPtr model1,
const SceneObjectPtr model2,
Eigen::Vector3f &  P1,
Eigen::Vector3f &  P2,
int *  trID1 = NULL,
int *  trID2 = NULL 
)
virtual

◆ checkCollision() [1/8]

bool VirtualRobot::CollisionChecker::checkCollision ( const SceneObjectSetPtr model1,
const SceneObjectSetPtr model2 
)
virtual

Test if the two models are colliding. Returns true on collision.

◆ checkCollision() [2/8]

bool VirtualRobot::CollisionChecker::checkCollision ( const std::vector< CollisionModelPtr > &  model1,
const CollisionModelPtr model2 
)
virtual

Test if the two models are colliding. Returns true on collision.

◆ checkCollision() [3/8]

bool VirtualRobot::CollisionChecker::checkCollision ( const CollisionModelPtr model1,
const SceneObjectSetPtr model2 
)
virtual

Test if the two models are colliding. Returns true on collision.

◆ checkCollision() [4/8]

bool VirtualRobot::CollisionChecker::checkCollision ( const CollisionModelPtr model1,
const CollisionModelPtr model2 
)
virtual

Test if the two models are colliding. Returns true on collision.

◆ checkCollision() [5/8]

bool VirtualRobot::CollisionChecker::checkCollision ( const SceneObjectPtr model1,
const SceneObjectSetPtr model2 
)
virtual

Test if the two models are colliding. Returns true on collision.

◆ checkCollision() [6/8]

bool VirtualRobot::CollisionChecker::checkCollision ( const SceneObjectPtr model1,
const SceneObjectPtr model2 
)
virtual

Test if the two models are colliding. Returns true on collision.

◆ checkCollision() [7/8]

bool VirtualRobot::CollisionChecker::checkCollision ( const CollisionModelPtr model,
const Eigen::Vector3f &  point,
float  tolerance 
)
virtual

checks collision between a sphere (modeled by point + tolerance). The check only performs a collision check of triangles, not if the sphere is inside the given model!

Parameters
model
point
tolerance
Returns
true, if the sphere is in collision with the model

◆ checkCollision() [8/8]

bool VirtualRobot::CollisionChecker::checkCollision ( const SceneObjectSetPtr modelSet,
const Eigen::Vector3f &  point,
float  tolerance 
)
virtual

◆ checkMultipleCollisions()

MultiCollisionResult VirtualRobot::CollisionChecker::checkMultipleCollisions ( CollisionModelPtr const &  model1,
CollisionModelPtr const &  model2 
)

If continuous collision detection (CCD) is supported, this method can be used to detect collisions on the path from the current pose of the collision models to the goal poses. true -> collision (then the time of contact [0..1] is stored to fStoreTOC)

◆ enableDebugOutput()

void VirtualRobot::CollisionChecker::enableDebugOutput ( bool  e)
inline

◆ getCollisionCheckerImplementation()

std::shared_ptr<CollisionCheckerPQP> VirtualRobot::CollisionChecker::getCollisionCheckerImplementation ( )
inline

◆ getContacts()

void VirtualRobot::CollisionChecker::getContacts ( const MathTools::Plane p,
const CollisionModelPtr colModel,
std::vector< MathTools::ContactPoint > &  storeContatcs,
float  maxDist = 1.0f 
)
virtual

Store all vertices of colModel whose distance to p is smaller than maxDist.

◆ getGlobalCollisionChecker()

CollisionCheckerPtr VirtualRobot::CollisionChecker::getGlobalCollisionChecker ( )
static

This is the global collision checker singleton.

◆ getRobotModels()

SceneObjectSetPtr VirtualRobot::CollisionChecker::getRobotModels ( const RobotPtr r)
protected

◆ isInitialized()

bool VirtualRobot::CollisionChecker::isInitialized ( )
inline

◆ IsSupported_CollisionDetection()

bool VirtualRobot::CollisionChecker::IsSupported_CollisionDetection ( )
static

Does the underlying collision detection library support discrete collision detection.

◆ IsSupported_ContinuousCollisionDetection()

bool VirtualRobot::CollisionChecker::IsSupported_ContinuousCollisionDetection ( )
static

Does the underlying collision detection library support continuous collision detection.

◆ IsSupported_DistanceCalculations()

bool VirtualRobot::CollisionChecker::IsSupported_DistanceCalculations ( )
static

Does the underlying collision detection library support distance calculations.

◆ IsSupported_Multithreading_MultipleColCheckers()

bool VirtualRobot::CollisionChecker::IsSupported_Multithreading_MultipleColCheckers ( )
static

Does the underlying collision detection library support multiple instances of the collision checker. E.g. one per thread.

◆ IsSupported_Multithreading_Threadsafe()

bool VirtualRobot::CollisionChecker::IsSupported_Multithreading_Threadsafe ( )
static

Does the underlying collision detection library support threadsafe access. E.g. multiple threads query the collision checker asynchronously.

◆ setAutomaticSizeCheck()

void VirtualRobot::CollisionChecker::setAutomaticSizeCheck ( bool  checkSizeOnColModelCreation)

Activates / Deactivates the automatic size check on col model creation. The size check can be useful when the UNITS definitions in inventor files result in different scalings of the 3D models. (Standard: true)

Friends And Related Function Documentation

◆ Cleanup

friend class Cleanup
friend

Field Documentation

◆ debugOutput

bool VirtualRobot::CollisionChecker::debugOutput