cd.c File Reference

Detailed Description

Implementation of the interface with the collision detection engine.

Different collision detection engines offer different performances and even different capabilities. Moreover, some applications require specific collision detection procedures (distance between bodies, interpretration, separating normal, etc). In general, the more the functionality of the collision detection engine the slower the process. Thus, a trade off must be reached for each application.

See Also

cd.h

Definition in file cd.c.

Functions
void	StoreCollisionInfoInt (FILE *f, char *fname, unsigned int objectID, Tmechanism *m, unsigned int nc, TCollisionInfo *c)
	Stores the information collected during last collision check into a file. More...
void	PrintCollisionInfoInt (THTransform *tl, Tmechanism *m, Tenvironment *e, unsigned int nc, TCollisionInfo *c)
	Prints information about the last collision check. More...
DT_Bool	CDCallBackInfo (void *client_data, void *obj1, void *obj2, const DT_CollData *cd)
	Callback called whenever a collision is detected. More...
DT_ShapeHandle	AddShape2Solid (Tpolyhedron *p)
	Adds a shape to the collision detection data. More...
boolean	SolidCorrection (THTransform *t, Tpolyhedron *p)
	Returns a transform that corrects the position of a polyhedron. More...
void	InitSolidCD (boolean parallel, Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TsolidCD *s)
	Initializes the Solid collision engine. More...
boolean	CheckCollisionSolid (boolean all, THTransform *tl, TLinkConf *def, TsolidCD *s)
	Determines if there is a collision using the SOLID collision engine. More...
void	DeleteSolidCD (TsolidCD *s)
	Deletes the Solid related collision information. More...
void	InitVcollideCD (Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TvcollideCD *vc)
	Initializes the Vcollide collision engine. More...
boolean	CheckCollisionVcollide (THTransform *tl, TvcollideCD *vc)
	Determines if there is a collision using the VCOLLIDE collision engine. More...
void	DeleteVcollideCD (TvcollideCD *vc)
	Deletes the Vcollide related collision information. More...
void	InitPQPCD (Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TpqpCD *p)
	Initializes the PQP collision engine. More...
boolean	CheckCollisionPQP (THTransform *tl, TpqpCD *p)
	Determines if there is a collision using the PQP collision engine. More...
void	DeletePQPCD (TpqpCD *p)
	Deletes the PQP related collision information. More...
void	InitCD (unsigned int engine, boolean parallel, Tmechanism *m, Tenvironment *e, boolean **checkCollisionsLL, boolean **checkCollisionsLO, TworldCD *cd)
	Initializes the collision detector. More...
boolean	LinkCanCollide (unsigned int l, unsigned int nl, unsigned int no, boolean **checkCollisionsLL, boolean **checkCollisionsLO)
	Identifies links than can collide. More...
boolean	ObstacleCanCollide (unsigned int o, unsigned int nl, boolean **checkCollisionsLO)
	Identifies obstacles than can collide. More...
unsigned int	GetCDEngine (TworldCD *cd)
	Determines the collision engine. More...
boolean	CheckCollision (boolean all, THTransform *tl, TLinkConf *def, THTransform *tlPrev, TLinkConf *defPrev, TworldCD *cd)
	Determines if there is a collision. More...
void	StoreCollisionInfo (FILE *f, char *fname, unsigned int objectID, Tmechanism *m, TworldCD *cd)
	Stores the information collected during last collision check into a file. More...
void	PrintCollisionInfo (THTransform *tl, Tmechanism *m, Tenvironment *e, TworldCD *cd)
	Prints some information collected during last collision check. More...
void	DeleteCD (TworldCD *cd)
	Collision information destructor. More...

Function Documentation

void StoreCollisionInfoInt	(	FILE *	f,
		char *	fname,
		unsigned int	objectID,
		Tmechanism *	m,
		unsigned int	nc,
		TCollisionInfo *	c
	)

Stores the information collected during last collision check into a file.

The information printed is in the form of a matlab script that can be directly used to generate contact variables and equations from the collision info. This relies on the existence of additiona matlab scripts describing the geometry of the objects.

We used this to generate contact aware cuik systems by using cuikmove to select the initial grasp. This is part of our grasping pipeline.

This function assumes that the object ot grasp is the last link in the world file.

See Also

StoreCollisionInfo for more details.

Parameters

f	File where to write the collision script.
fname	File from where we obtained the world information. The last part of the filename (from the last '_' to the end) is assumed to be the name of the object. This name is the one used to call the sub-matlab scripts giving contact points and normals on the object.
objectID	Identifier of the object being manipulated. This is assumed to be a link. Use NO_UINT to print all the collisions (wihtout any filter).
m	The mechanism description.
nc	Number of detected collisions.
c	The array of detected collisions.

Definition at line 2038 of file cd.c.

References GetLinkName(), GetMechanismLink(), HTransformPrint(), TCollisionInfo::id1, TCollisionInfo::id2, TCollisionInfo::isLink1, TCollisionInfo::isLink2, NO_UINT, TCollisionInfo::t1, and TCollisionInfo::t2.

Referenced by StoreCollisionInfo().

void PrintCollisionInfoInt	(	THTransform *	tl,
		Tmechanism *	m,
		Tenvironment *	e,
		unsigned int	nc,
		TCollisionInfo *	c
	)

Prints information about the last collision check.

See PrintCollisionInfo for more details.

Parameters

tl	The transforms positioning the links.
m	The mechanism description.
e	The environment description.
nc	Number of detected collisions.
c	The array of detected collisions.

Definition at line 2102 of file cd.c.

References GetLinkName(), GetMechanismLink(), GetObstacleName(), HTransformApply(), HTransformDelete(), HTransformInverse(), TCollisionInfo::isLink1, and TCollisionInfo::isLink2.

Referenced by PrintCollisionInfo().

DT_Bool CDCallBackInfo	(	void *	client_data,
		void *	obj1,
		void *	obj2,
		const DT_CollData *	cd
	)

Callback called whenever a collision is detected. This is the version where we print info about the collisions: colliding links, collision points, and normal in the collision.

We only print information about the first detected collision.

Parameters

client_data	Point to user data given when defining the intraction between objects.
obj1	Pointer to the object given when creating the object.
obj2	Pointer to the object given when creating the object.
cd	Collision detection information provided by Solid including the collision points and normal in global coordinates.

Definition at line 455 of file cd.c.

References TsolidObj::bodyID, TsolidCD::collision, FALSE, HTransformCopy(), HTransformIdentity(), TCollisionInfo::id1, TCollisionInfo::id2, TCollisionInfo::isLink1, TCollisionInfo::isLink2, TsolidObj::linkID, TsolidCD::mc, MEM_DUP, TsolidCD::nc, NO_UINT, Norm(), TCollisionInfo::normal, TsolidCD::object, TCollisionInfo::point, ScaleVector(), TsolidCD::simple, TsolidObj::t, TCollisionInfo::t1, TCollisionInfo::t2, TRUE, and ZERO.

Referenced by InitSolidCD().

DT_ShapeHandle AddShape2Solid

(

Tpolyhedron *

p

)

Adds the shape to the current collision detection. Note that the collision detection structure is not passed as a parameter to this function since we use SOLID that relies on global variables (not so good programming style).

Parameters

p	The convex polyhedron to add to the collision detection structure.

Returns

The new SOLID shape handle.

Definition at line 524 of file cd.c.

References CYLINDER, DECOR_SHAPE, Error(), GetOFFInfo(), GetPolyhedronRadius(), GetPolyhedronStatus(), GetPolyhedronType(), LINE, OFF, SEGMENTS, and SPHERE.

Referenced by InitSolidCD().

boolean SolidCorrection	(	THTransform *	t,
		Tpolyhedron *	p
	)

The SOLID collision detection software define shapes in a pre-defined positions (spheres centered at 0, cylinders aligned with the Y axis,...). This function returns a homogeneous matrix that places the convex polyhedron in its correct position from the default position of the collision detector.

Parameters

t	The returned correction transform.
p	The convex polyhedron.

Returns

TRUE if the correction is not the identity.

Definition at line 584 of file cd.c.

References CYLINDER, DECOR_SHAPE, Error(), GetPolyhedronCenter(), GetPolyhedronDefiningPoint(), GetPolyhedronStatus(), GetPolyhedronType(), HTransformIdentity(), HTransformIsIdentity(), HTransformProduct(), HTransformRz2(), HTransformTxyz(), HTransformTy(), HTransformX2Vect(), LINE, OFF, SEGMENTS, and SPHERE.

Referenced by InitSolidCD().

void InitSolidCD	(	boolean	parallel,
		Tmechanism *	m,
		Tenvironment *	e,
		boolean **	checkCollisionsLL,
		boolean **	checkCollisionsLO,
		TsolidCD *	s
	)

Initializes the Solid collision engine.

Parameters

parallel	TRUE if we are in a parallel situation, where different collision detection queries can be executed in parallel. In this case, we configure Solid just to detect the collision, but not to inform about the collision point (apparently this part of Solid is not re-entrant).
m	The machanism.
e	The enviroment.
checkCollisionsLL	Link-link collision to be checked.
checkCollisionsLO	Link-obstacle collision to be checked.
s	The solid collision information.

Definition at line 627 of file cd.c.

References AddShape2Solid(), TsolidObj::bodyID, CDCallBackInfo(), TsolidCD::collision, TsolidObj::correction, DECOR_SHAPE, GetEnvironmentNObstacles(), GetLinkBody(), GetLinkBodyStatus(), GetMechanismLink(), GetMechanismNBodies(), GetMechanismNLinks(), GetObstacleShape(), GetObstacleShapeStatus(), TsolidObj::handler, HTransform2GLMatrix(), HTransformIdentity(), TsolidObj::id, LinkCanCollide(), TsolidObj::linkID, LinkNBodies(), TsolidCD::mc, TsolidCD::nc, NEW, NO_UINT, TsolidCD::np, TsolidCD::object, ObstacleCanCollide(), TsolidObj::respClass, TsolidCD::respTable, TsolidCD::scene, TsolidObj::shape, TsolidCD::simple, SolidCorrection(), TsolidObj::t, TsolidObj::tc, and TRUE.

Referenced by InitCD().

boolean CheckCollisionSolid	(	boolean	all,
		THTransform *	tl,
		TLinkConf *	def,
		TsolidCD *	s
	)

Determines if there is a collision using the SOLID collision engine.

Parameters

all	TRUE if we want to detect all the collision. FALSE just to detect the first one.
tl	The transforms to apply to the links.
def	The deformation parameters for each link.
s	The solid collision information.

Returns

TRUE if there is a collision.

Definition at line 778 of file cd.c.

References TsolidObj::bodyID, TsolidCD::collision, TsolidObj::correction, EmptyLinkConf(), GetLinkConfTransform(), TsolidObj::handler, HTransform2GLMatrix(), HTransformCopy(), HTransformDelete(), HTransformProduct(), TsolidObj::linkID, TsolidCD::nc, NO_UINT, TsolidCD::np, TsolidCD::object, TsolidCD::respTable, TsolidCD::scene, TsolidCD::simple, TsolidObj::t, and TsolidObj::tc.

Referenced by CheckCollision().

void DeleteSolidCD

(

TsolidCD *

s

)

Deletes the Solid related collision information.

Parameters

s	The solid collision information to delete.

Definition at line 835 of file cd.c.

References TsolidCD::collision, TsolidObj::handler, HTransformDelete(), TsolidCD::np, TsolidCD::object, TsolidCD::respTable, TsolidCD::scene, TsolidObj::shape, TsolidObj::t, and TsolidObj::tc.

Referenced by DeleteCD().

void InitVcollideCD	(	Tmechanism *	m,
		Tenvironment *	e,
		boolean **	checkCollisionsLL,
		boolean **	checkCollisionsLO,
		TvcollideCD *	vc
	)

Initializes the Vcollide collision engine.

Parameters

m	The machanism.
e	The enviroment.
checkCollisionsLL	Link-link collision to be checked.
checkCollisionsLO	Link-obstacle collision to be checked.
vc	The Vcollide collision information.

Definition at line 861 of file cd.c.

References ActivateCollisionsVcollide(), AddPolyhedron2Vcollide(), TvcollideCD::bodyID, DeactivateCollisionsVcollide(), DECOR_SHAPE, GetEnvironmentNObstacles(), GetLinkBody(), GetLinkBodyStatus(), GetMechanismLink(), GetMechanismNBodies(), GetMechanismNLinks(), GetObstacleShape(), GetObstacleShapeStatus(), GetOFFInfo(), InitVcollide(), LinkCanCollide(), TvcollideCD::linkID, LinkNBodies(), NEW, NO_UINT, TvcollideCD::np, ObstacleCanCollide(), TvcollideCD::vc, and TvcollideCD::vcID.

Referenced by InitCD().

boolean CheckCollisionVcollide	(	THTransform *	tl,
		TvcollideCD *	vc
	)

Determines if there is a collision using the VCOLLIDE collision engine.

Parameters

tl	The transforms to apply to the links.
vc	The Vcollide collision information.

Returns

TRUE if there is a collision.

Definition at line 958 of file cd.c.

References TvcollideCD::linkID, MoveVcollideObject(), NO_UINT, TvcollideCD::np, TvcollideCD::vc, TvcollideCD::vcID, and VcollideTest().

Referenced by CheckCollision().

void DeleteVcollideCD

(

TvcollideCD *

vc

)

Deletes the Vcollide related collision information.

Parameters

vc	The Vcollide collision information to delete.

Definition at line 974 of file cd.c.

References TvcollideCD::bodyID, DeleteVcollide(), DeleteVcollideObject(), TvcollideCD::linkID, TvcollideCD::np, TvcollideCD::vc, and TvcollideCD::vcID.

Referenced by DeleteCD().

void InitPQPCD	(	Tmechanism *	m,
		Tenvironment *	e,
		boolean **	checkCollisionsLL,
		boolean **	checkCollisionsLO,
		TpqpCD *	p
	)

Initializes the PQP collision engine.

Parameters

m	The machanism.
e	The enviroment.
checkCollisionsLL	Link-link collision to be checked.
checkCollisionsLO	Link-obstacle collision to be checked.
p	The PQP collision information.

Definition at line 992 of file cd.c.

References TpqpCD::bodyID, DECOR_SHAPE, DefinePQPModel(), GetEnvironmentNObstacles(), GetLinkBody(), GetLinkBodyStatus(), GetMechanismLink(), GetMechanismNBodies(), GetMechanismNLinks(), GetObstacleShape(), GetObstacleShapeStatus(), GetOFFInfo(), LinkCanCollide(), TpqpCD::linkID, LinkNBodies(), TpqpCD::model, NEW, NO_UINT, TpqpCD::np, TpqpCD::nPairs, ObstacleCanCollide(), TpqpCD::p1, and TpqpCD::p2.

Referenced by InitCD().

boolean CheckCollisionPQP	(	THTransform *	tl,
		TpqpCD *	p
	)

Determines if there is a collision using the PQP collision engine.

Parameters

tl	The transforms to apply to the links.
p	The PQP collision information.

Returns

TRUE if there is a collision.

Definition at line 1093 of file cd.c.

References FALSE, TpqpCD::linkID, TpqpCD::model, NO_UINT, TpqpCD::p1, TpqpCD::p2, and PQPTest().

Referenced by CheckCollision().

void DeletePQPCD

(

TpqpCD *

p

)

Deletes the PQP related collision information.

Parameters

p	The PQP collision information.

Definition at line 1126 of file cd.c.

References TpqpCD::bodyID, DeletePQPModel(), TpqpCD::linkID, TpqpCD::model, TpqpCD::np, TpqpCD::p1, and TpqpCD::p2.

Referenced by DeleteCD().

void InitCD	(	unsigned int	engine,
		boolean	parallel,
		Tmechanism *	m,
		Tenvironment *	e,
		boolean **	checkCollisionsLL,
		boolean **	checkCollisionsLO,
		TworldCD *	cd
	)

Initializes the collision detector.

Parameters

engine	The engine to use
parallel	TRUE if we are in a contex of parallel execution, where different collision queries can be executed in parallel.
m	The machanism.
e	The enviroment.
checkCollisionsLL	Link-link collision to be checked.
checkCollisionsLO	Link-obstacle collision to be checked.
cd	The collision information to define.

Definition at line 2135 of file cd.c.

References BULLET, TworldCD::bullet, C_BULLET, C_FCL, TworldCD::engine, Error(), FCL, TworldCD::fcl, InitPQPCD(), InitSolidCD(), InitVcollideCD(), NEW, NOCD, PQP, TworldCD::pqp, RIGIDCLL, TworldCD::rigidCLL, SOLID, TworldCD::solid, VCOLLIDE, and TworldCD::vcollide.

Referenced by InitWorldCD().

boolean LinkCanCollide	(	unsigned int	l,
		unsigned int	nl,
		unsigned int	no,
		boolean **	checkCollisionsLL,
		boolean **	checkCollisionsLO
	)

Identifies links that can collide either with other links or with obstacles.

Parameters

l	The identifier of the link to check.
nl	Number of links.
no	Number of obstacles.
checkCollisionsLL	Allowed collisions between links.
checkCollisionsLO	Allowed collisions between links and obstacles.

Returns

TRUE if the link collision is activated with respect to any other link or obstacle.

Definition at line 2218 of file cd.c.

References FALSE.

Referenced by AnyCollision(), InitPQPCD(), InitSolidCD(), and InitVcollideCD().

boolean ObstacleCanCollide	(	unsigned int	o,
		unsigned int	nl,
		boolean **	checkCollisionsLO
	)

Identifies obstacles that can collide a given link..

Parameters

o	The identifier of the obstacle to check.
nl	Number of obstacles.
checkCollisionsLO	Allowed collisions between links and obstacles.

Returns

TRUE if the obstacle collision is activated with respect to any link.

Definition at line 2240 of file cd.c.

References FALSE.

Referenced by InitPQPCD(), InitSolidCD(), and InitVcollideCD().

unsigned int GetCDEngine

(

TworldCD *

cd

)

Determines the collision library used.

Parameters

cd	The collision information to query.

Returns

The identifier of the collision detection library in use.

Definition at line 2255 of file cd.c.

References TworldCD::engine.

Referenced by MoveAndCheckCD(), and WorldContinuousCD().

boolean CheckCollision	(	boolean	all,
		THTransform *	tl,
		TLinkConf *	def,
		THTransform *	tlPrev,
		TLinkConf *	defPrev,
		TworldCD *	cd
	)

Determines if there is a collision.

Parameters

all	TRUE if we want to detect all the collision. FALSE just to detect the first one. Right now this flag is only honored when using the Solid or the FCL collision libraries. The rest only detect the first collision and stop irrespectively of the value of this paramater.
tl	The transforms to apply to the links.
def	Deformation parameter. IMPORTANT: Up to now SOLID is the only collision detection engine that uses the deformation.
tlPrev	The transforms to apply to the links in the previous configuration. Only used for the continuous collision checker (C_FCL). The rest of collision detection libraries are discrete and ignore this parameter.
defPrev	Deformation parameter in the previous configuration.
cd	The collision information to use in the check.

Todo:

Implement link deformation in CD engines other than SOLID.

Returns

TRUE if there is a collision.

Definition at line 2260 of file cd.c.

References BULLET, TworldCD::bullet, C_BULLET, C_FCL, CheckCollisionPQP(), CheckCollisionSolid(), CheckCollisionVcollide(), TworldCD::engine, Error(), FALSE, FCL, TworldCD::fcl, NOCD, PQP, TworldCD::pqp, RIGIDCLL, TworldCD::rigidCLL, SOLID, TworldCD::solid, VCOLLIDE, and TworldCD::vcollide.

Referenced by MoveAndCheckCDFromTransforms().

void StoreCollisionInfo	(	FILE *	f,
		char *	fname,
		unsigned int	objectID,
		Tmechanism *	m,
		TworldCD *	cd
	)

Stores information collected during last collision check into a file. This is only used from the cuikmove application, when using the "save" option. The information to be stored is collected during the collision detection and latter compleated in a call to PrintCollisionInfo. In other words, this function can only be used after detecting a collision and after calling PrintCollisionInfo. The cuikmove alrady coordinates these steps: it first uses MoveWorldDOF and if the user press the "Save" button, it then calls this function.

Up to now this only works for the SOLID collision engine.

See StoreCollisionInfoInt for more deatils.

Parameters

f	File where to write the collision script.
fname	File from where we obtained the world information. The last part of the filename (from the last '_' to the end) is assumed to be the name of the object. This name is the one used to call the sub-matlab scripts giving contact points and normals on the object.
objectID	Identifier of the object being manipulated. This is assumed to be a link. Use NO_UINT to print all the information without any filtering.
m	The mechanism description.
cd	The collision information.

Definition at line 2346 of file cd.c.

References BULLET, TworldCD::bullet, TsolidCD::collision, TworldCD::engine, Error(), FCL, TworldCD::fcl, TsolidCD::nc, SOLID, TworldCD::solid, and StoreCollisionInfoInt().

Referenced by PrintWorldCollisionInfo().

void PrintCollisionInfo	(	THTransform *	tl,
		Tmechanism *	m,
		Tenvironment *	e,
		TworldCD *	cd
	)

Prints some information collected during last collision check.

Up to now this only works for the SOLID collision engine.

This is used when obtain contact information while interactively moving the robots.

Parameters

tl	The transforms positioning the links.
m	The mechanism description.
e	The environment description.
cd	The collision information.

Definition at line 2374 of file cd.c.

References BULLET, TworldCD::bullet, TsolidCD::collision, TworldCD::engine, Error(), FCL, TworldCD::fcl, TsolidCD::nc, PrintCollisionInfoInt(), SOLID, and TworldCD::solid.

Referenced by MoveAndCheckCDFromTransforms().

void DeleteCD

(

TworldCD *

cd

)

Releases the structures used for collision detection.

Parameters

cd	The collision detection information to be released.

Definition at line 2403 of file cd.c.

References BULLET, TworldCD::bullet, C_BULLET, C_FCL, DeletePQPCD(), DeleteSolidCD(), DeleteVcollideCD(), TworldCD::engine, Error(), FCL, TworldCD::fcl, NOCD, PQP, TworldCD::pqp, RIGIDCLL, TworldCD::rigidCLL, SOLID, TworldCD::solid, VCOLLIDE, and TworldCD::vcollide.

Referenced by DeleteWorldCD(), and InitWorldCD().