[ODE] Bug either in ODE or in implementation (more likely?)

[Sawyer]

I am trying to use the Gazebo Robot simulator which uses ODE.  For most 
situations this has worked very well but I am having problems getting 
reliable sensor returns from trimeshes.

This issue only occurs when using trimesh support.  The sensors are modeled as 
rays using ode.  The problem occurs when a ray has multiple collisions with a 
single (although large and complex) trimesh.  It seems to return the distance 
of a random collision, ie. not the closest one as would be the expected 
behavior, I have looked through the code of both packages but am getting lost 
in the details of collision_trimesh_ray.cpp and it's interaction with gazebo, 
below I have copied the code that is handling the collision detection within 
gazebo hoping that somebody more familiar with ode can spot a problem, also 
listed below are some screenshots highlighting the issue, as well as showing 
correct handling of other object types.

I have tried the versioned releases of both packages as well as the CVS 
downloads of both.  For ode this means ode-0.5, and the CVS branch downloaded 
on 3-17-05.

ode-0.5 + gazebo 0.5.x (including cvs)
http://mt-elbert.irgeek.com/~tobor/RayProximityBug1.jpg
http://mt-elbert.irgeek.com/~tobor/RayProximityBug2.jpg
http://mt-elbert.irgeek.com/~tobor/RayProximityBug3.jpg

ode-cvs 3-17-05 + gazebo 0.5.1
http://mt-elbert.irgeek.com/~tobor/RayProximityBugCVS.jpg

These are the only two functions that ask ode for collision information.  Also 
as far as I can tell gazebo is basically returning the depth of a random 
triangle collision depending on what order they are processed in.  I have not 
seen problems with any object type other than trimesh.  Any help is greatly 
appreciated.

Here is the code block from gazebo dealing with collisions:
in file: RayProximity.cc
//////////////////////////////////////////////////////////////////////////////
// Update the sensor information
void RayProximity::Update()
{
  int i;
  GzPose pose;
  RayGeom *ray;
  GzVector a, b;

  // Get the pose of the sensor body (global cs)
  pose = this->body->GetPose();

  // Reset the ray lengths and mark the geoms as dirty (so they get
  // redrawn)
  for (i = 0; i < this->rayCount; i++)
  {
    ray = this->rays[i];
    ray->dirty = true;
    ray->contactDepth = DBL_MAX;
    ray->contactRetro = 0.0;
    ray->contactFiducial = -1;

    // Update the ray endpoints (global cs)
    a = GzCoordPositionAdd(ray->pos_a, pose.pos, pose.rot);
    b = GzCoordPositionAdd(ray->pos_b, pose.pos, pose.rot);
    ray->Set(a, GzVectorUnit(GzVectorSub(b, a)));
  }

  // Do collision detection
  dSpaceCollide2( ( dGeomID ) ( this->superSpaceId ),
                  ( dGeomID ) ( this->world->GetSpaceId() ),
                  this, &UpdateCallback );

  return;
}

//////////////////////////////////////////////////////////////////////////////
// Callback for ray intersection test
void RayProximity::UpdateCallback( void *data, dGeomID o1, dGeomID o2 )
{
  int n;
  dContactGeom contact;
  Geom *geom1, *geom2;
  RayGeom *rayGeom;
  Geom *hitGeom;
  RayProximity *self;

  self = (RayProximity*) data;

  // Check space
  if ( dGeomIsSpace( o1 ) || dGeomIsSpace( o2 ) )
  {
    if (dGeomGetSpace(o1) == self->superSpaceId || dGeomGetSpace(o2) == 
self->superSpaceId)
    {
      dSpaceCollide2( o1, o2, self, &UpdateCallback );
    }
    if (dGeomGetSpace(o1) == self->raySpaceId || dGeomGetSpace(o2) == 
self->raySpaceId)
    {
      dSpaceCollide2( o1, o2, self, &UpdateCallback );
    }
  }
  else
  {
    geom1 = NULL;
    geom2 = NULL;

    // Get pointers to the underlying geoms
    if (dGeomGetClass(o1) == dGeomTransformClass)
      geom1 = (Geom*) dGeomGetData(dGeomTransformGetGeom(o1));
    else
      geom1 = (Geom*) dGeomGetData(o1);

    if (dGeomGetClass(o2) == dGeomTransformClass)
      geom2 = (Geom*) dGeomGetData(dGeomTransformGetGeom(o2));
    else
      geom2 = (Geom*) dGeomGetData(o2);

    assert(geom1 && geom2);

    rayGeom = NULL;
    hitGeom = NULL;

    // Figure out which one is a ray; note that this assumes
    // that the ODE dRayClass is used *soley* by the RayGeom.
    if (dGeomGetClass(o1) == dRayClass)
    {
      rayGeom = (RayGeom*) geom1;
      hitGeom = geom2;
    }

    if (dGeomGetClass(o2) == dRayClass)
    {
      assert(rayGeom == NULL);
      rayGeom = (RayGeom*) geom2;
      hitGeom = geom1;
    }

    // Check for ray/geom intersections
    if ( rayGeom && hitGeom )
    {
      n = dCollide(o1, o2, 1, &contact, sizeof(contact));
      if ( n > 0 )
      {
        if (contact.depth < rayGeom->contactDepth)
        {
          //printf("%p %p %f \n", o1, o2, contact.depth);
          rayGeom->contactDepth = contact.depth;
          rayGeom->contactRetro = hitGeom->GetRetro();
          rayGeom->contactFiducial = hitGeom->GetFiducial();
        }
      }
    }
  }
  return;
}

Thanks again,
Sawyer Larkin