one more for the gipper

1 files changed, 1103 insertions, 0 deletions

diff --git a/libraries/ode-0.9/ode/src/collision_kernel.cpp b/libraries/ode-0.9/ode/src/collision_kernel.cpp
new file mode 100644
index 0000000..b885603
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/collision_kernel.cpp
@@ -0,0 +1,1103 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+core collision functions and data structures, plus part of the public API
+for geometry objects
+
+*/
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/objects.h>
+#include <ode/odemath.h>
+#include "collision_kernel.h"
+#include "collision_util.h"
+#include "collision_std.h"
+#include "collision_transform.h"
+#include "collision_trimesh_internal.h"
+
+#if dTRIMESH_GIMPACT
+#include <GIMPACT/gimpact.h>
+#endif
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+//****************************************************************************
+// helper functions for dCollide()ing a space with another geom
+
+// this struct records the parameters passed to dCollideSpaceGeom()
+
+// Allocate and free posr - we cache a single posr to avoid thrashing
+static dxPosR* s_cachedPosR = 0;
+
+dxPosR* dAllocPosr()
+{
+        dxPosR* retPosR;
+        if (s_cachedPosR)
+        {
+                retPosR = s_cachedPosR;
+                s_cachedPosR = 0;
+        }
+        else
+        {
+                retPosR = (dxPosR*) dAlloc (sizeof(dxPosR));
+        }
+        return retPosR;
+}
+
+void dFreePosr(dxPosR* oldPosR)
+{
+        if (oldPosR)
+        {
+                if (s_cachedPosR)
+                {
+                        dFree(s_cachedPosR, sizeof(dxPosR));
+                }
+                s_cachedPosR = oldPosR;
+        }
+}
+
+void dClearPosrCache(void)
+{
+        if (s_cachedPosR)
+        {
+                dFree(s_cachedPosR, sizeof(dxPosR));
+                s_cachedPosR = 0;
+        }
+}
+
+struct SpaceGeomColliderData {
+  int flags;                    // space left in contacts array
+  dContactGeom *contact;
+  int skip;
+};
+
+
+static void space_geom_collider (void *data, dxGeom *o1, dxGeom *o2)
+{
+  SpaceGeomColliderData *d = (SpaceGeomColliderData*) data;
+  if (d->flags & NUMC_MASK) {
+    int n = dCollide (o1,o2,d->flags,d->contact,d->skip);
+    d->contact = CONTACT (d->contact,d->skip*n);
+    d->flags -= n;
+  }
+}
+
+
+static int dCollideSpaceGeom (dxGeom *o1, dxGeom *o2, int flags,
+                              dContactGeom *contact, int skip)
+{
+  SpaceGeomColliderData data;
+  data.flags = flags;
+  data.contact = contact;
+  data.skip = skip;
+  dSpaceCollide2 (o1,o2,&data,&space_geom_collider);
+  return (flags & NUMC_MASK) - (data.flags & NUMC_MASK);
+}
+
+//****************************************************************************
+// dispatcher for the N^2 collider functions
+
+// function pointers and modes for n^2 class collider functions
+
+struct dColliderEntry {
+  dColliderFn *fn;      // collider function, 0 = no function available
+  int reverse;          // 1 = reverse o1 and o2
+};
+static dColliderEntry colliders[dGeomNumClasses][dGeomNumClasses];
+static int colliders_initialized = 0;
+
+
+// setCollider() will refuse to write over a collider entry once it has
+// been written.
+
+static void setCollider (int i, int j, dColliderFn *fn)
+{
+  if (colliders[i][j].fn == 0) {
+    colliders[i][j].fn = fn;
+    colliders[i][j].reverse = 0;
+  }
+  if (colliders[j][i].fn == 0) {
+    colliders[j][i].fn = fn;
+    colliders[j][i].reverse = 1;
+  }
+}
+
+
+static void setAllColliders (int i, dColliderFn *fn)
+{
+  for (int j=0; j<dGeomNumClasses; j++) setCollider (i,j,fn);
+}
+
+
+static void initColliders()
+{
+  int i,j;
+
+  if (colliders_initialized) return;
+  colliders_initialized = 1;
+
+  memset (colliders,0,sizeof(colliders));
+
+  // setup space colliders
+  for (i=dFirstSpaceClass; i <= dLastSpaceClass; i++) {
+    for (j=0; j < dGeomNumClasses; j++) {
+      setCollider (i,j,&dCollideSpaceGeom);
+    }
+  }
+
+  setCollider (dSphereClass,dSphereClass,&dCollideSphereSphere);
+  setCollider (dSphereClass,dBoxClass,&dCollideSphereBox);
+  setCollider (dSphereClass,dPlaneClass,&dCollideSpherePlane);
+  setCollider (dBoxClass,dBoxClass,&dCollideBoxBox);
+  setCollider (dBoxClass,dPlaneClass,&dCollideBoxPlane);
+  setCollider (dCapsuleClass,dSphereClass,&dCollideCapsuleSphere);
+  setCollider (dCapsuleClass,dBoxClass,&dCollideCapsuleBox);
+  setCollider (dCapsuleClass,dCapsuleClass,&dCollideCapsuleCapsule);
+  setCollider (dCapsuleClass,dPlaneClass,&dCollideCapsulePlane);
+  setCollider (dRayClass,dSphereClass,&dCollideRaySphere);
+  setCollider (dRayClass,dBoxClass,&dCollideRayBox);
+  setCollider (dRayClass,dCapsuleClass,&dCollideRayCapsule);
+  setCollider (dRayClass,dPlaneClass,&dCollideRayPlane);
+  setCollider (dRayClass,dCylinderClass,&dCollideRayCylinder);
+#if dTRIMESH_ENABLED
+  setCollider (dTriMeshClass,dSphereClass,&dCollideSTL);
+  setCollider (dTriMeshClass,dBoxClass,&dCollideBTL);
+  setCollider (dTriMeshClass,dRayClass,&dCollideRTL);
+  setCollider (dTriMeshClass,dTriMeshClass,&dCollideTTL);
+  setCollider (dTriMeshClass,dCapsuleClass,&dCollideCCTL);
+  setCollider (dTriMeshClass,dPlaneClass,&dCollideTrimeshPlane);
+  setCollider (dCylinderClass,dTriMeshClass,&dCollideCylinderTrimesh);
+#endif
+  setCollider (dCylinderClass,dBoxClass,&dCollideCylinderBox);
+  setCollider (dCylinderClass,dSphereClass,&dCollideCylinderSphere);
+  setCollider (dCylinderClass,dPlaneClass,&dCollideCylinderPlane);
+  //setCollider (dCylinderClass,dCylinderClass,&dCollideCylinderCylinder);
+
+//--> Convex Collision
+  setCollider (dConvexClass,dPlaneClass,&dCollideConvexPlane);
+  setCollider (dSphereClass,dConvexClass,&dCollideSphereConvex);
+  setCollider (dConvexClass,dBoxClass,&dCollideConvexBox);
+  setCollider (dConvexClass,dCapsuleClass,&dCollideConvexCapsule);
+  setCollider (dConvexClass,dConvexClass,&dCollideConvexConvex);
+  setCollider (dRayClass,dConvexClass,&dCollideRayConvex);
+//<-- Convex Collision
+
+//--> dHeightfield Collision
+  setCollider (dHeightfieldClass,dRayClass,&dCollideHeightfield);
+  setCollider (dHeightfieldClass,dSphereClass,&dCollideHeightfield);
+  setCollider (dHeightfieldClass,dBoxClass,&dCollideHeightfield);
+  setCollider (dHeightfieldClass,dCapsuleClass,&dCollideHeightfield);
+  setCollider (dHeightfieldClass,dCylinderClass,&dCollideHeightfield);
+  setCollider (dHeightfieldClass,dConvexClass,&dCollideHeightfield);
+#if dTRIMESH_ENABLED
+  setCollider (dHeightfieldClass,dTriMeshClass,&dCollideHeightfield);
+#endif
+//<-- dHeightfield Collision
+
+  setAllColliders (dGeomTransformClass,&dCollideTransform);
+}
+
+
+/*
+ *      NOTE!
+ *      If it is necessary to add special processing mode without contact generation
+ *      use NULL contact parameter value as indicator, not zero in flags.
+ */
+int dCollide (dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact,
+              int skip)
+{
+  dAASSERT(o1 && o2 && contact);
+  dUASSERT(colliders_initialized,"colliders array not initialized");
+  dUASSERT(o1->type >= 0 && o1->type < dGeomNumClasses,"bad o1 class number");
+  dUASSERT(o2->type >= 0 && o2->type < dGeomNumClasses,"bad o2 class number");
+  // Even though comparison for greater or equal to one is used in all the 
+  // other places, here it is more logical to check for greater than zero
+  // because function does not require any specific number of contact slots - 
+  // it must be just a positive.
+  dUASSERT((flags & NUMC_MASK) > 0, "no contacts requested"); 
+
+  // Extra precaution for zero contact count in parameters
+  if ((flags & NUMC_MASK) == 0) return 0;
+  // no contacts if both geoms are the same
+  if (o1 == o2) return 0;
+
+  // no contacts if both geoms on the same body, and the body is not 0
+  if (o1->body == o2->body && o1->body) return 0;
+
+  o1->recomputePosr();
+  o2->recomputePosr();
+
+  dColliderEntry *ce = &colliders[o1->type][o2->type];
+  int count = 0;
+  if (ce->fn) {
+    if (ce->reverse) {
+      count = (*ce->fn) (o2,o1,flags,contact,skip);
+      for (int i=0; i<count; i++) {
+        dContactGeom *c = CONTACT(contact,skip*i);
+        c->normal[0] = -c->normal[0];
+        c->normal[1] = -c->normal[1];
+        c->normal[2] = -c->normal[2];
+        dxGeom *tmp = c->g1;
+        c->g1 = c->g2;
+        c->g2 = tmp;
+        int tmpint = c->side1;
+        c->side1 = c->side2;
+        c->side2 = tmpint;
+      }
+    }
+    else {
+      count = (*ce->fn) (o1,o2,flags,contact,skip);
+    }
+  }
+  return count;
+}
+
+//****************************************************************************
+// dxGeom
+
+dxGeom::dxGeom (dSpaceID _space, int is_placeable)
+{
+  initColliders();
+
+  // setup body vars. invalid type of -1 must be changed by the constructor.
+  type = -1;
+  gflags = GEOM_DIRTY | GEOM_AABB_BAD | GEOM_ENABLED;
+  if (is_placeable) gflags |= GEOM_PLACEABLE;
+  data = 0;
+  body = 0;
+  body_next = 0;
+  if (is_placeable) {
+        final_posr = dAllocPosr();
+    dSetZero (final_posr->pos,4);
+    dRSetIdentity (final_posr->R);
+  }
+  else {
+    final_posr = 0;
+  }
+  offset_posr = 0;
+
+  // setup space vars
+  next = 0;
+  tome = 0;
+  parent_space = 0;
+  dSetZero (aabb,6);
+  category_bits = ~0;
+  collide_bits = ~0;
+
+  // put this geom in a space if required
+  if (_space) dSpaceAdd (_space,this);
+}
+
+
+dxGeom::~dxGeom()
+{
+   if (parent_space) dSpaceRemove (parent_space,this);
+   if ((gflags & GEOM_PLACEABLE) && (!body || (body && offset_posr)))
+     dFreePosr(final_posr);
+   if (offset_posr) dFreePosr(offset_posr);
+   bodyRemove();
+}
+
+
+int dxGeom::AABBTest (dxGeom *o, dReal aabb[6])
+{
+  return 1;
+}
+
+
+void dxGeom::bodyRemove()
+{
+  if (body) {
+    // delete this geom from body list
+    dxGeom **last = &body->geom, *g = body->geom;
+    while (g) {
+      if (g == this) {
+        *last = g->body_next;
+        break;
+      }
+      last = &g->body_next;
+      g = g->body_next;
+    }
+    body = 0;
+    body_next = 0;
+  }
+}
+
+inline void myswap(dReal& a, dReal& b) { dReal t=b; b=a; a=t; }
+
+
+inline void matrixInvert(const dMatrix3& inMat, dMatrix3& outMat)
+{
+        memcpy(outMat, inMat, sizeof(dMatrix3));
+        // swap _12 and _21
+        myswap(outMat[0 + 4*1], outMat[1 + 4*0]);
+        // swap _31 and _13
+        myswap(outMat[2 + 4*0], outMat[0 + 4*2]);
+        // swap _23 and _32
+        myswap(outMat[1 + 4*2], outMat[2 + 4*1]);
+}
+
+void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr)
+{
+        dMatrix3 inv_offset;
+        matrixInvert(offset_posr.R, inv_offset);
+
+        dMULTIPLY0_333(body_posr.R, final_posr.R, inv_offset);
+        dVector3 world_offset;
+        dMULTIPLY0_331(world_offset, body_posr.R, offset_posr.pos);
+        body_posr.pos[0] = final_posr.pos[0] - world_offset[0];
+        body_posr.pos[1] = final_posr.pos[1] - world_offset[1];
+        body_posr.pos[2] = final_posr.pos[2] - world_offset[2];
+}
+
+void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr)
+{
+        dMatrix3 inv_body;
+        matrixInvert(body_posr.R, inv_body);
+
+        dMULTIPLY0_333(offset_posr.R, inv_body, world_posr.R);
+        dVector3 world_offset;
+        world_offset[0] = world_posr.pos[0] - body_posr.pos[0];
+        world_offset[1] = world_posr.pos[1] - body_posr.pos[1];
+        world_offset[2] = world_posr.pos[2] - body_posr.pos[2];
+        dMULTIPLY0_331(offset_posr.pos, inv_body, world_offset);
+}
+
+void dxGeom::computePosr()
+{
+  // should only be recalced if we need to - ie offset from a body
+  dIASSERT(offset_posr);  
+  dIASSERT(body);
+  
+  dMULTIPLY0_331 (final_posr->pos,body->posr.R,offset_posr->pos);
+  final_posr->pos[0] += body->posr.pos[0];
+  final_posr->pos[1] += body->posr.pos[1];
+  final_posr->pos[2] += body->posr.pos[2];
+  dMULTIPLY0_333 (final_posr->R,body->posr.R,offset_posr->R);
+}
+
+//****************************************************************************
+// misc
+
+dxGeom *dGeomGetBodyNext (dxGeom *geom)
+{
+  return geom->body_next;
+}
+
+//****************************************************************************
+// public API for geometry objects
+
+#define CHECK_NOT_LOCKED(space) \
+  dUASSERT (!(space && space->lock_count), \
+            "invalid operation for geom in locked space");
+
+
+void dGeomDestroy (dxGeom *g)
+{
+  dAASSERT (g);
+  delete g;
+}
+
+
+void dGeomSetData (dxGeom *g, void *data)
+{
+  dAASSERT (g);
+  g->data = data;
+}
+
+
+void *dGeomGetData (dxGeom *g)
+{
+  dAASSERT (g);
+  return g->data;
+}
+
+
+void dGeomSetBody (dxGeom *g, dxBody *b)
+{
+  dAASSERT (g);
+  dUASSERT (b == NULL || (g->gflags & GEOM_PLACEABLE),"geom must be placeable");
+  CHECK_NOT_LOCKED (g->parent_space);
+
+  if (b) {
+    if (!g->body) dFreePosr(g->final_posr);
+    if (g->body != b) {
+      if (g->offset_posr) {
+        dFreePosr(g->offset_posr);
+        g->offset_posr = 0;
+      }
+      g->final_posr = &b->posr;
+      g->bodyRemove();
+      g->bodyAdd (b);
+    }
+    dGeomMoved (g);
+  }
+  else {
+    if (g->body) {
+      if (g->offset_posr)
+      {
+        // if we're offset, we already have our own final position, make sure its updated
+        g->recomputePosr();
+        dFreePosr(g->offset_posr);
+        g->offset_posr = 0;
+      }
+      else
+      {
+        g->final_posr = dAllocPosr();
+        memcpy (g->final_posr->pos,g->body->posr.pos,sizeof(dVector3));
+        memcpy (g->final_posr->R,g->body->posr.R,sizeof(dMatrix3));
+      }
+      g->bodyRemove();
+    }
+    // dGeomMoved() should not be called if the body is being set to 0, as the
+    // new position of the geom is set to the old position of the body, so the
+    // effective position of the geom remains unchanged.
+  }
+}
+
+
+dBodyID dGeomGetBody (dxGeom *g)
+{
+  dAASSERT (g);
+  return g->body;
+}
+
+
+void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (g->offset_posr) {
+    // move body such that body+offset = position
+        dVector3 world_offset;
+        dMULTIPLY0_331(world_offset, g->body->posr.R, g->offset_posr->pos);
+        dBodySetPosition(g->body,
+            x - world_offset[0],
+            y - world_offset[1],
+            z - world_offset[2]);
+  }
+  else if (g->body) {
+    // this will call dGeomMoved (g), so we don't have to
+    dBodySetPosition (g->body,x,y,z);
+  }
+  else {
+    g->final_posr->pos[0] = x;
+    g->final_posr->pos[1] = y;
+    g->final_posr->pos[2] = z;
+    dGeomMoved (g);
+  }
+}
+
+
+void dGeomSetRotation (dxGeom *g, const dMatrix3 R)
+{
+  dAASSERT (g && R);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (g->offset_posr) {
+    g->recomputePosr();
+    // move body such that body+offset = rotation
+    dxPosR new_final_posr;
+    dxPosR new_body_posr;
+    memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
+    memcpy(new_final_posr.R, R, sizeof(dMatrix3));
+    getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);
+    dBodySetRotation(g->body, new_body_posr.R);
+    dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);
+  }
+  else if (g->body) {
+    // this will call dGeomMoved (g), so we don't have to
+    dBodySetRotation (g->body,R);
+  }
+  else {
+    memcpy (g->final_posr->R,R,sizeof(dMatrix3));
+    dGeomMoved (g);
+  }
+}
+
+
+void dGeomSetQuaternion (dxGeom *g, const dQuaternion quat)
+{
+  dAASSERT (g && quat);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (g->offset_posr) {
+    g->recomputePosr();
+    // move body such that body+offset = rotation
+    dxPosR new_final_posr;
+    dxPosR new_body_posr;
+    dQtoR (quat, new_final_posr.R);
+    memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
+    
+    getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);
+    dBodySetRotation(g->body, new_body_posr.R);
+    dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);
+  }
+  if (g->body) {
+    // this will call dGeomMoved (g), so we don't have to
+    dBodySetQuaternion (g->body,quat);
+  }
+  else {
+    dQtoR (quat, g->final_posr->R);
+    dGeomMoved (g);
+  }
+}
+
+
+const dReal * dGeomGetPosition (dxGeom *g)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  g->recomputePosr();
+  return g->final_posr->pos;
+}
+
+
+void dGeomCopyPosition(dxGeom *g, dVector3 pos)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  g->recomputePosr();
+  const dReal* src = g->final_posr->pos;
+  pos[0] = src[0];
+  pos[1] = src[1];
+  pos[2] = src[2];
+}
+
+
+const dReal * dGeomGetRotation (dxGeom *g)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  g->recomputePosr();
+  return g->final_posr->R;
+}
+
+
+void dGeomCopyRotation(dxGeom *g, dMatrix3 R)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  g->recomputePosr();
+  const dReal* src = g->final_posr->R;
+  R[0]  = src[0];
+  R[1]  = src[1];
+  R[2]  = src[2];
+  R[4]  = src[4];
+  R[5]  = src[5];
+  R[6]  = src[6];
+  R[8]  = src[8];
+  R[9]  = src[9];
+  R[10] = src[10];
+}
+
+
+void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  if (g->body && !g->offset_posr) {
+    const dReal * body_quat = dBodyGetQuaternion (g->body);
+    quat[0] = body_quat[0];
+    quat[1] = body_quat[1];
+    quat[2] = body_quat[2];
+    quat[3] = body_quat[3];
+  }
+  else {
+    g->recomputePosr();
+    dRtoQ (g->final_posr->R, quat);
+  }
+}
+
+
+void dGeomGetAABB (dxGeom *g, dReal aabb[6])
+{
+  dAASSERT (g);
+  dAASSERT (aabb);
+  g->recomputeAABB();
+  memcpy (aabb,g->aabb,6 * sizeof(dReal));
+}
+
+
+int dGeomIsSpace (dxGeom *g)
+{
+  dAASSERT (g);
+  return IS_SPACE(g);
+}
+
+
+dSpaceID dGeomGetSpace (dxGeom *g)
+{
+  dAASSERT (g);
+  return g->parent_space;
+}
+
+
+int dGeomGetClass (dxGeom *g)
+{
+  dAASSERT (g);
+  return g->type;
+}
+
+
+void dGeomSetCategoryBits (dxGeom *g, unsigned long bits)
+{
+  dAASSERT (g);
+  CHECK_NOT_LOCKED (g->parent_space);
+  g->category_bits = bits;
+}
+
+
+void dGeomSetCollideBits (dxGeom *g, unsigned long bits)
+{
+  dAASSERT (g);
+  CHECK_NOT_LOCKED (g->parent_space);
+  g->collide_bits = bits;
+}
+
+
+unsigned long dGeomGetCategoryBits (dxGeom *g)
+{
+  dAASSERT (g);
+  return g->category_bits;
+}
+
+
+unsigned long dGeomGetCollideBits (dxGeom *g)
+{
+  dAASSERT (g);
+  return g->collide_bits;
+}
+
+
+void dGeomEnable (dxGeom *g)
+{
+        dAASSERT (g);
+        g->gflags |= GEOM_ENABLED;
+}
+
+void dGeomDisable (dxGeom *g)
+{
+        dAASSERT (g);
+        g->gflags &= ~GEOM_ENABLED;
+}
+
+int dGeomIsEnabled (dxGeom *g)
+{
+        dAASSERT (g);
+        return (g->gflags & GEOM_ENABLED) != 0;
+}
+
+
+//****************************************************************************
+// C interface that lets the user make new classes. this interface is a lot
+// more cumbersome than C++ subclassing, which is what is used internally
+// in ODE. this API is mainly to support legacy code.
+
+static int num_user_classes = 0;
+static dGeomClass user_classes [dMaxUserClasses];
+
+
+struct dxUserGeom : public dxGeom {
+  void *user_data;
+
+  dxUserGeom (int class_num);
+  ~dxUserGeom();
+  void computeAABB();
+  int AABBTest (dxGeom *o, dReal aabb[6]);
+};
+
+
+dxUserGeom::dxUserGeom (int class_num) : dxGeom (0,1)
+{
+  type = class_num;
+  int size = user_classes[type-dFirstUserClass].bytes;
+  user_data = dAlloc (size);
+  memset (user_data,0,size);
+}
+
+
+dxUserGeom::~dxUserGeom()
+{
+  dGeomClass *c = &user_classes[type-dFirstUserClass];
+  if (c->dtor) c->dtor (this);
+  dFree (user_data,c->bytes);
+}
+
+
+void dxUserGeom::computeAABB()
+{
+  user_classes[type-dFirstUserClass].aabb (this,aabb);
+}
+
+
+int dxUserGeom::AABBTest (dxGeom *o, dReal aabb[6])
+{
+  dGeomClass *c = &user_classes[type-dFirstUserClass];
+  if (c->aabb_test) return c->aabb_test (this,o,aabb);
+  else return 1;
+}
+
+
+static int dCollideUserGeomWithGeom (dxGeom *o1, dxGeom *o2, int flags,
+                                     dContactGeom *contact, int skip)
+{
+  // this generic collider function is called the first time that a user class
+  // tries to collide against something. it will find out the correct collider
+  // function and then set the colliders array so that the correct function is
+  // called directly the next time around.
+
+  int t1 = o1->type;    // note that o1 is a user geom
+  int t2 = o2->type;    // o2 *may* be a user geom
+
+  // find the collider function to use. if o1 does not know how to collide with
+  // o2, then o2 might know how to collide with o1 (provided that it is a user
+  // geom).
+  dColliderFn *fn = user_classes[t1-dFirstUserClass].collider (t2);
+  int reverse = 0;
+  if (!fn && t2 >= dFirstUserClass && t2 <= dLastUserClass) {
+    fn = user_classes[t2-dFirstUserClass].collider (t1);
+    reverse = 1;
+  }
+
+  // set the colliders array so that the correct function is called directly
+  // the next time around. note that fn can be 0 here if no collider was found,
+  // which means that dCollide() will always return 0 for this case.
+  colliders[t1][t2].fn = fn;
+  colliders[t1][t2].reverse = reverse;
+  colliders[t2][t1].fn = fn;
+  colliders[t2][t1].reverse = !reverse;
+
+  // now call the collider function indirectly through dCollide(), so that
+  // contact reversing is properly handled.
+  return dCollide (o1,o2,flags,contact,skip);
+}
+
+
+int dCreateGeomClass (const dGeomClass *c)
+{
+  dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class");
+
+  if (num_user_classes >= dMaxUserClasses) {
+    dDebug (0,"too many user classes, you must increase the limit and "
+              "recompile ODE");
+  }
+  user_classes[num_user_classes] = *c;
+  int class_number = num_user_classes + dFirstUserClass;
+  initColliders();
+  setAllColliders (class_number,&dCollideUserGeomWithGeom);
+
+  num_user_classes++;
+  return class_number;
+}
+
+
+void * dGeomGetClassData (dxGeom *g)
+{
+  dUASSERT (g && g->type >= dFirstUserClass &&
+            g->type <= dLastUserClass,"not a custom class");
+  dxUserGeom *user = (dxUserGeom*) g;
+  return user->user_data;
+}
+
+
+dGeomID dCreateGeom (int classnum)
+{
+  dUASSERT (classnum >= dFirstUserClass &&
+            classnum <= dLastUserClass,"not a custom class");
+  return new dxUserGeom (classnum);
+}
+
+
+
+/* ************************************************************************ */
+/* geom offset from body */
+
+void dGeomCreateOffset (dxGeom *g)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  dUASSERT (g->body, "geom must be on a body");  
+  if (g->offset_posr)
+  {
+        return; // already created
+  }
+  dIASSERT (g->final_posr == &g->body->posr);
+  
+  g->final_posr = dAllocPosr();
+  g->offset_posr = dAllocPosr();
+  dSetZero (g->offset_posr->pos,4);
+  dRSetIdentity (g->offset_posr->R);
+  
+  g->gflags |= GEOM_POSR_BAD;
+}
+
+void dGeomSetOffsetPosition (dxGeom *g, dReal x, dReal y, dReal z)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  dUASSERT (g->body, "geom must be on a body");  
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (!g->offset_posr) 
+  {
+        dGeomCreateOffset(g);
+  }
+  g->offset_posr->pos[0] = x;
+  g->offset_posr->pos[1] = y;
+  g->offset_posr->pos[2] = z;
+  dGeomMoved (g);
+}
+
+void dGeomSetOffsetRotation (dxGeom *g, const dMatrix3 R)
+{
+  dAASSERT (g && R);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  dUASSERT (g->body, "geom must be on a body");  
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (!g->offset_posr) 
+  {
+        dGeomCreateOffset (g);
+  }
+  memcpy (g->offset_posr->R,R,sizeof(dMatrix3));
+  dGeomMoved (g);
+}
+
+void dGeomSetOffsetQuaternion (dxGeom *g, const dQuaternion quat)
+{
+  dAASSERT (g && quat);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  dUASSERT (g->body, "geom must be on a body");  
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (!g->offset_posr) 
+  {
+        dGeomCreateOffset (g);
+  }
+  dQtoR (quat, g->offset_posr->R);
+  dGeomMoved (g);
+}
+
+void dGeomSetOffsetWorldPosition (dxGeom *g, dReal x, dReal y, dReal z)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  dUASSERT (g->body, "geom must be on a body");  
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (!g->offset_posr) 
+  {
+        dGeomCreateOffset(g);
+  }
+  dBodyGetPosRelPoint(g->body, x, y, z, g->offset_posr->pos);
+  dGeomMoved (g);
+}
+
+void dGeomSetOffsetWorldRotation (dxGeom *g, const dMatrix3 R)
+{
+  dAASSERT (g && R);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  dUASSERT (g->body, "geom must be on a body");  
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (!g->offset_posr) 
+  {
+        dGeomCreateOffset (g);
+  }
+  g->recomputePosr();
+  
+  dxPosR new_final_posr;
+  memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
+  memcpy(new_final_posr.R, R, sizeof(dMatrix3));
+  
+  getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr);
+  dGeomMoved (g);
+}
+
+void dGeomSetOffsetWorldQuaternion (dxGeom *g, const dQuaternion quat)
+{
+  dAASSERT (g && quat);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  dUASSERT (g->body, "geom must be on a body");  
+  CHECK_NOT_LOCKED (g->parent_space);
+  if (!g->offset_posr) 
+  {
+        dGeomCreateOffset (g);
+  }
+
+  g->recomputePosr();
+  
+  dxPosR new_final_posr;
+  memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
+  dQtoR (quat, new_final_posr.R);
+  
+  getWorldOffsetPosr(g->body->posr, new_final_posr, *g->offset_posr);
+  dGeomMoved (g);
+}
+
+void dGeomClearOffset(dxGeom *g)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  if (g->offset_posr)
+  {
+    dIASSERT(g->body);
+    // no longer need an offset posr
+        dFreePosr(g->offset_posr);
+        g->offset_posr = 0;
+    // the geom will now share the position of the body
+    dFreePosr(g->final_posr);
+    g->final_posr = &g->body->posr;
+    // geom has moved
+    g->gflags &= ~GEOM_POSR_BAD;
+    dGeomMoved (g);
+  }
+}
+
+int dGeomIsOffset(dxGeom *g)
+{
+  dAASSERT (g);
+  return ((0 != g->offset_posr) ? 1 : 0);
+}
+
+static const dVector3 OFFSET_POSITION_ZERO = { 0.0f, 0.0f, 0.0f, 0.0f };
+
+const dReal * dGeomGetOffsetPosition (dxGeom *g)
+{
+  dAASSERT (g);
+  if (g->offset_posr)
+  {
+    return g->offset_posr->pos;
+  }
+  return OFFSET_POSITION_ZERO;
+}
+
+void dGeomCopyOffsetPosition (dxGeom *g, dVector3 pos)
+{
+  dAASSERT (g);
+  if (g->offset_posr)
+  {
+    const dReal* src = g->offset_posr->pos;
+    pos[0] = src[0];
+         pos[1] = src[1];
+         pos[2] = src[2];
+  }
+  else
+  {
+    pos[0] = 0;
+         pos[1] = 0;
+         pos[2] = 0;
+  }
+}
+
+static const dMatrix3 OFFSET_ROTATION_ZERO = 
+{ 
+        1.0f, 0.0f, 0.0f, 0.0f, 
+        0.0f, 1.0f, 0.0f, 0.0f, 
+        0.0f, 0.0f, 1.0f, 0.0f, 
+};
+
+const dReal * dGeomGetOffsetRotation (dxGeom *g)
+{
+  dAASSERT (g);
+  if (g->offset_posr)
+  {
+    return g->offset_posr->R;
+  }
+  return OFFSET_ROTATION_ZERO;
+}
+
+void dGeomCopyOffsetRotation (dxGeom *g, dMatrix3 R)
+{
+        dAASSERT (g);
+        if (g->offset_posr)
+        {
+                const dReal* src = g->final_posr->R;
+                R[0]  = src[0];
+                R[1]  = src[1];
+                R[2]  = src[2];
+                R[4]  = src[4];
+                R[5]  = src[5];
+                R[6]  = src[6];
+                R[8]  = src[8];
+                R[9]  = src[9];
+                R[10] = src[10];
+        }
+        else
+        {
+                R[0]  = OFFSET_ROTATION_ZERO[0];
+                R[1]  = OFFSET_ROTATION_ZERO[1];
+                R[2]  = OFFSET_ROTATION_ZERO[2];
+                R[4]  = OFFSET_ROTATION_ZERO[4];
+                R[5]  = OFFSET_ROTATION_ZERO[5];
+                R[6]  = OFFSET_ROTATION_ZERO[6];
+                R[8]  = OFFSET_ROTATION_ZERO[8];
+                R[9]  = OFFSET_ROTATION_ZERO[9];
+                R[10] = OFFSET_ROTATION_ZERO[10];
+        }
+}
+
+void dGeomGetOffsetQuaternion (dxGeom *g, dQuaternion result)
+{
+  dAASSERT (g);
+  if (g->offset_posr)
+  {
+    dRtoQ (g->offset_posr->R, result);
+  }
+  else
+  {
+    dSetZero (result,4);
+    result[0] = 1;
+  }
+}
+
+//****************************************************************************
+// initialization and shutdown routines - allocate and initialize data,
+// cleanup before exiting
+
+extern void opcode_collider_cleanup();
+
+void dInitODE()
+{
+#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
+        gimpact_init();
+#endif
+}
+
+void dCloseODE()
+{
+  colliders_initialized = 0;
+  num_user_classes = 0;
+  dClearPosrCache();
+
+#if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
+  gimpact_terminate();
+#endif
+
+#if dTRIMESH_ENABLED && dTRIMESH_OPCODE
+  // Free up static allocations in opcode
+  opcode_collider_cleanup();
+#endif
+}