[ODE] Space additions for dRay

[David McClurg]

Found a problem with the AABB setup.  Here's the fix...

int dcTriListCollider::CollideRay(dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
  dcAABBTreeCollider& Collider = AABBCollider;

  /* Get ray */
  dVector3 o, d;
  dGeomRayGet(RayGeom, o, d);
  dcVector3 RayOrigin(o), RayDirection(d);
  dReal RayLength = dGeomRayGetLength(RayGeom);

  /* Make AABB */
  dReal tempAABB[6];
  dAABBRay(RayGeom, tempAABB);
  Opcode::CollisionAABB Box;
  Box.mExtents.x = (tempAABB[1]-tempAABB[0])/2;
  Box.mExtents.y = (tempAABB[3]-tempAABB[2])/2;
  Box.mExtents.z = (tempAABB[5]-tempAABB[4])/2;
  Box.mCenter.x = tempAABB[0] + Box.mExtents.x;
  Box.mCenter.y = tempAABB[2] + Box.mExtents.y;
  Box.mCenter.z = tempAABB[4] + Box.mExtents.z;

  /* Intersect */
  Collider.Collide((Opcode::AABBNoLeafTree*)BVTree.GetTree(), Box);

  /* Retrieve data */
  int TriangleIDCount = Collider.Contacts.size();

  if (TriangleIDCount != 0){
    dArray<int>& TriangleIDs = Collider.Contacts;

    Callback(RayGeom, TriangleIDs);

    /* Creating minimum contacts */
    int OutTriCount = 0;
    for (int i = 0; i < TriangleIDCount; i++){
      const int& TriIndex = TriangleIDs[i];

      if (!Valid(RayGeom, TriIndex)) continue;

      const dcVector3& v0 = Vertices[Indices[TriIndex * 3 + 0]];
      const dcVector3& v1 = Vertices[Indices[TriIndex * 3 + 1]];
      const dcVector3& v2 = Vertices[Indices[TriIndex * 3 + 2]];

      dcPlane TriPlane(v0, v1, v2);
      const dcVector3& ContactNormal = TriPlane.Normal;

      dReal Denom = ContactNormal.DotProduct(RayDirection);
      if (dFabs(Denom) < 0.00001f){
        continue;	// Ray never hits
      }

      dReal ContactDepth = ContactNormal.DotProduct(RayOrigin);
      ContactDepth = (TriPlane.Distance - ContactDepth) / Denom;
      if (ContactDepth < 0 || ContactDepth > RayLength){
        continue;	// Ray hits but not within boundaries
      }

      const dcVector3 ContactPos = RayOrigin + RayDirection * ContactDepth;

      // Inside triangle?
      int Index;
      for (Index = 0; Index < 3; Index++){
        const dcVector3& v0 = Vertices[Indices[TriIndex * 3 + Index]];
        const dcVector3& v1 = Vertices[Indices[TriIndex * 3 + (Index + 1) % 3]];
        dcPlane Plane(v0, v1, v1 - TriPlane.Normal);

        if (!Plane.Contains(ContactPos)){
          break;
        }
      }
      if (Index == 3){
        dContactGeom& OutContact = *Contacts;
        if (OutTriCount == 0 || ContactDepth < OutContact.depth) {
          ((dcVector3&)OutContact.pos) = ContactPos;
          ((dcVector3&)OutContact.normal) = ContactNormal;
          OutContact.depth = ContactDepth;
          OutTriCount = 1;
        }
      }
    }

    if (OutTriCount != 0){
      dContactGeom& OutContact = *Contacts;
      OutContact.g1 = RayGeom;
      OutContact.g2 = Geometry;
#ifdef GENERATEBODIES
      OutContact.b1 = dGeomGetBody(RayGeom);
      OutContact.b2 = dGeomGetBody(Geometry);
#endif	//GENERATEBODIES
      return 1;
    }
    else return 0;
  }
  else return 0;
}