lovr-ODE/ode/demo/demo_collision.cpp

/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: [email protected]   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

static 
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);
        }
    }
}


static 
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

static 
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();
}


static 
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.5f*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();
}


static 
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();
}


static 
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

static 
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();
}


static 
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.5f*s[0]) > tol &&
            dFabs(dFabs(q[1]) - 0.5f*s[1]) > tol &&
            dFabs(dFabs(q[2]) - 0.5f*s[2]) > tol) {
            FAILED();
        }
        // also check normal signs
        if (dCalcVectorDot3(q3, contact.normal) > 0) FAILED();

        draw_all_objects(space);
    }

    PASSED();
}


static 
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
*/
static 
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();
}


static 
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

static 
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

static 
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

static 
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, DS_SIMULATION_DEFAULT_WIDTH, DS_SIMULATION_DEFAULT_HEIGHT, &fn);
    }
    else {
        // do all tests noninteractively
        dsInitializeConsole(argc, argv);

        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");
            }
        }

        dsFinalizeConsole();
    }
}

//****************************************************************************

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;
}