void internal_raycastCollisionCallback(void* data, dGeomID o0,
dGeomID o1)
{
if (dGeomIsSpace(o0) || dGeomIsSpace(o1))
{
// Colliding a space with either a geom or another space.
dSpaceCollide2(o0, o1, data,
&internal_raycastCollisionCallback);
}
else
{
// Colliding two geoms.
// Sometimes we get a case where the ray geom is passed in
// as both objects, which is stupid.
if (o0 == o1)
{
return ;
}
// Get a pointer to the ODESimulator.
ODESimulator* sim = (ODESimulator*) data;
// Get pointers to the two geoms' GeomData structure. One
// of these (the one NOT belonging to the ray geom)
// will always be non-NULL.
GeomData* geomData0 = ((GeomData*) dGeomGetData(o0));
GeomData* geomData1 = ((GeomData*) dGeomGetData(o1));
// Find the contact group of the collided Solid.
unsigned int geomContactGroup = defaults::shape::contactGroup;
if (geomData0)
{
geomContactGroup = geomData0->shape->contactGroup;
}
else
{
geomContactGroup = geomData1->shape->contactGroup;
}
// Check if the two Solids' contact groups generate contacts
// when they collide.
bool makeContacts = sim->groupsMakeContacts(
geomContactGroup, sim->internal_getRayContactGroup());
if (!makeContacts)
{
return ;
}
// Now actually test for collision between the two geoms.
// This is a fairly expensive operation.
dContactGeom contactArray[defaults::ode::maxRaycastContacts];
int numContacts = dCollide(o0, o1,
defaults::ode::maxRaycastContacts, contactArray,
sizeof(dContactGeom));
if (0 == numContacts)
{
return ;
}
else
{
// These two geoms must be intersecting. We will store
// only the closest RaycastResult.
int closest = 0;
for (int i = 0; i < numContacts; ++i)
{
if (contactArray[i].depth < contactArray[closest].depth)
{
closest = i;
}
}
// Only one of the geoms will be part of a Solid we
// want to store; the other is the ray.
Solid* solid = NULL;
if (geomData0)
{
solid = geomData0->solid;
}
else
{
solid = geomData1->solid;
}
Point3r intersection((real) contactArray[closest].pos[0],
(real) contactArray[closest].pos[1],
(real) contactArray[closest].pos[2]);
Vec3r normal((real) contactArray[closest].normal[0],
(real) contactArray[closest].normal[1],
(real) contactArray[closest].normal[2]);
sim->internal_addRaycastResult(solid, intersection,
normal, (real) contactArray[closest].depth);
}
}
}
