[ODE] Fix for bug in dGeomBoxPointDepth()

[John Miles]

The dGeomBoxPointDepth() function is documented to work for points both
inside and outside the box geom.  However, because the current
implementation always returns the minimum distance between the point and any
side of the box, it often returns meaningless results for input points
outside the box.

This implementation behaves identically to the current one for points inside
the box, but uses a different approximation for exterior-point distances.
The *largest* positive distance between the point and any side needs to be
returned in this case.  (Consider an exterior point that's only slightly
above the top plane of a box, but several units to the left.  The previous
implementation would have returned the distance from the point to the top
plane, which is grossly incorrect, rather than the left plane.)

Apart from any user applications that call dGeomBoxPointDepth(), the
function appears to be used only by the dTerrainY/Z contribution, as far as
I can tell.  This rewritten version (below) has been tested with dTerrainY
and seems to work fine.   It still isn't formally "right," but the
approximation it returns for exterior-point depths is at least reasonable.
Can someone please add it to CVS?

-- jm

----------snip from collision_std.cpp------------

dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
   dUASSERT (g && g->type == dBoxClass,"argument not a box");
   dxBox *b = (dxBox*) g;

   //
   // Set p = (x,y,z) relative to box center
   //
   // This will be (0,0,0) if the point is at
(side[0]/2,side[1]/2,side[2]/2)
   //

   dVector3 p,q;

   p[0] = x - b->pos[0];
   p[1] = y - b->pos[1];
   p[2] = z - b->pos[2];

   //
   // Rotate p into box's coordinate frame, so we can
   // treat the OBB as an AABB
   //

   dMULTIPLY1_331 (q,b->R,p);

   //
   // Record distance from point to each successive box side, and see
   // if the point is inside all six sides
   //

   dReal dist[6];
   int   i;

   bool inside = true;

   for (i=0; i < 3; i++)
     {
     dReal side = b->side[i] * REAL(0.5);

     dist[i  ] = side - q[i];
     dist[i+3] = side + q[i];

     if ((dist[i] < 0) || (dist[i+3] < 0))
        {
        inside = false;
        }
     }

   //
   // If point is inside the box, the depth is the smallest positive
distance
   // to any side
   //

   if (inside)
      {
      dReal smallest_dist = (dReal) (unsigned) -1;

      for (i=0; i < 6; i++)
         {
         if (dist[i] < smallest_dist) smallest_dist = dist[i];
         }

      return smallest_dist;
      }

   //
   // Otherwise, if point is outside the box, the depth is the largest
   // distance to any side
   //

   dReal largest_dist = 0;

   for (i=0; i < 6; i++)
      {
      if (dist[i] > largest_dist) largest_dist = dist[i];
      }

   return -largest_dist;
}
--------end snip--------