[ODE] Bug in Ray-Trimesh collision implementation

[Sawyer]

Thanks this is exactly the information I needed to know.  That said, it didn't 
work.

I tried 2 variations on a theme, I have not noticed any difference it looks 
like all the work on gazebo's end happens in the callback function, if I need 
to add the function(s) calls before:

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

Please let me know, however in the callback function they are already checking 
which geom is the ray so I thought the additions should go there.  

Here are the two compiling and non-crashing variations I tried, neither of 
them fixed the problem.

(RayProximity::UpdateCallback)
    // Check for ray/geom intersections
    if ( rayGeom && hitGeom )
    {
      dGeomRaySetParams((dxGeom*)rayGeom, 1, 1);
      dGeomRaySetClosestHit((dxGeom*)rayGeom, 1);
      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();
        }
      }
    }

and

    // Check for ray/geom intersections
    if ( rayGeom && hitGeom )
    {
      dGeomRaySetClosestHit((dxGeom*)rayGeom, 1);
      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();
        }
      }
    }

Should I make sure the second geom is a trimesh and not run the function(s) 
for other object types?  

Do I need to increase the number of collisions processed (if I just increase 
the number above 1 I get a run-time crash, I would need to look further into 
the size of the memory passed).

Thanks again, you've already been a great help in figuring this out.

Sawyer Larkin

Adam D. Moss wrote:
>Sawyer wrote:
>> 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
>
>ODE doesn't make any guarantees about the order of the contacts returned
>or that only the 'closest' contact will be returned -- it's up to you
>to sort the contacts by depth if that's what you actually want.
>
>That said, there are some undocumented ODE features that will
>probably help you out, that are currently (I believe) specific to
>ray-trimesh collisions:
>
>void dGeomRaySetParams (dGeomID g, int FirstContact, int BackfaceCull);
>void dGeomRayGetParams (dGeomID g, int *FirstContact, int *BackfaceCull);
>void dGeomRaySetClosestHit (dGeomID g, int closestHit);
>int dGeomRayGetClosestHit (dGeomID g);