add ode

1 files changed, 1463 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/demo/demo_collision.cpp b/libs/ode-0.16.1/ode/demo/demo_collision.cpp
new file mode 100644
index 0000000..e45d106
--- /dev/null
+++ b/libs/ode-0.16.1/ode/demo/demo_collision.cpp
@@ -0,0 +1,1463 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+collision tests. if this program is run without any arguments it will
+perform all the tests multiple times, with different random data for each
+test. if this program is given a test number it will run that test
+graphically/interactively, in which case the space bar can be used to
+change the random test conditions.
+
+*/
+
+
+#include <ode/ode.h>
+#include <drawstuff/drawstuff.h>
+#include "texturepath.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
+#endif
+// select correct drawing functions
+#ifdef dDOUBLE
+#define dsDrawSphere dsDrawSphereD
+#define dsDrawBox dsDrawBoxD
+#define dsDrawLine dsDrawLineD
+#define dsDrawCapsule dsDrawCapsuleD
+#define dsDrawCylinder dsDrawCylinderD
+#endif
+
+//****************************************************************************
+// test infrastructure, including constants and macros
+
+#define TEST_REPS1 1000		// run each test this many times (first batch)
+#define TEST_REPS2 10000	// run each test this many times (second batch)
+const dReal tol = 1e-8;		// tolerance used for numerical checks
+#define MAX_TESTS 1000		// maximum number of test slots
+#define Z_OFFSET 2		// z offset for drawing (to get above ground)
+
+//using namespace ode;
+
+// test function. returns 1 if the test passed or 0 if it failed
+typedef int test_function_t();
+
+struct TestSlot {
+  int number;			// number of test
+  const char *name;			// name of test
+  int failcount;
+  test_function_t *test_fn;
+  int last_failed_line;
+};
+TestSlot testslot[MAX_TESTS];
+
+
+// globals used by the test functions
+int graphical_test=0;		// show graphical results of this test, 0=none
+int current_test;		// currently execiting test
+int draw_all_objects_called;
+
+
+#define MAKE_TEST(number,function) \
+  if (testslot[number].name) dDebug (0,"test number already used"); \
+  if (number <= 0 || number >= MAX_TESTS) dDebug (0,"bad test number"); \
+  testslot[number].name = # function; \
+  testslot[number].test_fn = function;
+
+#define FAILED() { if (graphical_test==0) { \
+  testslot[current_test].last_failed_line=__LINE__; return 0; } }
+#define PASSED() { return 1; }
+
+//****************************************************************************
+// globals
+
+/* int dBoxBox (const dVector3 p1, const dMatrix3 R1,
+	     const dVector3 side1, const dVector3 p2,
+	     const dMatrix3 R2, const dVector3 side2,
+	     dVector3 normal, dReal *depth, int *code,
+	     int maxc, dContactGeom *contact, int skip); */
+
+void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
+			   const dVector3 pb, const dVector3 ub,
+			   dReal *alpha, dReal *beta);
+
+//****************************************************************************
+// draw all objects in a space, and draw all the collision contact points
+
+void nearCallback (void *, dGeomID o1, dGeomID o2)
+{
+  int i,j,n;
+  const int N = 100;
+  dContactGeom contact[N];
+
+  if (dGeomGetClass (o2) == dRayClass) {
+    n = dCollide (o2,o1,N,&contact[0],sizeof(dContactGeom));
+  }
+  else {
+    n = dCollide (o1,o2,N,&contact[0],sizeof(dContactGeom));
+  }
+  if (n > 0) {
+    dMatrix3 RI;
+    dRSetIdentity (RI);
+    const dReal ss[3] = {0.01,0.01,0.01};
+    for (i=0; i<n; i++) {
+      contact[i].pos[2] += Z_OFFSET;
+      dsDrawBox (contact[i].pos,RI,ss);
+      dVector3 n;
+      for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j];
+      dsDrawLine (contact[i].pos,n);
+    }
+  }
+}
+
+
+void draw_all_objects (dSpaceID space)
+{
+  int i, j;
+
+  draw_all_objects_called = 1;
+  if (!graphical_test) return;
+  int n = dSpaceGetNumGeoms (space);
+
+  // draw all contact points
+  dsSetColor (0,1,1);
+  dSpaceCollide (space,0,&nearCallback);
+
+  // draw all rays
+  for (i=0; i<n; i++) {
+    dGeomID g = dSpaceGetGeom (space,i);
+    if (dGeomGetClass (g) == dRayClass) {
+      dsSetColor (1,1,1);
+      dVector3 origin,dir;
+      dGeomRayGet (g,origin,dir);
+      origin[2] += Z_OFFSET;
+      dReal length = dGeomRayGetLength (g);
+      for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j];
+      dsDrawLine (origin,dir);
+      dsSetColor (0,0,1);
+      dsDrawSphere (origin,dGeomGetRotation(g),0.01);
+    }
+  }
+
+  // draw all other objects
+  for (i=0; i<n; i++) {
+    dGeomID g = dSpaceGetGeom (space,i);
+    dVector3 pos;
+    if (dGeomGetClass (g) != dPlaneClass) {
+      memcpy (pos,dGeomGetPosition(g),sizeof(pos));
+      pos[2] += Z_OFFSET;
+    }
+
+    switch (dGeomGetClass (g)) {
+
+    case dSphereClass: {
+      dsSetColorAlpha (1,0,0,0.8);
+      dReal radius = dGeomSphereGetRadius (g);
+      dsDrawSphere (pos,dGeomGetRotation(g),radius);
+      break;
+    }
+
+    case dBoxClass: {
+      dsSetColorAlpha (1,1,0,0.8);
+      dVector3 sides;
+      dGeomBoxGetLengths (g,sides);
+      dsDrawBox (pos,dGeomGetRotation(g),sides);
+      break;
+    }
+
+    case dCapsuleClass: {
+      dsSetColorAlpha (0,1,0,0.8);
+      dReal radius,length;
+      dGeomCapsuleGetParams (g,&radius,&length);
+      dsDrawCapsule (pos,dGeomGetRotation(g),length,radius);
+      break;
+    }
+    case dCylinderClass: {
+      dsSetColorAlpha (0,1,0,0.8);
+      dReal radius,length;
+      dGeomCylinderGetParams (g,&radius,&length);
+      dsDrawCylinder (pos,dGeomGetRotation(g),length,radius);
+      break;
+    }
+
+    case dPlaneClass: {
+      dVector4 n;
+      dMatrix3 R,sides;
+      dVector3 pos2;
+      dGeomPlaneGetParams (g,n);
+      dRFromZAxis (R,n[0],n[1],n[2]);
+      for (j=0; j<3; j++) pos[j] = n[j]*n[3];
+      pos[2] += Z_OFFSET;
+      sides[0] = 2;
+      sides[1] = 2;
+      sides[2] = 0.001;
+      dsSetColor (1,0,1);
+      for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j];
+      dsDrawLine (pos,pos2);
+      dsSetColorAlpha (1,0,1,0.8);
+      dsDrawBox (pos,R,sides);
+      break;
+    }
+
+    }
+  }
+}
+
+//****************************************************************************
+// point depth tests
+
+int test_sphere_point_depth()
+{
+  int j;
+  dVector3 p,q;
+  dMatrix3 R;
+  dReal r,d;
+
+  dSimpleSpace space(0);
+  dGeomID sphere = dCreateSphere (0,1);
+  dSpaceAdd (space,sphere);
+
+  // ********** make a random sphere of radius r at position p
+
+  r = dRandReal()+0.1;
+  dGeomSphereSetRadius (sphere,r);
+  dMakeRandomVector (p,3,1.0);
+  dGeomSetPosition (sphere,p[0],p[1],p[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (sphere,R);
+
+  // ********** test center point has depth r
+
+  if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();
+
+  // ********** test point on surface has depth 0
+
+  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
+  dNormalize3 (q);
+  for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
+  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();
+
+  // ********** test point at random depth
+
+  d = (dRandReal()*2-1) * r;
+  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
+  dNormalize3 (q);
+  for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
+  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();
+
+  PASSED();
+}
+
+
+int test_box_point_depth()
+{
+  int i,j;
+  dVector3 s,p,q,q2;	// s = box sides
+  dMatrix3 R;
+  dReal ss,d;		// ss = smallest side
+
+  dSimpleSpace space(0);
+  dGeomID box = dCreateBox (0,1,1,1);
+  dSpaceAdd (space,box);
+
+  // ********** make a random box
+
+  for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
+  dGeomBoxSetLengths (box,s[0],s[1],s[2]);
+  dMakeRandomVector (p,3,1.0);
+  dGeomSetPosition (box,p[0],p[1],p[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (box,R);
+
+  // ********** test center point has depth of smallest side
+
+  ss = 1e9;
+  for (j=0; j<3; j++) if (s[j] < ss) ss = s[j];
+  if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol)
+    FAILED();
+
+  // ********** test point on surface has depth 0
+
+  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
+  i = dRandInt (3);
+  if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i];
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q2[j] += p[j];
+  if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED();
+
+  // ********** test points outside box have -ve depth
+
+  for (j=0; j<3; j++) {
+    q[j] = 0.5*s[j] + dRandReal() + 0.01;
+    if (dRandReal() > 0.5) q[j] = -q[j];
+  }
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q2[j] += p[j];
+  if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED();
+
+  // ********** test points inside box have +ve depth
+
+  for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5);
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q2[j] += p[j];
+  if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED();
+
+  // ********** test random depth of point aligned along axis (up to ss deep)
+
+  i = dRandInt (3);
+  for (j=0; j<3; j++) q[j] = 0;
+  d = (dRandReal()*(ss*0.5+1)-1);
+  q[i] = s[i]*0.5 - d;
+  if (dRandReal() > 0.5) q[i] = -q[i];
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q2[j] += p[j];
+  if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED();
+
+  PASSED();
+}
+
+
+int test_ccylinder_point_depth()
+{
+  int j;
+  dVector3 p,a;
+  dMatrix3 R;
+  dReal r,l,beta,x,y,d;
+
+  dSimpleSpace space(0);
+  dGeomID ccyl = dCreateCapsule (0,1,1);
+  dSpaceAdd (space,ccyl);
+
+  // ********** make a random ccyl
+
+  r = dRandReal()*0.5 + 0.01;
+  l = dRandReal()*1 + 0.01;
+  dGeomCapsuleSetParams (ccyl,r,l);
+  dMakeRandomVector (p,3,1.0);
+  dGeomSetPosition (ccyl,p[0],p[1],p[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (ccyl,R);
+
+  // ********** test point on axis has depth of 'radius'
+
+  beta = dRandReal()-0.5;
+  for (j=0; j<3; j++) a[j] = p[j] + l*beta*R[j*4+2];
+  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - r) >= tol)
+    FAILED();
+
+  // ********** test point on surface (excluding caps) has depth 0
+
+  beta = dRandReal()*2*M_PI;
+  x = r*sin(beta);
+  y = r*cos(beta);
+  beta = dRandReal()-0.5;
+  for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
+  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
+
+  // ********** test point on surface of caps has depth 0
+
+  for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
+  dNormalize3 (a);
+  if (dCalcVectorDot3_14(a,R+2) > 0) {
+    for (j=0; j<3; j++) a[j] = p[j] + a[j]*r + l*0.5*R[j*4+2];
+  }
+  else {
+    for (j=0; j<3; j++) a[j] = p[j] + a[j]*r - l*0.5*R[j*4+2];
+  }
+  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2])) >= tol) FAILED();
+
+  // ********** test point inside ccyl has positive depth
+
+  for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
+  dNormalize3 (a);
+  beta = dRandReal()-0.5;
+  for (j=0; j<3; j++) a[j] = p[j] + a[j]*r*0.99 + l*beta*R[j*4+2];
+  if (dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) < 0) FAILED();
+
+  // ********** test point depth (1)
+
+  d = (dRandReal()*2-1) * r;
+  beta = dRandReal()*2*M_PI;
+  x = (r-d)*sin(beta);
+  y = (r-d)*cos(beta);
+  beta = dRandReal()-0.5;
+  for (j=0; j<3; j++) a[j] = p[j] + x*R[j*4+0] + y*R[j*4+1] + l*beta*R[j*4+2];
+  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
+    FAILED();
+
+  // ********** test point depth (2)
+
+  d = (dRandReal()*2-1) * r;
+  for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
+  dNormalize3 (a);
+  if (dCalcVectorDot3_14(a,R+2) > 0) {
+    for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) + l*0.5*R[j*4+2];
+  }
+  else {
+    for (j=0; j<3; j++) a[j] = p[j] + a[j]*(r-d) - l*0.5*R[j*4+2];
+  }
+  if (dFabs(dGeomCapsulePointDepth (ccyl,a[0],a[1],a[2]) - d) >= tol)
+    FAILED();
+
+  PASSED();
+}
+
+
+int test_plane_point_depth()
+{
+  int j;
+  dVector3 n,p,q,a,b;	// n = plane normal
+  dReal d;
+
+  dSimpleSpace space(0);
+  dGeomID plane = dCreatePlane (0,0,0,1,0);
+  dSpaceAdd (space,plane);
+
+  // ********** make a random plane
+
+  for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
+  dNormalize3 (n);
+  d = dRandReal() - 0.5;
+  dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
+  dPlaneSpace (n,p,q);
+
+  // ********** test point on plane has depth 0
+
+  a[0] = dRandReal() - 0.5;
+  a[1] = dRandReal() - 0.5;
+  a[2] = 0;
+  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
+  if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED();
+
+  // ********** test arbitrary depth point
+
+  a[0] = dRandReal() - 0.5;
+  a[1] = dRandReal() - 0.5;
+  a[2] = dRandReal() - 0.5;
+  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
+  if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol)
+    FAILED();
+
+  // ********** test depth-1 point
+
+  a[0] = dRandReal() - 0.5;
+  a[1] = dRandReal() - 0.5;
+  a[2] = -1;
+  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
+  if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED();
+
+  PASSED();
+}
+
+//****************************************************************************
+// ray tests
+
+int test_ray_and_sphere()
+{
+  int j;
+  dContactGeom contact;
+  dVector3 p,q,q2,n,v1;
+  dMatrix3 R;
+  dReal r,k;
+
+  dSimpleSpace space(0);
+  dGeomID ray = dCreateRay (0,0);
+  dGeomID sphere = dCreateSphere (0,1);
+  dSpaceAdd (space,ray);
+  dSpaceAdd (space,sphere);
+
+  // ********** make a random sphere of radius r at position p
+
+  r = dRandReal()+0.1;
+  dGeomSphereSetRadius (sphere,r);
+  dMakeRandomVector (p,3,1.0);
+  dGeomSetPosition (sphere,p[0],p[1],p[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (sphere,R);
+
+  // ********** test zero length ray just inside sphere
+
+  dGeomRaySetLength (ray,0);
+  dMakeRandomVector (q,3,1.0);
+  dNormalize3 (q);
+  for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
+  dGeomSetPosition (ray,q[0],q[1],q[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (ray,R);
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test zero length ray just outside that sphere
+
+  dGeomRaySetLength (ray,0);
+  dMakeRandomVector (q,3,1.0);
+  dNormalize3 (q);
+  for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
+  dGeomSetPosition (ray,q[0],q[1],q[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (ray,R);
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test finite length ray totally contained inside the sphere
+
+  dMakeRandomVector (q,3,1.0);
+  dNormalize3 (q);
+  k = dRandReal();
+  for (j=0; j<3; j++) q[j] = k*r*0.99 * q[j] + p[j];
+  dMakeRandomVector (q2,3,1.0);
+  dNormalize3 (q2);
+  k = dRandReal();
+  for (j=0; j<3; j++) q2[j] = k*r*0.99 * q2[j] + p[j];
+  for (j=0; j<3; j++) n[j] = q2[j] - q[j];
+  dNormalize3 (n);
+  dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
+  dGeomRaySetLength (ray,dCalcPointsDistance3(q,q2));
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test finite length ray totally outside the sphere
+
+  dMakeRandomVector (q,3,1.0);
+  dNormalize3 (q);
+  do {
+    dMakeRandomVector (n,3,1.0);
+    dNormalize3 (n);
+  }
+  while (dCalcVectorDot3(n,q) < 0);	// make sure normal goes away from sphere
+  for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
+  dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
+  dGeomRaySetLength (ray,100);
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test ray from outside to just above surface
+
+  dMakeRandomVector (q,3,1.0);
+  dNormalize3 (q);
+  for (j=0; j<3; j++) n[j] = -q[j];
+  for (j=0; j<3; j++) q2[j] = 2*r * q[j] + p[j];
+  dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
+  dGeomRaySetLength (ray,0.99*r);
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test ray from outside to just below surface
+
+  dGeomRaySetLength (ray,1.01*r);
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+  for (j=0; j<3; j++) q2[j] = r * q[j] + p[j];
+  if (dCalcPointsDistance3 (contact.pos,q2) > tol) FAILED();
+
+  // ********** test contact point distance for random rays
+
+  dMakeRandomVector (q,3,1.0);
+  dNormalize3 (q);
+  k = dRandReal()+0.5;
+  for (j=0; j<3; j++) q[j] = k*r * q[j] + p[j];
+  dMakeRandomVector (n,3,1.0);
+  dNormalize3 (n);
+  dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
+  dGeomRaySetLength (ray,100);
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom))) {
+    k = dCalcPointsDistance3 (contact.pos,dGeomGetPosition(sphere));
+    if (dFabs(k - r) > tol) FAILED();
+    // also check normal signs
+    if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
+    // also check depth of contact point
+    if (dFabs (dGeomSpherePointDepth
+	       (sphere,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
+      FAILED();
+
+    draw_all_objects (space);
+  }
+
+  // ********** test tangential grazing - miss
+
+  dMakeRandomVector (q,3,1.0);
+  dNormalize3 (q);
+  dPlaneSpace (q,n,v1);
+  for (j=0; j<3; j++) q[j] = 1.01*r * q[j] + p[j];
+  for (j=0; j<3; j++) q[j] -= n[j];
+  dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
+  dGeomRaySetLength (ray,2);
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test tangential grazing - hit
+
+  dMakeRandomVector (q,3,1.0);
+  dNormalize3 (q);
+  dPlaneSpace (q,n,v1);
+  for (j=0; j<3; j++) q[j] = 0.99*r * q[j] + p[j];
+  for (j=0; j<3; j++) q[j] -= n[j];
+  dGeomRaySet (ray,q[0],q[1],q[2],n[0],n[1],n[2]);
+  dGeomRaySetLength (ray,2);
+  if (dCollide (ray,sphere,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+
+  PASSED();
+}
+
+
+int test_ray_and_box()
+{
+  int i,j;
+  dContactGeom contact;
+  dVector3 s,p,q,n,q2,q3,q4;		// s = box sides
+  dMatrix3 R;
+  dReal k;
+
+  dSimpleSpace space(0);
+  dGeomID ray = dCreateRay (0,0);
+  dGeomID box = dCreateBox (0,1,1,1);
+  dSpaceAdd (space,ray);
+  dSpaceAdd (space,box);
+
+  // ********** make a random box
+
+  for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
+  dGeomBoxSetLengths (box,s[0],s[1],s[2]);
+  dMakeRandomVector (p,3,1.0);
+  dGeomSetPosition (box,p[0],p[1],p[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (box,R);
+
+  // ********** test zero length ray just inside box
+
+  dGeomRaySetLength (ray,0);
+  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
+  i = dRandInt (3);
+  if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q2[j] += p[j];
+  dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (ray,R);
+  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test zero length ray just outside box
+
+  dGeomRaySetLength (ray,0);
+  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
+  i = dRandInt (3);
+  if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q2[j] += p[j];
+  dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (ray,R);
+  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test finite length ray totally contained inside the box
+
+  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q2[j] += p[j];
+  for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
+  dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
+  for (j=0; j<3; j++) q4[j] += p[j];
+  for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
+  dNormalize3 (n);
+  dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
+  dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4));
+  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test finite length ray totally outside the box
+
+  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
+  i = dRandInt (3);
+  if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
+  dNormalize3 (q2);
+  dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
+  dGeomRaySetLength (ray,10);
+  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test ray from outside to just above surface
+
+  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
+  i = dRandInt (3);
+  if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
+  k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
+  for (j=0; j<3; j++) q2[j] = -q2[j];
+  dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
+  dGeomRaySetLength (ray,k*0.99);
+  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test ray from outside to just below surface
+
+  dGeomRaySetLength (ray,k*1.01);
+  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+
+  // ********** test contact point position for random rays
+
+  for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
+  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
+  for (j=0; j<3; j++) q2[j] += p[j];
+  for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
+  dNormalize3 (q3);
+  dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
+  dGeomRaySetLength (ray,10);
+  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
+    // check depth of contact point
+    if (dFabs (dGeomBoxPointDepth
+	       (box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
+      FAILED();
+    // check position of contact point
+    for (j=0; j<3; j++) contact.pos[j] -= p[j];
+    dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
+    if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
+	 dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
+	 dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
+      FAILED();
+    }
+    // also check normal signs
+    if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED();
+
+    draw_all_objects (space);
+  }
+
+  PASSED();
+}
+
+
+int test_ray_and_ccylinder()
+{
+  int j;
+  dContactGeom contact;
+  dVector3 p,a,b,n;
+  dMatrix3 R;
+  dReal r,l,k,x,y;
+
+  dSimpleSpace space(0);
+  dGeomID ray = dCreateRay (0,0);
+  dGeomID ccyl = dCreateCapsule (0,1,1);
+  dSpaceAdd (space,ray);
+  dSpaceAdd (space,ccyl);
+
+  // ********** make a random capped cylinder
+
+  r = dRandReal()*0.5 + 0.01;
+  l = dRandReal()*1 + 0.01;
+  dGeomCapsuleSetParams (ccyl,r,l);
+  dMakeRandomVector (p,3,1.0);
+  dGeomSetPosition (ccyl,p[0],p[1],p[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (ccyl,R);
+
+  // ********** test ray completely within ccyl
+
+  for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
+  dNormalize3 (a);
+  k = (dRandReal()-0.5)*l;
+  for (j=0; j<3; j++) a[j] = p[j] + r*0.99*a[j] + k*0.99*R[j*4+2];
+  for (j=0; j<3; j++) b[j] = dRandReal()-0.5;
+  dNormalize3 (b);
+  k = (dRandReal()-0.5)*l;
+  for (j=0; j<3; j++) b[j] = p[j] + r*0.99*b[j] + k*0.99*R[j*4+2];
+  dGeomRaySetLength (ray,dCalcPointsDistance3(a,b));
+  for (j=0; j<3; j++) b[j] -= a[j];
+  dNormalize3 (b);
+  dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
+  if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test ray outside ccyl that just misses (between caps)
+
+  k = dRandReal()*2*M_PI;
+  x = sin(k);
+  y = cos(k);
+  for (j=0; j<3; j++) a[j] = x*R[j*4+0] + y*R[j*4+1];
+  k = (dRandReal()-0.5)*l;
+  for (j=0; j<3; j++) b[j] = -a[j]*r*2 + k*R[j*4+2] + p[j];
+  dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
+  dGeomRaySetLength (ray,r*0.99);
+  if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test ray outside ccyl that just hits (between caps)
+
+  dGeomRaySetLength (ray,r*1.01);
+  if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+  // check depth of contact point
+  if (dFabs (dGeomCapsulePointDepth
+	     (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
+    FAILED();
+
+  // ********** test ray outside ccyl that just misses (caps)
+
+  for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
+  dNormalize3 (a);
+  if (dCalcVectorDot3_14(a,R+2) < 0) {
+    for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r + l*0.5*R[j*4+2];
+  }
+  else {
+    for (j=0; j<3; j++) b[j] = p[j] - a[j]*2*r - l*0.5*R[j*4+2];
+  }
+  dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
+  dGeomRaySetLength (ray,r*0.99);
+  if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test ray outside ccyl that just hits (caps)
+
+  dGeomRaySetLength (ray,r*1.01);
+  if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+  // check depth of contact point
+  if (dFabs (dGeomCapsulePointDepth
+	     (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
+    FAILED();
+
+  // ********** test random rays
+
+  for (j=0; j<3; j++) a[j] = dRandReal()-0.5;
+  for (j=0; j<3; j++) n[j] = dRandReal()-0.5;
+  dNormalize3 (n);
+  dGeomRaySet (ray,a[0],a[1],a[2],n[0],n[1],n[2]);
+  dGeomRaySetLength (ray,10);
+
+  if (dCollide (ray,ccyl,1,&contact,sizeof(dContactGeom))) {
+    // check depth of contact point
+    if (dFabs (dGeomCapsulePointDepth
+	       (ccyl,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
+      FAILED();
+
+    // check normal signs
+    if (dCalcVectorDot3 (n,contact.normal) > 0) FAILED();
+
+    draw_all_objects (space);
+  }
+
+  PASSED();
+}
+
+/*
+  Test rays within the cylinder
+  -completely inside
+  -exiting through side
+  -exiting through cap
+  -exiting through corner
+  Test rays outside the cylinder
+*/
+int test_ray_and_cylinder()
+{
+  dVector3 a,b;
+
+  dSimpleSpace space(0);
+  dGeomID ray = dCreateRay(space,4);
+
+  // The first thing that happens is the ray is
+  // rotated into cylinder coordinates.  We'll trust that's
+  // done right.  The major axis is in the z-dir.
+
+
+  // Random tests
+  /*b[0]=4*dRandReal()-2;
+  b[1]=4*dRandReal()-2;
+  b[2]=4*dRandReal()-2;
+  a[0]=2*dRandReal()-1;
+  a[1]=2*dRandReal()-1;
+  a[2]=2*dRandReal()-1;*/
+  
+  // Inside out
+  b[0]=dRandReal()-0.5;
+  b[1]=dRandReal()-0.5;
+  b[2]=dRandReal()-0.5;
+  a[0]=2*dRandReal()-1;
+  a[1]=2*dRandReal()-1;
+  a[2]=2*dRandReal()-1;
+
+  // Outside in
+  /*b[0]=4*dRandReal()-2;
+  b[1]=4*dRandReal()-2;
+  b[2]=4*dRandReal()-2;
+  a[0]=-b[0];
+  a[1]=-b[1];
+  a[2]=-b[2];*/
+
+  
+  dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
+  // This is just for visual inspection right now.
+  //if (dCollide (ray,cyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+
+  draw_all_objects (space);
+
+  PASSED();
+}
+
+
+int test_ray_and_plane()
+{
+  int j;
+  dContactGeom contact;
+  dVector3 n,p,q,a,b,g,h;		// n,d = plane parameters
+  dMatrix3 R;
+  dReal d;
+
+  dSimpleSpace space(0);
+  dGeomID ray = dCreateRay (0,0);
+  dGeomID plane = dCreatePlane (0,0,0,1,0);
+  dSpaceAdd (space,ray);
+  dSpaceAdd (space,plane);
+
+  // ********** make a random plane
+
+  for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
+  dNormalize3 (n);
+  d = dRandReal() - 0.5;
+  dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
+  dPlaneSpace (n,p,q);
+
+  // ********** test finite length ray below plane
+
+  dGeomRaySetLength (ray,0.09);
+  a[0] = dRandReal()-0.5;
+  a[1] = dRandReal()-0.5;
+  a[2] = -dRandReal()*0.5 - 0.1;
+  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
+  dGeomSetPosition (ray,b[0],b[1],b[2]);
+  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
+		      dRandReal()*2-1,dRandReal()*10-5);
+  dGeomSetRotation (ray,R);
+  if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test finite length ray above plane
+
+  a[0] = dRandReal()-0.5;
+  a[1] = dRandReal()-0.5;
+  a[2] = dRandReal()*0.5 + 0.01;
+  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
+  g[0] = dRandReal()-0.5;
+  g[1] = dRandReal()-0.5;
+  g[2] = dRandReal() + 0.01;
+  for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
+  dNormalize3 (h);
+  dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
+  dGeomRaySetLength (ray,10);
+  if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test finite length ray that intersects plane
+
+  a[0] = dRandReal()-0.5;
+  a[1] = dRandReal()-0.5;
+  a[2] = dRandReal()-0.5;
+  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
+  g[0] = dRandReal()-0.5;
+  g[1] = dRandReal()-0.5;
+  g[2] = dRandReal()-0.5;
+  for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j];
+  dNormalize3 (h);
+  dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
+  dGeomRaySetLength (ray,10);
+  if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) {
+    // test that contact is on plane surface
+    if (dFabs (dCalcVectorDot3(contact.pos,n) - d) > tol) FAILED();
+    // also check normal signs
+    if (dCalcVectorDot3 (h,contact.normal) > 0) FAILED();
+    // also check contact point depth
+    if (dFabs (dGeomPlanePointDepth
+	       (plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
+      FAILED();
+
+    draw_all_objects (space);
+  }
+
+  // ********** test ray that just misses
+
+  for (j=0; j<3; j++) b[j] = (1+d)*n[j];
+  for (j=0; j<3; j++) h[j] = -n[j];
+  dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]);
+  dGeomRaySetLength (ray,0.99);
+  if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED();
+
+  // ********** test ray that just hits
+
+  dGeomRaySetLength (ray,1.01);
+  if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+
+  // ********** test polarity with typical ground plane
+
+  dGeomPlaneSetParams (plane,0,0,1,0);
+  for (j=0; j<3; j++) a[j] = 0.1;
+  for (j=0; j<3; j++) b[j] = 0;
+  a[2] = 1;
+  b[2] = -1;
+  dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
+  dGeomRaySetLength (ray,2);
+  if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+  if (dFabs (contact.depth - 1) > tol) FAILED();
+  a[2] = -1;
+  b[2] = 1;
+  dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]);
+  if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+  if (dFabs (contact.depth - 1) > tol) FAILED();
+
+  PASSED();
+}
+
+//****************************************************************************
+// a really inefficient, but hopefully correct implementation of
+// dBoxTouchesBox(), that does 144 edge-face tests.
+
+// return 1 if edge v1 -> v2 hits the rectangle described by p1,p2,p3
+
+static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
+			       dVector3 p1, dVector3 p2, dVector3 p3)
+{
+    int k;
+    dVector3 u1, u2, n, tmp;
+
+    for (k=0; k < 3; k++) u1[k] = p3[k] - p1[k];
+    for (k=0; k < 3; k++) u2[k] = p2[k] - p1[k];
+
+    dReal d1 = dSqrt(dCalcVectorDot3(u1, u1));
+    dReal d2 = dSqrt(dCalcVectorDot3(u2, u2));
+    dNormalize3(u1);
+    dNormalize3(u2);
+
+    dReal error;
+#ifdef dSINGLE
+    const dReal uEpsilon = 1e-5, pEpsilon = 1e-6, tmpEpsilon = 1.5e-4;
+#else
+    const dReal uEpsilon = 1e-6, pEpsilon = 1e-8, tmpEpsilon = 1e-6;
+#endif
+
+    error = dFabs(dCalcVectorDot3(u1, u2));
+    if (error > uEpsilon) dDebug(0, "bad u1/u2");
+
+    dCalcVectorCross3(n, u1, u2);
+
+    for (k=0; k < 3; k++) tmp[k] = v2[k] - v1[k];
+
+    dReal d = -dCalcVectorDot3(n, p1);
+
+    error = dFabs(dCalcVectorDot3(n, p1) + d);
+    if (error > pEpsilon) dDebug(0, "bad n wrt p1");
+
+    error = dFabs(dCalcVectorDot3(n, p2) + d);
+    if (error > pEpsilon) dDebug(0, "bad n wrt p2");
+
+    error = dFabs(dCalcVectorDot3(n, p3) + d);
+    if (error > pEpsilon) dDebug(0, "bad n wrt p3");
+
+    dReal alpha = -(d + dCalcVectorDot3(n, v1)) / dCalcVectorDot3(n, tmp);
+    for (k=0; k < 3; k++) tmp[k] = v1[k] + alpha * (v2[k] - v1[k]);
+
+    error = dFabs(dCalcVectorDot3(n, tmp) + d);
+    if (error > tmpEpsilon) dDebug(0, "bad tmp");
+
+    if (alpha < 0) return 0;
+    if (alpha > 1) return 0;
+
+    for (k=0; k < 3; k++) tmp[k] -= p1[k];
+    dReal a1 = dCalcVectorDot3(u1, tmp);
+    dReal a2 = dCalcVectorDot3(u2, tmp);
+    if (a1 < 0 || a2 < 0 || a1 > d1 || a2 > d2) return 0;
+
+    return 1;
+}
+
+
+// return 1 if box 1 is completely inside box 2
+
+static int box1inside2 (const dVector3 p1, const dMatrix3 R1,
+			const dVector3 side1, const dVector3 p2,
+			const dMatrix3 R2, const dVector3 side2)
+{
+  for (int i=-1; i<=1; i+=2) {
+    for (int j=-1; j<=1; j+=2) {
+      for (int k=-1; k<=1; k+=2) {
+	dVector3 v,vv;
+	v[0] = i*0.5*side1[0];
+	v[1] = j*0.5*side1[1];
+	v[2] = k*0.5*side1[2];
+	dMultiply0_331 (vv,R1,v);
+	vv[0] += p1[0] - p2[0];
+	vv[1] += p1[1] - p2[1];
+	vv[2] += p1[2] - p2[2];
+	for (int axis=0; axis < 3; axis++) {
+	  dReal z = dCalcVectorDot3_14(vv,R2+axis);
+	  if (z < (-side2[axis]*0.5) || z > (side2[axis]*0.5)) return 0;
+	}
+      }
+    }
+  }
+  return 1;
+}
+
+
+// test if any edge from box 1 hits a face from box 2
+
+static int testBoxesTouch2 (const dVector3 p1, const dMatrix3 R1,
+			    const dVector3 side1, const dVector3 p2,
+			    const dMatrix3 R2, const dVector3 side2)
+{
+  int j,k,j1,j2;
+
+  // for 6 faces from box 2
+  for (int fd=0; fd<3; fd++) {		// direction for face
+
+    for (int fo=0; fo<2; fo++) {	// offset of face
+      // get four points on the face. first get 2 indexes that are not fd
+      int k1=0,k2=0;
+      if (fd==0) { k1 = 1; k2 = 2; }
+      if (fd==1) { k1 = 0; k2 = 2; }
+      if (fd==2) { k1 = 0; k2 = 1; }
+      dVector3 fp[4],tmp;
+      k=0;
+      for (j1=-1; j1<=1; j1+=2) {
+	for (j2=-1; j2<=1; j2+=2) {
+	  fp[k][k1] = j1;
+	  fp[k][k2] = j2;
+	  fp[k][fd] = fo*2-1;
+	  k++;
+	}
+      }
+      for (j=0; j<4; j++) {
+	for (k=0; k<3; k++) fp[j][k] *= 0.5*side2[k];
+	dMultiply0_331 (tmp,R2,fp[j]);
+	for (k=0; k<3; k++) fp[j][k] = tmp[k] + p2[k];
+      }
+
+      // for 8 vertices
+      dReal v1[3];
+      for (v1[0]=-1; v1[0] <= 1; v1[0] += 2) {
+	for (v1[1]=-1; v1[1] <= 1; v1[1] += 2) {
+	  for (v1[2]=-1; v1[2] <= 1; v1[2] += 2) {
+	    // for all possible +ve leading edges from those vertices
+	    for (int ei=0; ei < 3; ei ++) {
+	      if (v1[ei] < 0) {
+		// get vertex1 -> vertex2 = an edge from box 1
+		dVector3 vv1,vv2;
+		for (k=0; k<3; k++) vv1[k] = v1[k] * 0.5*side1[k];
+		for (k=0; k<3; k++) vv2[k] = (v1[k] + (k==ei)*2)*0.5*side1[k];
+		dVector3 vertex1,vertex2;
+		dMultiply0_331 (vertex1,R1,vv1);
+		dMultiply0_331 (vertex2,R1,vv2);
+		for (k=0; k<3; k++) vertex1[k] += p1[k];
+		for (k=0; k<3; k++) vertex2[k] += p1[k];
+
+		// see if vertex1 -> vertex2 interesects face
+		if (edgeIntersectsRect (vertex1,vertex2,fp[0],fp[1],fp[2]))
+		  return 1;
+	      }
+	    }
+	  }
+	}
+      }
+    }
+  }
+
+  if (box1inside2 (p1,R1,side1,p2,R2,side2)) return 1;
+  if (box1inside2 (p2,R2,side2,p1,R1,side1)) return 1;
+
+  return 0;
+}
+
+//****************************************************************************
+// dBoxTouchesBox() test
+
+int test_dBoxTouchesBox()
+{
+  int k,bt1,bt2;
+  dVector3 p1,p2,side1,side2;
+  dMatrix3 R1,R2;
+
+  dSimpleSpace space(0);
+  dGeomID box1 = dCreateBox (0,1,1,1);
+  dSpaceAdd (space,box1);
+  dGeomID box2 = dCreateBox (0,1,1,1);
+  dSpaceAdd (space,box2);
+
+  dMakeRandomVector (p1,3,0.5);
+  dMakeRandomVector (p2,3,0.5);
+  for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
+  for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
+  dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+  dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+
+  dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
+  dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
+  dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
+  dGeomSetRotation (box1,R1);
+  dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
+  dGeomSetRotation (box2,R2);
+  draw_all_objects (space);
+
+  int t1 = testBoxesTouch2 (p1,R1,side1,p2,R2,side2);
+  int t2 = testBoxesTouch2 (p2,R2,side2,p1,R1,side1);
+  bt1 = t1 || t2;
+  bt2 = dBoxTouchesBox (p1,R1,side1,p2,R2,side2);
+
+  if (bt1 != bt2) FAILED();
+
+  /*
+    // some more debugging info if necessary
+    if (bt1 && bt2) printf ("agree - boxes touch\n");
+    if (!bt1 && !bt2) printf ("agree - boxes don't touch\n");
+    if (bt1 && !bt2) printf ("disagree - boxes touch but dBoxTouchesBox "
+			     "says no\n");
+    if (!bt1 && bt2) printf ("disagree - boxes don't touch but dBoxTouchesBox "
+			     "says yes\n");
+  */
+
+  PASSED();
+}
+
+//****************************************************************************
+// test box-box collision
+
+int test_dBoxBox()
+{
+  int k,bt;
+  dVector3 p1,p2,side1,side2,normal,normal2;
+  dMatrix3 R1,R2;
+  dReal depth,depth2;
+  int code;
+  dContactGeom contact[48];
+
+  dSimpleSpace space(0);
+  dGeomID box1 = dCreateBox (0,1,1,1);
+  dSpaceAdd (space,box1);
+  dGeomID box2 = dCreateBox (0,1,1,1);
+  dSpaceAdd (space,box2);
+
+  dMakeRandomVector (p1,3,0.5);
+  dMakeRandomVector (p2,3,0.5);
+  for (k=0; k<3; k++) side1[k] = dRandReal() + 0.01;
+  for (k=0; k<3; k++) side2[k] = dRandReal() + 0.01;
+
+  dRFromAxisAndAngle (R1,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+  dRFromAxisAndAngle (R2,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+		      dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+
+  // dRSetIdentity (R1);	// we can also try this
+  // dRSetIdentity (R2);
+
+  dGeomBoxSetLengths (box1,side1[0],side1[1],side1[2]);
+  dGeomBoxSetLengths (box2,side2[0],side2[1],side2[2]);
+  dGeomSetPosition (box1,p1[0],p1[1],p1[2]);
+  dGeomSetRotation (box1,R1);
+  dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
+  dGeomSetRotation (box2,R2);
+
+  code = 0;
+  depth = 0;
+  bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal,&depth,&code,8,contact,
+		sizeof(dContactGeom));
+  if (bt==1) {
+    p2[0] += normal[0] * 0.96 * depth;
+    p2[1] += normal[1] * 0.96 * depth;
+    p2[2] += normal[2] * 0.96 * depth;
+    bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
+		  sizeof(dContactGeom));
+
+    /*
+    dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
+    draw_all_objects (space);
+    */
+
+    if (bt != 1) {
+      FAILED();
+      dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
+      draw_all_objects (space);
+    }
+
+    p2[0] += normal[0] * 0.08 * depth;
+    p2[1] += normal[1] * 0.08 * depth;
+    p2[2] += normal[2] * 0.08 * depth;
+    bt = dBoxBox (p1,R1,side1,p2,R2,side2,normal2,&depth2,&code,8,contact,
+		  sizeof(dContactGeom));
+    if (bt != 0) FAILED();
+
+    // dGeomSetPosition (box2,p2[0],p2[1],p2[2]);
+    // draw_all_objects (space);
+  }
+
+  // printf ("code=%2d  depth=%.4f  ",code,depth);
+
+  PASSED();
+}
+
+//****************************************************************************
+// graphics
+
+int space_pressed = 0;
+
+
+// start simulation - set viewpoint
+
+static void start()
+{
+  dAllocateODEDataForThread(dAllocateMaskAll);
+
+  static float xyz[3] = {2.4807,-1.8023,2.7600};
+  static float hpr[3] = {141.5000,-18.5000,0.0000};
+  dsSetViewpoint (xyz,hpr);
+}
+
+
+// called when a key pressed
+
+static void command (int cmd)
+{
+  if (cmd == ' ') space_pressed = 1;
+}
+
+
+// simulation loop
+
+static void simLoop (int)
+{
+  do {
+    draw_all_objects_called = 0;
+    unsigned long seed = dRandGetSeed();
+    testslot[graphical_test].test_fn();
+    if (draw_all_objects_called) {
+      if (space_pressed) space_pressed = 0; else dRandSetSeed (seed);
+    }
+  }
+  while (!draw_all_objects_called);
+}
+
+//****************************************************************************
+// do all the tests
+
+void do_tests (int argc, char **argv)
+{
+  int i,j;
+
+  // process command line arguments
+  if (argc >= 2) {
+    graphical_test = atoi (argv[1]);
+  }
+
+  if (graphical_test) {
+    // do one test gaphically and interactively
+
+    if (graphical_test < 1 || graphical_test >= MAX_TESTS ||
+	!testslot[graphical_test].name) {
+      dError (0,"invalid test number");
+    }
+
+    printf ("performing test: %s\n",testslot[graphical_test].name);
+
+    // setup pointers to drawstuff callback functions
+    dsFunctions fn;
+    fn.version = DS_VERSION;
+    fn.start = &start;
+    fn.step = &simLoop;
+    fn.command = &command;
+    fn.stop = 0;
+    fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
+
+    dsSetSphereQuality (3);
+    dsSetCapsuleQuality (8);
+    dsSimulationLoop (argc,argv,1280,900,&fn);
+  }
+  else {
+    // do all tests noninteractively
+
+    for (i=0; i<MAX_TESTS; i++) testslot[i].number = i;
+
+    // first put the active tests into a separate array
+    int n=0;
+    for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) n++;
+    TestSlot **ts = (TestSlot**) malloc (n * sizeof(TestSlot*));
+    j = 0;
+    for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) ts[j++] = testslot+i;
+    if (j != n) dDebug (0,"internal");
+
+    // do two test batches. the first test batch has far fewer reps and will
+    // catch problems quickly. if all tests in the first batch passes, the
+    // second batch is run.
+
+    for (i=0; i<n; i++) ts[i]->failcount = 0;
+    int total_reps=0;
+    for (int batch=0; batch<2; batch++) {
+      int reps = (batch==0) ? TEST_REPS1 : TEST_REPS2;
+      total_reps += reps;
+      printf ("testing batch %d (%d reps)...\n",batch+1,reps);
+
+      // run tests
+      for (j=0; j<reps; j++) {
+	for (i=0; i<n; i++) {
+	  current_test = ts[i]->number;
+	  if (ts[i]->test_fn() != 1) ts[i]->failcount++;
+	}
+      }
+
+      // check for failures
+      int total_fail_count=0;
+      for (i=0; i<n; i++) total_fail_count += ts[i]->failcount;
+      if (total_fail_count) break;
+    }
+
+    // print results
+    for (i=0; i<n; i++) {
+      printf ("%3d: %-30s: ",ts[i]->number,ts[i]->name);
+      if (ts[i]->failcount) {
+	printf ("FAILED (%.2f%%) at line %d\n",
+		double(ts[i]->failcount)/double(total_reps)*100.0,
+		ts[i]->last_failed_line);
+      }
+      else {
+	printf ("ok\n");
+      }
+    }
+  }
+}
+
+//****************************************************************************
+
+int main (int argc, char **argv)
+{
+  // setup all tests
+
+  memset (testslot,0,sizeof(testslot));
+  dInitODE2(0);
+
+  MAKE_TEST(1,test_sphere_point_depth);
+  MAKE_TEST(2,test_box_point_depth);
+  MAKE_TEST(3,test_ccylinder_point_depth);
+  MAKE_TEST(4,test_plane_point_depth);
+
+  MAKE_TEST(10,test_ray_and_sphere);
+  MAKE_TEST(11,test_ray_and_box);
+  MAKE_TEST(12,test_ray_and_ccylinder);
+  MAKE_TEST(13,test_ray_and_plane);
+  MAKE_TEST(14,test_ray_and_cylinder);
+
+  MAKE_TEST(100,test_dBoxTouchesBox);
+  MAKE_TEST(101,test_dBoxBox);
+
+  do_tests (argc,argv);
+  dCloseODE();
+  return 0;
+}