GUD improvements

Blocks3D.vcproj

@@ -185,6 +185,7 @@
 				OutputFile="./Blocks3D-Debug.exe"
 				LinkIncremental="2"
 				SuppressStartupBanner="true"
+				AdditionalLibraryDirectories=""
 				GenerateDebugInformation="true"
 				ProgramDatabaseFile=".\Debug/Blocks3D.pdb"
 				SubSystem="1"

src/include/ode/README

@@ -0,0 +1,18 @@
+
+this is the public C interface to the ODE library.
+
+all these files should be includable from C, i.e. they should not use any
+C++ features. everything should be protected with
+
+	#ifdef __cplusplus
+	extern "C" {
+	#endif
+
+	...
+
+	#ifdef __cplusplus
+	}
+	#endif
+
+the only exceptions are the odecpp.h and odecpp_collisioh.h files, which define a C++ wrapper for
+the C interface. remember to keep this in sync!

src/include/ode/collision.h

@@ -0,0 +1,189 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_COLLISION_H_
+#define _ODE_COLLISION_H_
+
+#include <ode/common.h>
+#include <ode/collision_space.h>
+#include <ode/contact.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* ************************************************************************ */
+/* general functions */
+
+void dGeomDestroy (dGeomID);
+void dGeomSetData (dGeomID, void *);
+void *dGeomGetData (dGeomID);
+void dGeomSetBody (dGeomID, dBodyID);
+dBodyID dGeomGetBody (dGeomID);
+void dGeomSetPosition (dGeomID, dReal x, dReal y, dReal z);
+void dGeomSetRotation (dGeomID, const dMatrix3 R);
+void dGeomSetQuaternion (dGeomID, const dQuaternion);
+const dReal * dGeomGetPosition (dGeomID);
+const dReal * dGeomGetRotation (dGeomID);
+void dGeomGetQuaternion (dGeomID, dQuaternion result);
+void dGeomGetAABB (dGeomID, dReal aabb[6]);
+int dGeomIsSpace (dGeomID);
+dSpaceID dGeomGetSpace (dGeomID);
+int dGeomGetClass (dGeomID);
+void dGeomSetCategoryBits (dGeomID, unsigned long bits);
+void dGeomSetCollideBits (dGeomID, unsigned long bits);
+unsigned long dGeomGetCategoryBits (dGeomID);
+unsigned long dGeomGetCollideBits (dGeomID);
+void dGeomEnable (dGeomID);
+void dGeomDisable (dGeomID);
+int dGeomIsEnabled (dGeomID);
+
+/* ************************************************************************ */
+/* collision detection */
+
+int dCollide (dGeomID o1, dGeomID o2, int flags, dContactGeom *contact,
+	      int skip);
+void dSpaceCollide (dSpaceID space, void *data, dNearCallback *callback);
+void dSpaceCollide2 (dGeomID o1, dGeomID o2, void *data,
+		     dNearCallback *callback);
+
+/* ************************************************************************ */
+/* standard classes */
+
+/* the maximum number of user classes that are supported */
+enum {
+  dMaxUserClasses = 4
+};
+
+/* class numbers - each geometry object needs a unique number */
+enum {
+  dSphereClass = 0,
+  dBoxClass,
+  dCCylinderClass,
+  dCylinderClass,
+  dPlaneClass,
+  dRayClass,
+  dGeomTransformClass,
+  dTriMeshClass,
+
+  dFirstSpaceClass,
+  dSimpleSpaceClass = dFirstSpaceClass,
+  dHashSpaceClass,
+  dQuadTreeSpaceClass,
+  dLastSpaceClass = dQuadTreeSpaceClass,
+
+  dFirstUserClass,
+  dLastUserClass = dFirstUserClass + dMaxUserClasses - 1,
+  dGeomNumClasses
+};
+
+
+dGeomID dCreateSphere (dSpaceID space, dReal radius);
+void dGeomSphereSetRadius (dGeomID sphere, dReal radius);
+dReal dGeomSphereGetRadius (dGeomID sphere);
+dReal dGeomSpherePointDepth (dGeomID sphere, dReal x, dReal y, dReal z);
+
+dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
+void dGeomBoxSetLengths (dGeomID box, dReal lx, dReal ly, dReal lz);
+void dGeomBoxGetLengths (dGeomID box, dVector3 result);
+dReal dGeomBoxPointDepth (dGeomID box, dReal x, dReal y, dReal z);
+
+dGeomID dCreatePlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
+void dGeomPlaneSetParams (dGeomID plane, dReal a, dReal b, dReal c, dReal d);
+void dGeomPlaneGetParams (dGeomID plane, dVector4 result);
+dReal dGeomPlanePointDepth (dGeomID plane, dReal x, dReal y, dReal z);
+
+dGeomID dCreateCCylinder (dSpaceID space, dReal radius, dReal length);
+void dGeomCCylinderSetParams (dGeomID ccylinder, dReal radius, dReal length);
+void dGeomCCylinderGetParams (dGeomID ccylinder, dReal *radius, dReal *length);
+dReal dGeomCCylinderPointDepth (dGeomID ccylinder, dReal x, dReal y, dReal z);
+
+dGeomID dCreateRay (dSpaceID space, dReal length);
+void dGeomRaySetLength (dGeomID ray, dReal length);
+dReal dGeomRayGetLength (dGeomID ray);
+void dGeomRaySet (dGeomID ray, dReal px, dReal py, dReal pz,
+		  dReal dx, dReal dy, dReal dz);
+void dGeomRayGet (dGeomID ray, dVector3 start, dVector3 dir);
+
+/*
+ * Set/get ray flags that influence ray collision detection.
+ * These flags are currently only noticed by the trimesh collider, because
+ * they can make a major differences there.
+ */
+void dGeomRaySetParams (dGeomID g, int FirstContact, int BackfaceCull);
+void dGeomRayGetParams (dGeomID g, int *FirstContact, int *BackfaceCull);
+void dGeomRaySetClosestHit (dGeomID g, int closestHit);
+int dGeomRayGetClosestHit (dGeomID g);
+
+#include "collision_trimesh.h"
+
+dGeomID dCreateGeomTransform (dSpaceID space);
+void dGeomTransformSetGeom (dGeomID g, dGeomID obj);
+dGeomID dGeomTransformGetGeom (dGeomID g);
+void dGeomTransformSetCleanup (dGeomID g, int mode);
+int dGeomTransformGetCleanup (dGeomID g);
+void dGeomTransformSetInfo (dGeomID g, int mode);
+int dGeomTransformGetInfo (dGeomID g);
+
+/* ************************************************************************ */
+/* utility functions */
+
+void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2,
+				const dVector3 b1, const dVector3 b2,
+				dVector3 cp1, dVector3 cp2);
+
+int dBoxTouchesBox (const dVector3 _p1, const dMatrix3 R1,
+		    const dVector3 side1, const dVector3 _p2,
+		    const dMatrix3 R2, const dVector3 side2);
+
+void dInfiniteAABB (dGeomID geom, dReal aabb[6]);
+void dCloseODE();
+
+/* ************************************************************************ */
+/* custom classes */
+
+typedef void dGetAABBFn (dGeomID, dReal aabb[6]);
+typedef int dColliderFn (dGeomID o1, dGeomID o2,
+			 int flags, dContactGeom *contact, int skip);
+typedef dColliderFn * dGetColliderFnFn (int num);
+typedef void dGeomDtorFn (dGeomID o);
+typedef int dAABBTestFn (dGeomID o1, dGeomID o2, dReal aabb[6]);
+
+typedef struct dGeomClass {
+  int bytes;
+  dGetColliderFnFn *collider;
+  dGetAABBFn *aabb;
+  dAABBTestFn *aabb_test;
+  dGeomDtorFn *dtor;
+} dGeomClass;
+
+int dCreateGeomClass (const dGeomClass *classptr);
+void * dGeomGetClassData (dGeomID);
+dGeomID dCreateGeom (int classnum);
+
+/* ************************************************************************ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/collision_space.h

@@ -0,0 +1,61 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_COLLISION_SPACE_H_
+#define _ODE_COLLISION_SPACE_H_
+
+#include <ode/common.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct dContactGeom;
+
+typedef void dNearCallback (void *data, dGeomID o1, dGeomID o2);
+
+
+dSpaceID dSimpleSpaceCreate (dSpaceID space);
+dSpaceID dHashSpaceCreate (dSpaceID space);
+dSpaceID dQuadTreeSpaceCreate (dSpaceID space, dVector3 Center, dVector3 Extents, int Depth);
+
+void dSpaceDestroy (dSpaceID);
+
+void dHashSpaceSetLevels (dSpaceID space, int minlevel, int maxlevel);
+void dHashSpaceGetLevels (dSpaceID space, int *minlevel, int *maxlevel);
+
+void dSpaceSetCleanup (dSpaceID space, int mode);
+int dSpaceGetCleanup (dSpaceID space);
+
+void dSpaceAdd (dSpaceID, dGeomID);
+void dSpaceRemove (dSpaceID, dGeomID);
+int dSpaceQuery (dSpaceID, dGeomID);
+void dSpaceClean (dSpaceID);
+int dSpaceGetNumGeoms (dSpaceID);
+dGeomID dSpaceGetGeom (dSpaceID, int i);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/collision_trimesh.h

@@ -0,0 +1,181 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ * TriMesh code by Erwin de Vries.
+ *
+ * Trimesh data.
+ * This is where the actual vertexdata (pointers), and BV tree is stored.
+ * Vertices should be single precision!
+ * This should be more sophisticated, so that the user can easyly implement
+ * another collision library, but this is a lot of work, and also costs some
+ * performance because some data has to be copied.
+ */
+
+#ifndef _ODE_COLLISION_TRIMESH_H_
+#define _ODE_COLLISION_TRIMESH_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * Data storage for triangle meshes.
+ */
+struct dxTriMeshData;
+typedef struct dxTriMeshData* dTriMeshDataID;
+
+/*
+ * These dont make much sense now, but they will later when we add more
+ * features.
+ */
+dTriMeshDataID dGeomTriMeshDataCreate();
+void dGeomTriMeshDataDestroy(dTriMeshDataID g);
+
+enum { TRIMESH_FACE_NORMALS, TRIMESH_LAST_TRANSFORMATION };
+void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* data);
+
+
+/*
+ * Build TriMesh data with single pricision used in vertex data .
+ */
+void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
+                                 const void* Vertices, int VertexStride, int VertexCount, 
+                                 const void* Indices, int IndexCount, int TriStride);
+/* same again with a normals array (used as trimesh-trimesh optimization) */
+void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
+                                  const void* Vertices, int VertexStride, int VertexCount, 
+                                  const void* Indices, int IndexCount, int TriStride,
+                                  const void* Normals);
+/*
+* Build TriMesh data with double pricision used in vertex data .
+*/
+void dGeomTriMeshDataBuildDouble(dTriMeshDataID g, 
+                                 const void* Vertices,  int VertexStride, int VertexCount, 
+                                 const void* Indices, int IndexCount, int TriStride);
+/* same again with a normals array (used as trimesh-trimesh optimization) */
+void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g, 
+                                  const void* Vertices,  int VertexStride, int VertexCount, 
+                                  const void* Indices, int IndexCount, int TriStride,
+                                  const void* Normals);
+
+/*
+ * Simple build. Single/double precision based on dSINGLE/dDOUBLE!
+ */
+void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
+                                 const dReal* Vertices, int VertexCount,
+                                 const int* Indices, int IndexCount);
+/* same again with a normals array (used as trimesh-trimesh optimization) */
+void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
+                                  const dReal* Vertices, int VertexCount,
+                                  const int* Indices, int IndexCount,
+                                  const int* Normals);
+/*
+ * Per triangle callback. Allows the user to say if he wants a collision with
+ * a particular triangle.
+ */
+typedef int dTriCallback(dGeomID TriMesh, dGeomID RefObject, int TriangleIndex);
+void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback);
+dTriCallback* dGeomTriMeshGetCallback(dGeomID g);
+
+/*
+ * Per object callback. Allows the user to get the list of triangles in 1
+ * shot. Maybe we should remove this one.
+ */
+typedef void dTriArrayCallback(dGeomID TriMesh, dGeomID RefObject, const int* TriIndices, int TriCount);
+void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback);
+dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g);
+
+/*
+ * Ray callback.
+ * Allows the user to say if a ray collides with a triangle on barycentric
+ * coords. The user can for example sample a texture with alpha transparency
+ * to determine if a collision should occur.
+ */
+typedef int dTriRayCallback(dGeomID TriMesh, dGeomID Ray, int TriangleIndex, dReal u, dReal v);
+void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback);
+dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g);
+
+/*
+ * Trimesh class
+ * Construction. Callbacks are optional.
+ */
+dGeomID dCreateTriMesh(dSpaceID space, dTriMeshDataID Data, dTriCallback* Callback, dTriArrayCallback* ArrayCallback, dTriRayCallback* RayCallback);
+
+void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data);
+
+// enable/disable/check temporal coherence
+void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable);
+int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass);
+
+/*
+ * Clears the internal temporal coherence caches. When a geom has its
+ * collision checked with a trimesh once, data is stored inside the trimesh.
+ * With large worlds with lots of seperate objects this list could get huge.
+ * We should be able to do this automagically.
+ */
+void dGeomTriMeshClearTCCache(dGeomID g);
+
+
+/*
+ * returns the TriMeshDataID
+ */
+dTriMeshDataID dGeomTriMeshGetTriMeshDataID(dGeomID g);
+
+/*
+ * Gets a triangle.
+ */
+void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2);
+
+/*
+ * Gets the point on the requested triangle and the given barycentric
+ * coordinates.
+ */
+void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out);
+
+/*
+
+This is how the strided data works:
+
+struct StridedVertex{
+	dVector3 Vertex;
+	// Userdata
+};
+int VertexStride = sizeof(StridedVertex);
+
+struct StridedTri{
+	int Indices[3];
+	// Userdata
+};
+int TriStride = sizeof(StridedTri);
+
+*/
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif	/* _ODE_COLLISION_TRIMESH_H_ */
+

src/include/ode/common.h

@@ -0,0 +1,321 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_COMMON_H_
+#define _ODE_COMMON_H_
+
+#include <ode/config.h>
+#include <ode/error.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* configuration stuff */
+
+/* the efficient alignment. most platforms align data structures to some
+ * number of bytes, but this is not always the most efficient alignment.
+ * for example, many x86 compilers align to 4 bytes, but on a pentium it
+ * is important to align doubles to 8 byte boundaries (for speed), and
+ * the 4 floats in a SIMD register to 16 byte boundaries. many other
+ * platforms have similar behavior. setting a larger alignment can waste
+ * a (very) small amount of memory. NOTE: this number must be a power of
+ * two. this is set to 16 by default.
+ */
+#define EFFICIENT_ALIGNMENT 16
+
+
+/* constants */
+
+/* pi and 1/sqrt(2) are defined here if necessary because they don't get
+ * defined in <math.h> on some platforms (like MS-Windows)
+ */
+
+#ifndef M_PI
+#define M_PI REAL(3.1415926535897932384626433832795029)
+#endif
+#ifndef M_SQRT1_2
+#define M_SQRT1_2 REAL(0.7071067811865475244008443621048490)
+#endif
+
+
+/* debugging:
+ *   IASSERT  is an internal assertion, i.e. a consistency check. if it fails
+ *            we want to know where.
+ *   UASSERT  is a user assertion, i.e. if it fails a nice error message
+ *            should be printed for the user.
+ *   AASSERT  is an arguments assertion, i.e. if it fails "bad argument(s)"
+ *            is printed.
+ *   DEBUGMSG just prints out a message
+ */
+
+#ifndef dNODEBUG
+#ifdef __GNUC__
+#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \
+  "assertion \"" #a "\" failed in %s() [%s]",__FUNCTION__,__FILE__);
+#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \
+  msg " in %s()", __FUNCTION__);
+#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \
+  msg " in %s()", __FUNCTION__);
+#else
+#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \
+  "assertion \"" #a "\" failed in %s:%d",__FILE__,__LINE__);
+#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \
+  msg " (%s:%d)", __FILE__,__LINE__);
+#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \
+  msg " (%s:%d)", __FILE__,__LINE__);
+#endif
+#else
+#define dIASSERT(a) ;
+#define dUASSERT(a,msg) ;
+#define dDEBUGMSG(msg) ;
+#endif
+#define dAASSERT(a) dUASSERT(a,"Bad argument(s)")
+
+/* floating point data type, vector, matrix and quaternion types */
+
+#if defined(dSINGLE)
+typedef float dReal;
+#elif defined(dDOUBLE)
+typedef double dReal;
+#else
+#error You must #define dSINGLE or dDOUBLE
+#endif
+
+
+/* round an integer up to a multiple of 4, except that 0 and 1 are unmodified
+ * (used to compute matrix leading dimensions)
+ */
+#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a))
+
+/* these types are mainly just used in headers */
+typedef dReal dVector3[4];
+typedef dReal dVector4[4];
+typedef dReal dMatrix3[4*3];
+typedef dReal dMatrix4[4*4];
+typedef dReal dMatrix6[8*6];
+typedef dReal dQuaternion[4];
+
+
+/* precision dependent scalar math functions */
+
+#if defined(dSINGLE)
+
+#define REAL(x) (x ## f)					/* form a constant */
+#define dRecip(x) ((float)(1.0f/(x)))				/* reciprocal */
+#define dSqrt(x) ((float)sqrtf(float(x)))			/* square root */
+#define dRecipSqrt(x) ((float)(1.0f/sqrtf(float(x))))		/* reciprocal square root */
+#define dSin(x) ((float)sinf(float(x)))				/* sine */
+#define dCos(x) ((float)cosf(float(x)))				/* cosine */
+#define dFabs(x) ((float)fabsf(float(x)))			/* absolute value */
+#define dAtan2(y,x) ((float)atan2f(float(y),float(x)))		/* arc tangent with 2 args */
+#define dFMod(a,b) ((float)fmodf(float(a),float(b)))		/* modulo */
+#define dCopySign(a,b) ((float)copysignf(float(a),float(b)))
+
+#elif defined(dDOUBLE)
+
+#define REAL(x) (x)
+#define dRecip(x) (1.0/(x))
+#define dSqrt(x) sqrt(x)
+#define dRecipSqrt(x) (1.0/sqrt(x))
+#define dSin(x) sin(x)
+#define dCos(x) cos(x)
+#define dFabs(x) fabs(x)
+#define dAtan2(y,x) atan2((y),(x))
+#define dFMod(a,b) (fmod((a),(b)))
+#define dCopySign(a,b) (copysign((a),(b)))
+
+#else
+#error You must #define dSINGLE or dDOUBLE
+#endif
+
+
+/* utility */
+
+
+/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */
+
+#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1)
+
+
+/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
+ * up to 15 bytes per allocation, depending on what alloca() returns.
+ */
+
+#define dALLOCA16(n) \
+  ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))
+
+
+/* internal object types (all prefixed with `dx') */
+
+struct dxWorld;		/* dynamics world */
+struct dxSpace;		/* collision space */
+struct dxBody;		/* rigid body (dynamics object) */
+struct dxGeom;		/* geometry (collision object) */
+struct dxJoint;
+struct dxJointNode;
+struct dxJointGroup;
+
+typedef struct dxWorld *dWorldID;
+typedef struct dxSpace *dSpaceID;
+typedef struct dxBody *dBodyID;
+typedef struct dxGeom *dGeomID;
+typedef struct dxJoint *dJointID;
+typedef struct dxJointGroup *dJointGroupID;
+
+
+/* error numbers */
+
+enum {
+  d_ERR_UNKNOWN = 0,		/* unknown error */
+  d_ERR_IASSERT,		/* internal assertion failed */
+  d_ERR_UASSERT,		/* user assertion failed */
+  d_ERR_LCP			/* user assertion failed */
+};
+
+
+/* joint type numbers */
+
+enum {
+  dJointTypeNone = 0,		/* or "unknown" */
+  dJointTypeBall,
+  dJointTypeHinge,
+  dJointTypeSlider,
+  dJointTypeContact,
+  dJointTypeUniversal,
+  dJointTypeHinge2,
+  dJointTypeFixed,
+  dJointTypeNull,
+  dJointTypeAMotor
+};
+
+
+/* an alternative way of setting joint parameters, using joint parameter
+ * structures and member constants. we don't actually do this yet.
+ */
+
+/*
+typedef struct dLimot {
+  int mode;
+  dReal lostop, histop;
+  dReal vel, fmax;
+  dReal fudge_factor;
+  dReal bounce, soft;
+  dReal suspension_erp, suspension_cfm;
+} dLimot;
+
+enum {
+  dLimotLoStop		= 0x0001,
+  dLimotHiStop		= 0x0002,
+  dLimotVel		= 0x0004,
+  dLimotFMax		= 0x0008,
+  dLimotFudgeFactor	= 0x0010,
+  dLimotBounce		= 0x0020,
+  dLimotSoft		= 0x0040
+};
+*/
+
+
+/* standard joint parameter names. why are these here? - because we don't want
+ * to include all the joint function definitions in joint.cpp. hmmmm.
+ * MSVC complains if we call D_ALL_PARAM_NAMES_X with a blank second argument,
+ * which is why we have the D_ALL_PARAM_NAMES macro as well. please copy and
+ * paste between these two.
+ */
+
+#define D_ALL_PARAM_NAMES(start) \
+  /* parameters for limits and motors */ \
+  dParamLoStop = start, \
+  dParamHiStop, \
+  dParamVel, \
+  dParamFMax, \
+  dParamFudgeFactor, \
+  dParamBounce, \
+  dParamCFM, \
+  dParamStopERP, \
+  dParamStopCFM, \
+  /* parameters for suspension */ \
+  dParamSuspensionERP, \
+  dParamSuspensionCFM,
+
+#define D_ALL_PARAM_NAMES_X(start,x) \
+  /* parameters for limits and motors */ \
+  dParamLoStop ## x = start, \
+  dParamHiStop ## x, \
+  dParamVel ## x, \
+  dParamFMax ## x, \
+  dParamFudgeFactor ## x, \
+  dParamBounce ## x, \
+  dParamCFM ## x, \
+  dParamStopERP ## x, \
+  dParamStopCFM ## x, \
+  /* parameters for suspension */ \
+  dParamSuspensionERP ## x, \
+  dParamSuspensionCFM ## x,
+
+enum {
+  D_ALL_PARAM_NAMES(0)
+  D_ALL_PARAM_NAMES_X(0x100,2)
+  D_ALL_PARAM_NAMES_X(0x200,3)
+
+  /* add a multiple of this constant to the basic parameter numbers to get
+   * the parameters for the second, third etc axes.
+   */
+  dParamGroup=0x100
+};
+
+
+/* angular motor mode numbers */
+
+enum{
+  dAMotorUser = 0,
+  dAMotorEuler = 1
+};
+
+
+/* joint force feedback information */
+
+typedef struct dJointFeedback {
+  dVector3 f1;		/* force applied to body 1 */
+  dVector3 t1;		/* torque applied to body 1 */
+  dVector3 f2;		/* force applied to body 2 */
+  dVector3 t2;		/* torque applied to body 2 */
+} dJointFeedback;
+
+
+/* private functions that must be implemented by the collision library:
+ * (1) indicate that a geom has moved, (2) get the next geom in a body list.
+ * these functions are called whenever the position of geoms connected to a
+ * body have changed, e.g. with dBodySetPosition(), dBodySetRotation(), or
+ * when the ODE step function updates the body state.
+ */
+
+void dGeomMoved (dGeomID);
+dGeomID dGeomGetBodyNext (dGeomID);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/compatibility.h

@@ -0,0 +1,40 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_COMPATIBILITY_H_
+#define _ODE_COMPATIBILITY_H_
+
+/*
+ * ODE's backward compatibility system ensures that as ODE's API
+ * evolves, user code will not break.
+ */
+
+/*
+ * These new rotation function names are more consistent with the
+ * rest of the API.
+ */
+#define dQtoR(q,R) dRfromQ((R),(q))
+#define dRtoQ(R,q) dQfromR((q),(R))
+#define dWtoDQ(w,q,dq) dDQfromW((dq),(w),(q))
+
+
+#endif

src/include/ode/config.h

@@ -0,0 +1,153 @@
+/* This file was autogenerated by Premake */
+#ifndef _ODE_CONFIG_H_
+#define _ODE_CONFIG_H_
+	
+
+/******************************************************************
+ * CONFIGURATON SETTINGS - you can change these, and then rebuild
+ *   ODE to modify the behavior of the library.
+ *
+ *   dSINGLE/dDOUBLE   - force ODE to use single-precision (float)
+ *                       or double-precision (double) for numbers
+ *
+ *   dCYLINDER_ENABLED - enable/disable cylinder support
+ *
+ *   dTRIMESH_ENABLED  - enable/disable trimesh support
+ *
+ ******************************************************************/
+
+#define dSINGLE 1
+
+#define dCYLINDER_ENABLED 1
+
+#define dTRIMESH_ENABLED 1
+
+
+
+/******************************************************************
+ * SYSTEM SETTINGS - you shouldn't need to change these. If you
+ *   run into an issue with these settings, please report it to
+ *   the ODE bug tracker at:
+ *      http://sf.net/tracker/?group_id=24884&atid=382799
+ ******************************************************************/
+
+/* Try to identify the platform */
+#if defined(_MSC_VER) || defined(__CYGWIN32__) || defined(__MINGW32__)
+  #define ODE_PLATFORM_WINDOWS
+#elif defined(__linux__)
+  #define ODE_PLATFORM_LINUX
+#elif defined(__APPLE__) && defined(__MACH__)
+  #define ODE_PLATFORM_OSX
+#else
+  #error "Need some help identifying the platform!"
+#endif
+
+/* Additional platform defines used in the code */
+#if defined(ODE_PLATFORM_WINDOWS) && !defined(WIN32)
+  #define WIN32
+#endif
+
+#if defined(__CYGWIN32__) || defined(__MINGW32__)
+  #define CYGWIN
+#endif
+
+#if defined(ODE_PLATFORM_OSX)
+  #define macintosh
+#endif
+
+
+/* Define a DLL export symbol for those platforms that need it */
+#if defined(ODE_PLATFORM_WINDOWS)
+  #if defined(ODE_DLL)
+    #define ODE_API __declspec(dllexport)
+  #elif !defined(ODE_LIB)
+    #define ODE_DLL_API __declspec(dllimport)
+  #endif
+#endif
+    
+#if !defined(ODE_API)
+  #define ODE_API
+#endif
+
+
+/* Pull in the standard headers */
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <malloc.h>
+#include <math.h>
+#include <string.h>
+#include <float.h>
+
+#if !defined(ODE_PLATFORM_WINDOWS)
+  #include <alloca.h>
+#endif
+
+
+/* Visual C does not define these functions */
+#if defined(_MSC_VER)
+  #define copysignf _copysign
+  #define copysign _copysign
+#endif
+
+
+/* Define a value for infinity */
+#if defined(HUGE_VALF)
+	#define ODE_INFINITY4 HUGE_VALF
+	#define ODE_INFINITY8 HUGE_VAL
+#elif defined(FLT_MAX)
+	#define ODE_INFINITY4 FLT_MAX
+	#define ODE_INFINITY8 DBL_MAX
+#else
+	static union { unsigned char __c[4]; float  __f; }  __ode_huge_valf = {{0,0,0x80,0x7f}};
+	static union { unsigned char __c[8]; double __d; }  __ode_huge_val  = {{0,0,0,0,0,0,0xf0,0x7f}};
+	#define ODE_INFINITY4 (__ode_huge_valf.__f)
+	#define ODE_INFINITY8 (__ode_huge_val.__d)
+#endif
+
+#if dSINGLE
+	#define dInfinity ODE_INFINITY4
+	#define dEpsilon  FLT_EPSILON
+#else
+	#define dInfinity ODE_INFINITY8
+	#define dEpsilon  DBL_EPSILON
+#endif
+
+
+/* Well-defined common data types...need to define for 64 bit systems */
+#if defined(_M_IA64) || defined(__ia64__) || defined(_M_AMD64) || defined(__x86_64__)
+  #define X86_64_SYSTEM   1
+  typedef int             int32;
+  typedef unsigned int    uint32;
+  typedef short           int16;
+  typedef unsigned short  uint16;
+  typedef char            int8;
+  typedef unsigned char   uint8;
+#else
+  typedef int             int32;
+  typedef unsigned int    uint32;
+  typedef short           int16;
+  typedef unsigned short  uint16;
+  typedef char            int8;
+  typedef unsigned char   uint8;
+#endif
+
+/* An integer type that can be safely cast to a pointer. This definition
+ * should be safe even on 64-bit systems */
+typedef size_t intP;
+
+
+/* The efficient alignment. most platforms align data structures to some
+ * number of bytes, but this is not always the most efficient alignment.
+ * for example, many x86 compilers align to 4 bytes, but on a pentium it is
+ * important to align doubles to 8 byte boundaries (for speed), and the 4
+ * floats in a SIMD register to 16 byte boundaries. many other platforms have
+ * similar behavior. setting a larger alignment can waste a (very) small
+ * amount of memory. NOTE: this number must be a power of two. */
+#define EFFICIENT_ALIGNMENT 16
+
+
+/* Define this if your system supports anonymous memory maps (linux does) */
+#define MMAP_ANONYMOUS
+
+#endif

src/include/ode/contact.h

@@ -0,0 +1,90 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_CONTACT_H_
+#define _ODE_CONTACT_H_
+
+#include <ode/common.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+enum {
+  dContactMu2		= 0x001,
+  dContactFDir1		= 0x002,
+  dContactBounce	= 0x004,
+  dContactSoftERP	= 0x008,
+  dContactSoftCFM	= 0x010,
+  dContactMotion1	= 0x020,
+  dContactMotion2	= 0x040,
+  dContactSlip1		= 0x080,
+  dContactSlip2		= 0x100,
+
+  dContactApprox0	= 0x0000,
+  dContactApprox1_1	= 0x1000,
+  dContactApprox1_2	= 0x2000,
+  dContactApprox1	= 0x3000
+};
+
+
+typedef struct dSurfaceParameters {
+  /* must always be defined */
+  int mode;
+  dReal mu;
+
+  /* only defined if the corresponding flag is set in mode */
+  dReal mu2;
+  dReal bounce;
+  dReal bounce_vel;
+  dReal soft_erp;
+  dReal soft_cfm;
+  dReal motion1,motion2;
+  dReal slip1,slip2;
+} dSurfaceParameters;
+
+
+/* contact info set by collision functions */
+
+typedef struct dContactGeom {
+  dVector3 pos;
+  dVector3 normal;
+  dReal depth;
+  dGeomID g1,g2;
+} dContactGeom;
+
+
+/* contact info used by contact joint */
+
+typedef struct dContact {
+  dSurfaceParameters surface;
+  dContactGeom geom;
+  dVector3 fdir1;
+} dContact;
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/error.h

@@ -0,0 +1,63 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* this comes from the `reuse' library. copy any changes back to the source */
+
+#ifndef _ODE_ERROR_H_
+#define _ODE_ERROR_H_
+
+#include <ode/config.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* all user defined error functions have this type. error and debug functions
+ * should not return.
+ */
+typedef void dMessageFunction (int errnum, const char *msg, va_list ap);
+
+/* set a new error, debug or warning handler. if fn is 0, the default handlers
+ * are used.
+ */
+void dSetErrorHandler (dMessageFunction *fn);
+void dSetDebugHandler (dMessageFunction *fn);
+void dSetMessageHandler (dMessageFunction *fn);
+
+/* return the current error, debug or warning handler. if the return value is
+ * 0, the default handlers are in place.
+ */
+dMessageFunction *dGetErrorHandler();
+dMessageFunction *dGetDebugHandler();
+dMessageFunction *dGetMessageHandler();
+
+/* generate a fatal error, debug trap or a message. */
+void dError (int num, const char *msg, ...);
+void dDebug (int num, const char *msg, ...);
+void dMessage (int num, const char *msg, ...);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/export-dif.h

@@ -0,0 +1,32 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_EXPORT_DIF_
+#define _ODE_EXPORT_DIF_
+
+#include <ode/common.h>
+
+
+void dWorldExportDIF (dWorldID w, FILE *file, const char *world_name);
+
+
+#endif

src/include/ode/mass.h

@@ -0,0 +1,107 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_MASS_H_
+#define _ODE_MASS_H_
+
+#include <ode/common.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct dMass;
+typedef struct dMass dMass;
+
+
+void dMassSetZero (dMass *);
+
+void dMassSetParameters (dMass *, dReal themass,
+			 dReal cgx, dReal cgy, dReal cgz,
+			 dReal I11, dReal I22, dReal I33,
+			 dReal I12, dReal I13, dReal I23);
+
+void dMassSetSphere (dMass *, dReal density, dReal radius);
+void dMassSetSphereTotal (dMass *, dReal total_mass, dReal radius);
+
+void dMassSetCappedCylinder (dMass *, dReal density, int direction,
+			     dReal radius, dReal length);
+void dMassSetCappedCylinderTotal (dMass *, dReal total_mass, int direction,
+				  dReal radius, dReal length);
+
+void dMassSetCylinder (dMass *, dReal density, int direction,
+		       dReal radius, dReal length);
+void dMassSetCylinderTotal (dMass *, dReal total_mass, int direction,
+			    dReal radius, dReal length);
+
+void dMassSetBox (dMass *, dReal density,
+		  dReal lx, dReal ly, dReal lz);
+void dMassSetBoxTotal (dMass *, dReal total_mass,
+		       dReal lx, dReal ly, dReal lz);
+
+void dMassAdjust (dMass *, dReal newmass);
+
+void dMassTranslate (dMass *, dReal x, dReal y, dReal z);
+
+void dMassRotate (dMass *, const dMatrix3 R);
+
+void dMassAdd (dMass *a, const dMass *b);
+
+
+
+struct dMass {
+  dReal mass;
+  dVector4 c;
+  dMatrix3 I;
+
+#ifdef __cplusplus
+  dMass()
+    { dMassSetZero (this); }
+  void setZero()
+    { dMassSetZero (this); }
+  void setParameters (dReal themass, dReal cgx, dReal cgy, dReal cgz,
+		      dReal I11, dReal I22, dReal I33,
+		      dReal I12, dReal I13, dReal I23)
+    { dMassSetParameters (this,themass,cgx,cgy,cgz,I11,I22,I33,I12,I13,I23); }
+  void setSphere (dReal density, dReal radius)
+    { dMassSetSphere (this,density,radius); }
+  void setCappedCylinder (dReal density, int direction, dReal a, dReal b)
+    { dMassSetCappedCylinder (this,density,direction,a,b); }
+  void setBox (dReal density, dReal lx, dReal ly, dReal lz)
+    { dMassSetBox (this,density,lx,ly,lz); }
+  void adjust (dReal newmass)
+    { dMassAdjust (this,newmass); }
+  void translate (dReal x, dReal y, dReal z)
+    { dMassTranslate (this,x,y,z); }
+  void rotate (const dMatrix3 R)
+    { dMassRotate (this,R); }
+  void add (const dMass *b)
+    { dMassAdd (this,b); }
+#endif
+};
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/matrix.h

@@ -0,0 +1,194 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* optimized and unoptimized vector and matrix functions */
+
+#ifndef _ODE_MATRIX_H_
+#define _ODE_MATRIX_H_
+
+#include <ode/common.h>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* set a vector/matrix of size n to all zeros, or to a specific value. */
+
+void dSetZero (dReal *a, int n);
+void dSetValue (dReal *a, int n, dReal value);
+
+
+/* get the dot product of two n*1 vectors. if n <= 0 then
+ * zero will be returned (in which case a and b need not be valid).
+ */
+
+dReal dDot (const dReal *a, const dReal *b, int n);
+
+
+/* get the dot products of (a0,b), (a1,b), etc and return them in outsum.
+ * all vectors are n*1. if n <= 0 then zeroes will be returned (in which case
+ * the input vectors need not be valid). this function is somewhat faster
+ * than calling dDot() for all of the combinations separately.
+ */
+
+/* NOT INCLUDED in the library for now.
+void dMultidot2 (const dReal *a0, const dReal *a1,
+		 const dReal *b, dReal *outsum, int n);
+*/
+
+
+/* matrix multiplication. all matrices are stored in standard row format.
+ * the digit refers to the argument that is transposed:
+ *   0:   A = B  * C   (sizes: A:p*r B:p*q C:q*r)
+ *   1:   A = B' * C   (sizes: A:p*r B:q*p C:q*r)
+ *   2:   A = B  * C'  (sizes: A:p*r B:p*q C:r*q)
+ * case 1,2 are equivalent to saying that the operation is A=B*C but
+ * B or C are stored in standard column format.
+ */
+
+void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
+void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
+void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p,int q,int r);
+
+
+/* do an in-place cholesky decomposition on the lower triangle of the n*n
+ * symmetric matrix A (which is stored by rows). the resulting lower triangle
+ * will be such that L*L'=A. return 1 on success and 0 on failure (on failure
+ * the matrix is not positive definite).
+ */
+
+int dFactorCholesky (dReal *A, int n);
+
+
+/* solve for x: L*L'*x = b, and put the result back into x.
+ * L is size n*n, b is size n*1. only the lower triangle of L is considered.
+ */
+
+void dSolveCholesky (const dReal *L, dReal *b, int n);
+
+
+/* compute the inverse of the n*n positive definite matrix A and put it in
+ * Ainv. this is not especially fast. this returns 1 on success (A was
+ * positive definite) or 0 on failure (not PD).
+ */
+
+int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n);
+
+
+/* check whether an n*n matrix A is positive definite, return 1/0 (yes/no).
+ * positive definite means that x'*A*x > 0 for any x. this performs a
+ * cholesky decomposition of A. if the decomposition fails then the matrix
+ * is not positive definite. A is stored by rows. A is not altered.
+ */
+
+int dIsPositiveDefinite (const dReal *A, int n);
+
+
+/* factorize a matrix A into L*D*L', where L is lower triangular with ones on
+ * the diagonal, and D is diagonal.
+ * A is an n*n matrix stored by rows, with a leading dimension of n rounded
+ * up to 4. L is written into the strict lower triangle of A (the ones are not
+ * written) and the reciprocal of the diagonal elements of D are written into
+ * d.
+ */
+void dFactorLDLT (dReal *A, dReal *d, int n, int nskip);
+
+
+/* solve L*x=b, where L is n*n lower triangular with ones on the diagonal,
+ * and x,b are n*1. b is overwritten with x.
+ * the leading dimension of L is `nskip'.
+ */
+void dSolveL1 (const dReal *L, dReal *b, int n, int nskip);
+
+
+/* solve L'*x=b, where L is n*n lower triangular with ones on the diagonal,
+ * and x,b are n*1. b is overwritten with x.
+ * the leading dimension of L is `nskip'.
+ */
+void dSolveL1T (const dReal *L, dReal *b, int n, int nskip);
+
+
+/* in matlab syntax: a(1:n) = a(1:n) .* d(1:n) */
+
+void dVectorScale (dReal *a, const dReal *d, int n);
+
+
+/* given `L', a n*n lower triangular matrix with ones on the diagonal,
+ * and `d', a n*1 vector of the reciprocal diagonal elements of an n*n matrix
+ * D, solve L*D*L'*x=b where x,b are n*1. x overwrites b.
+ * the leading dimension of L is `nskip'.
+ */
+
+void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip);
+
+
+/* given an L*D*L' factorization of an n*n matrix A, return the updated
+ * factorization L2*D2*L2' of A plus the following "top left" matrix:
+ *
+ *    [ b a' ]     <-- b is a[0]
+ *    [ a 0  ]     <-- a is a[1..n-1]
+ *
+ *   - L has size n*n, its leading dimension is nskip. L is lower triangular
+ *     with ones on the diagonal. only the lower triangle of L is referenced.
+ *   - d has size n. d contains the reciprocal diagonal elements of D.
+ *   - a has size n.
+ * the result is written into L, except that the left column of L and d[0]
+ * are not actually modified. see ldltaddTL.m for further comments. 
+ */
+void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip);
+
+
+/* given an L*D*L' factorization of a permuted matrix A, produce a new
+ * factorization for row and column `r' removed.
+ *   - A has size n1*n1, its leading dimension in nskip. A is symmetric and
+ *     positive definite. only the lower triangle of A is referenced.
+ *     A itself may actually be an array of row pointers.
+ *   - L has size n2*n2, its leading dimension in nskip. L is lower triangular
+ *     with ones on the diagonal. only the lower triangle of L is referenced.
+ *   - d has size n2. d contains the reciprocal diagonal elements of D.
+ *   - p is a permutation vector. it contains n2 indexes into A. each index
+ *     must be in the range 0..n1-1.
+ *   - r is the row/column of L to remove.
+ * the new L will be written within the old L, i.e. will have the same leading
+ * dimension. the last row and column of L, and the last element of d, are
+ * undefined on exit.
+ *
+ * a fast O(n^2) algorithm is used. see ldltremove.m for further comments.
+ */
+void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
+		  int n1, int n2, int r, int nskip);
+
+
+/* given an n*n matrix A (with leading dimension nskip), remove the r'th row
+ * and column by moving elements. the new matrix will have the same leading
+ * dimension. the last row and column of A are untouched on exit.
+ */
+void dRemoveRowCol (dReal *A, int n, int nskip, int r);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/memory.h

@@ -0,0 +1,59 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* this comes from the `reuse' library. copy any changes back to the source */
+
+#ifndef _ODE_MEMORY_H_
+#define _ODE_MEMORY_H_
+
+#include "ode/config.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* function types to allocate and free memory */
+typedef void * dAllocFunction (size_t size);
+typedef void * dReallocFunction (void *ptr, size_t oldsize, size_t newsize);
+typedef void dFreeFunction (void *ptr, size_t size);
+
+/* set new memory management functions. if fn is 0, the default handlers are
+ * used. */
+void dSetAllocHandler (dAllocFunction *fn);
+void dSetReallocHandler (dReallocFunction *fn);
+void dSetFreeHandler (dFreeFunction *fn);
+
+/* get current memory management functions */
+dAllocFunction *dGetAllocHandler ();
+dReallocFunction *dGetReallocHandler ();
+dFreeFunction *dGetFreeHandler ();
+
+/* allocate and free memory. */
+void * dAlloc (size_t size);
+void * dRealloc (void *ptr, size_t oldsize, size_t newsize);
+void dFree (void *ptr, size_t size);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/misc.h

@@ -0,0 +1,85 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* miscellaneous math functions. these are mostly useful for testing */
+
+#ifndef _ODE_MISC_H_
+#define _ODE_MISC_H_
+
+#include <ode/common.h>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* return 1 if the random number generator is working. */
+int dTestRand();
+
+/* return next 32 bit random number. this uses a not-very-random linear
+ * congruential method.
+ */
+unsigned long dRand();
+
+/* get and set the current random number seed. */
+unsigned long  dRandGetSeed();
+void dRandSetSeed (unsigned long s);
+
+/* return a random integer between 0..n-1. the distribution will get worse
+ * as n approaches 2^32.
+ */
+int dRandInt (int n);
+
+/* return a random real number between 0..1 */
+dReal dRandReal();
+
+/* print out a matrix */
+#ifdef __cplusplus
+void dPrintMatrix (const dReal *A, int n, int m, char *fmt = "%10.4f ",
+		   FILE *f=stdout);
+#else
+void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f);
+#endif
+
+/* make a random vector with entries between +/- range. A has n elements. */
+void dMakeRandomVector (dReal *A, int n, dReal range);
+
+/* make a random matrix with entries between +/- range. A has size n*m. */
+void dMakeRandomMatrix (dReal *A, int n, int m, dReal range);
+
+/* clear the upper triangle of a square matrix */
+void dClearUpperTriangle (dReal *A, int n);
+
+/* return the maximum element difference between the two n*m matrices */
+dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m);
+
+/* return the maximum element difference between the lower triangle of two
+ * n*n matrices */
+dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/objects.h

@@ -0,0 +1,264 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_OBJECTS_H_
+#define _ODE_OBJECTS_H_
+
+#include <ode/common.h>
+#include <ode/mass.h>
+#include <ode/contact.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* world */
+
+dWorldID dWorldCreate();
+void dWorldDestroy (dWorldID);
+
+void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z);
+void dWorldGetGravity (dWorldID, dVector3 gravity);
+void dWorldSetERP (dWorldID, dReal erp);
+dReal dWorldGetERP (dWorldID);
+void dWorldSetCFM (dWorldID, dReal cfm);
+dReal dWorldGetCFM (dWorldID);
+void dWorldStep (dWorldID, dReal stepsize);
+void dWorldImpulseToForce (dWorldID, dReal stepsize,
+			   dReal ix, dReal iy, dReal iz, dVector3 force);
+
+/* World QuickStep functions */
+
+void dWorldQuickStep (dWorldID w, dReal stepsize);
+void dWorldSetQuickStepNumIterations (dWorldID, int num);
+int dWorldGetQuickStepNumIterations (dWorldID);
+void dWorldSetQuickStepW (dWorldID, dReal param);
+dReal dWorldGetQuickStepW (dWorldID);
+
+/* World contact parameter functions */
+
+void dWorldSetContactMaxCorrectingVel (dWorldID, dReal vel);
+dReal dWorldGetContactMaxCorrectingVel (dWorldID);
+void dWorldSetContactSurfaceLayer (dWorldID, dReal depth);
+dReal dWorldGetContactSurfaceLayer (dWorldID);
+
+/* StepFast1 functions */
+
+void dWorldStepFast1(dWorldID, dReal stepsize, int maxiterations);
+void dWorldSetAutoEnableDepthSF1(dWorldID, int autoEnableDepth);
+int dWorldGetAutoEnableDepthSF1(dWorldID);
+
+/* Auto-disable functions */
+
+dReal dWorldGetAutoDisableLinearThreshold (dWorldID);
+void  dWorldSetAutoDisableLinearThreshold (dWorldID, dReal linear_threshold);
+dReal dWorldGetAutoDisableAngularThreshold (dWorldID);
+void  dWorldSetAutoDisableAngularThreshold (dWorldID, dReal angular_threshold);
+int   dWorldGetAutoDisableSteps (dWorldID);
+void  dWorldSetAutoDisableSteps (dWorldID, int steps);
+dReal dWorldGetAutoDisableTime (dWorldID);
+void  dWorldSetAutoDisableTime (dWorldID, dReal time);
+int   dWorldGetAutoDisableFlag (dWorldID);
+void  dWorldSetAutoDisableFlag (dWorldID, int do_auto_disable);
+
+dReal dBodyGetAutoDisableLinearThreshold (dBodyID);
+void  dBodySetAutoDisableLinearThreshold (dBodyID, dReal linear_threshold);
+dReal dBodyGetAutoDisableAngularThreshold (dBodyID);
+void  dBodySetAutoDisableAngularThreshold (dBodyID, dReal angular_threshold);
+int   dBodyGetAutoDisableSteps (dBodyID);
+void  dBodySetAutoDisableSteps (dBodyID, int steps);
+dReal dBodyGetAutoDisableTime (dBodyID);
+void  dBodySetAutoDisableTime (dBodyID, dReal time);
+int   dBodyGetAutoDisableFlag (dBodyID);
+void  dBodySetAutoDisableFlag (dBodyID, int do_auto_disable);
+void  dBodySetAutoDisableDefaults (dBodyID);
+
+/* bodies */
+
+dBodyID dBodyCreate (dWorldID);
+void dBodyDestroy (dBodyID);
+
+void  dBodySetData (dBodyID, void *data);
+void *dBodyGetData (dBodyID);
+
+void dBodySetPosition   (dBodyID, dReal x, dReal y, dReal z);
+void dBodySetRotation   (dBodyID, const dMatrix3 R);
+void dBodySetQuaternion (dBodyID, const dQuaternion q);
+void dBodySetLinearVel  (dBodyID, dReal x, dReal y, dReal z);
+void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z);
+const dReal * dBodyGetPosition   (dBodyID);
+const dReal * dBodyGetRotation   (dBodyID);	/* ptr to 4x3 rot matrix */
+const dReal * dBodyGetQuaternion (dBodyID);
+const dReal * dBodyGetLinearVel  (dBodyID);
+const dReal * dBodyGetAngularVel (dBodyID);
+
+void dBodySetMass (dBodyID, const dMass *mass);
+void dBodyGetMass (dBodyID, dMass *mass);
+
+void dBodyAddForce            (dBodyID, dReal fx, dReal fy, dReal fz);
+void dBodyAddTorque           (dBodyID, dReal fx, dReal fy, dReal fz);
+void dBodyAddRelForce         (dBodyID, dReal fx, dReal fy, dReal fz);
+void dBodyAddRelTorque        (dBodyID, dReal fx, dReal fy, dReal fz);
+void dBodyAddForceAtPos       (dBodyID, dReal fx, dReal fy, dReal fz,
+			                dReal px, dReal py, dReal pz);
+void dBodyAddForceAtRelPos    (dBodyID, dReal fx, dReal fy, dReal fz,
+			                dReal px, dReal py, dReal pz);
+void dBodyAddRelForceAtPos    (dBodyID, dReal fx, dReal fy, dReal fz,
+			                dReal px, dReal py, dReal pz);
+void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,
+			                dReal px, dReal py, dReal pz);
+
+const dReal * dBodyGetForce   (dBodyID);
+const dReal * dBodyGetTorque  (dBodyID);
+void dBodySetForce  (dBodyID b, dReal x, dReal y, dReal z);
+void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z);
+
+void dBodyGetRelPointPos    (dBodyID, dReal px, dReal py, dReal pz,
+			     dVector3 result);
+void dBodyGetRelPointVel    (dBodyID, dReal px, dReal py, dReal pz,
+			     dVector3 result);
+void dBodyGetPointVel       (dBodyID, dReal px, dReal py, dReal pz,
+			     dVector3 result);
+void dBodyGetPosRelPoint    (dBodyID, dReal px, dReal py, dReal pz,
+			     dVector3 result);
+void dBodyVectorToWorld     (dBodyID, dReal px, dReal py, dReal pz,
+			     dVector3 result);
+void dBodyVectorFromWorld   (dBodyID, dReal px, dReal py, dReal pz,
+			     dVector3 result);
+
+void dBodySetFiniteRotationMode (dBodyID, int mode);
+void dBodySetFiniteRotationAxis (dBodyID, dReal x, dReal y, dReal z);
+
+int dBodyGetFiniteRotationMode (dBodyID);
+void dBodyGetFiniteRotationAxis (dBodyID, dVector3 result);
+
+int dBodyGetNumJoints (dBodyID b);
+dJointID dBodyGetJoint (dBodyID, int index);
+
+void dBodyEnable (dBodyID);
+void dBodyDisable (dBodyID);
+int dBodyIsEnabled (dBodyID);
+
+void dBodySetGravityMode (dBodyID b, int mode);
+int dBodyGetGravityMode (dBodyID b);
+
+
+/* joints */
+
+dJointID dJointCreateBall (dWorldID, dJointGroupID);
+dJointID dJointCreateHinge (dWorldID, dJointGroupID);
+dJointID dJointCreateSlider (dWorldID, dJointGroupID);
+dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact *);
+dJointID dJointCreateHinge2 (dWorldID, dJointGroupID);
+dJointID dJointCreateUniversal (dWorldID, dJointGroupID);
+dJointID dJointCreateFixed (dWorldID, dJointGroupID);
+dJointID dJointCreateNull (dWorldID, dJointGroupID);
+dJointID dJointCreateAMotor (dWorldID, dJointGroupID);
+
+void dJointDestroy (dJointID);
+
+dJointGroupID dJointGroupCreate (int max_size);
+void dJointGroupDestroy (dJointGroupID);
+void dJointGroupEmpty (dJointGroupID);
+
+void dJointAttach (dJointID, dBodyID body1, dBodyID body2);
+void dJointSetData (dJointID, void *data);
+void *dJointGetData (dJointID);
+int dJointGetType (dJointID);
+dBodyID dJointGetBody (dJointID, int index);
+
+void dJointSetFeedback (dJointID, dJointFeedback *);
+dJointFeedback *dJointGetFeedback (dJointID);
+
+void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z);
+void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z);
+void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z);
+void dJointSetHingeParam (dJointID, int parameter, dReal value);
+void dJointAddHingeTorque(dJointID joint, dReal torque);
+void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z);
+void dJointSetSliderParam (dJointID, int parameter, dReal value);
+void dJointAddSliderForce(dJointID joint, dReal force);
+void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z);
+void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z);
+void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z);
+void dJointSetHinge2Param (dJointID, int parameter, dReal value);
+void dJointAddHinge2Torques(dJointID joint, dReal torque1, dReal torque2);
+void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z);
+void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z);
+void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z);
+void dJointSetUniversalParam (dJointID, int parameter, dReal value);
+void dJointAddUniversalTorques(dJointID joint, dReal torque1, dReal torque2);
+void dJointSetFixed (dJointID);
+void dJointSetAMotorNumAxes (dJointID, int num);
+void dJointSetAMotorAxis (dJointID, int anum, int rel,
+			  dReal x, dReal y, dReal z);
+void dJointSetAMotorAngle (dJointID, int anum, dReal angle);
+void dJointSetAMotorParam (dJointID, int parameter, dReal value);
+void dJointSetAMotorMode (dJointID, int mode);
+void dJointAddAMotorTorques (dJointID, dReal torque1, dReal torque2, dReal torque3);
+
+void dJointGetBallAnchor (dJointID, dVector3 result);
+void dJointGetBallAnchor2 (dJointID, dVector3 result);
+void dJointGetHingeAnchor (dJointID, dVector3 result);
+void dJointGetHingeAnchor2 (dJointID, dVector3 result);
+void dJointGetHingeAxis (dJointID, dVector3 result);
+dReal dJointGetHingeParam (dJointID, int parameter);
+dReal dJointGetHingeAngle (dJointID);
+dReal dJointGetHingeAngleRate (dJointID);
+dReal dJointGetSliderPosition (dJointID);
+dReal dJointGetSliderPositionRate (dJointID);
+void dJointGetSliderAxis (dJointID, dVector3 result);
+dReal dJointGetSliderParam (dJointID, int parameter);
+void dJointGetHinge2Anchor (dJointID, dVector3 result);
+void dJointGetHinge2Anchor2 (dJointID, dVector3 result);
+void dJointGetHinge2Axis1 (dJointID, dVector3 result);
+void dJointGetHinge2Axis2 (dJointID, dVector3 result);
+dReal dJointGetHinge2Param (dJointID, int parameter);
+dReal dJointGetHinge2Angle1 (dJointID);
+dReal dJointGetHinge2Angle1Rate (dJointID);
+dReal dJointGetHinge2Angle2Rate (dJointID);
+void dJointGetUniversalAnchor (dJointID, dVector3 result);
+void dJointGetUniversalAnchor2 (dJointID, dVector3 result);
+void dJointGetUniversalAxis1 (dJointID, dVector3 result);
+void dJointGetUniversalAxis2 (dJointID, dVector3 result);
+dReal dJointGetUniversalParam (dJointID, int parameter);
+dReal dJointGetUniversalAngle1 (dJointID);
+dReal dJointGetUniversalAngle2 (dJointID);
+dReal dJointGetUniversalAngle1Rate (dJointID);
+dReal dJointGetUniversalAngle2Rate (dJointID);
+int dJointGetAMotorNumAxes (dJointID);
+void dJointGetAMotorAxis (dJointID, int anum, dVector3 result);
+int dJointGetAMotorAxisRel (dJointID, int anum);
+dReal dJointGetAMotorAngle (dJointID, int anum);
+dReal dJointGetAMotorAngleRate (dJointID, int anum);
+dReal dJointGetAMotorParam (dJointID, int parameter);
+int dJointGetAMotorMode (dJointID);
+
+int dAreConnected (dBodyID, dBodyID);
+int dAreConnectedExcluding (dBodyID, dBodyID, int joint_type);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/ode.h

@@ -0,0 +1,47 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_ODE_H_
+#define _ODE_ODE_H_
+
+/* include *everything* here */
+
+#include <ode/config.h>
+#include <ode/compatibility.h>
+#include <ode/common.h>
+#include <ode/contact.h>
+#include <ode/error.h>
+#include <ode/memory.h>
+#include <ode/odemath.h>
+#include <ode/matrix.h>
+#include <ode/timer.h>
+#include <ode/rotation.h>
+#include <ode/mass.h>
+#include <ode/misc.h>
+#include <ode/objects.h>
+#include <ode/odecpp.h>
+#include <ode/collision_space.h>
+#include <ode/collision.h>
+#include <ode/odecpp_collision.h>
+#include <ode/export-dif.h>
+
+#endif

src/include/ode/odecpp.h

@@ -0,0 +1,621 @@
+/*************************************************************************
+ *									 *
+ * 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.			 *
+ *									 *
+ *************************************************************************/
+
+/* C++ interface for non-collision stuff */
+
+
+#ifndef _ODE_ODECPP_H_
+#define _ODE_ODECPP_H_
+#ifdef __cplusplus
+
+#include <ode/error.h>
+
+
+class dWorld {
+  dWorldID _id;
+
+  // intentionally undefined, don't use these
+  dWorld (const dWorld &);
+  void operator= (const dWorld &);
+
+public:
+  dWorld()
+    { _id = dWorldCreate(); }
+  ~dWorld()
+    { dWorldDestroy (_id); }
+
+  dWorldID id() const
+    { return _id; }
+  operator dWorldID() const
+    { return _id; }
+
+  void setGravity (dReal x, dReal y, dReal z)
+    { dWorldSetGravity (_id,x,y,z); }
+  void getGravity (dVector3 g) const
+    { dWorldGetGravity (_id,g); }
+
+  void setERP (dReal erp)
+    { dWorldSetERP(_id, erp); }
+  dReal getERP() const
+    { return dWorldGetERP(_id); }
+
+  void setCFM (dReal cfm)
+    { dWorldSetCFM(_id, cfm); }
+  dReal getCFM() const
+    { return dWorldGetCFM(_id); }
+
+  void step (dReal stepsize)
+    { dWorldStep (_id,stepsize); }
+
+  void stepFast1 (dReal stepsize, int maxiterations)
+    { dWorldStepFast1 (_id,stepsize,maxiterations); }
+  void setAutoEnableDepthSF1(dWorldID, int depth)
+    { dWorldSetAutoEnableDepthSF1 (_id, depth); }
+  int getAutoEnableDepthSF1(dWorldID)
+    { return dWorldGetAutoEnableDepthSF1 (_id); }
+
+  void  setAutoDisableLinearThreshold (dReal threshold) 
+    { dWorldSetAutoDisableLinearThreshold (_id,threshold); }
+  dReal getAutoDisableLinearThreshold()
+    { return dWorldGetAutoDisableLinearThreshold (_id); }
+  void setAutoDisableAngularThreshold (dReal threshold)
+    { dWorldSetAutoDisableAngularThreshold (_id,threshold); }
+  dReal getAutoDisableAngularThreshold()
+    { return dWorldGetAutoDisableAngularThreshold (_id); }
+  void setAutoDisableSteps (int steps)
+    { dWorldSetAutoDisableSteps (_id,steps); }
+  int getAutoDisableSteps()
+    { return dWorldGetAutoDisableSteps (_id); }
+  void setAutoDisableTime (dReal time)
+    { dWorldSetAutoDisableTime (_id,time); }
+  dReal getAutoDisableTime()
+    { return dWorldGetAutoDisableTime (_id); }
+  void setAutoDisableFlag (int do_auto_disable)
+    { dWorldSetAutoDisableFlag (_id,do_auto_disable); }
+  int getAutoDisableFlag()
+    { return dWorldGetAutoDisableFlag (_id); }
+
+  void impulseToForce (dReal stepsize, dReal ix, dReal iy, dReal iz,
+		       dVector3 force)
+    { dWorldImpulseToForce (_id,stepsize,ix,iy,iz,force); }
+};
+
+
+class dBody {
+  dBodyID _id;
+
+  // intentionally undefined, don't use these
+  dBody (const dBody &);
+  void operator= (const dBody &);
+
+public:
+  dBody()
+    { _id = 0; }
+  dBody (dWorldID world)
+    { _id = dBodyCreate (world); }
+  ~dBody()
+    { if (_id) dBodyDestroy (_id); }
+
+  void create (dWorldID world) {
+    if (_id) dBodyDestroy (_id);
+    _id = dBodyCreate (world);
+  }
+
+  dBodyID id() const
+    { return _id; }
+  operator dBodyID() const
+    { return _id; }
+
+  void setData (void *data)
+    { dBodySetData (_id,data); }
+  void *getData() const
+    { return dBodyGetData (_id); }
+
+  void setPosition (dReal x, dReal y, dReal z)
+    { dBodySetPosition (_id,x,y,z); }
+  void setRotation (const dMatrix3 R)
+    { dBodySetRotation (_id,R); }
+  void setQuaternion (const dQuaternion q)
+    { dBodySetQuaternion (_id,q); }
+  void setLinearVel  (dReal x, dReal y, dReal z)
+    { dBodySetLinearVel (_id,x,y,z); }
+  void setAngularVel (dReal x, dReal y, dReal z)
+    { dBodySetAngularVel (_id,x,y,z); }
+
+  const dReal * getPosition() const
+    { return dBodyGetPosition (_id); }
+  const dReal * getRotation() const
+    { return dBodyGetRotation (_id); }
+  const dReal * getQuaternion() const
+    { return dBodyGetQuaternion (_id); }
+  const dReal * getLinearVel() const
+    { return dBodyGetLinearVel (_id); }
+  const dReal * getAngularVel() const
+    { return dBodyGetAngularVel (_id); }
+
+  void setMass (const dMass *mass)
+    { dBodySetMass (_id,mass); }
+  void getMass (dMass *mass) const
+    { dBodyGetMass (_id,mass); }
+
+  void addForce (dReal fx, dReal fy, dReal fz)
+    { dBodyAddForce (_id, fx, fy, fz); }
+  void addTorque (dReal fx, dReal fy, dReal fz)
+    { dBodyAddTorque (_id, fx, fy, fz); }
+  void addRelForce (dReal fx, dReal fy, dReal fz)
+    { dBodyAddRelForce (_id, fx, fy, fz); }
+  void addRelTorque (dReal fx, dReal fy, dReal fz)
+    { dBodyAddRelTorque (_id, fx, fy, fz); }
+  void addForceAtPos (dReal fx, dReal fy, dReal fz,
+		      dReal px, dReal py, dReal pz)
+    { dBodyAddForceAtPos (_id, fx, fy, fz, px, py, pz); }
+  void addForceAtRelPos (dReal fx, dReal fy, dReal fz,
+		      dReal px, dReal py, dReal pz)
+    { dBodyAddForceAtRelPos (_id, fx, fy, fz, px, py, pz); }
+  void addRelForceAtPos (dReal fx, dReal fy, dReal fz,
+			 dReal px, dReal py, dReal pz)
+    { dBodyAddRelForceAtPos (_id, fx, fy, fz, px, py, pz); }
+  void addRelForceAtRelPos (dReal fx, dReal fy, dReal fz,
+			    dReal px, dReal py, dReal pz)
+    { dBodyAddRelForceAtRelPos (_id, fx, fy, fz, px, py, pz); }
+
+  const dReal * getForce() const
+    { return dBodyGetForce(_id); }
+  const dReal * getTorque() const
+    { return dBodyGetTorque(_id); }
+  void setForce (dReal x, dReal y, dReal z)
+    { dBodySetForce (_id,x,y,z); }
+  void setTorque (dReal x, dReal y, dReal z)
+    { dBodySetTorque (_id,x,y,z); }
+
+  void enable()
+    { dBodyEnable (_id); }
+  void disable()
+    { dBodyDisable (_id); }
+  int isEnabled() const
+    { return dBodyIsEnabled (_id); }
+
+  void getRelPointPos (dReal px, dReal py, dReal pz, dVector3 result) const
+    { dBodyGetRelPointPos (_id, px, py, pz, result); }
+  void getRelPointVel (dReal px, dReal py, dReal pz, dVector3 result) const
+    { dBodyGetRelPointVel (_id, px, py, pz, result); }
+  void getPointVel (dReal px, dReal py, dReal pz, dVector3 result) const
+    { dBodyGetPointVel (_id,px,py,pz,result); }
+  void getPosRelPoint (dReal px, dReal py, dReal pz, dVector3 result) const
+    { dBodyGetPosRelPoint (_id,px,py,pz,result); }
+  void vectorToWorld (dReal px, dReal py, dReal pz, dVector3 result) const
+    { dBodyVectorToWorld (_id,px,py,pz,result); }
+  void vectorFromWorld (dReal px, dReal py, dReal pz, dVector3 result) const
+    { dBodyVectorFromWorld (_id,px,py,pz,result); }
+
+  void setFiniteRotationMode (int mode)
+    { dBodySetFiniteRotationMode (_id, mode); }
+  void setFiniteRotationAxis (dReal x, dReal y, dReal z)
+    { dBodySetFiniteRotationAxis (_id, x, y, z); }
+
+  int getFiniteRotationMode() const
+    { return dBodyGetFiniteRotationMode (_id); }
+  void getFiniteRotationAxis (dVector3 result) const
+    { dBodyGetFiniteRotationAxis (_id, result); }
+
+  int getNumJoints() const
+    { return dBodyGetNumJoints (_id); }
+  dJointID getJoint (int index) const
+    { return dBodyGetJoint (_id, index); }
+
+  void setGravityMode (int mode)
+    { dBodySetGravityMode (_id,mode); }
+  int getGravityMode() const
+    { return dBodyGetGravityMode (_id); }
+
+  int isConnectedTo (dBodyID body) const
+    { return dAreConnected (_id, body); }
+
+  void  setAutoDisableLinearThreshold (dReal threshold) 
+    { dBodySetAutoDisableLinearThreshold (_id,threshold); }
+  dReal getAutoDisableLinearThreshold()
+    { return dBodyGetAutoDisableLinearThreshold (_id); }
+  void setAutoDisableAngularThreshold (dReal threshold)
+    { dBodySetAutoDisableAngularThreshold (_id,threshold); }
+  dReal getAutoDisableAngularThreshold()
+    { return dBodyGetAutoDisableAngularThreshold (_id); }
+  void setAutoDisableSteps (int steps)
+    { dBodySetAutoDisableSteps (_id,steps); }
+  int getAutoDisableSteps()
+    { return dBodyGetAutoDisableSteps (_id); }
+  void setAutoDisableTime (dReal time)
+    { dBodySetAutoDisableTime (_id,time); }
+  dReal getAutoDisableTime()
+    { return dBodyGetAutoDisableTime (_id); }
+  void setAutoDisableFlag (int do_auto_disable)
+    { dBodySetAutoDisableFlag (_id,do_auto_disable); }
+  int getAutoDisableFlag()
+    { return dBodyGetAutoDisableFlag (_id); }
+};
+
+
+class dJointGroup {
+  dJointGroupID _id;
+
+  // intentionally undefined, don't use these
+  dJointGroup (const dJointGroup &);
+  void operator= (const dJointGroup &);
+
+public:
+  dJointGroup (int dummy_arg=0)
+    { _id = dJointGroupCreate (0); }
+  ~dJointGroup()
+    { dJointGroupDestroy (_id); }
+  void create (int dummy_arg=0) {
+    if (_id) dJointGroupDestroy (_id);
+    _id = dJointGroupCreate (0);
+  }
+
+  dJointGroupID id() const
+    { return _id; }
+  operator dJointGroupID() const
+    { return _id; }
+
+  void empty()
+    { dJointGroupEmpty (_id); }
+};
+
+
+class dJoint {
+private:
+  // intentionally undefined, don't use these
+  dJoint (const dJoint &) ;
+  void operator= (const dJoint &);
+
+protected:
+  dJointID _id;
+
+public:
+  dJoint()
+    { _id = 0; }
+  ~dJoint()
+    { if (_id) dJointDestroy (_id); }
+
+  dJointID id() const
+    { return _id; }
+  operator dJointID() const
+    { return _id; }
+
+  void attach (dBodyID body1, dBodyID body2)
+    { dJointAttach (_id, body1, body2); }
+
+  void setData (void *data)
+    { dJointSetData (_id, data); }
+  void *getData() const
+    { return dJointGetData (_id); }
+
+  int getType() const
+    { return dJointGetType (_id); }
+
+  dBodyID getBody (int index) const
+    { return dJointGetBody (_id, index); }
+};
+
+
+class dBallJoint : public dJoint {
+private:
+  // intentionally undefined, don't use these
+  dBallJoint (const dBallJoint &);
+  void operator= (const dBallJoint &);
+
+public:
+  dBallJoint() { }
+  dBallJoint (dWorldID world, dJointGroupID group=0)
+    { _id = dJointCreateBall (world, group); }
+
+  void create (dWorldID world, dJointGroupID group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateBall (world, group);
+  }
+
+  void setAnchor (dReal x, dReal y, dReal z)
+    { dJointSetBallAnchor (_id, x, y, z); }
+  void getAnchor (dVector3 result) const
+    { dJointGetBallAnchor (_id, result); }
+  void getAnchor2 (dVector3 result) const
+    { dJointGetBallAnchor2 (_id, result); }
+} ;
+
+
+class dHingeJoint : public dJoint {
+  // intentionally undefined, don't use these
+  dHingeJoint (const dHingeJoint &);
+  void operator = (const dHingeJoint &);
+
+public:
+  dHingeJoint() { }
+  dHingeJoint (dWorldID world, dJointGroupID group=0)
+    { _id = dJointCreateHinge (world, group); }
+
+  void create (dWorldID world, dJointGroupID group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateHinge (world, group);
+  }
+
+  void setAnchor (dReal x, dReal y, dReal z)
+    { dJointSetHingeAnchor (_id, x, y, z); }
+  void getAnchor (dVector3 result) const
+    { dJointGetHingeAnchor (_id, result); }
+  void getAnchor2 (dVector3 result) const
+    { dJointGetHingeAnchor2 (_id, result); }
+
+  void setAxis (dReal x, dReal y, dReal z)
+    { dJointSetHingeAxis (_id, x, y, z); }
+  void getAxis (dVector3 result) const
+    { dJointGetHingeAxis (_id, result); }
+
+  dReal getAngle() const
+    { return dJointGetHingeAngle (_id); }
+  dReal getAngleRate() const
+    { return dJointGetHingeAngleRate (_id); }
+
+  void setParam (int parameter, dReal value)
+    { dJointSetHingeParam (_id, parameter, value); }
+  dReal getParam (int parameter) const
+    { return dJointGetHingeParam (_id, parameter); }
+
+  void addTorque (dReal torque)
+	{ dJointAddHingeTorque(_id, torque); }
+};
+
+
+class dSliderJoint : public dJoint {
+  // intentionally undefined, don't use these
+  dSliderJoint (const dSliderJoint &);
+  void operator = (const dSliderJoint &);
+
+public:
+  dSliderJoint() { }
+  dSliderJoint (dWorldID world, dJointGroupID group=0)
+    { _id = dJointCreateSlider (world, group); }
+
+  void create (dWorldID world, dJointGroupID group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateSlider (world, group);
+  }
+
+  void setAxis (dReal x, dReal y, dReal z)
+    { dJointSetSliderAxis (_id, x, y, z); }
+  void getAxis (dVector3 result) const
+    { dJointGetSliderAxis (_id, result); }
+
+  dReal getPosition() const
+    { return dJointGetSliderPosition (_id); }
+  dReal getPositionRate() const
+    { return dJointGetSliderPositionRate (_id); }
+
+  void setParam (int parameter, dReal value)
+    { dJointSetSliderParam (_id, parameter, value); }
+  dReal getParam (int parameter) const
+    { return dJointGetSliderParam (_id, parameter); }
+
+  void addForce (dReal force)
+	{ dJointAddSliderForce(_id, force); }
+};
+
+
+class dUniversalJoint : public dJoint {
+  // intentionally undefined, don't use these
+  dUniversalJoint (const dUniversalJoint &);
+  void operator = (const dUniversalJoint &);
+
+public:
+  dUniversalJoint() { }
+  dUniversalJoint (dWorldID world, dJointGroupID group=0)
+    { _id = dJointCreateUniversal (world, group); }
+
+  void create (dWorldID world, dJointGroupID group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateUniversal (world, group);
+  }
+
+  void setAnchor (dReal x, dReal y, dReal z)
+    { dJointSetUniversalAnchor (_id, x, y, z); }
+  void setAxis1 (dReal x, dReal y, dReal z)
+    { dJointSetUniversalAxis1 (_id, x, y, z); }
+  void setAxis2 (dReal x, dReal y, dReal z)
+    { dJointSetUniversalAxis2 (_id, x, y, z); }
+  void setParam (int parameter, dReal value)
+    { dJointSetUniversalParam (_id, parameter, value); }
+
+  void getAnchor (dVector3 result) const
+    { dJointGetUniversalAnchor (_id, result); }
+  void getAnchor2 (dVector3 result) const
+    { dJointGetUniversalAnchor2 (_id, result); }
+  void getAxis1 (dVector3 result) const
+    { dJointGetUniversalAxis1 (_id, result); }
+  void getAxis2 (dVector3 result) const
+    { dJointGetUniversalAxis2 (_id, result); }
+  dReal getParam (int parameter) const
+    { return dJointGetUniversalParam (_id, parameter); }
+  dReal getAngle1() const
+    { return dJointGetUniversalAngle1 (_id); }
+  dReal getAngle1Rate() const
+    { return dJointGetUniversalAngle1Rate (_id); }
+  dReal getAngle2() const
+    { return dJointGetUniversalAngle2 (_id); }
+  dReal getAngle2Rate() const
+    { return dJointGetUniversalAngle2Rate (_id); }
+
+  void addTorques (dReal torque1, dReal torque2)
+	{ dJointAddUniversalTorques(_id, torque1, torque2); }
+};
+
+
+class dHinge2Joint : public dJoint {
+  // intentionally undefined, don't use these
+  dHinge2Joint (const dHinge2Joint &);
+  void operator = (const dHinge2Joint &);
+
+public:
+  dHinge2Joint() { }
+  dHinge2Joint (dWorldID world, dJointGroupID group=0)
+    { _id = dJointCreateHinge2 (world, group); }
+
+  void create (dWorldID world, dJointGroupID group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateHinge2 (world, group);
+  }
+
+  void setAnchor (dReal x, dReal y, dReal z)
+    { dJointSetHinge2Anchor (_id, x, y, z); }
+  void setAxis1 (dReal x, dReal y, dReal z)
+    { dJointSetHinge2Axis1 (_id, x, y, z); }
+  void setAxis2 (dReal x, dReal y, dReal z)
+    { dJointSetHinge2Axis2 (_id, x, y, z); }
+
+  void getAnchor (dVector3 result) const
+    { dJointGetHinge2Anchor (_id, result); }
+  void getAnchor2 (dVector3 result) const
+    { dJointGetHinge2Anchor2 (_id, result); }
+  void getAxis1 (dVector3 result) const
+    { dJointGetHinge2Axis1 (_id, result); }
+  void getAxis2 (dVector3 result) const
+    { dJointGetHinge2Axis2 (_id, result); }
+
+  dReal getAngle1() const
+    { return dJointGetHinge2Angle1 (_id); }
+  dReal getAngle1Rate() const
+    { return dJointGetHinge2Angle1Rate (_id); }
+  dReal getAngle2Rate() const
+    { return dJointGetHinge2Angle2Rate (_id); }
+
+  void setParam (int parameter, dReal value)
+    { dJointSetHinge2Param (_id, parameter, value); }
+  dReal getParam (int parameter) const
+    { return dJointGetHinge2Param (_id, parameter); }
+
+  void addTorques(dReal torque1, dReal torque2)
+	{ dJointAddHinge2Torques(_id, torque1, torque2); }
+};
+
+
+class dFixedJoint : public dJoint {
+  // intentionally undefined, don't use these
+  dFixedJoint (const dFixedJoint &);
+  void operator = (const dFixedJoint &);
+
+public:
+  dFixedJoint() { }
+  dFixedJoint (dWorldID world, dJointGroupID group=0)
+    { _id = dJointCreateFixed (world, group); }
+
+  void create (dWorldID world, dJointGroupID group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateFixed (world, group);
+  }
+
+  void set()
+    { dJointSetFixed (_id); }
+};
+
+
+class dContactJoint : public dJoint {
+  // intentionally undefined, don't use these
+  dContactJoint (const dContactJoint &);
+  void operator = (const dContactJoint &);
+
+public:
+  dContactJoint() { }
+  dContactJoint (dWorldID world, dJointGroupID group, dContact *contact)
+    { _id = dJointCreateContact (world, group, contact); }
+
+  void create (dWorldID world, dJointGroupID group, dContact *contact) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateContact (world, group, contact);
+  }
+};
+
+
+class dNullJoint : public dJoint {
+  // intentionally undefined, don't use these
+  dNullJoint (const dNullJoint &);
+  void operator = (const dNullJoint &);
+
+public:
+  dNullJoint() { }
+  dNullJoint (dWorldID world, dJointGroupID group=0)
+    { _id = dJointCreateNull (world, group); }
+
+  void create (dWorldID world, dJointGroupID group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateNull (world, group);
+  }
+};
+
+
+class dAMotorJoint : public dJoint {
+  // intentionally undefined, don't use these
+  dAMotorJoint (const dAMotorJoint &);
+  void operator = (const dAMotorJoint &);
+
+public:
+  dAMotorJoint() { }
+  dAMotorJoint (dWorldID world, dJointGroupID group=0)
+    { _id = dJointCreateAMotor (world, group); }
+
+  void create (dWorldID world, dJointGroupID group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateAMotor (world, group);
+  }
+
+  void setMode (int mode)
+    { dJointSetAMotorMode (_id, mode); }
+  int getMode() const
+    { return dJointGetAMotorMode (_id); }
+
+  void setNumAxes (int num)
+    { dJointSetAMotorNumAxes (_id, num); }
+  int getNumAxes() const
+    { return dJointGetAMotorNumAxes (_id); }
+
+  void setAxis (int anum, int rel, dReal x, dReal y, dReal z)
+    { dJointSetAMotorAxis (_id, anum, rel, x, y, z); }
+  void getAxis (int anum, dVector3 result) const
+    { dJointGetAMotorAxis (_id, anum, result); }
+  int getAxisRel (int anum) const
+    { return dJointGetAMotorAxisRel (_id, anum); }
+
+  void setAngle (int anum, dReal angle)
+    { dJointSetAMotorAngle (_id, anum, angle); }
+  dReal getAngle (int anum) const
+    { return dJointGetAMotorAngle (_id, anum); }
+  dReal getAngleRate (int anum)
+    { return dJointGetAMotorAngleRate (_id,anum); }
+
+  void setParam (int parameter, dReal value)
+    { dJointSetAMotorParam (_id, parameter, value); }
+  dReal getParam (int parameter) const
+    { return dJointGetAMotorParam (_id, parameter); }
+
+  void addTorques(dReal torque1, dReal torque2, dReal torque3)
+	{ dJointAddAMotorTorques(_id, torque1, torque2, torque3); }
+};
+
+
+#endif
+#endif

src/include/ode/odecpp_collision.h

@@ -0,0 +1,346 @@
+/*************************************************************************
+ *									 *
+ * 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.			 *
+ *									 *
+ *************************************************************************/
+
+/* C++ interface for new collision API */
+
+
+#ifndef _ODE_ODECPP_COLLISION_H_
+#define _ODE_ODECPP_COLLISION_H_
+#ifdef __cplusplus
+
+#include <ode/error.h>
+
+
+class dGeom {
+  // intentionally undefined, don't use these
+  dGeom (dGeom &);
+  void operator= (dGeom &);
+
+protected:
+  dGeomID _id;
+
+public:
+  dGeom()
+    { _id = 0; }
+  ~dGeom()
+    { if (_id) dGeomDestroy (_id); }
+
+  dGeomID id() const
+    { return _id; }
+  operator dGeomID() const
+    { return _id; }
+
+  void destroy() {
+    if (_id) dGeomDestroy (_id);
+    _id = 0;
+  }
+
+  int getClass() const
+    { return dGeomGetClass (_id); }
+
+  dSpaceID getSpace() const
+    { return dGeomGetSpace (_id); }
+
+  void setData (void *data)
+    { dGeomSetData (_id,data); }
+  void *getData() const
+    { return dGeomGetData (_id); }
+
+  void setBody (dBodyID b)
+    { dGeomSetBody (_id,b); }
+  dBodyID getBody() const
+    { return dGeomGetBody (_id); }
+
+  void setPosition (dReal x, dReal y, dReal z)
+    { dGeomSetPosition (_id,x,y,z); }
+  const dReal * getPosition() const
+    { return dGeomGetPosition (_id); }
+
+  void setRotation (const dMatrix3 R)
+    { dGeomSetRotation (_id,R); }
+  const dReal * getRotation() const
+    { return dGeomGetRotation (_id); }
+    
+  void setQuaternion (const dQuaternion quat)
+    { dGeomSetQuaternion (_id,quat); }
+  void getQuaternion (dQuaternion quat) const
+    { dGeomGetQuaternion (_id,quat); }
+
+  void getAABB (dReal aabb[6]) const
+    { dGeomGetAABB (_id, aabb); }
+
+  int isSpace()
+    { return dGeomIsSpace (_id); }
+
+  void setCategoryBits (unsigned long bits)
+    { dGeomSetCategoryBits (_id, bits); }
+  void setCollideBits (unsigned long bits)
+    { dGeomSetCollideBits (_id, bits); }
+  unsigned long getCategoryBits()
+    { return dGeomGetCategoryBits (_id); }
+  unsigned long getCollideBits()
+    { return dGeomGetCollideBits (_id); }
+
+  void enable()
+    { dGeomEnable (_id); }
+  void disable()
+    { dGeomDisable (_id); }
+  int isEnabled()
+    { return dGeomIsEnabled (_id); }
+
+  void collide2 (dGeomID g, void *data, dNearCallback *callback)
+    { dSpaceCollide2 (_id,g,data,callback); }
+};
+
+
+class dSpace : public dGeom {
+  // intentionally undefined, don't use these
+  dSpace (dSpace &);
+  void operator= (dSpace &);
+
+protected:
+  // the default constructor is protected so that you
+  // can't instance this class. you must instance one
+  // of its subclasses instead.
+  dSpace () { _id = 0; }
+
+public:
+  dSpaceID id() const
+    { return (dSpaceID) _id; }
+  operator dSpaceID() const
+    { return (dSpaceID) _id; }
+
+  void setCleanup (int mode)
+    { dSpaceSetCleanup (id(), mode); }
+  int getCleanup()
+    { return dSpaceGetCleanup (id()); }
+
+  void add (dGeomID x)
+    { dSpaceAdd (id(), x); }
+  void remove (dGeomID x)
+    { dSpaceRemove (id(), x); }
+  int query (dGeomID x)
+    { return dSpaceQuery (id(),x); }
+
+  int getNumGeoms()
+    { return dSpaceGetNumGeoms (id()); }
+  dGeomID getGeom (int i)
+    { return dSpaceGetGeom (id(),i); }
+
+  void collide (void *data, dNearCallback *callback)
+    { dSpaceCollide (id(),data,callback); }
+};
+
+
+class dSimpleSpace : public dSpace {
+  // intentionally undefined, don't use these
+  dSimpleSpace (dSimpleSpace &);
+  void operator= (dSimpleSpace &);
+
+public:
+  dSimpleSpace (dSpaceID space)
+    { _id = (dGeomID) dSimpleSpaceCreate (space); }
+};
+
+
+class dHashSpace : public dSpace {
+  // intentionally undefined, don't use these
+  dHashSpace (dHashSpace &);
+  void operator= (dHashSpace &);
+
+public:
+  dHashSpace (dSpaceID space)
+    { _id = (dGeomID) dHashSpaceCreate (space); }
+  void setLevels (int minlevel, int maxlevel)
+    { dHashSpaceSetLevels (id(),minlevel,maxlevel); }
+};
+
+
+class dQuadTreeSpace : public dSpace {
+  // intentionally undefined, don't use these
+  dQuadTreeSpace (dQuadTreeSpace &);
+  void operator= (dQuadTreeSpace &);
+
+public:
+  dQuadTreeSpace (dSpaceID space, dVector3 center, dVector3 extents, int depth)
+    { _id = (dGeomID) dQuadTreeSpaceCreate (space,center,extents,depth); }
+};
+
+
+class dSphere : public dGeom {
+  // intentionally undefined, don't use these
+  dSphere (dSphere &);
+  void operator= (dSphere &);
+
+public:
+  dSphere () { }
+  dSphere (dSpaceID space, dReal radius)
+    { _id = dCreateSphere (space, radius); }
+
+  void create (dSpaceID space, dReal radius) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreateSphere (space, radius);
+  }
+
+  void setRadius (dReal radius)
+    { dGeomSphereSetRadius (_id, radius); }
+  dReal getRadius() const
+    { return dGeomSphereGetRadius (_id); }
+};
+
+
+class dBox : public dGeom {
+  // intentionally undefined, don't use these
+  dBox (dBox &);
+  void operator= (dBox &);
+
+public:
+  dBox () { }
+  dBox (dSpaceID space, dReal lx, dReal ly, dReal lz)
+    { _id = dCreateBox (space,lx,ly,lz); }
+
+  void create (dSpaceID space, dReal lx, dReal ly, dReal lz) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreateBox (space,lx,ly,lz);
+  }
+
+  void setLengths (dReal lx, dReal ly, dReal lz)
+    { dGeomBoxSetLengths (_id, lx, ly, lz); }
+  void getLengths (dVector3 result) const
+    { dGeomBoxGetLengths (_id,result); }
+};
+
+
+class dPlane : public dGeom {
+  // intentionally undefined, don't use these
+  dPlane (dPlane &);
+  void operator= (dPlane &);
+
+public:
+  dPlane() { }
+  dPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d)
+    { _id = dCreatePlane (space,a,b,c,d); }
+
+  void create (dSpaceID space, dReal a, dReal b, dReal c, dReal d) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreatePlane (space,a,b,c,d);
+  }
+
+  void setParams (dReal a, dReal b, dReal c, dReal d)
+    { dGeomPlaneSetParams (_id, a, b, c, d); }
+  void getParams (dVector4 result) const
+    { dGeomPlaneGetParams (_id,result); }
+};
+
+
+class dCCylinder : public dGeom {
+  // intentionally undefined, don't use these
+  dCCylinder (dCCylinder &);
+  void operator= (dCCylinder &);
+
+public:
+  dCCylinder() { }
+  dCCylinder (dSpaceID space, dReal radius, dReal length)
+    { _id = dCreateCCylinder (space,radius,length); }
+
+  void create (dSpaceID space, dReal radius, dReal length) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreateCCylinder (space,radius,length);
+  }
+
+  void setParams (dReal radius, dReal length)
+    { dGeomCCylinderSetParams (_id, radius, length); }
+  void getParams (dReal *radius, dReal *length) const
+    { dGeomCCylinderGetParams (_id,radius,length); }
+};
+
+
+class dRay : public dGeom {
+  // intentionally undefined, don't use these
+  dRay (dRay &);
+  void operator= (dRay &);
+
+public:
+  dRay() { }
+  dRay (dSpaceID space, dReal length)
+    { _id = dCreateRay (space,length); }
+
+  void create (dSpaceID space, dReal length) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreateRay (space,length);
+  }
+
+  void setLength (dReal length)
+    { dGeomRaySetLength (_id, length); }
+  dReal getLength()
+    { return dGeomRayGetLength (_id); }
+
+  void set (dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz)
+    { dGeomRaySet (_id, px, py, pz, dx, dy, dz); }
+  void get (dVector3 start, dVector3 dir)
+    { dGeomRayGet (_id, start, dir); }
+
+  void setParams (int firstContact, int backfaceCull)
+    { dGeomRaySetParams (_id, firstContact, backfaceCull); }
+  void getParams (int *firstContact, int *backfaceCull)
+    { dGeomRayGetParams (_id, firstContact, backfaceCull); }
+  void setClosestHit (int closestHit)
+    { dGeomRaySetClosestHit (_id, closestHit); }
+  int getClosestHit()
+    { return dGeomRayGetClosestHit (_id); }
+};
+
+
+class dGeomTransform : public dGeom {
+  // intentionally undefined, don't use these
+  dGeomTransform (dGeomTransform &);
+  void operator= (dGeomTransform &);
+
+public:
+  dGeomTransform() { }
+  dGeomTransform (dSpaceID space)
+    { _id = dCreateGeomTransform (space); }
+
+  void create (dSpaceID space=0) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreateGeomTransform (space);
+  }
+
+  void setGeom (dGeomID geom)
+    { dGeomTransformSetGeom (_id, geom); }
+  dGeomID getGeom() const
+    { return dGeomTransformGetGeom (_id); }
+
+  void setCleanup (int mode)
+    { dGeomTransformSetCleanup (_id,mode); }
+  int getCleanup ()
+    { return dGeomTransformGetCleanup (_id); }
+
+  void setInfo (int mode)
+    { dGeomTransformSetInfo (_id,mode); }
+  int getInfo()
+    { return dGeomTransformGetInfo (_id); }
+};
+
+
+#endif
+#endif

src/include/ode/odecpp_old.h

@@ -0,0 +1,316 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* this is the old C++ interface, the new C++ interface is not quite
+ * compatible with this. but this file is kept around in case you were
+ * using the old interface.
+ */
+
+#ifndef _ODE_ODECPP_H_
+#define _ODE_ODECPP_H_
+#ifdef __cplusplus
+
+#include <ode/error.h>
+
+
+class dWorld {
+  dWorldID _id;
+
+  dWorld (dWorld &) { dDebug (0,"bad"); }
+  void operator= (dWorld &) { dDebug (0,"bad"); }
+
+public:
+  dWorld()
+    { _id = dWorldCreate(); }
+  ~dWorld()
+    { dWorldDestroy (_id); }
+  dWorldID id()
+    { return _id; }
+
+  void setGravity (dReal x, dReal y, dReal z)
+    { dWorldSetGravity (_id,x,y,z); }
+  void getGravity (dVector3 g)
+    { dWorldGetGravity (_id,g); }
+  void step (dReal stepsize)
+    { dWorldStep (_id,stepsize); }
+};
+
+
+class dBody {
+  dBodyID _id;
+
+  dBody (dBody &) { dDebug (0,"bad"); }
+  void operator= (dBody &) { dDebug (0,"bad"); }
+
+public:
+  dBody()
+    { _id = 0; }
+  dBody (dWorld &world)
+    { _id = dBodyCreate (world.id()); }
+  ~dBody()
+    { dBodyDestroy (_id); }
+  void create (dWorld &world)
+    { if (_id) dBodyDestroy (_id); _id = dBodyCreate (world.id()); }
+  dBodyID id()
+    { return _id; }
+
+  void setData (void *data)
+    { dBodySetData (_id,data); }
+  void *getData()
+    { return dBodyGetData (_id); }
+
+  void setPosition (dReal x, dReal y, dReal z)
+    { dBodySetPosition (_id,x,y,z); }
+  void setRotation (const dMatrix3 R)
+    { dBodySetRotation (_id,R); }
+  void setQuaternion (const dQuaternion q)
+    { dBodySetQuaternion (_id,q); }
+  void setLinearVel  (dReal x, dReal y, dReal z)
+    { dBodySetLinearVel (_id,x,y,z); }
+  void setAngularVel (dReal x, dReal y, dReal z)
+    { dBodySetAngularVel (_id,x,y,z); }
+
+  const dReal * getPosition()
+    { return dBodyGetPosition (_id); }
+  const dReal * getRotation()
+    { return dBodyGetRotation (_id); }
+  const dReal * getQuaternion()
+    { return dBodyGetQuaternion (_id); }
+  const dReal * getLinearVel()
+    { return dBodyGetLinearVel (_id); }
+  const dReal * getAngularVel()
+    { return dBodyGetAngularVel (_id); }
+
+  void setMass (const dMass *mass)
+    { dBodySetMass (_id,mass); }
+  void getMass (dMass *mass)
+    { dBodyGetMass (_id,mass); }
+
+  void addForce (dReal fx, dReal fy, dReal fz)
+    { dBodyAddForce (_id, fx, fy, fz); }
+  void addTorque (dReal fx, dReal fy, dReal fz)
+    { dBodyAddTorque (_id, fx, fy, fz); }
+  void addRelForce (dReal fx, dReal fy, dReal fz)
+    { dBodyAddRelForce (_id, fx, fy, fz); }
+  void addRelTorque (dReal fx, dReal fy, dReal fz)
+    { dBodyAddRelTorque (_id, fx, fy, fz); }
+  void addForceAtPos (dReal fx, dReal fy, dReal fz,
+		      dReal px, dReal py, dReal pz)
+    { dBodyAddForceAtPos (_id, fx, fy, fz, px, py, pz); }
+  void addRelForceAtPos (dReal fx, dReal fy, dReal fz,
+			 dReal px, dReal py, dReal pz)
+    { dBodyAddRelForceAtPos (_id, fx, fy, fz, px, py, pz); }
+  void addRelForceAtRelPos (dReal fx, dReal fy, dReal fz,
+			    dReal px, dReal py, dReal pz)
+    { dBodyAddRelForceAtRelPos (_id, fx, fy, fz, px, py, pz); }
+
+  void getRelPointPos (dReal px, dReal py, dReal pz, dVector3 result)
+    { dBodyGetRelPointPos (_id, px, py, pz, result); }
+  void getRelPointVel (dReal px, dReal py, dReal pz, dVector3 result)
+    { dBodyGetRelPointVel (_id, px, py, pz, result); }
+
+  int isConnectedTo (const dBody &b)
+    { return dAreConnected (_id,b._id); }
+};
+
+
+class dJointGroup {
+  dJointGroupID _id;
+
+  dJointGroup (dJointGroup &) { dDebug (0,"bad"); }
+  void operator= (dJointGroup &) { dDebug (0,"bad"); }
+
+public:
+  dJointGroup()
+    { _id = 0; }
+  dJointGroup (int max_size)
+    { _id = dJointGroupCreate (max_size); }
+  ~dJointGroup()
+    { dJointGroupDestroy (_id); }
+  void create (int max_size)
+    { if (_id) dJointGroupDestroy (_id); _id = dJointGroupCreate (max_size); }
+  dJointGroupID id()
+    { return _id; }
+
+  void empty()
+    { dJointGroupEmpty (_id); }
+};
+
+
+class dJoint {
+  dJointID _id;
+
+  dJoint (dJoint &) { dDebug (0,"bad"); }
+  void operator= (dJoint &) { dDebug (0,"bad"); }
+
+public:
+  dJoint()
+    { _id = 0; }
+  ~dJoint()
+    { dJointDestroy (_id); }
+  dJointID id()
+    { return _id; }
+
+  void createBall (dWorld &world, dJointGroup *group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateBall (world.id(), group ? group->id() : 0);
+  }
+  void createHinge (dWorld &world, dJointGroup *group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateHinge (world.id(), group ? group->id() : 0);
+  }
+  void createSlider (dWorld &world, dJointGroup *group=0) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateSlider (world.id(), group ? group->id() : 0);
+  }
+  void createContact (dWorld &world, dJointGroup *group, dContact *contact) {
+    if (_id) dJointDestroy (_id);
+    _id = dJointCreateContact (world.id(), group ? group->id() : 0, contact);
+  }
+
+  void attach (dBody &body1, dBody &body2)
+    { dJointAttach (_id, body1.id(), body2.id()); }
+
+  void setBallAnchor (dReal x, dReal y, dReal z)
+    { dJointSetBallAnchor (_id, x, y, z); }
+  void setHingeAnchor (dReal x, dReal y, dReal z)
+    { dJointSetHingeAnchor (_id, x, y, z); }
+
+  void setHingeAxis (dReal x, dReal y, dReal z)
+    { dJointSetHingeAxis (_id, x, y, z); }
+  void setSliderAxis (dReal x, dReal y, dReal z)
+    { dJointSetSliderAxis (_id, x, y, z); }
+
+  void getBallAnchor (dVector3 result)
+    { dJointGetBallAnchor (_id, result); }
+  void getHingeAnchor (dVector3 result)
+    { dJointGetHingeAnchor (_id, result); }
+
+  void getHingeAxis (dVector3 result)
+    { dJointGetHingeAxis (_id, result); }
+  void getSliderAxis (dVector3 result)
+    { dJointGetSliderAxis (_id, result); }
+};
+
+
+class dSpace {
+  dSpaceID _id;
+
+  dSpace (dSpace &) { dDebug (0,"bad"); }
+  void operator= (dSpace &) { dDebug (0,"bad"); }
+
+public:
+  dSpace ()
+    { _id = dHashSpaceCreate(); }
+  ~dSpace()
+    { dSpaceDestroy (_id); }
+  dSpaceID id()
+    { return _id; }
+  void collide (void *data, dNearCallback *callback)
+    { dSpaceCollide (_id,data,callback); }
+};
+
+
+class dGeom {
+  dGeomID _id;
+
+  dGeom (dGeom &) { dDebug (0,"bad"); }
+  void operator= (dGeom &) { dDebug (0,"bad"); }
+
+public:
+  dGeom()
+    { _id = 0; }
+  ~dGeom()
+    { dGeomDestroy (_id); }
+  dGeomID id()
+    { return _id; }
+
+  void createSphere (dSpace &space, dReal radius) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreateSphere (space.id(),radius);
+  }
+
+  void createBox (dSpace &space, dReal lx, dReal ly, dReal lz) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreateBox (space.id(),lx,ly,lz);
+  }
+
+  void createPlane (dSpace &space, dReal a, dReal b, dReal c, dReal d) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreatePlane (space.id(),a,b,c,d);
+  }
+
+  void createCCylinder (dSpace &space, dReal radius, dReal length) {
+    if (_id) dGeomDestroy (_id);
+    _id = dCreateCCylinder (space.id(),radius,length);
+  }
+
+  void destroy() {
+    if (_id) dGeomDestroy (_id);
+    _id = 0;
+  }
+
+  int getClass()
+    { return dGeomGetClass (_id); }
+
+  dReal sphereGetRadius()
+    { return dGeomSphereGetRadius (_id); }
+
+  void boxGetLengths (dVector3 result)
+    { dGeomBoxGetLengths (_id,result); }
+
+  void planeGetParams (dVector4 result)
+    { dGeomPlaneGetParams (_id,result); }
+
+  void CCylinderGetParams (dReal *radius, dReal *length)
+    { dGeomCCylinderGetParams (_id,radius,length); }
+
+  void setData (void *data)
+    { dGeomSetData (_id,data); }
+
+  void *getData()
+    { return dGeomGetData (_id); }
+
+  void setBody (dBody &b)
+    { dGeomSetBody (_id,b.id()); }
+  void setBody (dBodyID b)
+    { dGeomSetBody (_id,b); }
+
+  dBodyID getBody()
+    { return dGeomGetBody (_id); }
+
+  void setPosition (dReal x, dReal y, dReal z)
+    { dGeomSetPosition (_id,x,y,z); }
+
+  void setRotation (const dMatrix3 R)
+    { dGeomSetRotation (_id,R); }
+
+  const dReal * getPosition()
+    { return dGeomGetPosition (_id); }
+
+  const dReal * getRotation()
+    { return dGeomGetRotation (_id); }
+};
+
+
+#endif
+#endif

src/include/ode/odemath.h

@@ -0,0 +1,240 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_ODEMATH_H_
+#define _ODE_ODEMATH_H_
+
+#include <ode/common.h>
+
+#ifdef __GNUC__
+#define PURE_INLINE extern inline
+#else
+#define PURE_INLINE inline
+#endif
+
+/*
+ * macro to access elements i,j in an NxM matrix A, independent of the
+ * matrix storage convention.
+ */
+#define dACCESS33(A,i,j) ((A)[(i)*4+(j)])
+
+
+/*
+ * 3-way dot product. dDOTpq means that elements of `a' and `b' are spaced
+ * p and q indexes apart respectively. dDOT() means dDOT11.
+ * in C++ we could use function templates to get all the versions of these
+ * functions - but on some compilers this will result in sub-optimal code.
+ */
+
+#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
+
+#ifdef __cplusplus
+
+PURE_INLINE dReal dDOT   (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,1); }
+PURE_INLINE dReal dDOT13 (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,3); }
+PURE_INLINE dReal dDOT31 (const dReal *a, const dReal *b) { return dDOTpq(a,b,3,1); }
+PURE_INLINE dReal dDOT33 (const dReal *a, const dReal *b) { return dDOTpq(a,b,3,3); }
+PURE_INLINE dReal dDOT14 (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,4); }
+PURE_INLINE dReal dDOT41 (const dReal *a, const dReal *b) { return dDOTpq(a,b,4,1); }
+PURE_INLINE dReal dDOT44 (const dReal *a, const dReal *b) { return dDOTpq(a,b,4,4); }
+
+#else
+
+#define dDOT(a,b)   dDOTpq(a,b,1,1)
+#define dDOT13(a,b) dDOTpq(a,b,1,3)
+#define dDOT31(a,b) dDOTpq(a,b,3,1)
+#define dDOT33(a,b) dDOTpq(a,b,3,3)
+#define dDOT14(a,b) dDOTpq(a,b,1,4)
+#define dDOT41(a,b) dDOTpq(a,b,4,1)
+#define dDOT44(a,b) dDOTpq(a,b,4,4)
+
+#endif /* __cplusplus */
+
+
+/*
+ * cross product, set a = b x c. dCROSSpqr means that elements of `a', `b'
+ * and `c' are spaced p, q and r indexes apart respectively.
+ * dCROSS() means dCROSS111. `op' is normally `=', but you can set it to
+ * +=, -= etc to get other effects.
+ */
+
+#define dCROSS(a,op,b,c) \
+  (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
+  (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
+  (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
+#define dCROSSpqr(a,op,b,c,p,q,r) \
+  (a)[  0] op ((b)[  q]*(c)[2*r] - (b)[2*q]*(c)[  r]); \
+  (a)[  p] op ((b)[2*q]*(c)[  0] - (b)[  0]*(c)[2*r]); \
+  (a)[2*p] op ((b)[  0]*(c)[  r] - (b)[  q]*(c)[  0]);
+#define dCROSS114(a,op,b,c) dCROSSpqr(a,op,b,c,1,1,4)
+#define dCROSS141(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,1)
+#define dCROSS144(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,4)
+#define dCROSS411(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,1)
+#define dCROSS414(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,4)
+#define dCROSS441(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,1)
+#define dCROSS444(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,4)
+
+
+/*
+ * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
+ * A is stored by rows, and has `skip' elements per row. the matrix is
+ * assumed to be already zero, so this does not write zero elements!
+ * if (plus,minus) is (+,-) then a positive version will be written.
+ * if (plus,minus) is (-,+) then a negative version will be written.
+ */
+
+#define dCROSSMAT(A,a,skip,plus,minus) \
+  (A)[1] = minus (a)[2]; \
+  (A)[2] = plus (a)[1]; \
+  (A)[(skip)+0] = plus (a)[2]; \
+  (A)[(skip)+2] = minus (a)[0]; \
+  (A)[2*(skip)+0] = minus (a)[1]; \
+  (A)[2*(skip)+1] = plus (a)[0];
+
+
+/*
+ * compute the distance between two 3-vectors
+ */
+
+#ifdef __cplusplus
+PURE_INLINE float dDISTANCE (const float a[3], const float b[3])
+	{ return (float) dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]) ); }
+PURE_INLINE double dDISTANCE (const double a[3], const double b[3])
+	{ return dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]) ); }
+#else
+#define dDISTANCE(a,b) \
+	(dSqrt( ((a)[0]-(b)[0])*((a)[0]-(b)[0]) + ((a)[1]-(b)[1])*((a)[1]-(b)[1]) + ((a)[2]-(b)[2])*((a)[2]-(b)[2]) ))
+#endif
+
+
+/*
+ * special case matrix multipication, with operator selection
+ */
+
+#define dMULTIPLYOP0_331(A,op,B,C) \
+  (A)[0] op dDOT((B),(C)); \
+  (A)[1] op dDOT((B+4),(C)); \
+  (A)[2] op dDOT((B+8),(C));
+#define dMULTIPLYOP1_331(A,op,B,C) \
+  (A)[0] op dDOT41((B),(C)); \
+  (A)[1] op dDOT41((B+1),(C)); \
+  (A)[2] op dDOT41((B+2),(C));
+#define dMULTIPLYOP0_133(A,op,B,C) \
+  (A)[0] op dDOT14((B),(C)); \
+  (A)[1] op dDOT14((B),(C+1)); \
+  (A)[2] op dDOT14((B),(C+2));
+#define dMULTIPLYOP0_333(A,op,B,C) \
+  (A)[0] op dDOT14((B),(C)); \
+  (A)[1] op dDOT14((B),(C+1)); \
+  (A)[2] op dDOT14((B),(C+2)); \
+  (A)[4] op dDOT14((B+4),(C)); \
+  (A)[5] op dDOT14((B+4),(C+1)); \
+  (A)[6] op dDOT14((B+4),(C+2)); \
+  (A)[8] op dDOT14((B+8),(C)); \
+  (A)[9] op dDOT14((B+8),(C+1)); \
+  (A)[10] op dDOT14((B+8),(C+2));
+#define dMULTIPLYOP1_333(A,op,B,C) \
+  (A)[0] op dDOT44((B),(C)); \
+  (A)[1] op dDOT44((B),(C+1)); \
+  (A)[2] op dDOT44((B),(C+2)); \
+  (A)[4] op dDOT44((B+1),(C)); \
+  (A)[5] op dDOT44((B+1),(C+1)); \
+  (A)[6] op dDOT44((B+1),(C+2)); \
+  (A)[8] op dDOT44((B+2),(C)); \
+  (A)[9] op dDOT44((B+2),(C+1)); \
+  (A)[10] op dDOT44((B+2),(C+2));
+#define dMULTIPLYOP2_333(A,op,B,C) \
+  (A)[0] op dDOT((B),(C)); \
+  (A)[1] op dDOT((B),(C+4)); \
+  (A)[2] op dDOT((B),(C+8)); \
+  (A)[4] op dDOT((B+4),(C)); \
+  (A)[5] op dDOT((B+4),(C+4)); \
+  (A)[6] op dDOT((B+4),(C+8)); \
+  (A)[8] op dDOT((B+8),(C)); \
+  (A)[9] op dDOT((B+8),(C+4)); \
+  (A)[10] op dDOT((B+8),(C+8));
+
+#ifdef __cplusplus
+
+#define DECL template <class TA, class TB, class TC> PURE_INLINE void
+
+DECL dMULTIPLY0_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_331(A,=,B,C) }
+DECL dMULTIPLY1_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_331(A,=,B,C) }
+DECL dMULTIPLY0_133(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_133(A,=,B,C) }
+DECL dMULTIPLY0_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_333(A,=,B,C) }
+DECL dMULTIPLY1_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_333(A,=,B,C) }
+DECL dMULTIPLY2_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP2_333(A,=,B,C) }
+
+DECL dMULTIPLYADD0_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_331(A,+=,B,C) }
+DECL dMULTIPLYADD1_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_331(A,+=,B,C) }
+DECL dMULTIPLYADD0_133(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_133(A,+=,B,C) }
+DECL dMULTIPLYADD0_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_333(A,+=,B,C) }
+DECL dMULTIPLYADD1_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_333(A,+=,B,C) }
+DECL dMULTIPLYADD2_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP2_333(A,+=,B,C) }
+
+#undef DECL
+
+#else
+
+#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
+#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
+#define dMULTIPLY0_133(A,B,C) dMULTIPLYOP0_133(A,=,B,C)
+#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C)
+#define dMULTIPLY1_333(A,B,C) dMULTIPLYOP1_333(A,=,B,C)
+#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C)
+
+#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C)
+#define dMULTIPLYADD1_331(A,B,C) dMULTIPLYOP1_331(A,+=,B,C)
+#define dMULTIPLYADD0_133(A,B,C) dMULTIPLYOP0_133(A,+=,B,C)
+#define dMULTIPLYADD0_333(A,B,C) dMULTIPLYOP0_333(A,+=,B,C)
+#define dMULTIPLYADD1_333(A,B,C) dMULTIPLYOP1_333(A,+=,B,C)
+#define dMULTIPLYADD2_333(A,B,C) dMULTIPLYOP2_333(A,+=,B,C)
+
+#endif
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length)
+ */
+void dNormalize3 (dVector3 a);
+void dNormalize4 (dVector4 a);
+
+
+/*
+ * given a unit length "normal" vector n, generate vectors p and q vectors
+ * that are an orthonormal basis for the plane space perpendicular to n.
+ * i.e. this makes p,q such that n,p,q are all perpendicular to each other.
+ * q will equal n x p. if n is not unit length then p will be unit length but
+ * q wont be.
+ */
+
+void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/rotation.h

@@ -0,0 +1,70 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_ROTATION_H_
+#define _ODE_ROTATION_H_
+
+#include <ode/common.h>
+#include <ode/compatibility.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+void dRSetIdentity (dMatrix3 R);
+
+void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
+			 dReal angle);
+
+void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi);
+
+void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
+		  dReal bx, dReal by, dReal bz);
+
+void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az);
+
+void dQSetIdentity (dQuaternion q);
+
+void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
+			 dReal angle);
+
+/* Quaternion multiplication, analogous to the matrix multiplication routines. */
+/* qa = rotate by qc, then qb */
+void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
+/* qa = rotate by qc, then by inverse of qb */
+void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
+/* qa = rotate by inverse of qc, then by qb */
+void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
+/* qa = rotate by inverse of qc, then by inverse of qb */
+void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc);
+
+void dRfromQ (dMatrix3 R, const dQuaternion q);
+void dQfromR (dQuaternion q, const dMatrix3 R);
+void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/include/ode/timer.h

@@ -0,0 +1,76 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_TIMER_H_
+#define _ODE_TIMER_H_
+
+#include <ode/config.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* stop watch objects */
+
+typedef struct dStopwatch {
+  double time;			/* total clock count */
+  unsigned long cc[2];		/* clock count since last `start' */
+} dStopwatch;
+
+void dStopwatchReset (dStopwatch *);
+void dStopwatchStart (dStopwatch *);
+void dStopwatchStop  (dStopwatch *);
+double dStopwatchTime (dStopwatch *);	/* returns total time in secs */
+
+
+/* code timers */
+
+void dTimerStart (const char *description);	/* pass a static string here */
+void dTimerNow (const char *description);	/* pass a static string here */
+void dTimerEnd();
+
+/* print out a timer report. if `average' is nonzero, print out the average
+ * time for each slot (this is only meaningful if the same start-now-end
+ * calls are being made repeatedly.
+ */
+void dTimerReport (FILE *fout, int average);
+
+
+/* resolution */
+
+/* returns the timer ticks per second implied by the timing hardware or API.
+ * the actual timer resolution may not be this great.
+ */
+double dTimerTicksPerSecond();
+
+/* returns an estimate of the actual timer resolution, in seconds. this may
+ * be greater than 1/ticks_per_second.
+ */
+double dTimerResolution();
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/source/DataModelV2/GuiRootInstance.cpp

@@ -213,7 +213,7 @@ GuiRootInstance::GuiRootInstance() : _message(""), _messageTime(0)
 	button->boxBegin = Vector2(0,215);
 	button->boxEnd = Vector2(80,235);
 	button->textOutlineColor = Color4(0.5F,0.5F,0.5F,0.5F);
-	button->textColor = Color3::white();
+	button->textColor = Color3(0,1,1);
 	button->boxColor = Color4::clear();
 	button->textSize = 12;
 	button->title = "Group";
@@ -229,7 +229,7 @@ GuiRootInstance::GuiRootInstance() : _message(""), _messageTime(0)
 	button->boxBegin = Vector2(0,240);
 	button->boxEnd = Vector2(80,260);
 	button->textOutlineColor = Color4(0.5F,0.5F,0.5F,0.5F);
-	button->textColor = Color3::white();
+	button->textColor = Color3(0,1,1);
 	button->boxColor = Color4::clear();
 	button->textSize = 12;
 	button->title = "UnGroup";
@@ -245,7 +245,7 @@ GuiRootInstance::GuiRootInstance() : _message(""), _messageTime(0)
 	button->boxBegin = Vector2(0,265);
 	button->boxEnd = Vector2(80,285);
 	button->textOutlineColor = Color4(0.5F,0.5F,0.5F,0.5F);
-	button->textColor = Color3::white();
+	button->textColor = Color3(0,1,1);
 	button->boxColor = Color4::clear();
 	button->textSize = 12;
 	button->title = "Duplicate";
@@ -520,11 +520,19 @@ void GuiRootInstance::update()
 			if(g_dataModel->getSelectionService()->getSelection()[i]->canDelete)
 			{
 				button->disabled = false;
-				button2->disabled = false;
-				button3->disabled = false;
 				button4->disabled = false;
 				button5->disabled = false;
 				button6->disabled = false;
+				
+
+				if (g_dataModel->getSelectionService()->getSelection().size() > 1){
+					button2->disabled = false;
+				}
+
+				if (g_dataModel->getSelectionService()->getSelection()[i]->getClassName() == "GroupInstance"){
+					button3->disabled = false;
+				}
+
 				break;
 			}
 	}

src/source/Listener/GUDButtonListener.cpp

@@ -16,7 +16,11 @@ void GUDButtonListener::onButton1MouseClick(BaseButtonInstance* button)
 		}
 	if(cont)
 	{
-		AudioPlayer::playSound(dingSound);
+		
+		if(button->disabled == false){
+			AudioPlayer::playSound(dingSound);
+		}
+
 		if(button->name == "Duplicate")
 		{
 			std::vector<Instance*> newinst;
@@ -36,28 +40,31 @@ void GUDButtonListener::onButton1MouseClick(BaseButtonInstance* button)
 		}
 		else if(button->name == "Group")
 		{
-			GroupInstance * inst = new GroupInstance();
-			inst->setParent(g_dataModel->getWorkspace());
-			for(size_t i = 0; i < g_dataModel->getSelectionService()->getSelection().size(); i++)
-			{
-				if(g_dataModel->getSelectionService()->getSelection()[i]->canDelete)
+			if (g_dataModel->getSelectionService()->getSelection().size() > 1){
+				GroupInstance * inst = new GroupInstance();
+				inst->setParent(g_dataModel->getWorkspace());
+				for(size_t i = 0; i < g_dataModel->getSelectionService()->getSelection().size(); i++)
 				{
-					g_dataModel->getSelectionService()->getSelection()[i]->setParent(inst);
-					if(PartInstance* part = dynamic_cast<PartInstance*>(g_dataModel->getSelectionService()->getSelection()[i]))
+					if(g_dataModel->getSelectionService()->getSelection()[i]->canDelete)
 					{
-						inst->primaryPart = part;
+						g_dataModel->getSelectionService()->getSelection()[i]->setParent(inst);
+						if(PartInstance* part = dynamic_cast<PartInstance*>(g_dataModel->getSelectionService()->getSelection()[i]))
+						{
+							inst->primaryPart = part;
+						}
 					}
 				}
-			}
-			g_dataModel->getSelectionService()->clearSelection();
-			g_dataModel->getSelectionService()->addSelected(inst);
+				g_dataModel->getSelectionService()->clearSelection();
+				g_dataModel->getSelectionService()->addSelected(inst);
+			}			
 		}
 		else if(button->name == "UnGroup")
 		{
 			std::vector<Instance*> newinst;
 			for(size_t i = 0; i < g_dataModel->getSelectionService()->getSelection().size(); i++)
 			{
-				if(g_dataModel->getSelectionService()->getSelection()[i]->canDelete)
+				Instance* selection = g_dataModel->getSelectionService()->getSelection()[i];
+				if(selection->canDelete && selection->getClassName() == "GroupInstance")
 				{
 					if(GroupInstance* model = dynamic_cast<GroupInstance*>(g_dataModel->getSelectionService()->getSelection()[i]))
 					{

src/source/ode/array.cpp

@@ -0,0 +1,80 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/config.h>
+#include <ode/memory.h>
+#include <ode/error.h>
+#include "array.h"
+
+
+static inline int roundUpToPowerOfTwo (int x)
+{
+  int i = 1;
+  while (i < x) i <<= 1;
+  return i;
+}
+
+
+void dArrayBase::_freeAll (int sizeofT)
+{
+  if (_data) {
+    if (_data == this+1) return;	// if constructLocalArray() was called
+    dFree (_data,_anum * sizeofT);
+  }
+}
+
+
+void dArrayBase::_setSize (int newsize, int sizeofT)
+{
+  if (newsize < 0) return;
+  if (newsize > _anum) {
+    if (_data == this+1) {
+      // this is a no-no, because constructLocalArray() was called
+      dDebug (0,"setSize() out of space in LOCAL array");
+    }
+    int newanum = roundUpToPowerOfTwo (newsize);
+    if (_data) _data = dRealloc (_data, _anum*sizeofT, newanum*sizeofT);
+    else _data = dAlloc (newanum*sizeofT);
+    _anum = newanum;
+  }
+  _size = newsize;
+}
+
+
+void * dArrayBase::operator new (size_t size)
+{
+  return dAlloc (size);
+}
+
+
+void dArrayBase::operator delete (void *ptr, size_t size)
+{
+  dFree (ptr,size);
+}
+
+
+void dArrayBase::constructLocalArray (int __anum)
+{
+  _size = 0;
+  _anum = __anum;
+  _data = this+1;
+}

src/source/ode/array.h

@@ -0,0 +1,135 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* this comes from the `reuse' library. copy any changes back to the source.
+ *
+ * Variable sized array template. The array is always stored in a contiguous
+ * chunk. The array can be resized. A size increase will cause more memory
+ * to be allocated, and may result in relocation of the array memory.
+ * A size decrease has no effect on the memory allocation.
+ *
+ * Array elements with constructors or destructors are not supported!
+ * But if you must have such elements, here's what to know/do:
+ *   - Bitwise copy is used when copying whole arrays.
+ *   - When copying individual items (via push(), insert() etc) the `='
+ *     (equals) operator is used. Thus you should define this operator to do
+ *     a bitwise copy. You should probably also define the copy constructor.
+ */
+
+
+#ifndef _ODE_ARRAY_H_
+#define _ODE_ARRAY_H_
+
+#include <ode/config.h>
+
+
+// this base class has no constructors or destructor, for your convenience.
+
+class dArrayBase {
+protected:
+  int _size;		// number of elements in `data'
+  int _anum;		// allocated number of elements in `data'
+  void *_data;		// array data
+
+  void _freeAll (int sizeofT);
+  void _setSize (int newsize, int sizeofT);
+  // set the array size to `newsize', allocating more memory if necessary.
+  // if newsize>_anum and is a power of two then this is guaranteed to
+  // set _size and _anum to newsize.
+
+public:
+  // not: dArrayBase () { _size=0; _anum=0; _data=0; }
+
+  int size() const { return _size; }
+  int allocatedSize() const { return _anum; }
+  void * operator new (size_t size);
+  void operator delete (void *ptr, size_t size);
+
+  void constructor() { _size=0; _anum=0; _data=0; }
+  // if this structure is allocated with malloc() instead of new, you can
+  // call this to set it up.
+
+  void constructLocalArray (int __anum);
+  // this helper function allows non-reallocating arrays to be constructed
+  // on the stack (or in the heap if necessary). this is something of a
+  // kludge and should be used with extreme care. this function acts like
+  // a constructor - it is called on uninitialized memory that will hold the
+  // Array structure and the data. __anum is the number of elements that
+  // are allocated. the memory MUST be allocated with size:
+  //   sizeof(ArrayBase) + __anum*sizeof(T)
+  // arrays allocated this way will never try to reallocate or free the
+  // memory - that's your job.
+};
+
+
+template <class T> class dArray : public dArrayBase {
+public:
+  void equals (const dArray<T> &x) {
+    setSize (x.size());
+    memcpy (_data,x._data,x._size * sizeof(T));
+  }
+
+  dArray () { constructor(); }
+  dArray (const dArray<T> &x) { constructor(); equals (x); }
+  ~dArray () { _freeAll(sizeof(T)); }
+  void setSize (int newsize) { _setSize (newsize,sizeof(T)); }
+  T *data() const { return (T*) _data; }
+  T & operator[] (int i) const { return ((T*)_data)[i]; }
+  void operator = (const dArray<T> &x) { equals (x); }
+
+  void push (const T item) {
+    if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
+    memcpy (&(((T*)_data)[_size-1]), &item, sizeof(T));
+  }
+
+  void swap (dArray<T> &x) {
+    int tmp1;
+    void *tmp2;
+    tmp1=_size; _size=x._size; x._size=tmp1;
+    tmp1=_anum; _anum=x._anum; x._anum=tmp1;
+    tmp2=_data; _data=x._data; x._data=tmp2;
+  }
+
+  // insert the item at the position `i'. if i<0 then add the item to the
+  // start, if i >= size then add the item to the end of the array.
+  void insert (int i, const T item) {
+    if (_size < _anum) _size++; else _setSize (_size+1,sizeof(T));
+    if (i >= (_size-1)) i = _size-1;	// add to end
+    else {
+      if (i < 0) i=0;			// add to start
+      int n = _size-1-i;
+      if (n>0) memmove (((T*)_data) + i+1, ((T*)_data) + i, n*sizeof(T));
+    }
+    ((T*)_data)[i] = item;
+  }
+
+  void remove (int i) {
+    if (i >= 0 && i < _size) {	// passing this test guarantees size>0
+      int n = _size-1-i;
+      if (n>0) memmove (((T*)_data) + i, ((T*)_data) + i+1, n*sizeof(T));
+      _size--;
+    }
+  }
+};
+
+
+#endif

src/source/ode/collision_kernel.cpp

@@ -0,0 +1,625 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 "collision_kernel.h"
+#include "collision_util.h"
+#include "collision_std.h"
+#include "collision_transform.h"
+#include "collision_trimesh_internal.h"
+
+#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()
+
+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 (dCCylinderClass,dSphereClass,&dCollideCCylinderSphere);
+  setCollider (dCCylinderClass,dBoxClass,&dCollideCCylinderBox);
+  setCollider (dCCylinderClass,dCCylinderClass,&dCollideCCylinderCCylinder);
+  setCollider (dCCylinderClass,dPlaneClass,&dCollideCCylinderPlane);
+  setCollider (dRayClass,dSphereClass,&dCollideRaySphere);
+  setCollider (dRayClass,dBoxClass,&dCollideRayBox);
+  setCollider (dRayClass,dCCylinderClass,&dCollideRayCCylinder);
+  setCollider (dRayClass,dPlaneClass,&dCollideRayPlane);
+#ifdef dTRIMESH_ENABLED
+  setCollider (dTriMeshClass,dSphereClass,&dCollideSTL);
+  setCollider (dTriMeshClass,dBoxClass,&dCollideBTL);
+  setCollider (dTriMeshClass,dRayClass,&dCollideRTL);
+  setCollider (dTriMeshClass,dTriMeshClass,&dCollideTTL);
+  setCollider (dTriMeshClass,dCCylinderClass,&dCollideCCTL);
+#endif
+  setAllColliders (dGeomTransformClass,&dCollideTransform);
+}
+
+
+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");
+
+  // 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;
+
+  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;
+      }
+    }
+    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) {
+    dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
+    pos = pr->pos;
+    R = pr->R;
+    dSetZero (pos,4);
+    dRSetIdentity (R);
+  }
+  else {
+    pos = 0;
+    R = 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) dFree (pos,sizeof(dxPosR));
+  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;
+  }
+}
+
+//****************************************************************************
+// 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 (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  CHECK_NOT_LOCKED (g->parent_space);
+
+  if (b) {
+    if (!g->body) dFree (g->pos,sizeof(dxPosR));
+    g->pos = b->pos;
+    g->R = b->R;
+    dGeomMoved (g);
+    if (g->body != b) {
+      g->bodyRemove();
+      g->bodyAdd (b);
+    }
+  }
+  else {
+    if (g->body) {
+      dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR));
+      g->pos = pr->pos;
+      g->R = pr->R;
+      memcpy (g->pos,g->body->pos,sizeof(dVector3));
+      memcpy (g->R,g->body->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->body) {
+    // this will call dGeomMoved (g), so we don't have to
+    dBodySetPosition (g->body,x,y,z);
+  }
+  else {
+    g->pos[0] = x;
+    g->pos[1] = y;
+    g->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->body) {
+    // this will call dGeomMoved (g), so we don't have to
+    dBodySetRotation (g->body,R);
+  }
+  else {
+    memcpy (g->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->body) {
+    // this will call dGeomMoved (g), so we don't have to
+    dBodySetQuaternion (g->body,quat);
+  }
+  else {
+    dQtoR (quat, g->R);
+    dGeomMoved (g);
+  }
+}
+
+
+const dReal * dGeomGetPosition (dxGeom *g)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  return g->pos;
+}
+
+
+const dReal * dGeomGetRotation (dxGeom *g)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  return g->R;
+}
+
+
+void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
+{
+  dAASSERT (g);
+  dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
+  if (g->body) {
+    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 {
+    dRtoQ (g->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);
+}
+
+//****************************************************************************
+// here is where we deallocate any memory that has been globally
+// allocated, or free other global resources.
+
+void dCloseODE()
+{
+  colliders_initialized = 0;
+  num_user_classes = 0;
+}

src/source/ode/collision_kernel.h

@@ -0,0 +1,202 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+internal data structures and functions for collision detection.
+
+*/
+
+#ifndef _ODE_COLLISION_KERNEL_H_
+#define _ODE_COLLISION_KERNEL_H_
+
+#include <ode/common.h>
+#include <ode/contact.h>
+#include <ode/collision.h>
+#include "objects.h"
+
+//****************************************************************************
+// constants and macros
+
+// mask for the number-of-contacts field in the dCollide() flags parameter
+#define NUMC_MASK (0xffff)
+
+#define IS_SPACE(geom) \
+  ((geom)->type >= dFirstSpaceClass && (geom)->type <= dLastSpaceClass)
+
+//****************************************************************************
+// geometry object base class
+
+// position vector and rotation matrix for geometry objects that are not
+// connected to bodies.
+
+struct dxPosR {
+  dVector3 pos;
+  dMatrix3 R;
+};
+
+
+// geom flags.
+//
+// GEOM_DIRTY means that the space data structures for this geom are
+// potentially not up to date. NOTE THAT all space parents of a dirty geom
+// are themselves dirty. this is an invariant that must be enforced.
+//
+// GEOM_AABB_BAD means that the cached AABB for this geom is not up to date.
+// note that GEOM_DIRTY does not imply GEOM_AABB_BAD, as the geom might
+// recalculate its own AABB but does not know how to update the space data
+// structures for the space it is in. but GEOM_AABB_BAD implies GEOM_DIRTY.
+// the valid combinations are: 0, GEOM_DIRTY, GEOM_DIRTY|GEOM_AABB_BAD.
+
+enum {
+  GEOM_DIRTY	= 1,	// geom is 'dirty', i.e. position unknown
+  GEOM_AABB_BAD	= 2,	// geom's AABB is not valid
+  GEOM_PLACEABLE = 4,	// geom is placeable
+  GEOM_ENABLED = 8,		// geom is enabled
+
+  // Ray specific
+  RAY_FIRSTCONTACT = 0x10000,
+  RAY_BACKFACECULL = 0x20000,
+  RAY_CLOSEST_HIT  = 0x40000
+};
+
+
+// geometry object base class. pos and R will either point to a separately
+// allocated buffer (if body is 0 - pos points to the dxPosR object) or to
+// the pos and R of the body (if body nonzero).
+// a dGeomID is a pointer to this object.
+
+struct dxGeom : public dBase {
+  int type;		// geom type number, set by subclass constructor
+  int gflags;		// flags used by geom and space
+  void *data;		// user-defined data pointer
+  dBodyID body;		// dynamics body associated with this object (if any)
+  dxGeom *body_next;	// next geom in body's linked list of associated geoms
+  dReal *pos;		// pointer to object's position vector
+  dReal *R;		// pointer to object's rotation matrix
+
+  // information used by spaces
+  dxGeom *next;		// next geom in linked list of geoms
+  dxGeom **tome;	// linked list backpointer
+  dxSpace *parent_space;// the space this geom is contained in, 0 if none
+  dReal aabb[6];	// cached AABB for this space
+  unsigned long category_bits,collide_bits;
+
+  dxGeom (dSpaceID _space, int is_placeable);
+  virtual ~dxGeom();
+
+  virtual void computeAABB()=0;
+  // compute the AABB for this object and put it in aabb. this function
+  // always performs a fresh computation, it does not inspect the
+  // GEOM_AABB_BAD flag.
+
+  virtual int AABBTest (dxGeom *o, dReal aabb[6]);
+  // test whether the given AABB object intersects with this object, return
+  // 1=yes, 0=no. this is used as an early-exit test in the space collision
+  // functions. the default implementation returns 1, which is the correct
+  // behavior if no more detailed implementation can be provided.
+
+  // utility functions
+
+  // compute the AABB only if it is not current. this function manipulates
+  // the GEOM_AABB_BAD flag.
+
+  void recomputeAABB() {
+    if (gflags & GEOM_AABB_BAD) {
+      computeAABB();
+      gflags &= ~GEOM_AABB_BAD;
+    }
+  }
+
+  // add and remove this geom from a linked list maintained by a space.
+
+  void spaceAdd (dxGeom **first_ptr) {
+    next = *first_ptr;
+    tome = first_ptr;
+    if (*first_ptr) (*first_ptr)->tome = &next;
+    *first_ptr = this;
+  }
+  void spaceRemove() {
+    if (next) next->tome = tome;
+    *tome = next;
+  }
+
+  // add and remove this geom from a linked list maintained by a body.
+
+  void bodyAdd (dxBody *b) {
+    body = b;
+    body_next = b->geom;
+    b->geom = this;
+  }
+  void bodyRemove();
+};
+
+//****************************************************************************
+// the base space class
+//
+// the contained geoms are divided into two kinds: clean and dirty.
+// the clean geoms have not moved since they were put in the list,
+// and their AABBs are valid. the dirty geoms have changed position, and
+// their AABBs are may not be valid. the two types are distinguished by the
+// GEOM_DIRTY flag. all dirty geoms come *before* all clean geoms in the list.
+
+struct dxSpace : public dxGeom {
+  int count;			// number of geoms in this space
+  dxGeom *first;		// first geom in list
+  int cleanup;			// cleanup mode, 1=destroy geoms on exit
+
+  // cached state for getGeom()
+  int current_index;		// only valid if current_geom != 0
+  dxGeom *current_geom;		// if 0 then there is no information
+
+  // locking stuff. the space is locked when it is currently traversing its
+  // internal data structures, e.g. in collide() and collide2(). operations
+  // that modify the contents of the space are not permitted when the space
+  // is locked.
+  int lock_count;
+
+  dxSpace (dSpaceID _space);
+  ~dxSpace();
+
+  void computeAABB();
+
+  void setCleanup (int mode);
+  int getCleanup();
+  int query (dxGeom *geom);
+  int getNumGeoms();
+  virtual dxGeom *getGeom (int i);
+
+  virtual void add (dxGeom *);
+  virtual void remove (dxGeom *);
+  virtual void dirty (dxGeom *);
+
+  virtual void cleanGeoms()=0;
+  // turn all dirty geoms into clean geoms by computing their AABBs and any
+  // other space data structures that are required. this should clear the
+  // GEOM_DIRTY and GEOM_AABB_BAD flags of all geoms.
+
+  virtual void collide (void *data, dNearCallback *callback)=0;
+  virtual void collide2 (void *data, dxGeom *geom, dNearCallback *callback)=0;
+};
+
+
+#endif

src/source/ode/collision_quadtreespace.cpp

@@ -0,0 +1,583 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// QuadTreeSpace by Erwin de Vries.
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/collision_space.h>
+#include <ode/collision.h>
+#include "collision_kernel.h"
+
+#include "collision_space_internal.h"
+
+
+#define AXIS0 0
+#define AXIS1 1
+#define UP 2
+
+//#define DRAWBLOCKS
+
+const int SPLITAXIS = 2;
+const int SPLITS = SPLITAXIS * SPLITAXIS;
+
+#define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED
+
+class Block{
+public:
+	dReal MinX, MaxX;
+	dReal MinZ, MaxZ;
+
+	dGeomID First;
+	int GeomCount;
+
+	Block* Parent;
+	Block* Children;
+
+	void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks);
+
+	void Collide(void* UserData, dNearCallback* Callback);
+	void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback);
+
+	void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback);
+	
+	void AddObject(dGeomID Object);
+	void DelObject(dGeomID Object);
+	void Traverse(dGeomID Object);
+
+	bool Inside(const dReal* AABB);
+	
+	Block* GetBlock(const dReal* AABB);
+	Block* GetBlockChild(const dReal* AABB);
+};
+
+
+#ifdef DRAWBLOCKS
+#include "..\..\Include\drawstuff\\drawstuff.h"
+
+static void DrawBlock(Block* Block){
+	dVector3 v[8];
+	v[0][AXIS0] = Block->MinX;
+	v[0][UP] = REAL(-1.0);
+	v[0][AXIS1] = Block->MinZ;
+	
+	v[1][AXIS0] = Block->MinX;
+	v[1][UP] = REAL(-1.0);
+	v[1][AXIS1] = Block->MaxZ;
+	
+	v[2][AXIS0] = Block->MaxX;
+	v[2][UP] = REAL(-1.0);
+	v[2][AXIS1] = Block->MinZ;
+	
+	v[3][AXIS0] = Block->MaxX;
+	v[3][UP] = REAL(-1.0);
+	v[3][AXIS1] = Block->MaxZ;
+	
+	v[4][AXIS0] = Block->MinX;
+	v[4][UP] = REAL(1.0);
+	v[4][AXIS1] = Block->MinZ;
+	
+	v[5][AXIS0] = Block->MinX;
+	v[5][UP] = REAL(1.0);
+	v[5][AXIS1] = Block->MaxZ;
+	
+	v[6][AXIS0] = Block->MaxX;
+	v[6][UP] = REAL(1.0);
+	v[6][AXIS1] = Block->MinZ;
+	
+	v[7][AXIS0] = Block->MaxX;
+	v[7][UP] = REAL(1.0);
+	v[7][AXIS1] = Block->MaxZ;
+	
+	// Bottom
+	dsDrawLine(v[0], v[1]);
+	dsDrawLine(v[1], v[3]);
+	dsDrawLine(v[3], v[2]);
+	dsDrawLine(v[2], v[0]);
+	
+	// Top
+	dsDrawLine(v[4], v[5]);
+	dsDrawLine(v[5], v[7]);
+	dsDrawLine(v[7], v[6]);
+	dsDrawLine(v[6], v[4]);
+	
+	// Sides
+	dsDrawLine(v[0], v[4]);
+	dsDrawLine(v[1], v[5]);
+	dsDrawLine(v[2], v[6]);
+	dsDrawLine(v[3], v[7]);
+}
+#endif	//DRAWBLOCKS
+
+
+void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){
+	GeomCount = 0;
+	First = 0;
+
+	MinX = Center[AXIS0] - Extents[AXIS0];
+	MaxX = Center[AXIS0] + Extents[AXIS0];
+
+	MinZ = Center[AXIS1] - Extents[AXIS1];
+	MaxZ = Center[AXIS1] + Extents[AXIS1];
+
+	this->Parent = Parent;
+	if (Depth > 0){
+		Children = Blocks;
+		Blocks += SPLITS;
+
+		dVector3 ChildExtents;
+		ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS;
+		ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS;
+		ChildExtents[UP] = Extents[UP];
+
+		for (int i = 0; i < SPLITAXIS; i++){
+			for (int j = 0; j < SPLITAXIS; j++){
+				int Index = i * SPLITAXIS + j;
+
+				dVector3 ChildCenter;
+				ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2);
+				ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2);
+				ChildCenter[UP] = Center[UP];
+				
+				Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks);
+			}
+		}
+	}
+	else Children = 0;
+}
+
+void Block::Collide(void* UserData, dNearCallback* Callback){
+#ifdef DRAWBLOCKS
+	DrawBlock(this);
+#endif
+	// Collide the local list
+	dxGeom* g = First;
+	while (g){
+		if (GEOM_ENABLED(g)){
+			Collide(g, g->next, UserData, Callback);
+		}
+		g = g->next;
+	}
+
+	// Recurse for children
+	if (Children){
+		for (int i = 0; i < SPLITS; i++){
+			if (Children[i].GeomCount <= 1){	// Early out
+				continue;
+			}
+			Children[i].Collide(UserData, Callback);
+		}
+	}
+}
+
+void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
+#ifdef DRAWBLOCKS
+	DrawBlock(this);
+#endif
+	// Collide against local list
+	while (g2){
+		if (GEOM_ENABLED(g2)){
+			collideAABBs (g1, g2, UserData, Callback);
+		}
+		g2 = g2->next;
+	}
+
+	// Collide against children
+	if (Children){
+		for (int i = 0; i < SPLITS; i++){
+			// Early out for empty blocks
+			if (Children[i].GeomCount == 0){
+				continue;
+			}
+
+			// Does the geom's AABB collide with the block?
+			// Dont do AABB tests for single geom blocks.
+			if (Children[i].GeomCount == 1 && Children[i].First){
+				//
+			}
+			else if (true){
+				if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX ||
+					g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX ||
+					g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ ||
+					g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue;
+			}
+			Children[i].Collide(g1, Children[i].First, UserData, Callback);
+		}
+	}
+}
+
+void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){
+	// Collide against local list
+	dxGeom* g2 = First;
+	while (g2){
+		if (GEOM_ENABLED(g2)){
+			collideAABBs (g1, g2, UserData, Callback);
+		}
+		g2 = g2->next;
+	}
+}
+
+void Block::AddObject(dGeomID Object){
+	// Add the geom
+	Object->next = First;
+	First = Object;
+	Object->tome = (dxGeom**)this;
+
+	// Now traverse upwards to tell that we have a geom
+	Block* Block = this;
+	do{
+		Block->GeomCount++;
+		Block = Block->Parent;
+	}
+	while (Block);
+}
+
+void Block::DelObject(dGeomID Object){
+	// Del the geom
+	dxGeom* g = First;
+	dxGeom* Last = 0;
+	while (g){
+		if (g == Object){
+			if (Last){
+				Last->next = g->next;
+			}
+			else First = g->next;
+
+			break;
+		}
+		Last = g;
+		g = g->next;
+	}
+
+	Object->tome = 0;
+
+	// Now traverse upwards to tell that we have lost a geom
+	Block* Block = this;
+	do{
+		Block->GeomCount--;
+		Block = Block->Parent;
+	}
+	while (Block);
+}
+
+void Block::Traverse(dGeomID Object){
+	Block* NewBlock = GetBlock(Object->aabb);
+
+	if (NewBlock != this){
+		// Remove the geom from the old block and add it to the new block.
+		// This could be more optimal, but the loss should be very small.
+		DelObject(Object);
+		NewBlock->AddObject(Object);
+	}
+}
+
+bool Block::Inside(const dReal* AABB){
+	return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ;
+}
+
+Block* Block::GetBlock(const dReal* AABB){
+	if (Inside(AABB)){
+		return GetBlockChild(AABB);	// Child or this will have a good block
+	}
+	else if (Parent){
+		return Parent->GetBlock(AABB);	// Parent has a good block
+	}
+	else return this;	// We are at the root, so we have little choice
+}
+
+Block* Block::GetBlockChild(const dReal* AABB){
+	if (Children){
+		for (int i = 0; i < SPLITS; i++){
+			if (Children[i].Inside(AABB)){
+				return Children[i].GetBlockChild(AABB);	// Child will have good block
+			}
+		}
+	}
+	return this;	// This is the best block
+}
+
+//****************************************************************************
+// quadtree space
+
+struct dxQuadTreeSpace : public dxSpace{
+	Block* Blocks;	// Blocks[0] is the root
+
+	dArray<dxGeom*> DirtyList;
+
+	dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth);
+	~dxQuadTreeSpace();
+
+	dxGeom* getGeom(int i);
+	
+	void add(dxGeom* g);
+	void remove(dxGeom* g);
+	void dirty(dxGeom* g);
+
+	void computeAABB();
+	
+	void cleanGeoms();
+	void collide(void* UserData, dNearCallback* Callback);
+	void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);
+
+	// Temp data
+	Block* CurrentBlock;	// Only used while enumerating
+	int* CurrentChild;	// Only used while enumerating
+	int CurrentLevel;	// Only used while enumerating
+	dxGeom* CurrentObject;	// Only used while enumerating
+	int CurrentIndex;
+};
+
+dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){
+	type = dQuadTreeSpaceClass;
+
+	int BlockCount = 0;
+	for (int i = 0; i <= Depth; i++){
+		BlockCount += (int)pow((double)SPLITS, (double)i);
+	}
+
+	Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
+	Block* Blocks = this->Blocks + 1;	// This pointer gets modified!
+
+	this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks);
+
+	CurrentBlock = 0;
+	CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
+	CurrentLevel = 0;
+	CurrentObject = 0;
+	CurrentIndex = -1;
+
+	// Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
+	aabb[0] = -dInfinity;
+	aabb[1] = dInfinity;
+	aabb[2] = -dInfinity;
+	aabb[3] = dInfinity;
+	aabb[4] = -dInfinity;
+	aabb[5] = dInfinity;
+}
+
+dxQuadTreeSpace::~dxQuadTreeSpace(){
+	int Depth = 0;
+	Block* Current = &Blocks[0];
+	while (Current){
+		Depth++;
+		Current = Current->Children;
+	}
+
+	int BlockCount = 0;
+	for (int i = 0; i < Depth; i++){
+		BlockCount += (int)pow((double)SPLITS, (double)i);
+	}
+
+	dFree(Blocks, BlockCount * sizeof(Block));
+	dFree(CurrentChild, (Depth + 1) * sizeof(int));
+}
+
+dxGeom* dxQuadTreeSpace::getGeom(int Index){
+	dUASSERT(Index >= 0 && Index < count, "index out of range");
+
+	//@@@
+	dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
+
+	return 0;
+
+	// This doesnt work
+
+	/*if (CurrentIndex == Index){
+		// Loop through all objects in the local list
+CHILDRECURSE:
+		if (CurrentObject){
+			dGeomID g = CurrentObject;
+			CurrentObject = CurrentObject->next;
+			CurrentIndex++;
+		
+#ifdef DRAWBLOCKS
+			DrawBlock(CurrentBlock);
+#endif	//DRAWBLOCKS
+			return g;
+		}
+		else{
+			// Now lets loop through our children. Starting at index 0.
+			if (CurrentBlock->Children){
+				CurrentChild[CurrentLevel] = 0;
+PARENTRECURSE:
+				for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
+					if (CurrentBlock->Children[i].GeomCount == 0){
+						continue;
+					}
+					CurrentBlock = &CurrentBlock->Children[i];
+					CurrentObject = CurrentBlock->First;
+				
+					i++;
+				
+					CurrentLevel++;
+					goto CHILDRECURSE;
+				}
+			}
+		}
+		
+		// Now lets go back to the parent so it can continue processing its other children.
+		if (CurrentBlock->Parent){
+			CurrentBlock = CurrentBlock->Parent;
+			CurrentLevel--;
+			goto PARENTRECURSE;
+		}
+	}
+	else{
+		CurrentBlock = &Blocks[0];
+		CurrentLevel = 0;
+		CurrentObject = CurrentObject;
+		CurrentIndex = 0;
+
+		// Other states are already set
+		CurrentObject = CurrentBlock->First;
+	}
+
+
+	if (current_geom && current_index == Index - 1){
+		//current_geom = current_geom->next; // next
+		current_index = Index;
+		return current_geom;
+	}
+	else for (int i = 0; i < Index; i++){	// this will be verrrrrrry slow
+		getGeom(i);
+	}*/
+
+	return 0;
+}
+
+void dxQuadTreeSpace::add(dxGeom* g){
+	CHECK_NOT_LOCKED (this);
+	dAASSERT(g);
+	dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space");
+
+	g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
+	DirtyList.push(g);
+
+	// add
+	g->parent_space = this;
+	Blocks[0].GetBlock(g->aabb)->AddObject(g);	// Add to best block
+	count++;
+	
+	// enumerator has been invalidated
+	current_geom = 0;
+	
+	dGeomMoved(this);
+}
+
+void dxQuadTreeSpace::remove(dxGeom* g){
+	CHECK_NOT_LOCKED(this);
+	dAASSERT(g);
+	dUASSERT(g->parent_space == this,"object is not in this space");
+	
+	// remove
+	((Block*)g->tome)->DelObject(g);
+	count--;
+
+	for (int i = 0; i < DirtyList.size(); i++){
+		if (DirtyList[i] == g){
+			DirtyList.remove(i);
+			break;
+		}
+	}
+	
+	// safeguard
+	g->next = 0;
+	g->tome = 0;
+	g->parent_space = 0;
+	
+	// enumerator has been invalidated
+	current_geom = 0;
+	
+	// the bounding box of this space (and that of all the parents) may have
+	// changed as a consequence of the removal.
+	dGeomMoved(this);
+}
+
+void dxQuadTreeSpace::dirty(dxGeom* g){
+	DirtyList.push(g);
+}
+
+void dxQuadTreeSpace::computeAABB(){
+	//
+}
+
+void dxQuadTreeSpace::cleanGeoms(){
+	// compute the AABBs of all dirty geoms, and clear the dirty flags
+	lock_count++;
+	
+	for (int i = 0; i < DirtyList.size(); i++){
+		dxGeom* g = DirtyList[i];
+		if (IS_SPACE(g)){
+			((dxSpace*)g)->cleanGeoms();
+		}
+		g->recomputeAABB();
+		g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
+
+		((Block*)g->tome)->Traverse(g);
+	}
+	DirtyList.setSize(0);
+
+	lock_count--;
+}
+
+void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
+  dAASSERT(Callback);
+
+  lock_count++;
+  cleanGeoms();
+
+  Blocks[0].Collide(UserData, Callback);
+
+  lock_count--;
+}
+
+void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){
+  dAASSERT(g1 && Callback);
+
+  lock_count++;
+  cleanGeoms();
+  g1->recomputeAABB();
+
+  if (g1->parent_space == this){
+	  // The block the geom is in
+	  Block* CurrentBlock = (Block*)g1->tome;
+	  
+	  // Collide against block and its children
+	  CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback);
+	  
+	  // Collide against parents
+	  while (true){
+		  CurrentBlock = CurrentBlock->Parent;
+		  if (!CurrentBlock){
+			  break;
+		  }
+		  CurrentBlock->CollideLocal(g1, UserData, Callback);
+	  }
+  }
+  else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback);
+
+  lock_count--;
+}
+
+dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){
+	return new dxQuadTreeSpace(space, Center, Extents, Depth);
+}

src/source/ode/collision_space.cpp

@@ -0,0 +1,785 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+spaces
+
+*/
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+#include <ode/collision_space.h>
+#include <ode/collision.h>
+#include "collision_kernel.h"
+
+#include "collision_space_internal.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+//****************************************************************************
+// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags
+// and moving it to the front of the space's list. all the parents of a
+// dirty geom also become dirty.
+
+void dGeomMoved (dxGeom *geom)
+{
+  dAASSERT (geom);
+  
+  // from the bottom of the space heirarchy up, process all clean geoms
+  // turning them into dirty geoms.
+  dxSpace *parent = geom->parent_space;
+
+  while (parent && (geom->gflags & GEOM_DIRTY)==0) {
+    CHECK_NOT_LOCKED (parent);
+    geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
+    parent->dirty (geom);
+    geom = parent;
+    parent = parent->parent_space;
+  }
+
+  // all the remaining dirty geoms must have their AABB_BAD flags set, to
+  // ensure that their AABBs get recomputed
+  while (geom) {
+    geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
+    CHECK_NOT_LOCKED (geom->parent_space);
+    geom = geom->parent_space;
+  }
+}
+
+#define GEOM_ENABLED(g) ((g)->gflags & GEOM_ENABLED)
+
+//****************************************************************************
+// dxSpace
+
+dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0)
+{
+  count = 0;
+  first = 0;
+  cleanup = 1;
+  current_index = 0;
+  current_geom = 0;
+  lock_count = 0;
+}
+
+
+dxSpace::~dxSpace()
+{
+  CHECK_NOT_LOCKED (this);
+  if (cleanup) {
+    // note that destroying each geom will call remove()
+    dxGeom *g,*n;
+    for (g = first; g; g=n) {
+      n = g->next;
+      dGeomDestroy (g);
+    }
+  }
+  else {
+    dxGeom *g,*n;
+    for (g = first; g; g=n) {
+      n = g->next;
+      remove (g);
+    }
+  }
+}
+
+
+void dxSpace::computeAABB()
+{
+  if (first) {
+    int i;
+    dReal a[6];
+    a[0] = dInfinity;
+    a[1] = -dInfinity;
+    a[2] = dInfinity;
+    a[3] = -dInfinity;
+    a[4] = dInfinity;
+    a[5] = -dInfinity;
+    for (dxGeom *g=first; g; g=g->next) {
+      g->recomputeAABB();
+      for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i];
+      for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i];
+    }
+    memcpy(aabb,a,6*sizeof(dReal));
+  }
+  else {
+    dSetZero (aabb,6);
+  }
+}
+
+
+void dxSpace::setCleanup (int mode)
+{
+  cleanup = (mode != 0);
+}
+
+
+int dxSpace::getCleanup()
+{
+  return cleanup;
+}
+
+
+int dxSpace::query (dxGeom *geom)
+{
+  dAASSERT (geom);
+  return (geom->parent_space == this);
+}
+
+
+int dxSpace::getNumGeoms()
+{
+  return count;
+}
+
+
+// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1
+
+dxGeom *dxSpace::getGeom (int i)
+{
+  dUASSERT (i >= 0 && i < count,"index out of range");
+  if (current_geom && current_index == i-1) {
+    current_geom = current_geom->next;
+    current_index = i;
+    return current_geom;
+  }
+  else {
+    dxGeom *g=first;
+    for (int j=0; j<i; j++) {
+      if (g) g = g->next; else return 0;
+    }
+    current_geom = g;
+    current_index = i;
+    return g;
+  }
+}
+
+
+void dxSpace::add (dxGeom *geom)
+{
+  CHECK_NOT_LOCKED (this);
+  dAASSERT (geom);
+  dUASSERT (geom->parent_space == 0 && geom->next == 0,
+	    "geom is already in a space");
+
+  // add
+  geom->parent_space = this;
+  geom->spaceAdd (&first);
+  count++;
+
+  // enumerator has been invalidated
+  current_geom = 0;
+
+  // new geoms are added to the front of the list and are always
+  // considered to be dirty. as a consequence, this space and all its
+  // parents are dirty too.
+  geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
+  dGeomMoved (this);
+}
+
+
+void dxSpace::remove (dxGeom *geom)
+{
+  CHECK_NOT_LOCKED (this);
+  dAASSERT (geom);
+  dUASSERT (geom->parent_space == this,"object is not in this space");
+
+  // remove
+  geom->spaceRemove();
+  count--;
+
+  // safeguard
+  geom->next = 0;
+  geom->tome = 0;
+  geom->parent_space = 0;
+
+  // enumerator has been invalidated
+  current_geom = 0;
+
+  // the bounding box of this space (and that of all the parents) may have
+  // changed as a consequence of the removal.
+  dGeomMoved (this);
+}
+
+
+void dxSpace::dirty (dxGeom *geom)
+{
+  geom->spaceRemove();
+  geom->spaceAdd (&first);
+}
+
+//****************************************************************************
+// simple space - reports all n^2 object intersections
+
+struct dxSimpleSpace : public dxSpace {
+  dxSimpleSpace (dSpaceID _space);
+  void cleanGeoms();
+  void collide (void *data, dNearCallback *callback);
+  void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
+};
+
+
+dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space)
+{
+  type = dSimpleSpaceClass;
+}
+
+
+void dxSimpleSpace::cleanGeoms()
+{
+  // compute the AABBs of all dirty geoms, and clear the dirty flags
+  lock_count++;
+  for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
+    if (IS_SPACE(g)) {
+      ((dxSpace*)g)->cleanGeoms();
+    }
+    g->recomputeAABB();
+    g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
+  }
+  lock_count--;
+}
+
+
+void dxSimpleSpace::collide (void *data, dNearCallback *callback)
+{
+  dAASSERT (callback);
+
+  lock_count++;
+  cleanGeoms();
+
+  // intersect all bounding boxes
+  for (dxGeom *g1=first; g1; g1=g1->next) {
+    if (GEOM_ENABLED(g1)){
+      for (dxGeom *g2=g1->next; g2; g2=g2->next) {
+	if (GEOM_ENABLED(g2)){
+	  collideAABBs (g1,g2,data,callback);
+	}
+      }
+    }
+  }
+
+  lock_count--;
+}
+
+
+void dxSimpleSpace::collide2 (void *data, dxGeom *geom,
+			      dNearCallback *callback)
+{
+  dAASSERT (geom && callback);
+
+  lock_count++;
+  cleanGeoms();
+  geom->recomputeAABB();
+
+  // intersect bounding boxes
+  for (dxGeom *g=first; g; g=g->next) {
+    if (GEOM_ENABLED(g)){
+      collideAABBs (g,geom,data,callback);
+    }
+  }
+
+  lock_count--;
+}
+
+//****************************************************************************
+// utility stuff for hash table space
+
+// kind of silly, but oh well...
+#ifndef MAXINT
+#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1))
+#endif
+
+
+// prime[i] is the largest prime smaller than 2^i
+#define NUM_PRIMES 31
+static long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L,
+  1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L,
+  524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L,
+  67108859L,134217689L,268435399L,536870909L,1073741789L};
+
+
+// an axis aligned bounding box in the hash table
+struct dxAABB {
+  dxAABB *next;		// next in the list of all AABBs
+  int level;		// the level this is stored in (cell size = 2^level)
+  int dbounds[6];	// AABB bounds, discretized to cell size
+  dxGeom *geom;		// corresponding geometry object (AABB stored here)
+  int index;		// index of this AABB, starting from 0
+};
+
+
+// a hash table node that represents an AABB that intersects a particular cell
+// at a particular level
+struct Node {
+  Node *next;		// next node in hash table collision list, 0 if none
+  int x,y,z;		// cell position in space, discretized to cell size
+  dxAABB *aabb;		// axis aligned bounding box that intersects this cell
+};
+
+
+// return the `level' of an AABB. the AABB will be put into cells at this
+// level - the cell size will be 2^level. the level is chosen to be the
+// smallest value such that the AABB occupies no more than 8 cells, regardless
+// of its placement. this means that:
+//	size/2 < q <= size
+// where q is the maximum AABB dimension.
+
+static int findLevel (dReal bounds[6])
+{
+  if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity ||
+      bounds[2] <= -dInfinity || bounds[3] >= dInfinity ||
+      bounds[4] <= -dInfinity || bounds[5] >= dInfinity) {
+    return MAXINT;
+  }
+
+  // compute q
+  dReal q,q2;
+  q = bounds[1] - bounds[0];	// x bounds
+  q2 = bounds[3] - bounds[2];	// y bounds
+  if (q2 > q) q = q2;
+  q2 = bounds[5] - bounds[4];	// z bounds
+  if (q2 > q) q = q2;
+
+  // find level such that 0.5 * 2^level < q <= 2^level
+  int level;
+  frexp (q,&level);	// q = (0.5 .. 1.0) * 2^level (definition of frexp)
+  return level;
+}
+
+
+// find a virtual memory address for a cell at the given level and x,y,z
+// position.
+// @@@ currently this is not very sophisticated, e.g. the scaling
+// factors could be better designed to avoid collisions, and they should
+// probably depend on the hash table physical size.
+
+static unsigned long getVirtualAddress (int level, int x, int y, int z)
+{
+  return level*1000 + x*100 + y*10 + z;
+}
+
+//****************************************************************************
+// hash space
+
+struct dxHashSpace : public dxSpace {
+  int global_minlevel;	// smallest hash table level to put AABBs in
+  int global_maxlevel;	// objects that need a level larger than this will be
+			// put in a "big objects" list instead of a hash table
+
+  dxHashSpace (dSpaceID _space);
+  void setLevels (int minlevel, int maxlevel);
+  void getLevels (int *minlevel, int *maxlevel);
+  void cleanGeoms();
+  void collide (void *data, dNearCallback *callback);
+  void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
+};
+
+
+dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space)
+{
+  type = dHashSpaceClass;
+  global_minlevel = -3;
+  global_maxlevel = 10;
+}
+
+
+void dxHashSpace::setLevels (int minlevel, int maxlevel)
+{
+  dAASSERT (minlevel <= maxlevel);
+  global_minlevel = minlevel;
+  global_maxlevel = maxlevel;
+}
+
+
+void dxHashSpace::getLevels (int *minlevel, int *maxlevel)
+{
+  if (minlevel) *minlevel = global_minlevel;
+  if (maxlevel) *maxlevel = global_maxlevel;
+}
+
+
+void dxHashSpace::cleanGeoms()
+{
+  // compute the AABBs of all dirty geoms, and clear the dirty flags
+  lock_count++;
+  for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
+    if (IS_SPACE(g)) {
+      ((dxSpace*)g)->cleanGeoms();
+    }
+    g->recomputeAABB();
+    g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
+  }
+  lock_count--;
+}
+
+
+void dxHashSpace::collide (void *data, dNearCallback *callback)
+{
+  dAASSERT(this && callback);
+  dxGeom *geom;
+  dxAABB *aabb;
+  int i,maxlevel;
+
+  // 0 or 1 geoms can't collide with anything
+  if (count < 2) return;
+
+  lock_count++;
+  cleanGeoms();
+
+  // create a list of auxiliary information for all geom axis aligned bounding
+  // boxes. set the level for all AABBs. put AABBs larger than the space's
+  // global_maxlevel in the big_boxes list, check everything else against
+  // that list at the end. for AABBs that are not too big, record the maximum
+  // level that we need.
+
+  int n = 0;			// number of AABBs in main list
+  dxAABB *first_aabb = 0;	// list of AABBs in hash table
+  dxAABB *big_boxes = 0;	// list of AABBs too big for hash table
+  maxlevel = global_minlevel - 1;
+  for (geom = first; geom; geom=geom->next) {
+    if (!GEOM_ENABLED(geom)){
+      continue;
+    }
+    dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB));
+    aabb->geom = geom;
+    // compute level, but prevent cells from getting too small
+    int level = findLevel (geom->aabb);
+    if (level < global_minlevel) level = global_minlevel;
+    if (level <= global_maxlevel) {
+      // aabb goes in main list
+      aabb->next = first_aabb;
+      first_aabb = aabb;
+      aabb->level = level;
+      if (level > maxlevel) maxlevel = level;
+      // cellsize = 2^level
+      dReal cellsize = (dReal) ldexp (1.0,level);
+      // discretize AABB position to cell size
+      for (i=0; i < 6; i++) aabb->dbounds[i] = (int)
+			      floor (geom->aabb[i]/cellsize);
+      // set AABB index
+      aabb->index = n;
+      n++;
+    }
+    else {
+      // aabb is too big, put it in the big_boxes list. we don't care about
+      // setting level, dbounds, index, or the maxlevel
+      aabb->next = big_boxes;
+      big_boxes = aabb;
+    }
+  }
+
+  // for `n' objects, an n*n array of bits is used to record if those objects
+  // have been intersection-tested against each other yet. this array can
+  // grow large with high n, but oh well...
+  int tested_rowsize = (n+7) >> 3;	// number of bytes needed for n bits
+  unsigned char *tested = (unsigned char *) alloca (n * tested_rowsize);
+  memset (tested,0,n * tested_rowsize);
+
+  // create a hash table to store all AABBs. each AABB may take up to 8 cells.
+  // we use chaining to resolve collisions, but we use a relatively large table
+  // to reduce the chance of collisions.
+
+  // compute hash table size sz to be a prime > 8*n
+  for (i=0; i<NUM_PRIMES; i++) {
+    if (prime[i] >= (8*n)) break;
+  }
+  if (i >= NUM_PRIMES) i = NUM_PRIMES-1;	// probably pointless
+  int sz = prime[i];
+
+  // allocate and initialize hash table node pointers
+  Node **table = (Node **) ALLOCA (sizeof(Node*) * sz);
+  for (i=0; i<sz; i++) table[i] = 0;
+
+  // add each AABB to the hash table (may need to add it to up to 8 cells)
+  for (aabb=first_aabb; aabb; aabb=aabb->next) {
+    int *dbounds = aabb->dbounds;
+    for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) {
+      for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) {
+	for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) {
+	  // get the hash index
+	  unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz;
+	  // add a new node to the hash table
+	  Node *node = (Node*) alloca (sizeof (Node));
+	  node->x = xi;
+	  node->y = yi;
+	  node->z = zi;
+	  node->aabb = aabb;
+	  node->next = table[hi];
+	  table[hi] = node;
+	}
+      }
+    }
+  }
+
+  // now that all AABBs are loaded into the hash table, we do the actual
+  // collision detection. for all AABBs, check for other AABBs in the
+  // same cells for collisions, and then check for other AABBs in all
+  // intersecting higher level cells.
+
+  int db[6];			// discrete bounds at current level
+  for (aabb=first_aabb; aabb; aabb=aabb->next) {
+    // we are searching for collisions with aabb
+    for (i=0; i<6; i++) db[i] = aabb->dbounds[i];
+    for (int level = aabb->level; level <= maxlevel; level++) {
+      for (int xi = db[0]; xi <= db[1]; xi++) {
+	for (int yi = db[2]; yi <= db[3]; yi++) {
+	  for (int zi = db[4]; zi <= db[5]; zi++) {
+	    // get the hash index
+	    unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz;
+	    // search all nodes at this index
+	    Node *node;
+	    for (node = table[hi]; node; node=node->next) {
+	      // node points to an AABB that may intersect aabb
+	      if (node->aabb == aabb) continue;
+	      if (node->aabb->level == level &&
+		  node->x == xi && node->y == yi && node->z == zi) {
+		// see if aabb and node->aabb have already been tested
+		// against each other
+		unsigned char mask;
+		if (aabb->index <= node->aabb->index) {
+		  i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3);
+		  mask = 1 << (node->aabb->index & 7);
+		}
+		else {
+		  i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3);
+		  mask = 1 << (aabb->index & 7);
+		}
+		dIASSERT (i >= 0 && i < (tested_rowsize*n));
+		if ((tested[i] & mask)==0) {
+		  collideAABBs (aabb->geom,node->aabb->geom,data,callback);
+		}
+		tested[i] |= mask;
+	      }
+	    }
+	  }
+	}
+      }
+      // get the discrete bounds for the next level up
+      for (i=0; i<6; i++) db[i] >>= 1;
+    }
+  }
+
+  // every AABB in the normal list must now be intersected against every
+  // AABB in the big_boxes list. so let's hope there are not too many objects
+  // in the big_boxes list.
+  for (aabb=first_aabb; aabb; aabb=aabb->next) {
+    for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) {
+      collideAABBs (aabb->geom,aabb2->geom,data,callback);
+    }
+  }
+
+  // intersected all AABBs in the big_boxes list together
+  for (aabb=big_boxes; aabb; aabb=aabb->next) {
+    for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) {
+      collideAABBs (aabb->geom,aabb2->geom,data,callback);
+    }
+  }
+
+  lock_count--;
+}
+
+
+void dxHashSpace::collide2 (void *data, dxGeom *geom,
+			    dNearCallback *callback)
+{
+  dAASSERT (geom && callback);
+  
+  // this could take advantage of the hash structure to avoid
+  // O(n2) complexity, but it does not yet.
+  
+  lock_count++;
+  cleanGeoms();
+  geom->recomputeAABB();
+  
+  // intersect bounding boxes
+  for (dxGeom *g=first; g; g=g->next) {
+    collideAABBs (g,geom,data,callback);
+  }
+  
+  lock_count--;
+}
+
+//****************************************************************************
+// space functions
+
+dxSpace *dSimpleSpaceCreate (dxSpace *space)
+{
+  return new dxSimpleSpace (space);
+}
+
+
+dxSpace *dHashSpaceCreate (dxSpace *space)
+{
+  return new dxHashSpace (space);
+}
+
+
+void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel)
+{
+  dAASSERT (space);
+  dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel");
+  dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
+  dxHashSpace *hspace = (dxHashSpace*) space;
+  hspace->setLevels (minlevel,maxlevel);
+}
+
+
+void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel)
+{
+  dAASSERT (space);
+  dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
+  dxHashSpace *hspace = (dxHashSpace*) space;
+  hspace->getLevels (minlevel,maxlevel);
+}
+
+
+void dSpaceDestroy (dxSpace *space)
+{
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  dGeomDestroy (space);
+}
+
+
+void dSpaceSetCleanup (dxSpace *space, int mode)
+{
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  space->setCleanup (mode);
+}
+
+
+int dSpaceGetCleanup (dxSpace *space)
+{
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  return space->getCleanup();
+}
+
+
+void dSpaceAdd (dxSpace *space, dxGeom *g)
+{
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  CHECK_NOT_LOCKED (space);
+  space->add (g);
+}
+
+
+void dSpaceRemove (dxSpace *space, dxGeom *g)
+{
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  CHECK_NOT_LOCKED (space);
+  space->remove (g);
+}
+
+
+int dSpaceQuery (dxSpace *space, dxGeom *g)
+{
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  return space->query (g);
+}
+
+void dSpaceClean (dxSpace *space){
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+
+  space->cleanGeoms();
+}
+
+int dSpaceGetNumGeoms (dxSpace *space)
+{
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  return space->getNumGeoms();
+}
+
+
+dGeomID dSpaceGetGeom (dxSpace *space, int i)
+{
+  dAASSERT (space);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  return space->getGeom (i);
+}
+
+
+void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback)
+{
+  dAASSERT (space && callback);
+  dUASSERT (dGeomIsSpace(space),"argument not a space");
+  space->collide (data,callback);
+}
+
+
+void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data,
+		     dNearCallback *callback)
+{
+  dAASSERT (g1 && g2 && callback);
+  dxSpace *s1,*s2;
+
+  // see if either geom is a space
+  if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0;
+  if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0;
+
+  // handle the four space/geom cases
+  if (s1) {
+    if (s2) {
+      // g1 and g2 are spaces.
+      if (s1==s2) {
+	// collide a space with itself --> interior collision
+	s1->collide (data,callback);
+      }
+      else {
+	// iterate through the space that has the fewest geoms, calling
+	// collide2 in the other space for each one.
+	if (s1->count < s2->count) {
+	  for (dxGeom *g = s1->first; g; g=g->next) {
+	    s2->collide2 (data,g,callback);
+	  }
+	}
+	else {
+	  for (dxGeom *g = s2->first; g; g=g->next) {
+	    s1->collide2 (data,g,callback);
+	  }
+	}
+      }
+    }
+    else {
+      // g1 is a space, g2 is a geom
+      s1->collide2 (data,g2,callback);
+    }
+  }
+  else {
+    if (s2) {
+      // g1 is a geom, g2 is a space
+      s2->collide2 (data,g1,callback);
+    }
+    else {
+      // g1 and g2 are geoms, call the callback directly
+      callback (data,g1,g2);
+    }
+  }
+}

src/source/ode/collision_space_internal.h

@@ -0,0 +1,84 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+stuff common to all spaces
+
+*/
+
+#ifndef _ODE_COLLISION_SPACE_INTERNAL_H_
+#define _ODE_COLLISION_SPACE_INTERNAL_H_
+
+#define ALLOCA(x) dALLOCA16(x)
+
+#define CHECK_NOT_LOCKED(space) \
+  dUASSERT ((space)==0 || (space)->lock_count==0, \
+	    "invalid operation for locked space");
+
+
+// collide two geoms together. for the hash table space, this is
+// called if the two AABBs inhabit the same hash table cells.
+// this only calls the callback function if the AABBs actually
+// intersect. if a geom has an AABB test function, that is called to
+// provide a further refinement of the intersection.
+//
+// NOTE: this assumes that the geom AABBs are valid on entry
+// and that both geoms are enabled.
+
+static void collideAABBs (dxGeom *g1, dxGeom *g2,
+			  void *data, dNearCallback *callback)
+{
+  dIASSERT((g1->gflags & GEOM_AABB_BAD)==0);
+  dIASSERT((g2->gflags & GEOM_AABB_BAD)==0);
+
+  // no contacts if both geoms on the same body, and the body is not 0
+  if (g1->body == g2->body && g1->body) return;
+
+  // test if the category and collide bitfields match
+  if ( ((g1->category_bits & g2->collide_bits) ||
+	(g2->category_bits & g1->collide_bits)) == 0) {
+    return;
+  }
+
+  // if the bounding boxes are disjoint then don't do anything
+  dReal *bounds1 = g1->aabb;
+  dReal *bounds2 = g2->aabb;
+  if (bounds1[0] > bounds2[1] ||
+      bounds1[1] < bounds2[0] ||
+      bounds1[2] > bounds2[3] ||
+      bounds1[3] < bounds2[2] ||
+      bounds1[4] > bounds2[5] ||
+      bounds1[5] < bounds2[4]) {
+    return;
+  }
+
+  // check if either object is able to prove that it doesn't intersect the
+  // AABB of the other
+  if (g1->AABBTest (g2,bounds2) == 0) return;
+  if (g2->AABBTest (g1,bounds1) == 0) return;
+
+  // the objects might actually intersect - call the space callback function
+  callback (data,g1,g2);
+}
+
+#endif

src/source/ode/collision_std.h

@@ -0,0 +1,68 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+the standard ODE geometry primitives.
+
+*/
+
+#ifndef _ODE_COLLISION_STD_H_
+#define _ODE_COLLISION_STD_H_
+
+#include <ode/common.h>
+#include "collision_kernel.h"
+
+
+// primitive collision functions - these have the dColliderFn interface, i.e.
+// the same interface as dCollide(). the first and second geom arguments must
+// have the specified types.
+
+int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags,
+			  dContactGeom *contact, int skip);
+int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
+		       dContactGeom *contact, int skip);
+int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
+			 dContactGeom *contact, int skip);
+int dCollideBoxBox (dxGeom *o1, dxGeom *o2, int flags,
+		    dContactGeom *contact, int skip);
+int dCollideBoxPlane (dxGeom *o1, dxGeom *o2,
+		      int flags, dContactGeom *contact, int skip);
+int dCollideCCylinderSphere (dxGeom *o1, dxGeom *o2, int flags,
+			     dContactGeom *contact, int skip);
+int dCollideCCylinderBox (dxGeom *o1, dxGeom *o2, int flags,
+			  dContactGeom *contact, int skip);
+int dCollideCCylinderCCylinder (dxGeom *o1, dxGeom *o2,
+				int flags, dContactGeom *contact, int skip);
+int dCollideCCylinderPlane (dxGeom *o1, dxGeom *o2, int flags,
+			    dContactGeom *contact, int skip);
+int dCollideRaySphere (dxGeom *o1, dxGeom *o2, int flags,
+		       dContactGeom *contact, int skip);
+int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
+		    dContactGeom *contact, int skip);
+int dCollideRayCCylinder (dxGeom *o1, dxGeom *o2,
+			  int flags, dContactGeom *contact, int skip);
+int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
+		      dContactGeom *contact, int skip);
+
+
+#endif

src/source/ode/collision_transform.cpp

@@ -0,0 +1,231 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+geom transform
+
+*/
+
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_transform.h"
+#include "collision_util.h"
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+//****************************************************************************
+// dxGeomTransform class
+
+struct dxGeomTransform : public dxGeom {
+  dxGeom *obj;		// object that is being transformed
+  int cleanup;		// 1 to destroy obj when destroyed
+  int infomode;		// 1 to put Tx geom in dContactGeom g1
+
+  // cached final object transform (body tx + relative tx). this is set by
+  // computeAABB(), and it is valid while the AABB is valid.
+  dVector3 final_pos;
+  dMatrix3 final_R;
+
+  dxGeomTransform (dSpaceID space);
+  ~dxGeomTransform();
+  void computeAABB();
+  void computeFinalTx();
+};
+
+
+dxGeomTransform::dxGeomTransform (dSpaceID space) : dxGeom (space,1)
+{
+  type = dGeomTransformClass;
+  obj = 0;
+  cleanup = 0;
+  infomode = 0;
+  dSetZero (final_pos,4);
+  dRSetIdentity (final_R);
+}
+
+
+dxGeomTransform::~dxGeomTransform()
+{
+  if (obj && cleanup) delete obj;
+}
+
+
+void dxGeomTransform::computeAABB()
+{
+  if (!obj) {
+    dSetZero (aabb,6);
+    return;
+  }
+
+  // backup the relative pos and R pointers of the encapsulated geom object
+  dReal *posbak = obj->pos;
+  dReal *Rbak = obj->R;
+
+  // compute temporary pos and R for the encapsulated geom object
+  computeFinalTx();
+  obj->pos = final_pos;
+  obj->R = final_R;
+
+  // compute the AABB
+  obj->computeAABB();
+  memcpy (aabb,obj->aabb,6*sizeof(dReal));
+
+  // restore the pos and R
+  obj->pos = posbak;
+  obj->R = Rbak;
+}
+
+
+// utility function for dCollideTransform() : compute final pos and R
+// for the encapsulated geom object
+
+void dxGeomTransform::computeFinalTx()
+{
+  dMULTIPLY0_331 (final_pos,R,obj->pos);
+  final_pos[0] += pos[0];
+  final_pos[1] += pos[1];
+  final_pos[2] += pos[2];
+  dMULTIPLY0_333 (final_R,R,obj->R);
+}
+
+//****************************************************************************
+// collider function:
+// this collides a transformed geom with another geom. the other geom can
+// also be a transformed geom, but this case is not handled specially.
+
+int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
+		       dContactGeom *contact, int skip)
+{
+  dIASSERT (skip >= (int)sizeof(dContactGeom));
+  dIASSERT (o1->type == dGeomTransformClass);
+
+  dxGeomTransform *tr = (dxGeomTransform*) o1;
+  if (!tr->obj) return 0;
+  dUASSERT (tr->obj->parent_space==0,
+	    "GeomTransform encapsulated object must not be in a space");
+  dUASSERT (tr->obj->body==0,
+	    "GeomTransform encapsulated object must not be attached "
+	    "to a body");
+
+  // backup the relative pos and R pointers of the encapsulated geom object,
+  // and the body pointer
+  dReal *posbak = tr->obj->pos;
+  dReal *Rbak = tr->obj->R;
+  dxBody *bodybak = tr->obj->body;
+
+  // compute temporary pos and R for the encapsulated geom object.
+  // note that final_pos and final_R are valid if no GEOM_AABB_BAD flag,
+  // because computeFinalTx() will have already been called in
+  // dxGeomTransform::computeAABB()
+
+  if (tr->gflags & GEOM_AABB_BAD) tr->computeFinalTx();
+  tr->obj->pos = tr->final_pos;
+  tr->obj->R = tr->final_R;
+  tr->obj->body = o1->body;
+
+  // do the collision
+  int n = dCollide (tr->obj,o2,flags,contact,skip);
+
+  // if required, adjust the 'g1' values in the generated contacts so that
+  // thay indicated the GeomTransform object instead of the encapsulated
+  // object.
+  if (tr->infomode) {
+    for (int i=0; i<n; i++) {
+      dContactGeom *c = CONTACT(contact,skip*i);
+      c->g1 = o1;
+    }
+  }
+
+  // restore the pos, R and body
+  tr->obj->pos = posbak;
+  tr->obj->R = Rbak;
+  tr->obj->body = bodybak;
+  return n;
+}
+
+//****************************************************************************
+// public API
+
+dGeomID dCreateGeomTransform (dSpaceID space)
+{
+  return new dxGeomTransform (space);
+}
+
+
+void dGeomTransformSetGeom (dGeomID g, dGeomID obj)
+{
+  dUASSERT (g && g->type == dGeomTransformClass,
+	    "argument not a geom transform");
+  dxGeomTransform *tr = (dxGeomTransform*) g;
+  if (tr->obj && tr->cleanup) delete tr->obj;
+  tr->obj = obj;
+}
+
+
+dGeomID dGeomTransformGetGeom (dGeomID g)
+{
+  dUASSERT (g && g->type == dGeomTransformClass,
+	    "argument not a geom transform");
+  dxGeomTransform *tr = (dxGeomTransform*) g;
+  return tr->obj;
+}
+
+
+void dGeomTransformSetCleanup (dGeomID g, int mode)
+{
+  dUASSERT (g && g->type == dGeomTransformClass,
+	    "argument not a geom transform");
+  dxGeomTransform *tr = (dxGeomTransform*) g;
+  tr->cleanup = mode;
+}
+
+
+int dGeomTransformGetCleanup (dGeomID g)
+{
+  dUASSERT (g && g->type == dGeomTransformClass,
+	    "argument not a geom transform");
+  dxGeomTransform *tr = (dxGeomTransform*) g;
+  return tr->cleanup;
+}
+
+
+void dGeomTransformSetInfo (dGeomID g, int mode)
+{
+  dUASSERT (g && g->type == dGeomTransformClass,
+	    "argument not a geom transform");
+  dxGeomTransform *tr = (dxGeomTransform*) g;
+  tr->infomode = mode;
+}
+
+
+int dGeomTransformGetInfo (dGeomID g)
+{
+  dUASSERT (g && g->type == dGeomTransformClass,
+	    "argument not a geom transform");
+  dxGeomTransform *tr = (dxGeomTransform*) g;
+  return tr->infomode;
+}

src/source/ode/collision_transform.h

@@ -0,0 +1,40 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+geom transform
+
+*/
+
+#ifndef _ODE_COLLISION_TRANSFORM_H_
+#define _ODE_COLLISION_TRANSFORM_H_
+
+#include <ode/common.h>
+#include "collision_kernel.h"
+
+
+int dCollideTransform (dxGeom *o1, dxGeom *o2, int flags,
+		       dContactGeom *contact, int skip);
+
+
+#endif

src/source/ode/collision_trimesh.cpp

@@ -0,0 +1,492 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_util.h"
+#define TRIMESH_INTERNAL
+#include "collision_trimesh_internal.h"
+
+// Trimesh data
+dxTriMeshData::dxTriMeshData(){
+#ifndef dTRIMESH_ENABLED
+  dUASSERT(g, "dTRIMESH_ENABLED is not defined. Trimesh geoms will not work");
+#endif
+}
+
+dxTriMeshData::~dxTriMeshData(){
+	//
+}
+
+void 
+dxTriMeshData::Build(const void* Vertices, int VertexStide, int VertexCount,
+		     const void* Indices, int IndexCount, int TriStride,
+		     const void* in_Normals,
+		     bool Single){
+	Mesh.SetNbTriangles(IndexCount / 3);
+	Mesh.SetNbVertices(VertexCount);
+	Mesh.SetPointers((IndexedTriangle*)Indices, (Point*)Vertices);
+	Mesh.SetStrides(TriStride, VertexStide);
+	Mesh.Single = Single;
+	
+	// Build tree
+	BuildSettings Settings;
+    // recommended in Opcode User Manual
+    //Settings.mRules = SPLIT_COMPLETE | SPLIT_SPLATTERPOINTS | SPLIT_GEOMCENTER;
+    // used in ODE, why?
+	Settings.mRules = SPLIT_BEST_AXIS;
+
+    // best compromise?
+    //Settings.mRules = SPLIT_BEST_AXIS | SPLIT_SPLATTER_POINTS | SPLIT_GEOM_CENTER;
+
+
+	OPCODECREATE TreeBuilder;
+	TreeBuilder.mIMesh = &Mesh;
+
+	TreeBuilder.mSettings = Settings;
+	TreeBuilder.mNoLeaf = true;
+	TreeBuilder.mQuantized = false;
+
+	TreeBuilder.mKeepOriginal = false;
+	TreeBuilder.mCanRemap = false;
+
+	BVTree.Build(TreeBuilder);
+
+	// compute model space AABB
+	dVector3 AABBMax, AABBMin;
+    AABBMax[0] = AABBMax[1] = AABBMax[2] = (dReal) -dInfinity;
+    AABBMin[0] = AABBMin[1] = AABBMin[2] = (dReal) dInfinity;
+	if( Single ) {
+        const char* verts = (const char*)Vertices;
+        for( int i = 0; i < VertexCount; ++i ) {
+            const float* v = (const float*)verts;
+            if( v[0] > AABBMax[0] ) AABBMax[0] = v[0];
+            if( v[1] > AABBMax[1] ) AABBMax[1] = v[1];
+            if( v[2] > AABBMax[2] ) AABBMax[2] = v[2];
+            if( v[0] < AABBMin[0] ) AABBMin[0] = v[0];
+            if( v[1] < AABBMin[1] ) AABBMin[1] = v[1];
+            if( v[2] < AABBMin[2] ) AABBMin[2] = v[2];
+            verts += VertexStide;
+        }
+    } else {
+        const char* verts = (const char*)Vertices;
+        for( int i = 0; i < VertexCount; ++i ) {
+            const double* v = (const double*)verts;
+        if( v[0] > AABBMax[0] ) AABBMax[0] = (dReal) v[0];
+        if( v[1] > AABBMax[1] ) AABBMax[1] = (dReal) v[1];
+        if( v[2] > AABBMax[2] ) AABBMax[2] = (dReal) v[2];
+        if( v[0] < AABBMin[0] ) AABBMin[0] = (dReal) v[0];
+        if( v[1] < AABBMin[1] ) AABBMin[1] = (dReal) v[1];
+        if( v[2] < AABBMin[2] ) AABBMin[2] = (dReal) v[2];
+            verts += VertexStide;
+        }
+    }
+    AABBCenter[0] = (AABBMin[0] + AABBMax[0]) * REAL(0.5);
+    AABBCenter[1] = (AABBMin[1] + AABBMax[1]) * REAL(0.5);
+    AABBCenter[2] = (AABBMin[2] + AABBMax[2]) * REAL(0.5);
+    AABBExtents[0] = AABBMax[0] - AABBCenter[0];
+    AABBExtents[1] = AABBMax[1] - AABBCenter[1];
+    AABBExtents[2] = AABBMax[2] - AABBCenter[2];
+
+    // user data (not used by OPCODE)
+    for (int i=0; i<3; i++)
+	for (int j=0; j<3; j++)
+	    last_trans.m[i][j] = 0;
+
+    Normals = (dReal *) in_Normals;
+}
+
+dTriMeshDataID dGeomTriMeshDataCreate(){
+	return new dxTriMeshData();
+}
+
+void dGeomTriMeshDataDestroy(dTriMeshDataID g){
+	delete g;
+}
+
+void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
+{
+	dUASSERT(g, "argument not trimesh data");
+
+    double *elem;
+    
+    switch (data_id) {
+    case TRIMESH_FACE_NORMALS:
+	g->Normals = (dReal *) in_data;
+	break;
+
+    case TRIMESH_LAST_TRANSFORMATION:
+	elem = (double *) in_data;
+	for (int i=0; i<4; i++)
+	    for (int j=0; j<4; j++)
+		g->last_trans.m[i][j] = (dReal) elem[i*4 + j];
+	
+	break;
+    default:
+	dUASSERT(data_id, "invalid data type");
+	break;
+    }
+
+    return;
+}
+
+
+void dGeomTriMeshDataBuildSingle1(dTriMeshDataID g,
+                                  const void* Vertices, int VertexStride, int VertexCount, 
+                                  const void* Indices, int IndexCount, int TriStride,
+                                  const void* Normals){
+    dUASSERT(g, "argument not trimesh data");
+
+    g->Build(Vertices, VertexStride, VertexCount, 
+	     Indices, IndexCount, TriStride, 
+	     Normals, 
+	     true);
+}
+
+
+void dGeomTriMeshDataBuildSingle(dTriMeshDataID g,
+
+				 const void* Vertices, int VertexStride, int VertexCount, 
+                                 const void* Indices, int IndexCount, int TriStride) {
+    dGeomTriMeshDataBuildSingle1(g, Vertices, VertexStride, VertexCount,
+                                 Indices, IndexCount, TriStride, (void*)NULL);
+}
+
+
+void dGeomTriMeshDataBuildDouble1(dTriMeshDataID g,
+                                  const void* Vertices, int VertexStride, int VertexCount, 
+                                  const void* Indices, int IndexCount, int TriStride,
+                                  const void* Normals){
+    dUASSERT(g, "argument not trimesh data");
+
+    g->Build(Vertices, VertexStride, VertexCount, 
+	     Indices, IndexCount, TriStride, 
+	     Normals, 
+	     false);
+}
+
+
+void dGeomTriMeshDataBuildDouble(dTriMeshDataID g,
+				 const void* Vertices, int VertexStride, int VertexCount, 
+                                 const void* Indices, int IndexCount, int TriStride) {
+    dGeomTriMeshDataBuildDouble1(g, Vertices, VertexStride, VertexCount,
+                                 Indices, IndexCount, TriStride, NULL);
+}
+
+
+void dGeomTriMeshDataBuildSimple1(dTriMeshDataID g,
+                                  const dReal* Vertices, int VertexCount, 
+                                  const int* Indices, int IndexCount,
+                                  const int* Normals){
+#ifdef dSINGLE
+    dGeomTriMeshDataBuildSingle1(g,
+                                 Vertices, 4 * sizeof(dReal), VertexCount, 
+                                 Indices, IndexCount, 3 * sizeof(unsigned int),
+                                 Normals);
+#else
+    dGeomTriMeshDataBuildDouble1(g, Vertices, 4 * sizeof(dReal), VertexCount, 
+                                 Indices, IndexCount, 3 * sizeof(unsigned int),
+                                 Normals);
+#endif
+}
+
+
+void dGeomTriMeshDataBuildSimple(dTriMeshDataID g,
+                                 const dReal* Vertices, int VertexCount, 
+                                 const int* Indices, int IndexCount) {
+    dGeomTriMeshDataBuildSimple1(g,
+                                 Vertices, VertexCount, Indices, IndexCount,
+                                 (const int*)NULL);
+}
+
+
+// Trimesh
+PlanesCollider dxTriMesh::_PlanesCollider;
+SphereCollider dxTriMesh::_SphereCollider;
+OBBCollider dxTriMesh::_OBBCollider;
+RayCollider dxTriMesh::_RayCollider;
+AABBTreeCollider dxTriMesh::_AABBTreeCollider;
+LSSCollider dxTriMesh::_LSSCollider;
+
+SphereCache dxTriMesh::defaultSphereCache;
+OBBCache dxTriMesh::defaultBoxCache;
+LSSCache dxTriMesh::defaultCCylinderCache;
+
+CollisionFaces dxTriMesh::Faces;
+
+dxTriMesh::dxTriMesh(dSpaceID Space, dTriMeshDataID Data) : dxGeom(Space, 1){
+	type = dTriMeshClass;
+
+	this->Data = Data;
+
+	_RayCollider.SetDestination(&Faces);
+
+	_PlanesCollider.SetTemporalCoherence(true);
+
+	_SphereCollider.SetTemporalCoherence(true);
+        _SphereCollider.SetPrimitiveTests(false);
+
+
+	_OBBCollider.SetTemporalCoherence(true);
+
+    // no first-contact test (i.e. return full contact info)
+	_AABBTreeCollider.SetFirstContact( false );     
+    // temporal coherence only works with "first conact" tests
+    _AABBTreeCollider.SetTemporalCoherence(false);
+    // Perform full BV-BV tests (true) or SAT-lite tests (false)
+	_AABBTreeCollider.SetFullBoxBoxTest( true );
+    // Perform full Primitive-BV tests (true) or SAT-lite tests (false)
+	_AABBTreeCollider.SetFullPrimBoxTest( true );
+	_LSSCollider.SetTemporalCoherence(false);
+
+	/* TC has speed/space 'issues' that don't make it a clear
+	   win by default on spheres/boxes. */
+	this->doSphereTC = false;
+	this->doBoxTC = false;
+	this->doCCylinderTC = false;
+
+    const char* msg;
+    if ((msg =_AABBTreeCollider.ValidateSettings()))
+        dDebug (d_ERR_UASSERT, msg, " (%s:%d)", __FILE__,__LINE__);
+	_LSSCollider.SetPrimitiveTests(false);
+	_LSSCollider.SetFirstContact(false);
+}
+
+dxTriMesh::~dxTriMesh(){
+	//
+}
+
+
+void dxTriMesh::ClearTCCache(){
+  /* dxTriMesh::ClearTCCache uses dArray's setSize(0) to clear the caches -
+     but the destructor isn't called when doing this, so we would leak.
+     So, call the previous caches' containers' destructors by hand first. */
+	int i, n;
+	n = SphereTCCache.size();
+	for( i = 0; i < n; ++i ) {
+	  SphereTCCache[i].~SphereTC();
+	}
+	SphereTCCache.setSize(0);
+	n = BoxTCCache.size();
+	for( i = 0; i < n; ++i ) {
+	  BoxTCCache[i].~BoxTC();
+	}
+	BoxTCCache.setSize(0);
+	n = CCylinderTCCache.size();
+	for( i = 0; i < n; ++i ) {
+	  CCylinderTCCache[i].~CCylinderTC();
+	}
+	CCylinderTCCache.setSize(0);
+}
+
+
+int dxTriMesh::AABBTest(dxGeom* g, dReal aabb[6]){
+	return 1;
+}
+
+
+void dxTriMesh::computeAABB() {
+        const dxTriMeshData* d = Data;
+        dVector3 c;
+    
+        dMULTIPLY0_331( c, R, d->AABBCenter );
+    
+        dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
+        dFabs(R[1] * Data->AABBExtents[1]) + 
+        dFabs(R[2] * Data->AABBExtents[2]);
+        dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
+        dFabs(R[5] * Data->AABBExtents[1]) + 
+        dFabs(R[6] * Data->AABBExtents[2]);
+        dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
+        dFabs(R[9] * Data->AABBExtents[1]) + 
+        dFabs(R[10] * Data->AABBExtents[2]);
+
+        aabb[0] = c[0] + pos[0] - xrange;
+        aabb[1] = c[0] + pos[0] + xrange;
+        aabb[2] = c[1] + pos[1] - yrange;
+        aabb[3] = c[1] + pos[1] + yrange;
+        aabb[4] = c[2] + pos[2] - zrange;
+        aabb[5] = c[2] + pos[2] + zrange;
+}
+
+dGeomID dCreateTriMesh(dSpaceID space, 
+		       dTriMeshDataID Data,
+		       dTriCallback* Callback,
+		       dTriArrayCallback* ArrayCallback,
+		       dTriRayCallback* RayCallback)
+{
+	dxTriMesh* Geom = new dxTriMesh(space, Data);
+	Geom->Callback = Callback;
+	Geom->ArrayCallback = ArrayCallback;
+	Geom->RayCallback = RayCallback;
+
+	return Geom;
+}
+
+void dGeomTriMeshSetCallback(dGeomID g, dTriCallback* Callback)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+	((dxTriMesh*)g)->Callback = Callback;
+}
+
+dTriCallback* dGeomTriMeshGetCallback(dGeomID g)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+	return ((dxTriMesh*)g)->Callback;
+}
+
+void dGeomTriMeshSetArrayCallback(dGeomID g, dTriArrayCallback* ArrayCallback)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+	((dxTriMesh*)g)->ArrayCallback = ArrayCallback;
+}
+
+dTriArrayCallback* dGeomTriMeshGetArrayCallback(dGeomID g)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+	return ((dxTriMesh*)g)->ArrayCallback;
+}
+
+void dGeomTriMeshSetRayCallback(dGeomID g, dTriRayCallback* Callback)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+	((dxTriMesh*)g)->RayCallback = Callback;
+}
+
+dTriRayCallback* dGeomTriMeshGetRayCallback(dGeomID g)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");	
+	return ((dxTriMesh*)g)->RayCallback;
+}
+
+void dGeomTriMeshSetData(dGeomID g, dTriMeshDataID Data)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+	((dxTriMesh*)g)->Data = Data;
+}
+
+void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+  
+	switch (geomClass)
+	{
+		case dSphereClass: 
+			((dxTriMesh*)g)->doSphereTC = (1 == enable);
+			break;
+		case dBoxClass:
+			((dxTriMesh*)g)->doBoxTC = (1 == enable);
+			break;
+		case dCCylinderClass:
+			((dxTriMesh*)g)->doCCylinderTC = (1 == enable);
+			break;
+	}
+}
+
+int dGeomTriMeshIsTCEnabled(dGeomID g, int geomClass)
+{
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+  
+	switch (geomClass)
+	{
+		case dSphereClass:
+			if (((dxTriMesh*)g)->doSphereTC)
+				return 1;
+			break;
+		case dBoxClass:
+			if (((dxTriMesh*)g)->doBoxTC)
+				return 1;
+			break;
+		case dCCylinderClass:
+			if (((dxTriMesh*)g)->doCCylinderTC)
+				return 1;
+			break;
+	}
+	return 0;
+}
+
+void dGeomTriMeshClearTCCache(dGeomID g){
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+
+	dxTriMesh* Geom = (dxTriMesh*)g;
+	Geom->ClearTCCache();
+}
+
+/*
+ * returns the TriMeshDataID
+ */
+dTriMeshDataID
+dGeomTriMeshGetTriMeshDataID(dGeomID g)
+{
+    dxTriMesh* Geom = (dxTriMesh*) g;
+    return Geom->Data;
+}
+
+// Getting data
+void dGeomTriMeshGetTriangle(dGeomID g, int Index, dVector3* v0, dVector3* v1, dVector3* v2){
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+
+	dxTriMesh* Geom = (dxTriMesh*)g;
+
+	const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
+	const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
+
+	dVector3 v[3];
+	FetchTriangle(Geom, Index, Position, Rotation, v);
+
+	if (v0){
+		(*v0)[0] = v[0][0];
+		(*v0)[1] = v[0][1];
+		(*v0)[2] = v[0][2];
+		(*v0)[3] = v[0][3];
+	}
+	if (v1){
+		(*v1)[0] = v[1][0];
+		(*v1)[1] = v[1][1];
+		(*v1)[2] = v[1][2];
+		(*v1)[3] = v[1][3];
+	}
+	if (v2){
+		(*v2)[0] = v[2][0];
+		(*v2)[1] = v[2][1];
+		(*v2)[2] = v[2][2];
+		(*v2)[3] = v[2][3];
+	}
+}
+
+void dGeomTriMeshGetPoint(dGeomID g, int Index, dReal u, dReal v, dVector3 Out){
+	dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
+
+	dxTriMesh* Geom = (dxTriMesh*)g;
+
+	const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
+	const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
+
+	dVector3 dv[3];
+        FetchTriangle(Geom, Index, Position, Rotation, dv);
+
+	GetPointFromBarycentric(dv, u, v, Out);
+}

src/source/ode/collision_trimesh_ccylinder.cpp

@@ -0,0 +1,985 @@
+/*************************************************************************
+*                                                                       *
+* 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.                     *
+*                                                                       *
+*************************************************************************/
+
+/*
+ *	Triangle-CCylinder(Capsule) collider by Alen Ladavac
+ *  Ported to ODE by Nguyen Binh
+ */
+
+// NOTES from Nguyen Binh
+//	14 Apr : Seem to be robust
+//       There is a problem when you use original Step and set contact friction
+//		surface.mu = dInfinity;
+//		More description : 
+//			When I dropped CCylinder over the bunny ears, it seems to stuck
+//			there for a while. I think the cause is when you set surface.mu = dInfinity;
+//			the friction force is too high so it just hang the capsule there.
+//			So the good cure for this is to set mu = around 1.5 (in my case)
+//		For StepFast1, this become as solid as rock : StepFast1 just approximate 
+//		friction force.
+
+// NOTES from Croteam's Alen
+//As a side note... there are some extra contacts that can be generated
+//on the edge between two triangles, and if the capsule penetrates deeply into
+//the triangle (usually happens with large mass or low FPS), some such
+//contacts can in some cases push the capsule away from the edge instead of
+//away from the two triangles. This shows up as capsule slowing down a bit
+//when hitting an edge while sliding along a flat tesselated grid of
+//triangles. This is only if capsule is standing upwards.
+
+//Same thing can appear whenever a smooth object (e.g sphere) hits such an
+//edge, and it needs to be solved as a special case probably. This is a
+//problem we are looking forward to address soon.
+
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_util.h"
+
+#define TRIMESH_INTERNAL
+#include "collision_trimesh_internal.h"
+
+// largest number, double or float
+#if defined(dSINGLE)
+#define MAX_REAL	FLT_MAX
+#define MIN_REAL	(-FLT_MAX)
+#else
+#define MAX_REAL	DBL_MAX
+#define MIN_REAL	(-DBL_MAX)
+#endif
+
+// To optimize before send contacts to dynamic part
+#define OPTIMIZE_CONTACTS
+
+// dVector3
+// r=a-b
+#define SUBSTRACT(a,b,r) \
+	(r)[0]=(a)[0] - (b)[0]; \
+	(r)[1]=(a)[1] - (b)[1]; \
+	(r)[2]=(a)[2] - (b)[2]; 
+
+
+// dVector3
+// a=b
+#define SET(a,b) \
+	(a)[0]=(b)[0]; \
+	(a)[1]=(b)[1]; \
+	(a)[2]=(b)[2]; 
+
+
+// dMatrix3
+// a=b
+#define SETM(a,b) \
+	(a)[0]=(b)[0]; \
+	(a)[1]=(b)[1]; \
+	(a)[2]=(b)[2]; \
+	(a)[3]=(b)[3]; \
+	(a)[4]=(b)[4]; \
+	(a)[5]=(b)[5]; \
+	(a)[6]=(b)[6]; \
+	(a)[7]=(b)[7]; \
+	(a)[8]=(b)[8]; \
+	(a)[9]=(b)[9]; \
+	(a)[10]=(b)[10]; \
+	(a)[11]=(b)[11];
+
+
+// dVector3
+// r=a+b
+#define ADD(a,b,r) \
+	(r)[0]=(a)[0] + (b)[0]; \
+	(r)[1]=(a)[1] + (b)[1]; \
+	(r)[2]=(a)[2] + (b)[2]; 
+
+
+// dMatrix3, int, dVector3
+// v=column a from m
+#define GETCOL(m,a,v) \
+	(v)[0]=(m)[(a)+0]; \
+	(v)[1]=(m)[(a)+4]; \
+	(v)[2]=(m)[(a)+8];
+
+
+// dVector4, dVector3
+// distance between plane p and point v
+#define POINTDISTANCE(p,v) \
+	( p[0]*v[0] + p[1]*v[1] + p[2]*v[2] + p[3] ); \
+
+
+// dVector4, dVector3, dReal
+// construct plane from normal and d
+#define CONSTRUCTPLANE(plane,normal,d) \
+	plane[0]=normal[0];\
+	plane[1]=normal[1];\
+	plane[2]=normal[2];\
+	plane[3]=d;
+
+
+// dVector3
+// length of vector a
+#define LENGTHOF(a) \
+	dSqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);\
+
+inline dReal _length2OfVector3(dVector3 v)
+{
+	return (v[0] * v[0] + v[1] * v[1] + v[2] * v[2] );
+}
+
+
+// Local contacts data
+typedef struct _sLocalContactData
+{
+	dVector3	vPos;
+	dVector3	vNormal;
+	dReal		fDepth;
+	int			nFlags; // 0 = filtered out, 1 = OK
+}sLocalContactData;
+
+static const int			gMaxLocalContacts = 32;
+static sLocalContactData	gLocalContacts[gMaxLocalContacts];
+static int					ctContacts = 0;
+
+// capsule data
+// real time data
+static dMatrix3  mCapsuleRotation;
+static dVector3   vCapsulePosition;
+static dVector3   vCapsuleAxis;
+// static data
+static dReal      vCapsuleRadius;
+static dReal      fCapsuleSize;
+
+// mesh data
+static  dMatrix4  mHullDstPl;
+static   dMatrix3  mTriMeshRot;
+static dVector3   mTriMeshPos;
+static dVector3   vE0, vE1, vE2;
+
+// Two geom
+dxGeom*	   gCylinder;
+dxGeom*	   gTriMesh;
+
+// global collider data
+static dVector3 vNormal;
+static dReal    fBestDepth;
+static dReal    fBestCenter;
+static dReal    fBestrt;
+static int		iBestAxis;
+static dVector3 vN = {0,0,0,0};
+
+static dVector3 vV0; 
+static dVector3 vV1;
+static dVector3 vV2;
+
+// ODE contact's specific
+static int iFlags;
+static dContactGeom *ContactGeoms;
+static int iStride;
+
+// Capsule lie on axis number 3 = (Z axis)
+static const int nCAPSULE_AXIS = 2;
+
+// Use to classify contacts to be "near" in position
+static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4
+// Use to classify contacts to be "near" in normal direction
+static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4
+
+
+// If this two contact can be classified as "near"
+inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2)
+{
+	int bPosNear = 0;
+	int bSameDir = 0;
+	dVector3	vDiff;
+
+	// First check if they are "near" in position
+	SUBSTRACT(c1.vPos,c2.vPos,vDiff);
+	if (  (dFabs(vDiff[0]) < fSameContactPositionEpsilon)
+		&&(dFabs(vDiff[1]) < fSameContactPositionEpsilon)
+		&&(dFabs(vDiff[2]) < fSameContactPositionEpsilon))
+	{
+		bPosNear = 1;
+	}
+
+	// Second check if they are "near" in normal direction
+	SUBSTRACT(c1.vNormal,c2.vNormal,vDiff);
+	if (  (dFabs(vDiff[0]) < fSameContactNormalEpsilon)
+		&&(dFabs(vDiff[1]) < fSameContactNormalEpsilon)
+		&&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) )
+	{
+		bSameDir = 1;
+	}
+
+	// Will be "near" if position and normal direction are "near"
+	return (bPosNear && bSameDir);
+}
+
+inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2)
+{
+	// The not better will be throw away
+	// You can change the selection criteria here
+	return (c1.fDepth > c2.fDepth);
+}
+
+// iterate through gLocalContacts and filtered out "near contact"
+inline void	_OptimizeLocalContacts()
+{
+	int nContacts = ctContacts;
+		
+	for (int i = 0; i < nContacts-1; i++)
+	{
+		for (int j = i+1; j < nContacts; j++)
+		{
+			if (_IsNearContacts(gLocalContacts[i],gLocalContacts[j]))
+			{
+				// If they are seem to be the samed then filtered 
+				// out the least penetrate one
+				if (_IsBetter(gLocalContacts[j],gLocalContacts[i]))
+				{
+					gLocalContacts[i].nFlags = 0; // filtered 1st contact
+				}
+				else
+				{
+					gLocalContacts[j].nFlags = 0; // filtered 2nd contact
+				}
+
+				// NOTE
+				// There is other way is to add two depth together but
+				// it not work so well. Why???
+			}
+		}
+	}
+}
+
+inline int	_ProcessLocalContacts()
+{
+	if (ctContacts == 0)
+	{
+		return 0;
+	}
+
+#ifdef OPTIMIZE_CONTACTS
+	if (ctContacts > 1)
+	{
+		// Can be optimized...
+		_OptimizeLocalContacts();
+	}
+#endif		
+
+	int iContact = 0;
+	dContactGeom* Contact = 0;
+
+	int nFinalContact = 0;
+
+	for (iContact = 0; iContact < ctContacts; iContact ++)
+	{
+		if (1 == gLocalContacts[iContact].nFlags)
+		{
+				Contact =  SAFECONTACT(iFlags, ContactGeoms, nFinalContact, iStride);
+				Contact->depth = gLocalContacts[iContact].fDepth;
+				SET(Contact->normal,gLocalContacts[iContact].vNormal);
+				SET(Contact->pos,gLocalContacts[iContact].vPos);
+				Contact->g1 = gCylinder;
+				Contact->g2 = gTriMesh;
+
+				nFinalContact++;
+		}
+	}
+	// debug
+	//if (nFinalContact != ctContacts)
+	//{
+	//	printf("[Info] %d contacts generated,%d  filtered.\n",ctContacts,ctContacts-nFinalContact);
+	//}
+
+	return nFinalContact;
+}
+
+BOOL _cldClipEdgeToPlane( dVector3 &vEpnt0, dVector3 &vEpnt1, const dVector4& plPlane)
+{
+	// calculate distance of edge points to plane
+	dReal fDistance0 = POINTDISTANCE( plPlane, vEpnt0 );
+	dReal fDistance1 = POINTDISTANCE( plPlane, vEpnt1 );
+
+	// if both points are behind the plane
+	if ( fDistance0 < 0 && fDistance1 < 0 ) 
+	{
+		// do nothing
+		return FALSE;
+		// if both points in front of the plane
+	} else if ( fDistance0 > 0 && fDistance1 > 0 ) 
+	{
+		// accept them
+		return TRUE;
+		// if we have edge/plane intersection
+	} else if ((fDistance0 > 0 && fDistance1 < 0) || ( fDistance0 < 0 && fDistance1 > 0)) 
+	{
+
+			// find intersection point of edge and plane
+			dVector3 vIntersectionPoint;
+			vIntersectionPoint[0]= vEpnt0[0]-(vEpnt0[0]-vEpnt1[0])*fDistance0/(fDistance0-fDistance1);
+			vIntersectionPoint[1]= vEpnt0[1]-(vEpnt0[1]-vEpnt1[1])*fDistance0/(fDistance0-fDistance1);
+			vIntersectionPoint[2]= vEpnt0[2]-(vEpnt0[2]-vEpnt1[2])*fDistance0/(fDistance0-fDistance1);
+
+			// clamp correct edge to intersection point
+			if ( fDistance0 < 0 ) 
+			{
+				SET(vEpnt0,vIntersectionPoint);
+			} else 
+			{
+				SET(vEpnt1,vIntersectionPoint);
+			}
+			return TRUE;
+		}
+		return TRUE;
+}
+
+static BOOL _cldTestAxis(const dVector3 &v0,
+						 const dVector3 &v1,
+						 const dVector3 &v2, 
+						 dVector3 vAxis, 
+						 int iAxis,
+						 BOOL bNoFlip = FALSE) 
+{
+
+	// calculate length of separating axis vector
+	dReal fL = LENGTHOF(vAxis);
+	// if not long enough
+	// TODO : dReal epsilon please
+	if ( fL < 1e-5f ) 
+	{
+		// do nothing
+		//iLastOutAxis = 0;
+		return TRUE;
+	}
+
+	// otherwise normalize it
+	dNormalize3(vAxis);
+
+	// project capsule on vAxis
+	dReal frc = dFabs(dDOT(vCapsuleAxis,vAxis))*(fCapsuleSize*REAL(0.5)-vCapsuleRadius) + vCapsuleRadius;
+
+	// project triangle on vAxis
+	dReal afv[3];
+	afv[0] = dDOT( vV0 , vAxis );
+	afv[1] = dDOT( vV1 , vAxis );
+	afv[2] = dDOT( vV2 , vAxis );
+
+	dReal fMin = MAX_REAL;
+	dReal fMax = MIN_REAL;
+
+	// for each vertex 
+	for(int i=0; i<3; i++) 
+	{
+		// find minimum
+		if (afv[i]<fMin) 
+		{
+			fMin = afv[i];
+		}
+		// find maximum
+		if (afv[i]>fMax) 
+		{
+			fMax = afv[i];
+		}
+	}
+
+	// find triangle's center of interval on axis
+	dReal fCenter = (fMin+fMax)*REAL(0.5);
+	// calculate triangles half interval 
+	dReal fTriangleRadius = (fMax-fMin)*REAL(0.5);
+
+	// if they do not overlap, 
+	if( dFabs(fCenter) > ( frc + fTriangleRadius ) ) 
+	{ 
+		// exit, we have no intersection
+		return FALSE; 
+	}
+
+	// calculate depth 
+	dReal fDepth = dFabs(fCenter) - (frc+fTriangleRadius);
+
+	// if greater then best found so far
+	if ( fDepth > fBestDepth ) 
+	{
+		// remember depth
+		fBestDepth  = fDepth;
+		fBestCenter = fCenter;
+		fBestrt     = fTriangleRadius;
+
+		vNormal[0]     = vAxis[0];
+		vNormal[1]     = vAxis[1];
+		vNormal[2]     = vAxis[2];
+
+		iBestAxis   = iAxis;
+
+		// flip normal if interval is wrong faced
+		if (fCenter<0 && !bNoFlip) 
+		{ 
+			vNormal[0] = -vNormal[0];
+			vNormal[1] = -vNormal[1];
+			vNormal[2] = -vNormal[2];
+
+			fBestCenter = -fCenter;
+		}
+	}
+
+	return TRUE;
+}
+
+// helper for less key strokes
+inline void _CalculateAxis(const dVector3& v1,
+						   const dVector3& v2,
+						   const dVector3& v3,
+						   const dVector3& v4,
+						   dVector3& r)
+{
+	dVector3 t1;
+	dVector3 t2;
+
+	SUBSTRACT(v1,v2,t1);
+	dCROSS(t2,=,t1,v3);
+	dCROSS(r,=,t2,v4);
+}
+
+static BOOL _cldTestSeparatingAxesOfCapsule(const dVector3 &v0,
+											const dVector3 &v1,
+											const dVector3 &v2) 
+{
+	// calculate caps centers in absolute space
+	dVector3 vCp0;
+	vCp0[0] = vCapsulePosition[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+	vCp0[1] = vCapsulePosition[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+	vCp0[2] = vCapsulePosition[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+
+	dVector3 vCp1;
+	vCp1[0] = vCapsulePosition[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+	vCp1[1] = vCapsulePosition[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+	vCp1[2] = vCapsulePosition[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+
+	// reset best axis
+	iBestAxis = 0;
+	// reset best depth
+	fBestDepth  = -MAX_REAL;
+	// reset separating axis vector
+	dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)};
+
+	// Epsilon value for checking axis vector length 
+	const dReal fEpsilon = 1e-6f;
+
+	// Translate triangle to Cc cord.
+	SUBSTRACT(v0 , vCapsulePosition, vV0);
+	SUBSTRACT(v1 , vCapsulePosition, vV1);
+	SUBSTRACT(v2 , vCapsulePosition, vV2);
+	
+	// We begin to test for 19 separating axis now
+	// I wonder does it help if we employ the method like ISA-GJK???
+	// Or at least we should do experiment and find what axis will
+	// be most likely to be separating axis to check it first.
+
+	// Original
+	// axis vN
+	//vAxis = -vN;
+	vAxis[0] = - vN[0];
+	vAxis[1] = - vN[1];
+	vAxis[2] = - vN[2];
+	if (!_cldTestAxis( v0, v1, v2, vAxis, 1, TRUE)) 
+	{ 
+		return FALSE; 
+	}
+
+	// axis CxE0 - Edge 0
+	dCROSS(vAxis,=,vCapsuleAxis,vE0);
+	//vAxis = dCROSS( vCapsuleAxis cross vE0 );
+	if( _length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 2)) { 
+			return FALSE; 
+		}
+	}
+
+	// axis CxE1 - Edge 1
+	dCROSS(vAxis,=,vCapsuleAxis,vE1);
+	//vAxis = ( vCapsuleAxis cross vE1 );
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 3)) { 
+			return FALSE; 
+		}
+	}
+
+	// axis CxE2 - Edge 2
+	//vAxis = ( vCapsuleAxis cross vE2 );
+	dCROSS(vAxis,=,vCapsuleAxis,vE2);
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 4)) { 
+			return FALSE; 
+		}
+	}
+
+	// first capsule point
+	// axis ((Cp0-V0) x E0) x E0
+	_CalculateAxis(vCp0,v0,vE0,vE0,vAxis);
+//	vAxis = ( ( vCp0-v0) cross vE0 ) cross vE0;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 5)) { 
+			return FALSE; 
+		}
+	}
+
+	// axis ((Cp0-V1) x E1) x E1
+	_CalculateAxis(vCp0,v1,vE1,vE1,vAxis);
+	//vAxis = ( ( vCp0-v1) cross vE1 ) cross vE1;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 6)) { 
+			return FALSE; 
+		}
+	}
+
+	// axis ((Cp0-V2) x E2) x E2
+	_CalculateAxis(vCp0,v2,vE2,vE2,vAxis);
+	//vAxis = ( ( vCp0-v2) cross vE2 ) cross vE2;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 7)) { 
+			return FALSE; 
+		}
+	}
+
+	// second capsule point
+	// axis ((Cp1-V0) x E0) x E0
+	_CalculateAxis(vCp1,v0,vE0,vE0,vAxis);	
+	//vAxis = ( ( vCp1-v0 ) cross vE0 ) cross vE0;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 8)) { 
+			return FALSE; 
+		}
+	}
+
+	// axis ((Cp1-V1) x E1) x E1
+	_CalculateAxis(vCp1,v1,vE1,vE1,vAxis);	
+	//vAxis = ( ( vCp1-v1 ) cross vE1 ) cross vE1;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 9)) { 
+			return FALSE; 
+		}
+	}
+
+	// axis ((Cp1-V2) x E2) x E2
+	_CalculateAxis(vCp1,v2,vE2,vE2,vAxis);	
+	//vAxis = ( ( vCp1-v2 ) cross vE2 ) cross vE2;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 10)) { 
+			return FALSE; 
+		}
+	}
+
+	// first vertex on triangle
+	// axis ((V0-Cp0) x C) x C
+	_CalculateAxis(v0,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);	
+	//vAxis = ( ( v0-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 11)) { 
+			return FALSE; 
+		}
+	}
+
+	// second vertex on triangle
+	// axis ((V1-Cp0) x C) x C
+	_CalculateAxis(v1,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);	
+	//vAxis = ( ( v1-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 12)) { 
+			return FALSE; 
+		}
+	}
+
+	// third vertex on triangle
+	// axis ((V2-Cp0) x C) x C
+	_CalculateAxis(v2,vCp0,vCapsuleAxis,vCapsuleAxis,vAxis);	
+	//vAxis = ( ( v2-vCp0 ) cross vCapsuleAxis ) cross vCapsuleAxis;
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 13)) { 
+			return FALSE; 
+		}
+	}
+
+	// Test as separating axes direction vectors between each triangle
+	// edge and each capsule's cap center
+
+	// first triangle vertex and first capsule point
+	//vAxis = v0 - vCp0;
+	SUBSTRACT(v0,vCp0,vAxis);
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 14)) { 
+			return FALSE; 
+		}
+	}
+
+	// second triangle vertex and first capsule point
+	//vAxis = v1 - vCp0;
+	SUBSTRACT(v1,vCp0,vAxis);
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 15)) { 
+			return FALSE; 
+		}
+	}
+
+	// third triangle vertex and first capsule point
+	//vAxis = v2 - vCp0;
+	SUBSTRACT(v2,vCp0,vAxis);
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 16)) { 
+			return FALSE; 
+		}
+	}
+
+	// first triangle vertex and second capsule point
+	//vAxis = v0 - vCp1;
+	SUBSTRACT(v0,vCp1,vAxis);
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 17)) { 
+			return FALSE; 
+		}
+	}
+
+	// second triangle vertex and second capsule point
+	//vAxis = v1 - vCp1;
+	SUBSTRACT(v1,vCp1,vAxis);
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 18)) { 
+			return FALSE; 
+		}
+	}
+
+	// third triangle vertex and second capsule point
+	//vAxis = v2 - vCp1;
+	SUBSTRACT(v2,vCp1,vAxis);
+	if(_length2OfVector3( vAxis ) > fEpsilon ) {
+		if (!_cldTestAxis( v0, v1, v2, vAxis, 19)) { 
+			return FALSE; 
+		}
+	}	
+
+	return TRUE;
+}
+
+// test one mesh triangle on intersection with capsule
+static void _cldTestOneTriangleVSCCylinder( const dVector3 &v0, 
+											const dVector3 &v1, 
+											const dVector3 &v2 )
+{
+
+	// calculate edges
+	SUBSTRACT(v1,v0,vE0);
+	SUBSTRACT(v2,v1,vE1);
+	SUBSTRACT(v0,v2,vE2);
+
+	dVector3	_minus_vE0;
+	SUBSTRACT(v0,v1,_minus_vE0);
+
+	// calculate poly normal
+	dCROSS(vN,=,vE1,_minus_vE0);
+	dNormalize3(vN);
+	
+	// create plane from triangle
+	dReal plDistance = -dDOT(v0,vN);
+	dVector4 plTrianglePlane;
+	CONSTRUCTPLANE(plTrianglePlane,vN,plDistance);
+	
+	// calculate capsule distance to plane
+	dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,vCapsulePosition);
+
+	// Capsule must be over positive side of triangle
+	if(fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/) 
+	{
+		// if not don't generate contacts
+		return;
+	}
+
+	dVector3 vPnt0;
+	SET	(vPnt0,v0);
+	dVector3 vPnt1;
+	SET	(vPnt1,v1);
+	dVector3 vPnt2;
+	SET	(vPnt2,v2);
+
+	if (fDistanceCapsuleCenterToPlane < 0 )
+	{
+		SET	(vPnt0,v0);
+		SET	(vPnt1,v2);
+		SET	(vPnt2,v1);
+	}
+
+	// do intersection test and find best separating axis
+	if(!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2) ) 
+	{
+		// if not found do nothing
+		return;
+	}
+
+	// if best separation axis is not found
+	if ( iBestAxis == 0 ) 
+	{
+		// this should not happen (we should already exit in that case)
+		ASSERT(FALSE);
+		// do nothing
+		return;
+	}
+
+	// calculate caps centers in absolute space
+	dVector3 vCposTrans;
+	vCposTrans[0] = vCapsulePosition[0] + vNormal[0]*vCapsuleRadius;
+	vCposTrans[1] = vCapsulePosition[1] + vNormal[1]*vCapsuleRadius;
+	vCposTrans[2] = vCapsulePosition[2] + vNormal[2]*vCapsuleRadius;
+
+	dVector3 vCEdgePoint0;
+	vCEdgePoint0[0]  = vCposTrans[0] + vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+	vCEdgePoint0[1]  = vCposTrans[1] + vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+	vCEdgePoint0[2]  = vCposTrans[2] + vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+    
+	dVector3 vCEdgePoint1;
+	vCEdgePoint1[0] = vCposTrans[0] - vCapsuleAxis[0]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+	vCEdgePoint1[1] = vCposTrans[1] - vCapsuleAxis[1]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+	vCEdgePoint1[2] = vCposTrans[2] - vCapsuleAxis[2]*(fCapsuleSize*REAL(0.5)-vCapsuleRadius);
+
+	// transform capsule edge points into triangle space
+	vCEdgePoint0[0] -= vPnt0[0];
+	vCEdgePoint0[1] -= vPnt0[1];
+	vCEdgePoint0[2] -= vPnt0[2];
+
+	vCEdgePoint1[0] -= vPnt0[0];
+	vCEdgePoint1[1] -= vPnt0[1];
+	vCEdgePoint1[2] -= vPnt0[2];
+
+	dVector4 plPlane;
+	dVector3 _minus_vN;
+	_minus_vN[0] = -vN[0];
+	_minus_vN[1] = -vN[1];
+	_minus_vN[2] = -vN[2];
+	// triangle plane
+	CONSTRUCTPLANE(plPlane,_minus_vN,0);
+	//plPlane = Plane4f( -vN, 0);
+
+	if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
+	{ 
+		return; 
+	}
+
+	// plane with edge 0
+	dVector3 vTemp;
+	dCROSS(vTemp,=,vN,vE0);
+	CONSTRUCTPLANE(plPlane, vTemp, 1e-5f);
+	if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
+	{ 
+		return; 
+	}
+
+	dCROSS(vTemp,=,vN,vE1);
+	CONSTRUCTPLANE(plPlane, vTemp, -(dDOT(vE0,vTemp)-1e-5f));
+	if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) 
+	{ 
+		return; 
+	}
+
+	dCROSS(vTemp,=,vN,vE2);
+	CONSTRUCTPLANE(plPlane, vTemp, 1e-5f);
+	if(!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { 
+		return; 
+	}
+
+	// return capsule edge points into absolute space
+	vCEdgePoint0[0] += vPnt0[0];
+	vCEdgePoint0[1] += vPnt0[1];
+	vCEdgePoint0[2] += vPnt0[2];
+
+	vCEdgePoint1[0] += vPnt0[0];
+	vCEdgePoint1[1] += vPnt0[1];
+	vCEdgePoint1[2] += vPnt0[2];
+
+	// calculate depths for both contact points
+	SUBSTRACT(vCEdgePoint0,vCapsulePosition,vTemp);
+	dReal fDepth0 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt);
+	SUBSTRACT(vCEdgePoint1,vCapsulePosition,vTemp);
+	dReal fDepth1 = dDOT(vTemp,vNormal) - (fBestCenter-fBestrt);
+
+	// clamp depths to zero
+	if(fDepth0 < 0) 
+	{
+		fDepth0 = 0.0f;
+	}
+
+	if(fDepth1 < 0 ) 
+	{
+		fDepth1 = 0.0f;
+	}
+
+	// Cached contacts's data
+	// contact 0
+	gLocalContacts[ctContacts].fDepth = fDepth0;
+	SET(gLocalContacts[ctContacts].vNormal,vNormal);
+	SET(gLocalContacts[ctContacts].vPos,vCEdgePoint0);
+	gLocalContacts[ctContacts].nFlags = 1;
+	ctContacts++;
+
+	// contact 1
+	gLocalContacts[ctContacts].fDepth = fDepth1;
+	SET(gLocalContacts[ctContacts].vNormal,vNormal);
+	SET(gLocalContacts[ctContacts].vPos,vCEdgePoint1);
+	gLocalContacts[ctContacts].nFlags = 1;
+	ctContacts++;
+
+}
+
+// capsule - trimesh by CroTeam
+// Ported by Nguyem Binh
+int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
+{
+	dxTriMesh* TriMesh = (dxTriMesh*)o1;
+	gCylinder = o2;
+	gTriMesh = o1;
+
+	const dMatrix3* pRot = (const dMatrix3*) dGeomGetRotation(gCylinder);
+	memcpy(mCapsuleRotation,pRot,sizeof(dMatrix3));
+
+	const dVector3* pDst = (const dVector3*)dGeomGetPosition(gCylinder);
+	memcpy(vCapsulePosition,pDst,sizeof(dVector3));
+
+	vCapsuleAxis[0] = mCapsuleRotation[0*4 + nCAPSULE_AXIS]; 
+	vCapsuleAxis[1] = mCapsuleRotation[1*4 + nCAPSULE_AXIS];
+	vCapsuleAxis[2] = mCapsuleRotation[2*4 + nCAPSULE_AXIS];
+
+	// Get size of CCylinder
+	dGeomCCylinderGetParams(gCylinder,&vCapsuleRadius,&fCapsuleSize);
+	fCapsuleSize += 2*vCapsuleRadius;
+
+	const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh);
+	memcpy(mTriMeshRot,pTriRot,sizeof(dMatrix3));
+
+	const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh);
+	memcpy(mTriMeshPos,pTriPos,sizeof(dVector3));	
+
+	// global info for contact creation
+	ctContacts		= 0;
+	iStride			=skip;
+	iFlags			=flags;
+	ContactGeoms	=contact;
+
+	// reset contact counter
+	ctContacts = 0;	
+
+	// reset best depth
+	fBestDepth  = - MAX_REAL;
+	fBestCenter = 0;
+	fBestrt     = 0;
+
+	// reset collision normal
+	vNormal[0] = REAL(0.0);
+	vNormal[1] = REAL(0.0);
+	vNormal[2] = REAL(0.0);
+
+	// Will it better to use LSS here? -> confirm Pierre.
+	 OBBCollider& Collider = TriMesh->_OBBCollider;
+
+	 Point cCenter(vCapsulePosition[0],vCapsulePosition[1],vCapsulePosition[2]);
+	 Point cExtents(vCapsuleRadius,vCapsuleRadius,fCapsuleSize/2);
+
+	 Matrix3x3 obbRot;
+
+	 obbRot[0][0] = mCapsuleRotation[0];
+	 obbRot[1][0] = mCapsuleRotation[1];
+	 obbRot[2][0] = mCapsuleRotation[2];
+
+	 obbRot[0][1] = mCapsuleRotation[4];
+	 obbRot[1][1] = mCapsuleRotation[5];
+	 obbRot[2][1] = mCapsuleRotation[6];
+
+	 obbRot[0][2] = mCapsuleRotation[8];
+	 obbRot[1][2] = mCapsuleRotation[9];
+	 obbRot[2][2] = mCapsuleRotation[10];
+
+	 OBB obbCCylinder(cCenter,cExtents,obbRot);
+
+	 Matrix4x4 CCylinderMatrix;
+	 MakeMatrix(vCapsulePosition, mCapsuleRotation, CCylinderMatrix);
+
+	 Matrix4x4 MeshMatrix;
+	 MakeMatrix(mTriMeshPos, mTriMeshRot, MeshMatrix);
+
+	 // TC results
+	 if (TriMesh->doBoxTC) {
+		 dxTriMesh::BoxTC* BoxTC = 0;
+		 for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
+			 if (TriMesh->BoxTCCache[i].Geom == gCylinder){
+				 BoxTC = &TriMesh->BoxTCCache[i];
+				 break;
+			 }
+		 }
+		 if (!BoxTC){
+			 TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
+
+			 BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
+			 BoxTC->Geom = gCylinder;
+			 BoxTC->FatCoeff = 1.0f;
+		 }
+
+		 // Intersect
+		 Collider.SetTemporalCoherence(true);
+		 Collider.Collide(*BoxTC, obbCCylinder, TriMesh->Data->BVTree, null, &MeshMatrix);
+	 }
+	 else {
+		 Collider.SetTemporalCoherence(false);
+		 Collider.Collide(dxTriMesh::defaultBoxCache, obbCCylinder, TriMesh->Data->BVTree, null,&MeshMatrix);
+	 }
+
+	 // Retrieve data
+	 int TriCount = Collider.GetNbTouchedPrimitives();
+	 const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
+
+	 if (TriCount != 0)
+	 {
+		 if (TriMesh->ArrayCallback != null)
+		 {
+			 TriMesh->ArrayCallback(TriMesh, gCylinder, Triangles, TriCount);
+		 }
+
+		int OutTriCount = 0;
+
+		// loop through all intersecting triangles
+		for (int i = 0; i < TriCount; i++)
+		{
+			if(ctContacts>=(iFlags & NUMC_MASK)) 
+			{
+				break;
+			}
+
+			const int& Triint = Triangles[i];
+			if (!Callback(TriMesh, gCylinder, Triint)) continue;
+
+
+			dVector3 dv[3];
+			FetchTriangle(TriMesh, Triint, mTriMeshPos, mTriMeshRot, dv);
+
+			// test this triangle
+			_cldTestOneTriangleVSCCylinder(dv[0],dv[1],dv[2]);
+			
+		}
+	 }
+
+	return _ProcessLocalContacts();
+}
+

src/source/ode/collision_trimesh_internal.h

@@ -0,0 +1,345 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+
+#ifndef _ODE_COLLISION_TRIMESH_INTERNAL_H_
+#define _ODE_COLLISION_TRIMESH_INTERNAL_H_
+
+int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideBTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideRTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideTTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideCCTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+//****************************************************************************
+// dxTriMesh class
+
+#ifdef TRIMESH_INTERNAL
+
+#include "collision_kernel.h"
+#include <ode/collision_trimesh.h>
+
+#define BAN_OPCODE_AUTOLINK
+#include "Opcode.h"
+using namespace Opcode;
+
+struct dxTriMeshData{
+	Model BVTree;
+	MeshInterface Mesh;
+
+    dxTriMeshData();
+    ~dxTriMeshData();
+    
+    void Build(const void* Vertices, int VertexStide, int VertexCount, 
+	       const void* Indices, int IndexCount, int TriStride, 
+	       const void* Normals, 
+	       bool Single);
+    
+        /* aabb in model space */
+        dVector3 AABBCenter;
+        dVector3 AABBExtents;
+
+    /* data for use in collison resolution */
+    const void* Normals;
+    Matrix4x4   last_trans;
+};
+
+
+struct dxTriMesh : public dxGeom{
+	// Callbacks
+	dTriCallback* Callback;
+	dTriArrayCallback* ArrayCallback;
+	dTriRayCallback* RayCallback;
+
+	// Data types
+	dxTriMeshData* Data;
+
+
+	// Colliders
+	static PlanesCollider _PlanesCollider;
+	static SphereCollider _SphereCollider;
+	static OBBCollider _OBBCollider;
+	static RayCollider _RayCollider;
+	static AABBTreeCollider _AABBTreeCollider;
+	static LSSCollider _LSSCollider;
+
+	// Some constants
+	static CollisionFaces Faces;
+
+	// Temporal coherence
+	struct SphereTC : public SphereCache{
+		dxGeom* Geom;
+	};
+	dArray<SphereTC> SphereTCCache;
+	static SphereCache defaultSphereCache;
+
+	struct BoxTC : public OBBCache{
+		dxGeom* Geom;
+	};
+	dArray<BoxTC> BoxTCCache;
+	static OBBCache defaultBoxCache;
+	
+	struct CCylinderTC : public LSSCache{
+		dxGeom* Geom;
+	};
+	dArray<CCylinderTC> CCylinderTCCache;
+	static LSSCache defaultCCylinderCache;
+
+	bool doSphereTC;
+	bool doBoxTC;
+	bool doCCylinderTC;
+
+	// Functions
+	dxTriMesh(dSpaceID Space, dTriMeshDataID Data);
+	~dxTriMesh();
+
+	void ClearTCCache();
+
+	int AABBTest(dxGeom* g, dReal aabb[6]);
+	void computeAABB();
+};
+
+// Fetches a contact
+inline dContactGeom* SAFECONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
+	dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff));
+	return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
+}
+
+// Fetches a triangle
+inline void FetchTriangle(dxTriMesh* TriMesh, int Index, dVector3 Out[3]){
+	VertexPointers VP;
+	TriMesh->Data->Mesh.GetTriangle(VP, Index);
+	for (int i = 0; i < 3; i++){
+		Out[i][0] = VP.Vertex[i]->x;
+		Out[i][1] = VP.Vertex[i]->y;
+		Out[i][2] = VP.Vertex[i]->z;
+		Out[i][3] = 0;
+	}
+}
+
+// Fetches a triangle
+inline void FetchTriangle(dxTriMesh* TriMesh, int Index, const dVector3 Position, const dMatrix3 Rotation, dVector3 Out[3]){
+	VertexPointers VP;
+	TriMesh->Data->Mesh.GetTriangle(VP, Index);
+	for (int i = 0; i < 3; i++){
+		dVector3 v;
+		v[0] = VP.Vertex[i]->x;
+		v[1] = VP.Vertex[i]->y;
+		v[2] = VP.Vertex[i]->z;
+		v[3] = 0;
+
+		dMULTIPLY0_331(Out[i], Rotation, v);
+		Out[i][0] += Position[0];
+		Out[i][1] += Position[1];
+		Out[i][2] += Position[2];
+		Out[i][3] = 0;
+	}
+}
+
+// Creates an OPCODE matrix from an ODE matrix
+inline Matrix4x4& MakeMatrix(const dVector3 Position, const dMatrix3 Rotation, Matrix4x4& Out){
+	Out.m[0][0] = Rotation[0];
+	Out.m[1][0] = Rotation[1];
+	Out.m[2][0] = Rotation[2];
+
+	Out.m[0][1] = Rotation[4];
+	Out.m[1][1] = Rotation[5];
+	Out.m[2][1] = Rotation[6];
+
+	Out.m[0][2] = Rotation[8];
+	Out.m[1][2] = Rotation[9];
+	Out.m[2][2] = Rotation[10];
+
+	Out.m[3][0] = Position[0];
+	Out.m[3][1] = Position[1];
+	Out.m[3][2] = Position[2];
+
+	Out.m[0][3] = 0.0f;
+	Out.m[1][3] = 0.0f;
+	Out.m[2][3] = 0.0f;
+	Out.m[3][3] = 1.0f;
+
+	return Out;
+}
+
+// Outputs a matrix to 3 vectors
+inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
+	Right[0] = Matrix[0 * 4 + 0];
+	Right[1] = Matrix[1 * 4 + 0];
+	Right[2] = Matrix[2 * 4 + 0];
+	Right[3] = REAL(0.0);
+	Up[0] = Matrix[0 * 4 + 1];
+	Up[1] = Matrix[1 * 4 + 1];
+	Up[2] = Matrix[2 * 4 + 1];
+	Up[3] = REAL(0.0);
+	Direction[0] = Matrix[0 * 4 + 2];
+	Direction[1] = Matrix[1 * 4 + 2];
+	Direction[2] = Matrix[2 * 4 + 2];
+	Direction[3] = REAL(0.0);
+}
+
+// Outputs a matrix to 3 vectors
+inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
+	Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
+}
+
+// Creates an OPCODE matrix from an ODE matrix
+inline Matrix4x4& MakeMatrix(dxGeom* g, Matrix4x4& Out){
+	const dVector3& Position = *(const dVector3*)dGeomGetPosition(g);
+	const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(g);
+	return MakeMatrix(Position, Rotation, Out);
+}
+
+// Finds barycentric
+inline void GetPointFromBarycentric(const dVector3 dv[3], dReal u, dReal v, dVector3 Out){
+	dReal w = REAL(1.0) - u - v;
+
+	Out[0] = (dv[0][0] * w) + (dv[1][0] * u) + (dv[2][0] * v);
+	Out[1] = (dv[0][1] * w) + (dv[1][1] * u) + (dv[2][1] * v);
+	Out[2] = (dv[0][2] * w) + (dv[1][2] * u) + (dv[2][2] * v);
+	Out[3] = (dv[0][3] * w) + (dv[1][3] * u) + (dv[2][3] * v);
+}
+
+// Performs a callback
+inline bool Callback(dxTriMesh* TriMesh, dxGeom* Object, int TriIndex){
+	if (TriMesh->Callback != null){
+		return (TriMesh->Callback(TriMesh, Object, TriIndex) != 0);
+	}
+	else return true;
+}
+
+// Some utilities
+template<class T> const T& dcMAX(const T& x, const T& y){
+	return x > y ? x : y;
+}
+
+template<class T> const T& dcMIN(const T& x, const T& y){
+	return x < y ? x : y;
+}
+
+dReal SqrDistancePointTri( const dVector3 p, const dVector3 triOrigin, 
+                           const dVector3 triEdge1, const dVector3 triEdge2,
+                           dReal* pfSParam = 0, dReal* pfTParam = 0 );
+
+dReal SqrDistanceSegments( const dVector3 seg1Origin, const dVector3 seg1Direction, 
+                           const dVector3 seg2Origin, const dVector3 seg2Direction,
+                           dReal* pfSegP0 = 0, dReal* pfSegP1 = 0 );
+
+dReal SqrDistanceSegTri( const dVector3 segOrigin, const dVector3 segEnd, 
+                         const dVector3 triOrigin, 
+                         const dVector3 triEdge1, const dVector3 triEdge2,
+                         dReal* t = 0, dReal* u = 0, dReal* v = 0 );
+
+inline
+void Vector3Subtract( const dVector3 left, const dVector3 right, dVector3 result )
+{
+    result[0] = left[0] - right[0];
+    result[1] = left[1] - right[1];
+    result[2] = left[2] - right[2];
+    result[3] = REAL(0.0);
+}
+
+inline
+void Vector3Add( const dVector3 left, const dVector3 right, dVector3 result )
+{
+    result[0] = left[0] + right[0];
+    result[1] = left[1] + right[1];
+    result[2] = left[2] + right[2];
+    result[3] = REAL(0.0);
+}
+
+inline
+void Vector3Negate( const dVector3 in, dVector3 out )
+{
+    out[0] = -in[0];
+    out[1] = -in[1];
+    out[2] = -in[2];
+    out[3] = REAL(0.0);
+}
+
+inline
+void Vector3Copy( const dVector3 in, dVector3 out )
+{
+    out[0] = in[0];
+    out[1] = in[1];
+    out[2] = in[2];
+    out[3] = REAL(0.0);
+}
+
+inline
+void Vector3Multiply( const dVector3 in, dReal scalar, dVector3 out )
+{
+    out[0] = in[0] * scalar;
+    out[1] = in[1] * scalar;
+    out[2] = in[2] * scalar;
+    out[3] = REAL(0.0);
+}
+
+inline
+void TransformVector3( const dVector3 in, 
+                       const dMatrix3 orientation, const dVector3 position, 
+                       dVector3 out )
+{
+    dMULTIPLY0_331( out, orientation, in );
+    out[0] += position[0];
+    out[1] += position[1];
+    out[2] += position[2];
+}
+
+//------------------------------------------------------------------------------
+/**
+  @brief Check for intersection between triangle and capsule.
+  
+  @param dist [out] Shortest distance squared between the triangle and 
+                    the capsule segment (central axis).
+  @param t    [out] t value of point on segment that's the shortest distance 
+                    away from the triangle, the coordinates of this point 
+                    can be found by (cap.seg.end - cap.seg.start) * t,
+                    or cap.seg.ipol(t).
+  @param u    [out] Barycentric coord on triangle.
+  @param v    [out] Barycentric coord on triangle.
+  @return True if intersection exists.
+  
+  The third Barycentric coord is implicit, ie. w = 1.0 - u - v
+  The Barycentric coords give the location of the point on the triangle
+  closest to the capsule (where the distance between the two shapes
+  is the shortest).
+*/
+inline
+bool IntersectCapsuleTri( const dVector3 segOrigin, const dVector3 segEnd, 
+                          const dReal radius, const dVector3 triOrigin, 
+                          const dVector3 triEdge0, const dVector3 triEdge1,
+                          dReal* dist, dReal* t, dReal* u, dReal* v )
+{
+    dReal sqrDist = SqrDistanceSegTri( segOrigin, segEnd, triOrigin, triEdge0, triEdge1, 
+                                       t, u, v );
+  
+    if ( dist )
+      *dist = sqrDist;
+    
+    return ( sqrDist <= (radius * radius) );
+}
+
+#endif	//TRIMESH_INTERNAL
+
+#endif	//_ODE_COLLISION_TRIMESH_INTERNAL_H_

src/source/ode/collision_trimesh_ray.cpp

@@ -0,0 +1,126 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_util.h"
+
+#define TRIMESH_INTERNAL
+#include "collision_trimesh_internal.h"
+
+int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){
+	dxTriMesh* TriMesh = (dxTriMesh*)g1;
+
+	const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
+	const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
+
+	RayCollider& Collider = TriMesh->_RayCollider;
+
+	dReal Length = dGeomRayGetLength(RayGeom);
+
+	int FirstContact, BackfaceCull;
+	dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull);
+	int ClosestHit = dGeomRayGetClosestHit(RayGeom);
+
+	Collider.SetFirstContact(FirstContact != 0);
+	Collider.SetClosestHit(ClosestHit != 0);
+	Collider.SetCulling(BackfaceCull != 0);
+	Collider.SetMaxDist(Length);
+
+	dVector3 Origin, Direction;
+	dGeomRayGet(RayGeom, Origin, Direction);
+
+	/* Make Ray */
+	Ray WorldRay;
+	WorldRay.mOrig.x = Origin[0];
+	WorldRay.mOrig.y = Origin[1];
+	WorldRay.mOrig.z = Origin[2];
+	WorldRay.mDir.x = Direction[0];
+	WorldRay.mDir.y = Direction[1];
+	WorldRay.mDir.z = Direction[2];
+
+	/* Intersect */
+	Matrix4x4 amatrix;
+        int TriCount = 0;
+        if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) {
+                TriCount = TriMesh->Faces.GetNbFaces();
+        }
+
+        if (TriCount == 0) {
+                return 0;
+        }
+	
+	const CollisionFace* Faces = TriMesh->Faces.GetFaces();
+
+	int OutTriCount = 0;
+	for (int i = 0; i < TriCount; i++) {
+		if (OutTriCount == (Flags & 0xffff)) {
+			break;
+		}
+		if (TriMesh->RayCallback == null ||
+                    TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID,
+                                         Faces[i].mU, Faces[i].mV)) {
+			const int& TriIndex = Faces[i].mFaceID;
+			if (!Callback(TriMesh, RayGeom, TriIndex)) {
+                                continue;
+                        }
+
+			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
+
+			dVector3 dv[3];
+			FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
+
+			float T = Faces[i].mDistance;
+			Contact->pos[0] = Origin[0] + (Direction[0] * T);
+			Contact->pos[1] = Origin[1] + (Direction[1] * T);
+			Contact->pos[2] = Origin[2] + (Direction[2] * T);
+			Contact->pos[3] = REAL(0.0);
+				
+			dVector3 vu;
+			vu[0] = dv[1][0] - dv[0][0];
+			vu[1] = dv[1][1] - dv[0][1];
+			vu[2] = dv[1][2] - dv[0][2];
+			vu[3] = REAL(0.0);
+				
+			dVector3 vv;
+			vv[0] = dv[2][0] - dv[0][0];
+			vv[1] = dv[2][1] - dv[0][1];
+			vv[2] = dv[2][2] - dv[0][2];
+			vv[3] = REAL(0.0);
+
+			dCROSS(Contact->normal, =, vv, vu);	// Reversed
+
+			dNormalize3(Contact->normal);
+
+			Contact->depth = T;
+			Contact->g1 = TriMesh;
+			Contact->g2 = RayGeom;
+				
+			OutTriCount++;
+		}
+	}
+	return OutTriCount;
+}

src/source/ode/collision_trimesh_sphere.cpp

@@ -0,0 +1,483 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// TriMesh code by Erwin de Vries.
+
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_util.h"
+
+#define TRIMESH_INTERNAL
+#include "collision_trimesh_internal.h"
+
+#define MERGECONTACTS
+
+// Ripped from Opcode 1.1.
+static bool GetContactData(const dVector3& Center, dReal Radius, const dVector3 Origin, const dVector3 Edge0, const dVector3 Edge1, dReal& Dist, float& u, float& v){
+	//calculate plane of triangle
+	dVector4 Plane;
+	dCROSS(Plane, =, Edge0, Edge1);
+	Plane[3] = dDOT(Plane, Origin);
+ 
+	//normalize
+        dNormalize4(Plane);
+  
+	/* If the center of the sphere is within the positive halfspace of the
+         * triangle's plane, allow a contact to be generated.
+         * If the center of the sphere made it into the positive halfspace of a
+         * back-facing triangle, then the physics update and/or velocity needs
+         * to be adjusted (penetration has occured anyway).
+         */
+  
+	float side = dDOT(Plane,Center) - Plane[3];
+  
+	if(side < 0.0f) {
+		return false;
+        }
+  
+        // now onto the bulk of the collision...
+
+	dVector3 Diff;
+	Diff[0] = Origin[0] - Center[0];
+	Diff[1] = Origin[1] - Center[1];
+	Diff[2] = Origin[2] - Center[2];
+	Diff[3] = Origin[3] - Center[3];
+
+	float A00 = dDOT(Edge0, Edge0);
+	float A01 = dDOT(Edge0, Edge1);
+	float A11 = dDOT(Edge1, Edge1);
+
+	float B0 = dDOT(Diff, Edge0);
+	float B1 = dDOT(Diff, Edge1);
+
+	float C = dDOT(Diff, Diff);
+
+	float Det = dFabs(A00 * A11 - A01 * A01);
+	u = A01 * B1 - A11 * B0;
+	v = A01 * B0 - A00 * B1;
+
+	float DistSq;
+
+	if (u + v <= Det){
+		if(u < REAL(0.0)){
+			if(v < REAL(0.0)){  // region 4
+				if(B0 < REAL(0.0)){
+					v = REAL(0.0);
+					if (-B0 >= A00){
+						u = REAL(1.0);
+						DistSq = A00 + REAL(2.0) * B0 + C;
+					}
+					else{
+						u = -B0 / A00;
+						DistSq = B0 * u + C;
+					}
+				}
+				else{
+					u = REAL(0.0);
+					if(B1 >= REAL(0.0)){
+						v = REAL(0.0);
+						DistSq = C;
+					}
+					else if(-B1 >= A11){
+						v = REAL(1.0);
+						DistSq = A11 + REAL(2.0) * B1 + C;
+					}
+					else{
+						v = -B1 / A11;
+						DistSq = B1 * v + C;
+					}
+				}
+			}
+			else{  // region 3
+				u = REAL(0.0);
+				if(B1 >= REAL(0.0)){
+					v = REAL(0.0);
+					DistSq = C;
+				}
+				else if(-B1 >= A11){
+					v = REAL(1.0);
+					DistSq = A11 + REAL(2.0) * B1 + C;
+				}
+				else{
+					v = -B1 / A11;
+					DistSq = B1 * v + C;
+				}
+			}
+		}
+		else if(v < REAL(0.0)){  // region 5
+			v = REAL(0.0);
+			if (B0 >= REAL(0.0)){
+				u = REAL(0.0);
+				DistSq = C;
+			}
+			else if (-B0 >= A00){
+				u = REAL(1.0);
+				DistSq = A00 + REAL(2.0) * B0 + C;
+			}
+			else{
+				u = -B0 / A00;
+				DistSq = B0 * u + C;
+			}
+		}
+		else{  // region 0
+			// minimum at interior point
+			if (Det == REAL(0.0)){
+				u = REAL(0.0);
+				v = REAL(0.0);
+				DistSq = FLT_MAX;
+			}
+			else{
+				float InvDet = REAL(1.0) / Det;
+				u *= InvDet;
+				v *= InvDet;
+				DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+			}
+		}
+	}
+	else{
+		float Tmp0, Tmp1, Numer, Denom;
+
+		if(u < REAL(0.0)){  // region 2
+			Tmp0 = A01 + B0;
+			Tmp1 = A11 + B1;
+			if (Tmp1 > Tmp0){
+				Numer = Tmp1 - Tmp0;
+				Denom = A00 - REAL(2.0) * A01 + A11;
+				if (Numer >= Denom){
+					u = REAL(1.0);
+					v = REAL(0.0);
+					DistSq = A00 + REAL(2.0) * B0 + C;
+				}
+				else{
+					u = Numer / Denom;
+					v = REAL(1.0) - u;
+					DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+				}
+			}
+			else{
+				u = REAL(0.0);
+				if(Tmp1 <= REAL(0.0)){
+					v = REAL(1.0);
+					DistSq = A11 + REAL(2.0) * B1 + C;
+				}
+				else if(B1 >= REAL(0.0)){
+					v = REAL(0.0);
+					DistSq = C;
+				}
+				else{
+					v = -B1 / A11;
+					DistSq = B1 * v + C;
+				}
+			}
+		}
+		else if(v < REAL(0.0)){  // region 6
+			Tmp0 = A01 + B1;
+			Tmp1 = A00 + B0;
+			if (Tmp1 > Tmp0){
+				Numer = Tmp1 - Tmp0;
+				Denom = A00 - REAL(2.0) * A01 + A11;
+				if (Numer >= Denom){
+					v = REAL(1.0);
+					u = REAL(0.0);
+					DistSq = A11 + REAL(2.0) * B1 + C;
+				}
+				else{
+					v = Numer / Denom;
+					u = REAL(1.0) - v;
+					DistSq =  u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+				}
+			}
+			else{
+				v = REAL(0.0);
+				if (Tmp1 <= REAL(0.0)){
+					u = REAL(1.0);
+					DistSq = A00 + REAL(2.0) * B0 + C;
+				}
+				else if(B0 >= REAL(0.0)){
+					u = REAL(0.0);
+					DistSq = C;
+				}
+				else{
+					u = -B0 / A00;
+					DistSq = B0 * u + C;
+				}
+			}
+		}
+		else{  // region 1
+			Numer = A11 + B1 - A01 - B0;
+			if (Numer <= REAL(0.0)){
+				u = REAL(0.0);
+				v = REAL(1.0);
+				DistSq = A11 + REAL(2.0) * B1 + C;
+			}
+			else{
+				Denom = A00 - REAL(2.0) * A01 + A11;
+				if (Numer >= Denom){
+					u = REAL(1.0);
+					v = REAL(0.0);
+					DistSq = A00 + REAL(2.0) * B0 + C;
+				}
+				else{
+					u = Numer / Denom;
+					v = REAL(1.0) - u;
+					DistSq = u * (A00 * u + A01 * v + REAL(2.0) * B0) + v * (A01 * u + A11 * v + REAL(2.0) * B1) + C;
+				}
+			}
+		}
+	}
+
+	Dist = dSqrt(dFabs(DistSq));
+
+	if (Dist <= Radius){
+		Dist = Radius - Dist;
+		return true;
+	}
+	else return false;
+}
+
+int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
+	dxTriMesh* TriMesh = (dxTriMesh*)g1;
+
+	// Init
+	const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
+	const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
+
+	SphereCollider& Collider = TriMesh->_SphereCollider;
+
+	const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
+	dReal Radius = dGeomSphereGetRadius(SphereGeom);
+
+	// Sphere
+	Sphere Sphere;
+	Sphere.mCenter.x = Position[0];
+	Sphere.mCenter.y = Position[1];
+	Sphere.mCenter.z = Position[2];
+	Sphere.mRadius = Radius;
+
+	Matrix4x4 amatrix;
+
+	// TC results
+	if (TriMesh->doSphereTC) {
+		dxTriMesh::SphereTC* sphereTC = 0;
+		for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
+			if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
+				sphereTC = &TriMesh->SphereTCCache[i];
+				break;
+			}
+		}
+
+		if (!sphereTC){
+			TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
+
+			sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
+			sphereTC->Geom = SphereGeom;
+		}
+		
+		// Intersect
+		Collider.SetTemporalCoherence(true);
+		Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null, 
+						 &MakeMatrix(TLPosition, TLRotation, amatrix));
+	}
+	else {
+		Collider.SetTemporalCoherence(false);
+		Collider.Collide(dxTriMesh::defaultSphereCache, Sphere, TriMesh->Data->BVTree, null, 
+						 &MakeMatrix(TLPosition, TLRotation, amatrix));
+ 	}
+
+	// get results
+	int TriCount = Collider.GetNbTouchedPrimitives();
+	const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
+
+	if (TriCount != 0){
+		if (TriMesh->ArrayCallback != null){
+			TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
+		}
+
+		int OutTriCount = 0;
+		for (int i = 0; i < TriCount; i++){
+			if (OutTriCount == (Flags & 0xffff)){
+				break;
+			}
+
+			const int& TriIndex = Triangles[i];
+
+			dVector3 dv[3];
+			FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
+
+			dVector3& v0 = dv[0];
+			dVector3& v1 = dv[1];
+			dVector3& v2 = dv[2];
+
+			dVector3 vu;
+			vu[0] = v1[0] - v0[0];
+			vu[1] = v1[1] - v0[1];
+			vu[2] = v1[2] - v0[2];
+			vu[3] = REAL(0.0);
+
+			dVector3 vv;
+			vv[0] = v2[0] - v0[0];
+			vv[1] = v2[1] - v0[1];
+			vv[2] = v2[2] - v0[2];
+			vv[3] = REAL(0.0);
+
+			dReal Depth;
+			float u, v;
+			if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
+				continue;	// Sphere doesnt hit triangle
+			}
+			dReal w = REAL(1.0) - u - v;
+
+			if (Depth < REAL(0.0)){
+				Depth = REAL(0.0);
+			}
+
+			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
+
+			Contact->pos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
+			Contact->pos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
+			Contact->pos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
+			Contact->pos[3] = REAL(0.0);
+
+			dVector4 Plane;
+			dCROSS(Plane, =, vv, vu);	// Reversed
+			Plane[3] = dDOT(Plane, v0);	// Using normal as plane.
+
+			dReal Area = dSqrt(dDOT(Plane, Plane));	// We can use this later
+			Plane[0] /= Area;
+			Plane[1] /= Area;
+			Plane[2] /= Area;
+			Plane[3] /= Area;
+
+			Contact->normal[0] = Plane[0];
+			Contact->normal[1] = Plane[1];
+			Contact->normal[2] = Plane[2];
+			Contact->normal[3] = REAL(0.0);
+
+			Contact->depth = Depth;
+
+			//Contact->g1 = TriMesh;
+			//Contact->g2 = SphereGeom;
+			
+			OutTriCount++;
+		}
+#ifdef MERGECONTACTS	// Merge all contacts into 1
+		if (OutTriCount != 0){
+			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
+			
+			if (OutTriCount != 1){
+				Contact->normal[0] *= Contact->depth;
+				Contact->normal[1] *= Contact->depth;
+				Contact->normal[2] *= Contact->depth;
+				Contact->normal[3] *= Contact->depth;
+
+				for (int i = 1; i < OutTriCount; i++){
+					dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
+					
+					Contact->pos[0] += TempContact->pos[0];
+					Contact->pos[1] += TempContact->pos[1];
+					Contact->pos[2] += TempContact->pos[2];
+					Contact->pos[3] += TempContact->pos[3];
+					
+					Contact->normal[0] += TempContact->normal[0] * TempContact->depth;
+					Contact->normal[1] += TempContact->normal[1] * TempContact->depth;
+					Contact->normal[2] += TempContact->normal[2] * TempContact->depth;
+					Contact->normal[3] += TempContact->normal[3] * TempContact->depth;
+				}
+			
+				Contact->pos[0] /= OutTriCount;
+				Contact->pos[1] /= OutTriCount;
+				Contact->pos[2] /= OutTriCount;
+				Contact->pos[3] /= OutTriCount;
+				
+				// Remember to divide in square space.
+				Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal) / OutTriCount);
+
+				dNormalize3(Contact->normal);
+			}
+
+			Contact->g1 = TriMesh;
+			Contact->g2 = SphereGeom;
+
+			return 1;
+		}
+		else return 0;
+#elif defined MERGECONTACTNORMALS	// Merge all normals, and distribute between all contacts
+		if (OutTriCount != 0){
+			if (OutTriCount != 1){
+				dVector3& Normal = SAFECONTACT(Flags, Contacts, 0, Stride)->normal;
+				Normal[0] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
+				Normal[1] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
+				Normal[2] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
+				Normal[3] *= SAFECONTACT(Flags, Contacts, 0, Stride)->depth;
+
+				for (int i = 1; i < OutTriCount; i++){
+					dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+					Normal[0] += Contact->normal[0] * Contact->depth;
+					Normal[1] += Contact->normal[1] * Contact->depth;
+					Normal[2] += Contact->normal[2] * Contact->depth;
+					Normal[3] += Contact->normal[3] * Contact->depth;
+				}
+				dNormalize3(Normal);
+
+				for (int i = 1; i < OutTriCount; i++){
+					dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+					Contact->normal[0] = Normal[0];
+					Contact->normal[1] = Normal[1];
+					Contact->normal[2] = Normal[2];
+					Contact->normal[3] = Normal[3];
+
+					Contact->g1 = TriMesh;
+					Contact->g2 = SphereGeom;
+				}
+			}
+			else{
+				SAFECONTACT(Flags, Contacts, 0, Stride)->g1 = TriMesh;
+				SAFECONTACT(Flags, Contacts, 0, Stride)->g2 = SphereGeom;
+			}
+
+			return OutTriCount;
+		}
+		else return 0;
+#else	//MERGECONTACTNORMALS	// Just gather penetration depths and return
+		for (int i = 0; i < OutTriCount; i++){
+			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
+
+			//Contact->depth = dSqrt(dDOT(Contact->normal, Contact->normal));
+
+			/*Contact->normal[0] /= Contact->depth;
+			Contact->normal[1] /= Contact->depth;
+			Contact->normal[2] /= Contact->depth;
+			Contact->normal[3] /= Contact->depth;*/
+
+			Contact->g1 = TriMesh;
+			Contact->g2 = SphereGeom;
+		}
+
+		return OutTriCount;
+#endif	// MERGECONTACTS
+	}
+	else return 0;
+}

src/source/ode/collision_util.cpp

@@ -0,0 +1,447 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+some useful collision utility stuff. this includes some API utility
+functions that are defined in the public header files.
+
+*/
+
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/odemath.h>
+#include "collision_util.h"
+
+//****************************************************************************
+
+int dCollideSpheres (dVector3 p1, dReal r1,
+		     dVector3 p2, dReal r2, dContactGeom *c)
+{
+  // printf ("d=%.2f  (%.2f %.2f %.2f) (%.2f %.2f %.2f) r1=%.2f r2=%.2f\n",
+  //	  d,p1[0],p1[1],p1[2],p2[0],p2[1],p2[2],r1,r2);
+
+  dReal d = dDISTANCE (p1,p2);
+  if (d > (r1 + r2)) return 0;
+  if (d <= 0) {
+    c->pos[0] = p1[0];
+    c->pos[1] = p1[1];
+    c->pos[2] = p1[2];
+    c->normal[0] = 1;
+    c->normal[1] = 0;
+    c->normal[2] = 0;
+    c->depth = r1 + r2;
+  }
+  else {
+    dReal d1 = dRecip (d);
+    c->normal[0] = (p1[0]-p2[0])*d1;
+    c->normal[1] = (p1[1]-p2[1])*d1;
+    c->normal[2] = (p1[2]-p2[2])*d1;
+    dReal k = REAL(0.5) * (r2 - r1 - d);
+    c->pos[0] = p1[0] + c->normal[0]*k;
+    c->pos[1] = p1[1] + c->normal[1]*k;
+    c->pos[2] = p1[2] + c->normal[2]*k;
+    c->depth = r1 + r2 - d;
+  }
+  return 1;
+}
+
+
+void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
+			   const dVector3 pb, const dVector3 ub,
+			   dReal *alpha, dReal *beta)
+{
+  dVector3 p;
+  p[0] = pb[0] - pa[0];
+  p[1] = pb[1] - pa[1];
+  p[2] = pb[2] - pa[2];
+  dReal uaub = dDOT(ua,ub);
+  dReal q1 =  dDOT(ua,p);
+  dReal q2 = -dDOT(ub,p);
+  dReal d = 1-uaub*uaub;
+  if (d <= REAL(0.0001)) {
+    // @@@ this needs to be made more robust
+    *alpha = 0;
+    *beta  = 0;
+  }
+  else {
+    d = dRecip(d);
+    *alpha = (q1 + uaub*q2)*d;
+    *beta  = (uaub*q1 + q2)*d;
+  }
+}
+
+
+// given two line segments A and B with endpoints a1-a2 and b1-b2, return the
+// points on A and B that are closest to each other (in cp1 and cp2).
+// in the case of parallel lines where there are multiple solutions, a
+// solution involving the endpoint of at least one line will be returned.
+// this will work correctly for zero length lines, e.g. if a1==a2 and/or
+// b1==b2.
+//
+// the algorithm works by applying the voronoi clipping rule to the features
+// of the line segments. the three features of each line segment are the two
+// endpoints and the line between them. the voronoi clipping rule states that,
+// for feature X on line A and feature Y on line B, the closest points PA and
+// PB between X and Y are globally the closest points if PA is in V(Y) and
+// PB is in V(X), where V(X) is the voronoi region of X.
+
+void dClosestLineSegmentPoints (const dVector3 a1, const dVector3 a2,
+				const dVector3 b1, const dVector3 b2,
+				dVector3 cp1, dVector3 cp2)
+{
+  dVector3 a1a2,b1b2,a1b1,a1b2,a2b1,a2b2,n;
+  dReal la,lb,k,da1,da2,da3,da4,db1,db2,db3,db4,det;
+
+#define SET2(a,b) a[0]=b[0]; a[1]=b[1]; a[2]=b[2];
+#define SET3(a,b,op,c) a[0]=b[0] op c[0]; a[1]=b[1] op c[1]; a[2]=b[2] op c[2];
+
+  // check vertex-vertex features
+
+  SET3 (a1a2,a2,-,a1);
+  SET3 (b1b2,b2,-,b1);
+  SET3 (a1b1,b1,-,a1);
+  da1 = dDOT(a1a2,a1b1);
+  db1 = dDOT(b1b2,a1b1);
+  if (da1 <= 0 && db1 >= 0) {
+    SET2 (cp1,a1);
+    SET2 (cp2,b1);
+    return;
+  }
+
+  SET3 (a1b2,b2,-,a1);
+  da2 = dDOT(a1a2,a1b2);
+  db2 = dDOT(b1b2,a1b2);
+  if (da2 <= 0 && db2 <= 0) {
+    SET2 (cp1,a1);
+    SET2 (cp2,b2);
+    return;
+  }
+
+  SET3 (a2b1,b1,-,a2);
+  da3 = dDOT(a1a2,a2b1);
+  db3 = dDOT(b1b2,a2b1);
+  if (da3 >= 0 && db3 >= 0) {
+    SET2 (cp1,a2);
+    SET2 (cp2,b1);
+    return;
+  }
+
+  SET3 (a2b2,b2,-,a2);
+  da4 = dDOT(a1a2,a2b2);
+  db4 = dDOT(b1b2,a2b2);
+  if (da4 >= 0 && db4 <= 0) {
+    SET2 (cp1,a2);
+    SET2 (cp2,b2);
+    return;
+  }
+
+  // check edge-vertex features.
+  // if one or both of the lines has zero length, we will never get to here,
+  // so we do not have to worry about the following divisions by zero.
+
+  la = dDOT(a1a2,a1a2);
+  if (da1 >= 0 && da3 <= 0) {
+    k = da1 / la;
+    SET3 (n,a1b1,-,k*a1a2);
+    if (dDOT(b1b2,n) >= 0) {
+      SET3 (cp1,a1,+,k*a1a2);
+      SET2 (cp2,b1);
+      return;
+    }
+  }
+
+  if (da2 >= 0 && da4 <= 0) {
+    k = da2 / la;
+    SET3 (n,a1b2,-,k*a1a2);
+    if (dDOT(b1b2,n) <= 0) {
+      SET3 (cp1,a1,+,k*a1a2);
+      SET2 (cp2,b2);
+      return;
+    }
+  }
+
+  lb = dDOT(b1b2,b1b2);
+  if (db1 <= 0 && db2 >= 0) {
+    k = -db1 / lb;
+    SET3 (n,-a1b1,-,k*b1b2);
+    if (dDOT(a1a2,n) >= 0) {
+      SET2 (cp1,a1);
+      SET3 (cp2,b1,+,k*b1b2);
+      return;
+    }
+  }
+
+  if (db3 <= 0 && db4 >= 0) {
+    k = -db3 / lb;
+    SET3 (n,-a2b1,-,k*b1b2);
+    if (dDOT(a1a2,n) <= 0) {
+      SET2 (cp1,a2);
+      SET3 (cp2,b1,+,k*b1b2);
+      return;
+    }
+  }
+
+  // it must be edge-edge
+
+  k = dDOT(a1a2,b1b2);
+  det = la*lb - k*k;
+  if (det <= 0) {
+    // this should never happen, but just in case...
+    SET2(cp1,a1);
+    SET2(cp2,b1);
+    return;
+  }
+  det = dRecip (det);
+  dReal alpha = (lb*da1 -  k*db1) * det;
+  dReal beta  = ( k*da1 - la*db1) * det;
+  SET3 (cp1,a1,+,alpha*a1a2);
+  SET3 (cp2,b1,+,beta*b1b2);
+
+# undef SET2
+# undef SET3
+}
+
+
+// a simple root finding algorithm is used to find the value of 't' that
+// satisfies:
+//		d|D(t)|^2/dt = 0
+// where:
+//		|D(t)| = |p(t)-b(t)|
+// where p(t) is a point on the line parameterized by t:
+//		p(t) = p1 + t*(p2-p1)
+// and b(t) is that same point clipped to the boundary of the box. in box-
+// relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components
+// each of which looks like this:
+//
+//	    t_lo     /
+//	      ______/    -->t
+//	     /     t_hi
+//	    /
+//
+// t_lo and t_hi are the t values where the line passes through the planes
+// corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt
+// in a piecewise fashion from t=0 to t=1, stopping at the point where
+// d|D(t)|^2/dt crosses from negative to positive.
+
+void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
+			    const dVector3 c, const dMatrix3 R,
+			    const dVector3 side,
+			    dVector3 lret, dVector3 bret)
+{
+  int i;
+
+  // compute the start and delta of the line p1-p2 relative to the box.
+  // we will do all subsequent computations in this box-relative coordinate
+  // system. we have to do a translation and rotation for each point.
+  dVector3 tmp,s,v;
+  tmp[0] = p1[0] - c[0];
+  tmp[1] = p1[1] - c[1];
+  tmp[2] = p1[2] - c[2];
+  dMULTIPLY1_331 (s,R,tmp);
+  tmp[0] = p2[0] - p1[0];
+  tmp[1] = p2[1] - p1[1];
+  tmp[2] = p2[2] - p1[2];
+  dMULTIPLY1_331 (v,R,tmp);
+
+  // mirror the line so that v has all components >= 0
+  dVector3 sign;
+  for (i=0; i<3; i++) {
+    if (v[i] < 0) {
+      s[i] = -s[i];
+      v[i] = -v[i];
+      sign[i] = -1;
+    }
+    else sign[i] = 1;
+  }
+
+  // compute v^2
+  dVector3 v2;
+  v2[0] = v[0]*v[0];
+  v2[1] = v[1]*v[1];
+  v2[2] = v[2]*v[2];
+
+  // compute the half-sides of the box
+  dReal h[3];
+  h[0] = REAL(0.5) * side[0];
+  h[1] = REAL(0.5) * side[1];
+  h[2] = REAL(0.5) * side[2];
+
+  // region is -1,0,+1 depending on which side of the box planes each
+  // coordinate is on. tanchor is the next t value at which there is a
+  // transition, or the last one if there are no more.
+  int region[3];
+  dReal tanchor[3];
+
+  // find the region and tanchor values for p1
+  for (i=0; i<3; i++) {
+    if (v[i] > 0) {
+      if (s[i] < -h[i]) {
+	region[i] = -1;
+	tanchor[i] = (-h[i]-s[i])/v[i];
+      }
+      else {
+	region[i] = (s[i] > h[i]);
+	tanchor[i] = (h[i]-s[i])/v[i];
+      }
+    }
+    else {
+      region[i] = 0;
+      tanchor[i] = 2;		// this will never be a valid tanchor
+    }
+  }
+
+  // compute d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point
+  dReal t=0;
+  dReal dd2dt = 0;
+  for (i=0; i<3; i++) dd2dt -= (region[i] ? v2[i] : 0) * tanchor[i];
+  if (dd2dt >= 0) goto got_answer;
+
+  do {
+    // find the point on the line that is at the next clip plane boundary
+    dReal next_t = 1;
+    for (i=0; i<3; i++) {
+      if (tanchor[i] > t && tanchor[i] < 1 && tanchor[i] < next_t)
+        next_t = tanchor[i];
+    }
+
+    // compute d|d|^2/dt for the next t
+    dReal next_dd2dt = 0;
+    for (i=0; i<3; i++) {
+      next_dd2dt += (region[i] ? v2[i] : 0) * (next_t - tanchor[i]);
+    }
+
+    // if the sign of d|d|^2/dt has changed, solution = the crossover point
+    if (next_dd2dt >= 0) {
+      dReal m = (next_dd2dt-dd2dt)/(next_t - t);
+      t -= dd2dt/m;
+      goto got_answer;
+    }
+
+    // advance to the next anchor point / region
+    for (i=0; i<3; i++) {
+      if (tanchor[i] == next_t) {
+	tanchor[i] = (h[i]-s[i])/v[i];
+	region[i]++;
+      }
+    }
+    t = next_t;
+    dd2dt = next_dd2dt;
+  }
+  while (t < 1);
+  t = 1;
+
+  got_answer:
+
+  // compute closest point on the line
+  for (i=0; i<3; i++) lret[i] = p1[i] + t*tmp[i];	// note: tmp=p2-p1
+
+  // compute closest point on the box
+  for (i=0; i<3; i++) {
+    tmp[i] = sign[i] * (s[i] + t*v[i]);
+    if (tmp[i] < -h[i]) tmp[i] = -h[i];
+    else if (tmp[i] > h[i]) tmp[i] = h[i];
+  }
+  dMULTIPLY0_331 (s,R,tmp);
+  for (i=0; i<3; i++) bret[i] = s[i] + c[i];
+}
+
+
+// given boxes (p1,R1,side1) and (p1,R1,side1), return 1 if they intersect
+// or 0 if not.
+
+int dBoxTouchesBox (const dVector3 p1, const dMatrix3 R1,
+		    const dVector3 side1, const dVector3 p2,
+		    const dMatrix3 R2, const dVector3 side2)
+{
+  // two boxes are disjoint if (and only if) there is a separating axis
+  // perpendicular to a face from one box or perpendicular to an edge from
+  // either box. the following tests are derived from:
+  //    "OBB Tree: A Hierarchical Structure for Rapid Interference Detection",
+  //    S.Gottschalk, M.C.Lin, D.Manocha., Proc of ACM Siggraph 1996.
+
+  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2.
+  // Qij is abs(Rij)
+  dVector3 p,pp;
+  dReal A1,A2,A3,B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
+    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33;
+
+  // get vector from centers of box 1 to box 2, relative to box 1
+  p[0] = p2[0] - p1[0];
+  p[1] = p2[1] - p1[1];
+  p[2] = p2[2] - p1[2];
+  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1
+
+  // get side lengths / 2
+  A1 = side1[0]*REAL(0.5); A2 = side1[1]*REAL(0.5); A3 = side1[2]*REAL(0.5);
+  B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
+
+  // for the following tests, excluding computation of Rij, in the worst case,
+  // 15 compares, 60 adds, 81 multiplies, and 24 absolutes.
+  // notation: R1=[u1 u2 u3], R2=[v1 v2 v3]
+
+  // separating axis = u1,u2,u3
+  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
+  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
+  if (dFabs(pp[0]) > (A1 + B1*Q11 + B2*Q12 + B3*Q13)) return 0;
+  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
+  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
+  if (dFabs(pp[1]) > (A2 + B1*Q21 + B2*Q22 + B3*Q23)) return 0;
+  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
+  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
+  if (dFabs(pp[2]) > (A3 + B1*Q31 + B2*Q32 + B3*Q33)) return 0;
+
+  // separating axis = v1,v2,v3
+  if (dFabs(dDOT41(R2+0,p)) > (A1*Q11 + A2*Q21 + A3*Q31 + B1)) return 0;
+  if (dFabs(dDOT41(R2+1,p)) > (A1*Q12 + A2*Q22 + A3*Q32 + B2)) return 0;
+  if (dFabs(dDOT41(R2+2,p)) > (A1*Q13 + A2*Q23 + A3*Q33 + B3)) return 0;
+
+  // separating axis = u1 x (v1,v2,v3)
+  if (dFabs(pp[2]*R21-pp[1]*R31) > A2*Q31 + A3*Q21 + B2*Q13 + B3*Q12) return 0;
+  if (dFabs(pp[2]*R22-pp[1]*R32) > A2*Q32 + A3*Q22 + B1*Q13 + B3*Q11) return 0;
+  if (dFabs(pp[2]*R23-pp[1]*R33) > A2*Q33 + A3*Q23 + B1*Q12 + B2*Q11) return 0;
+
+  // separating axis = u2 x (v1,v2,v3)
+  if (dFabs(pp[0]*R31-pp[2]*R11) > A1*Q31 + A3*Q11 + B2*Q23 + B3*Q22) return 0;
+  if (dFabs(pp[0]*R32-pp[2]*R12) > A1*Q32 + A3*Q12 + B1*Q23 + B3*Q21) return 0;
+  if (dFabs(pp[0]*R33-pp[2]*R13) > A1*Q33 + A3*Q13 + B1*Q22 + B2*Q21) return 0;
+
+  // separating axis = u3 x (v1,v2,v3)
+  if (dFabs(pp[1]*R11-pp[0]*R21) > A1*Q21 + A2*Q11 + B2*Q33 + B3*Q32) return 0;
+  if (dFabs(pp[1]*R12-pp[0]*R22) > A1*Q22 + A2*Q12 + B1*Q33 + B3*Q31) return 0;
+  if (dFabs(pp[1]*R13-pp[0]*R23) > A1*Q23 + A2*Q13 + B1*Q32 + B2*Q31) return 0;
+
+  return 1;
+}
+
+//****************************************************************************
+// other utility functions
+
+void dInfiniteAABB (dxGeom *geom, dReal aabb[6])
+{
+  aabb[0] = -dInfinity;
+  aabb[1] = dInfinity;
+  aabb[2] = -dInfinity;
+  aabb[3] = dInfinity;
+  aabb[4] = -dInfinity;
+  aabb[5] = dInfinity;
+}

src/source/ode/collision_util.h

@@ -0,0 +1,72 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+some useful collision utility stuff.
+
+*/
+
+#ifndef _ODE_COLLISION_UTIL_H_
+#define _ODE_COLLISION_UTIL_H_
+
+#include <ode/common.h>
+#include <ode/contact.h>
+
+
+// given a pointer `p' to a dContactGeom, return the dContactGeom at
+// p + skip bytes.
+#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
+
+
+// if the spheres (p1,r1) and (p2,r2) collide, set the contact `c' and
+// return 1, else return 0.
+
+int dCollideSpheres (dVector3 p1, dReal r1,
+		     dVector3 p2, dReal r2, dContactGeom *c);
+
+
+// given two lines
+//    qa = pa + alpha* ua
+//    qb = pb + beta * ub
+// where pa,pb are two points, ua,ub are two unit length vectors, and alpha,
+// beta go from [-inf,inf], return alpha and beta such that qa and qb are
+// as close as possible
+
+void dLineClosestApproach (const dVector3 pa, const dVector3 ua,
+			   const dVector3 pb, const dVector3 ub,
+			   dReal *alpha, dReal *beta);
+
+
+// given a line segment p1-p2 and a box (center 'c', rotation 'R', side length
+// vector 'side'), compute the points of closest approach between the box
+// and the line. return these points in 'lret' (the point on the line) and
+// 'bret' (the point on the box). if the line actually penetrates the box
+// then the solution is not unique, but only one solution will be returned.
+// in this case the solution points will coincide.
+
+void dClosestLineBoxPoints (const dVector3 p1, const dVector3 p2,
+			    const dVector3 c, const dMatrix3 R,
+			    const dVector3 side,
+			    dVector3 lret, dVector3 bret);
+
+#endif

src/source/ode/error.cpp

@@ -0,0 +1,165 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/config.h>
+#include <ode/error.h>
+
+
+static dMessageFunction *error_function = 0;
+static dMessageFunction *debug_function = 0;
+static dMessageFunction *message_function = 0;
+
+
+extern "C" void dSetErrorHandler (dMessageFunction *fn)
+{
+  error_function = fn;
+}
+
+
+extern "C" void dSetDebugHandler (dMessageFunction *fn)
+{
+  debug_function = fn;
+}
+
+
+extern "C" void dSetMessageHandler (dMessageFunction *fn)
+{
+  message_function = fn;
+}
+
+
+extern "C" dMessageFunction *dGetErrorHandler()
+{
+  return error_function;
+}
+
+
+extern "C" dMessageFunction *dGetDebugHandler()
+{
+  return debug_function;
+}
+
+
+extern "C" dMessageFunction *dGetMessageHandler()
+{
+  return message_function;
+}
+
+
+static void printMessage (int num, const char *msg1, const char *msg2,
+			  va_list ap)
+{
+  fflush (stderr);
+  fflush (stdout);
+  if (num) fprintf (stderr,"\n%s %d: ",msg1,num);
+  else fprintf (stderr,"\n%s: ",msg1);
+  vfprintf (stderr,msg2,ap);
+  fprintf (stderr,"\n");
+  fflush (stderr);
+}
+
+//****************************************************************************
+// unix
+
+#ifndef WIN32
+
+extern "C" void dError (int num, const char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  if (error_function) error_function (num,msg,ap);
+  else printMessage (num,"ODE Error",msg,ap);
+  exit (1);
+}
+
+
+extern "C" void dDebug (int num, const char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  if (debug_function) debug_function (num,msg,ap);
+  else printMessage (num,"ODE INTERNAL ERROR",msg,ap);
+  // *((char *)0) = 0;   ... commit SEGVicide
+  abort();
+}
+
+
+extern "C" void dMessage (int num, const char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  if (message_function) message_function (num,msg,ap);
+  else printMessage (num,"ODE Message",msg,ap);
+}
+
+#endif
+
+//****************************************************************************
+// windows
+
+#ifdef WIN32
+
+#include "windows.h"
+
+
+extern "C" void dError (int num, const char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  if (error_function) error_function (num,msg,ap);
+  else {
+    char s[1000],title[100];
+    _snprintf (title,sizeof(title),"ODE Error %d",num);
+    vsnprintf (s,sizeof(s),msg,ap);
+    s[sizeof(s)-1] = 0;
+    MessageBox(0,s,title,MB_OK | MB_ICONWARNING);
+  }
+  exit (1);
+}
+
+
+extern "C" void dDebug (int num, const char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  if (debug_function) debug_function (num,msg,ap);
+  else {
+    char s[1000],title[100];
+    _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num);
+    vsnprintf (s,sizeof(s),msg,ap);
+    s[sizeof(s)-1] = 0;
+    MessageBox(0,s,title,MB_OK | MB_ICONSTOP);
+  }
+  abort();
+}
+
+
+extern "C" void dMessage (int num, const char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  if (message_function) message_function (num,msg,ap);
+  else printMessage (num,"ODE Message",msg,ap);
+}
+
+
+#endif

src/source/ode/export-dif.cpp

@@ -0,0 +1,533 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+ * Export a DIF (Dynamics Interchange Format) file.
+ */
+
+
+// @@@ TODO:
+//	* export all spaces, and geoms in spaces, not just ones attached to bodies
+//	  (separate export function?)
+//	* say the space each geom is in, so reader can construct space heirarchy
+//	* limot --> separate out into limits and motors?
+//	* make sure ODE-specific parameters divided out
+
+
+#include "ode/ode.h"
+#include "objects.h"
+#include "joint.h"
+#include "collision_kernel.h"
+
+//***************************************************************************
+// utility
+
+struct PrintingContext {
+	FILE *file;		// file to write to
+	int precision;		// digits of precision to print
+	int indent;		// number of levels of indent
+
+	void printIndent();
+	void printReal (dReal x);
+	void print (const char *name, int x);
+	void print (const char *name, dReal x);
+	void print (const char *name, const dReal *x, int n=3);
+	void print (const char *name, const char *x=0);
+	void printNonzero (const char *name, dReal x);
+	void printNonzero (const char *name, const dReal x[3]);
+};
+
+
+void PrintingContext::printIndent()
+{
+	for (int i=0; i<indent; i++) fputc ('\t',file);
+}
+
+
+void PrintingContext::print (const char *name, int x)
+{
+	printIndent();
+	fprintf (file,"%s = %d,\n",name,x);
+}
+
+
+void PrintingContext::printReal (dReal x)
+{
+	if (x==dInfinity) {
+		fprintf (file,"inf");
+	}
+	else if (x==-dInfinity) {
+		fprintf (file,"-inf");
+	}
+	else {
+		fprintf (file,"%.*g",precision,x);
+	}
+}
+
+
+void PrintingContext::print (const char *name, dReal x)
+{
+	printIndent();
+	fprintf (file,"%s = ",name);
+	printReal (x);
+	fprintf (file,",\n");
+}
+
+
+void PrintingContext::print (const char *name, const dReal *x, int n)
+{
+	printIndent();
+	fprintf (file,"%s = {",name);
+	for (int i=0; i<n; i++) {
+		printReal (x[i]);
+		if (i < n-1) fputc (',',file);
+	}
+	fprintf (file,"},\n");
+}
+
+
+void PrintingContext::print (const char *name, const char *x)
+{
+	printIndent();
+	if (x) {
+		fprintf (file,"%s = \"%s\",\n",name,x);
+	}
+	else {
+		fprintf (file,"%s\n",name);
+	}
+}
+
+
+void PrintingContext::printNonzero (const char *name, dReal x)
+{
+	if (x != 0) print (name,x);
+}
+
+
+void PrintingContext::printNonzero (const char *name, const dReal x[3])
+{
+	if (x[0] != 0 && x[1] != 0 && x[2] != 0) print (name,x);
+}
+
+//***************************************************************************
+// joints
+
+
+static void printLimot (PrintingContext &c, dxJointLimitMotor &limot, int num)
+{
+	if (num >= 0) {
+		c.printIndent();
+		fprintf (c.file,"limit%d = {\n",num);
+	}
+	else {
+		c.print ("limit = {");
+	}
+	c.indent++;
+	c.print ("low_stop",limot.lostop);
+	c.print ("high_stop",limot.histop);
+	c.printNonzero ("bounce",limot.bounce);
+	c.print ("ODE = {");
+	c.indent++;
+	c.printNonzero ("stop_erp",limot.stop_erp);
+	c.printNonzero ("stop_cfm",limot.stop_cfm);
+	c.indent--;
+	c.print ("},");
+	c.indent--;
+	c.print ("},");
+
+	if (num >= 0) {
+		c.printIndent();
+		fprintf (c.file,"motor%d = {\n",num);
+	}
+	else {
+		c.print ("motor = {");
+	}
+	c.indent++;
+	c.printNonzero ("vel",limot.vel);
+	c.printNonzero ("fmax",limot.fmax);
+	c.print ("ODE = {");
+	c.indent++;
+	c.printNonzero ("fudge_factor",limot.fudge_factor);
+	c.printNonzero ("normal_cfm",limot.normal_cfm);
+	c.indent--;
+	c.print ("},");
+	c.indent--;
+	c.print ("},");
+}
+
+
+static const char *getJointName (dxJoint *j)
+{
+	switch (j->vtable->typenum) {
+		case dJointTypeBall: return "ball";
+		case dJointTypeHinge: return "hinge";
+		case dJointTypeSlider: return "slider";
+		case dJointTypeContact: return "contact";
+		case dJointTypeUniversal: return "universal";
+		case dJointTypeHinge2: return "ODE_hinge2";
+		case dJointTypeFixed: return "fixed";
+		case dJointTypeNull: return "null";
+		case dJointTypeAMotor: return "ODE_angular_motor";
+	}
+	return "unknown";
+}
+
+
+static void printBall (PrintingContext &c, dxJoint *j)
+{
+	dxJointBall *b = (dxJointBall*) j;
+	c.print ("anchor1",b->anchor1);
+	c.print ("anchor2",b->anchor2);
+}
+
+
+static void printHinge (PrintingContext &c, dxJoint *j)
+{
+	dxJointHinge *h = (dxJointHinge*) j;
+	c.print ("anchor1",h->anchor1);
+	c.print ("anchor2",h->anchor2);
+	c.print ("axis1",h->axis1);
+	c.print ("axis2",h->axis2);
+	c.print ("qrel",h->qrel,4);
+	printLimot (c,h->limot,-1);
+}
+
+
+static void printSlider (PrintingContext &c, dxJoint *j)
+{
+	dxJointSlider *s = (dxJointSlider*) j;
+	c.print ("axis1",s->axis1);
+	c.print ("qrel",s->qrel,4);
+	c.print ("offset",s->offset);
+	printLimot (c,s->limot,-1);
+}
+
+
+static void printContact (PrintingContext &c, dxJoint *j)
+{
+	dxJointContact *ct = (dxJointContact*) j;
+	int mode = ct->contact.surface.mode;
+	c.print ("pos",ct->contact.geom.pos);
+	c.print ("normal",ct->contact.geom.normal);
+	c.print ("depth",ct->contact.geom.depth);
+	//@@@ may want to write the geoms g1 and g2 that are involved, for debugging.
+	//    to do this we must have written out all geoms in all spaces, not just
+	//    geoms that are attached to bodies.
+	c.print ("mu",ct->contact.surface.mu);
+	if (mode & dContactMu2) c.print ("mu2",ct->contact.surface.mu2);
+	if (mode & dContactBounce) c.print ("bounce",ct->contact.surface.bounce);
+	if (mode & dContactBounce) c.print ("bounce_vel",ct->contact.surface.bounce_vel);
+	if (mode & dContactSoftERP) c.print ("soft_ERP",ct->contact.surface.soft_erp);
+	if (mode & dContactSoftCFM) c.print ("soft_CFM",ct->contact.surface.soft_cfm);
+	if (mode & dContactMotion1) c.print ("motion1",ct->contact.surface.motion1);
+	if (mode & dContactMotion2) c.print ("motion2",ct->contact.surface.motion2);
+	if (mode & dContactSlip1) c.print ("slip1",ct->contact.surface.slip1);
+	if (mode & dContactSlip2) c.print ("slip2",ct->contact.surface.slip2);
+	int fa = 0;		// friction approximation code
+	if (mode & dContactApprox1_1) fa |= 1;
+	if (mode & dContactApprox1_2) fa |= 2;
+	if (fa) c.print ("friction_approximation",fa);
+	if (mode & dContactFDir1) c.print ("fdir1",ct->contact.fdir1);
+}
+
+
+static void printUniversal (PrintingContext &c, dxJoint *j)
+{
+	dxJointUniversal *u = (dxJointUniversal*) j;
+	c.print ("anchor1",u->anchor1);
+	c.print ("anchor2",u->anchor2);
+	c.print ("axis1",u->axis1);
+	c.print ("axis2",u->axis2);
+	c.print ("qrel1",u->qrel1,4);
+	c.print ("qrel2",u->qrel2,4);
+	printLimot (c,u->limot1,1);
+	printLimot (c,u->limot2,2);
+}
+
+
+static void printHinge2 (PrintingContext &c, dxJoint *j)
+{
+	dxJointHinge2 *h = (dxJointHinge2*) j;
+	c.print ("anchor1",h->anchor1);
+	c.print ("anchor2",h->anchor2);
+	c.print ("axis1",h->axis1);
+	c.print ("axis2",h->axis2);
+	c.print ("v1",h->v1);	//@@@ much better to write out 'qrel' here, if it's available
+	c.print ("v2",h->v2);
+	c.print ("susp_erp",h->susp_erp);
+	c.print ("susp_cfm",h->susp_cfm);
+	printLimot (c,h->limot1,1);
+	printLimot (c,h->limot2,2);
+}
+
+
+static void printFixed (PrintingContext &c, dxJoint *j)
+{
+	dxJointFixed *f = (dxJointFixed*) j;
+	c.print ("qrel",f->qrel);
+	c.print ("offset",f->offset);
+}
+
+
+static void printAMotor (PrintingContext &c, dxJoint *j)
+{
+	dxJointAMotor *a = (dxJointAMotor*) j;
+	c.print ("num",a->num);
+	c.print ("mode",a->mode);
+	c.printIndent();
+	fprintf (c.file,"rel = {%d,%d,%d},\n",a->rel[0],a->rel[1],a->rel[2]);
+	c.print ("axis1",a->axis[0]);
+	c.print ("axis2",a->axis[1]);
+	c.print ("axis3",a->axis[2]);
+	for (int i=0; i<3; i++) printLimot (c,a->limot[i],i+1);
+	c.print ("angle1",a->angle[0]);
+	c.print ("angle2",a->angle[1]);
+	c.print ("angle3",a->angle[2]);
+}
+
+//***************************************************************************
+// geometry
+
+static void printGeom (PrintingContext &c, dxGeom *g);
+
+static void printSphere (PrintingContext &c, dxGeom *g)
+{
+	c.print ("type","sphere");
+	c.print ("radius",dGeomSphereGetRadius (g));
+}
+
+
+static void printBox (PrintingContext &c, dxGeom *g)
+{
+	dVector3 sides;
+	dGeomBoxGetLengths (g,sides);
+	c.print ("type","box");
+	c.print ("sides",sides);
+}
+
+
+
+static void printCCylinder (PrintingContext &c, dxGeom *g)
+{
+	dReal radius,length;
+	dGeomCCylinderGetParams (g,&radius,&length);
+	c.print ("type","capsule");
+	c.print ("radius",radius);
+	c.print ("length",length);
+}
+
+
+static void printPlane (PrintingContext &c, dxGeom *g)
+{
+	dVector4 e;
+	dGeomPlaneGetParams (g,e);
+	c.print ("type","plane");
+	c.print ("normal",e);
+	c.print ("d",e[3]);
+}
+
+
+
+static void printRay (PrintingContext &c, dxGeom *g)
+{
+	dReal length = dGeomRayGetLength (g);
+	c.print ("type","ray");
+	c.print ("length",length);
+}
+
+
+
+static void printGeomTransform (PrintingContext &c, dxGeom *g)
+{
+	dxGeom *g2 = dGeomTransformGetGeom (g);
+	const dReal *pos = dGeomGetPosition (g2);
+	dQuaternion q;
+	dGeomGetQuaternion (g2,q);
+	c.print ("type","transform");
+	c.print ("pos",pos);
+	c.print ("q",q,4);
+	c.print ("geometry = {");
+	c.indent++;
+	printGeom (c,g2);
+	c.indent--;
+	c.print ("}");
+}
+
+
+
+static void printTriMesh (PrintingContext &c, dxGeom *g)
+{
+	c.print ("type","trimesh");
+	//@@@ i don't think that the trimesh accessor functions are really
+	//    sufficient to read out all the triangle data, and anyway we
+	//    should have a method of not duplicating trimesh data that is
+	//    shared.
+}
+
+
+static void printGeom (PrintingContext &c, dxGeom *g)
+{
+	unsigned long category = dGeomGetCategoryBits (g);
+	if (category != (unsigned long)(~0)) {
+		c.printIndent();
+		fprintf (c.file,"category_bits = %lu\n",category);
+	}
+	unsigned long collide = dGeomGetCollideBits (g);
+	if (collide != (unsigned long)(~0)) {
+		c.printIndent();
+		fprintf (c.file,"collide_bits = %lu\n",collide);
+	}
+	if (!dGeomIsEnabled (g)) {
+		c.print ("disabled",1);
+	}
+	switch (g->type) {
+		case dSphereClass: printSphere (c,g); break;
+		case dBoxClass: printBox (c,g); break;
+		case dCCylinderClass: printCCylinder (c,g); break;
+		case dPlaneClass: printPlane (c,g); break;
+		case dRayClass: printRay (c,g); break;
+		case dGeomTransformClass: printGeomTransform (c,g); break;
+		case dTriMeshClass: printTriMesh (c,g); break;
+	}
+}
+
+//***************************************************************************
+// world
+
+void dWorldExportDIF (dWorldID w, FILE *file, const char *prefix)
+{
+	PrintingContext c;
+	c.file = file;
+#if defined(dSINGLE)
+	c.precision = 7;
+#else
+	c.precision = 15;
+#endif
+	c.indent = 1;
+
+	fprintf (file,"-- Dynamics Interchange Format v0.1\n\n%sworld = dynamics.world {\n",prefix);
+	c.print ("gravity",w->gravity);
+	c.print ("ODE = {");
+	c.indent++;
+	c.print ("ERP",w->global_erp);
+	c.print ("CFM",w->global_cfm);
+	c.print ("auto_disable = {");
+	c.indent++;
+	c.print ("linear_threshold",w->adis.linear_threshold);
+	c.print ("angular_threshold",w->adis.angular_threshold);
+	c.print ("idle_time",w->adis.idle_time);
+	c.print ("idle_steps",w->adis.idle_steps);
+	fprintf (file,"\t\t},\n\t},\n}\n");
+	c.indent -= 3;
+
+	// bodies
+	int num = 0;
+	fprintf (file,"%sbody = {}\n",prefix);
+	for (dxBody *b=w->firstbody; b; b=(dxBody*)b->next) {
+		b->tag = num;
+		fprintf (file,"%sbody[%d] = dynamics.body {\n\tworld = %sworld,\n",prefix,num,prefix);
+		c.indent++;
+		c.print ("pos",b->pos);
+		c.print ("q",b->q,4);
+		c.print ("lvel",b->lvel);
+		c.print ("avel",b->avel);
+		c.print ("mass",b->mass.mass);
+		fprintf (file,"\tI = {{");
+		for (int i=0; i<3; i++) {
+			for (int j=0; j<3; j++) {
+				c.printReal (b->mass.I[i*4+j]);
+				if (j < 2) fputc (',',file);
+			}
+			if (i < 2) fprintf (file,"},{");
+		}
+		fprintf (file,"}},\n");
+		c.printNonzero ("com",b->mass.c);
+		c.print ("ODE = {");
+		c.indent++;
+		if (b->flags & dxBodyFlagFiniteRotation) c.print ("finite_rotation",1);
+		if (b->flags & dxBodyDisabled) c.print ("disabled",1);
+		if (b->flags & dxBodyNoGravity) c.print ("no_gravity",1);
+		if (b->flags & dxBodyAutoDisable) {
+			c.print ("auto_disable = {");
+			c.indent++;
+			c.print ("linear_threshold",b->adis.linear_threshold);
+			c.print ("angular_threshold",b->adis.angular_threshold);
+			c.print ("idle_time",b->adis.idle_time);
+			c.print ("idle_steps",b->adis.idle_steps);
+			c.print ("time_left",b->adis_timeleft);
+			c.print ("steps_left",b->adis_stepsleft);
+			c.indent--;
+			c.print ("},");
+		}
+		c.printNonzero ("facc",b->facc);
+		c.printNonzero ("tacc",b->tacc);
+		if (b->flags & dxBodyFlagFiniteRotationAxis) {
+			c.print ("finite_rotation_axis",b->finite_rot_axis);
+		}
+		c.indent--;
+		c.print ("},");
+		if (b->geom) {
+			c.print ("geometry = {");
+			c.indent++;
+			for (dxGeom *g=b->geom; g; g=g->body_next) {
+				c.print ("{");
+				c.indent++;
+				printGeom (c,g);
+				c.indent--;
+				c.print ("},");
+			}
+			c.indent--;
+			c.print ("},");
+		}
+		c.indent--;
+		c.print ("}");
+		num++;
+	}
+
+	// joints
+	num = 0;
+	fprintf (file,"%sjoint = {}\n",prefix);
+	for (dxJoint *j=w->firstjoint; j; j=(dxJoint*)j->next) {
+		c.indent++;
+		const char *name = getJointName (j);
+		fprintf (file,
+			"%sjoint[%d] = dynamics.%s_joint {\n"
+			"\tworld = %sworld,\n"
+			"\tbody = {%sbody[%d]"
+			,prefix,num,name,prefix,prefix,j->node[0].body->tag);
+		if (j->node[1].body) fprintf (file,",%sbody[%d]",prefix,j->node[1].body->tag);
+		fprintf (file,"},\n");
+		switch (j->vtable->typenum) {
+			case dJointTypeBall: printBall (c,j); break;
+			case dJointTypeHinge: printHinge (c,j); break;
+			case dJointTypeSlider: printSlider (c,j); break;
+			case dJointTypeContact: printContact (c,j); break;
+			case dJointTypeUniversal: printUniversal (c,j); break;
+			case dJointTypeHinge2: printHinge2 (c,j); break;
+			case dJointTypeFixed: printFixed (c,j); break;
+			case dJointTypeAMotor: printAMotor (c,j); break;
+		}		
+		c.indent--;
+		c.print ("}");
+		num++;
+	}
+}

src/source/ode/fastdot.c

@@ -0,0 +1,30 @@
+/* generated code, do not edit. */
+
+#include "ode/matrix.h"
+
+
+dReal dDot (const dReal *a, const dReal *b, int n)
+{  
+  dReal p0,q0,m0,p1,q1,m1,sum;
+  sum = 0;
+  n -= 2;
+  while (n >= 0) {
+    p0 = a[0]; q0 = b[0];
+    m0 = p0 * q0;
+    p1 = a[1]; q1 = b[1];
+    m1 = p1 * q1;
+    sum += m0;
+    sum += m1;
+    a += 2;
+    b += 2;
+    n -= 2;
+  }
+  n += 2;
+  while (n > 0) {
+    sum += (*a) * (*b);
+    a++;
+    b++;
+    n--;
+  }
+  return sum;
+}

src/source/ode/fastldlt.c

@@ -0,0 +1,381 @@
+/* generated code, do not edit. */
+
+#include "ode/matrix.h"
+
+/* solve L*X=B, with B containing 1 right hand sides.
+ * L is an n*n lower triangular matrix with ones on the diagonal.
+ * L is stored by rows and its leading dimension is lskip.
+ * B is an n*1 matrix that contains the right hand sides.
+ * B is stored by columns and its leading dimension is also lskip.
+ * B is overwritten with X.
+ * this processes blocks of 2*2.
+ * if this is in the factorizer source file, n must be a multiple of 2.
+ */
+
+static void dSolveL1_1 (const dReal *L, dReal *B, int n, int lskip1)
+{  
+  /* declare variables - Z matrix, p and q vectors, etc */
+  dReal Z11,m11,Z21,m21,p1,q1,p2,*ex;
+  const dReal *ell;
+  int i,j;
+  /* compute all 2 x 1 blocks of X */
+  for (i=0; i < n; i+=2) {
+    /* compute all 2 x 1 block of X, from rows i..i+2-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    Z21=0;
+    ell = L + i*lskip1;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-2; j >= 0; j -= 2) {
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[0];
+      q1=ex[0];
+      m11 = p1 * q1;
+      p2=ell[lskip1];
+      m21 = p2 * q1;
+      Z11 += m11;
+      Z21 += m21;
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[1];
+      q1=ex[1];
+      m11 = p1 * q1;
+      p2=ell[1+lskip1];
+      m21 = p2 * q1;
+      /* advance pointers */
+      ell += 2;
+      ex += 2;
+      Z11 += m11;
+      Z21 += m21;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 2;
+    for (; j > 0; j--) {
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[0];
+      q1=ex[0];
+      m11 = p1 * q1;
+      p2=ell[lskip1];
+      m21 = p2 * q1;
+      /* advance pointers */
+      ell += 1;
+      ex += 1;
+      Z11 += m11;
+      Z21 += m21;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+    p1 = ell[lskip1];
+    Z21 = ex[1] - Z21 - p1*Z11;
+    ex[1] = Z21;
+    /* end of outer loop */
+  }
+}
+
+/* solve L*X=B, with B containing 2 right hand sides.
+ * L is an n*n lower triangular matrix with ones on the diagonal.
+ * L is stored by rows and its leading dimension is lskip.
+ * B is an n*2 matrix that contains the right hand sides.
+ * B is stored by columns and its leading dimension is also lskip.
+ * B is overwritten with X.
+ * this processes blocks of 2*2.
+ * if this is in the factorizer source file, n must be a multiple of 2.
+ */
+
+static void dSolveL1_2 (const dReal *L, dReal *B, int n, int lskip1)
+{  
+  /* declare variables - Z matrix, p and q vectors, etc */
+  dReal Z11,m11,Z12,m12,Z21,m21,Z22,m22,p1,q1,p2,q2,*ex;
+  const dReal *ell;
+  int i,j;
+  /* compute all 2 x 2 blocks of X */
+  for (i=0; i < n; i+=2) {
+    /* compute all 2 x 2 block of X, from rows i..i+2-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    Z12=0;
+    Z21=0;
+    Z22=0;
+    ell = L + i*lskip1;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-2; j >= 0; j -= 2) {
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[0];
+      q1=ex[0];
+      m11 = p1 * q1;
+      q2=ex[lskip1];
+      m12 = p1 * q2;
+      p2=ell[lskip1];
+      m21 = p2 * q1;
+      m22 = p2 * q2;
+      Z11 += m11;
+      Z12 += m12;
+      Z21 += m21;
+      Z22 += m22;
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[1];
+      q1=ex[1];
+      m11 = p1 * q1;
+      q2=ex[1+lskip1];
+      m12 = p1 * q2;
+      p2=ell[1+lskip1];
+      m21 = p2 * q1;
+      m22 = p2 * q2;
+      /* advance pointers */
+      ell += 2;
+      ex += 2;
+      Z11 += m11;
+      Z12 += m12;
+      Z21 += m21;
+      Z22 += m22;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 2;
+    for (; j > 0; j--) {
+      /* compute outer product and add it to the Z matrix */
+      p1=ell[0];
+      q1=ex[0];
+      m11 = p1 * q1;
+      q2=ex[lskip1];
+      m12 = p1 * q2;
+      p2=ell[lskip1];
+      m21 = p2 * q1;
+      m22 = p2 * q2;
+      /* advance pointers */
+      ell += 1;
+      ex += 1;
+      Z11 += m11;
+      Z12 += m12;
+      Z21 += m21;
+      Z22 += m22;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+    Z12 = ex[lskip1] - Z12;
+    ex[lskip1] = Z12;
+    p1 = ell[lskip1];
+    Z21 = ex[1] - Z21 - p1*Z11;
+    ex[1] = Z21;
+    Z22 = ex[1+lskip1] - Z22 - p1*Z12;
+    ex[1+lskip1] = Z22;
+    /* end of outer loop */
+  }
+}
+
+
+void dFactorLDLT (dReal *A, dReal *d, int n, int nskip1)
+{  
+  int i,j;
+  dReal sum,*ell,*dee,dd,p1,p2,q1,q2,Z11,m11,Z21,m21,Z22,m22;
+  if (n < 1) return;
+  
+  for (i=0; i<=n-2; i += 2) {
+    /* solve L*(D*l)=a, l is scaled elements in 2 x i block at A(i,0) */
+    dSolveL1_2 (A,A+i*nskip1,i,nskip1);
+    /* scale the elements in a 2 x i block at A(i,0), and also */
+    /* compute Z = the outer product matrix that we'll need. */
+    Z11 = 0;
+    Z21 = 0;
+    Z22 = 0;
+    ell = A+i*nskip1;
+    dee = d;
+    for (j=i-6; j >= 0; j -= 6) {
+      p1 = ell[0];
+      p2 = ell[nskip1];
+      dd = dee[0];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[0] = q1;
+      ell[nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[1];
+      p2 = ell[1+nskip1];
+      dd = dee[1];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[1] = q1;
+      ell[1+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[2];
+      p2 = ell[2+nskip1];
+      dd = dee[2];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[2] = q1;
+      ell[2+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[3];
+      p2 = ell[3+nskip1];
+      dd = dee[3];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[3] = q1;
+      ell[3+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[4];
+      p2 = ell[4+nskip1];
+      dd = dee[4];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[4] = q1;
+      ell[4+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      p1 = ell[5];
+      p2 = ell[5+nskip1];
+      dd = dee[5];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[5] = q1;
+      ell[5+nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      ell += 6;
+      dee += 6;
+    }
+    /* compute left-over iterations */
+    j += 6;
+    for (; j > 0; j--) {
+      p1 = ell[0];
+      p2 = ell[nskip1];
+      dd = dee[0];
+      q1 = p1*dd;
+      q2 = p2*dd;
+      ell[0] = q1;
+      ell[nskip1] = q2;
+      m11 = p1*q1;
+      m21 = p2*q1;
+      m22 = p2*q2;
+      Z11 += m11;
+      Z21 += m21;
+      Z22 += m22;
+      ell++;
+      dee++;
+    }
+    /* solve for diagonal 2 x 2 block at A(i,i) */
+    Z11 = ell[0] - Z11;
+    Z21 = ell[nskip1] - Z21;
+    Z22 = ell[1+nskip1] - Z22;
+    dee = d + i;
+    /* factorize 2 x 2 block Z,dee */
+    /* factorize row 1 */
+    dee[0] = dRecip(Z11);
+    /* factorize row 2 */
+    sum = 0;
+    q1 = Z21;
+    q2 = q1 * dee[0];
+    Z21 = q2;
+    sum += q1*q2;
+    dee[1] = dRecip(Z22 - sum);
+    /* done factorizing 2 x 2 block */
+    ell[nskip1] = Z21;
+  }
+  /* compute the (less than 2) rows at the bottom */
+  switch (n-i) {
+    case 0:
+    break;
+    
+    case 1:
+    dSolveL1_1 (A,A+i*nskip1,i,nskip1);
+    /* scale the elements in a 1 x i block at A(i,0), and also */
+    /* compute Z = the outer product matrix that we'll need. */
+    Z11 = 0;
+    ell = A+i*nskip1;
+    dee = d;
+    for (j=i-6; j >= 0; j -= 6) {
+      p1 = ell[0];
+      dd = dee[0];
+      q1 = p1*dd;
+      ell[0] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[1];
+      dd = dee[1];
+      q1 = p1*dd;
+      ell[1] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[2];
+      dd = dee[2];
+      q1 = p1*dd;
+      ell[2] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[3];
+      dd = dee[3];
+      q1 = p1*dd;
+      ell[3] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[4];
+      dd = dee[4];
+      q1 = p1*dd;
+      ell[4] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      p1 = ell[5];
+      dd = dee[5];
+      q1 = p1*dd;
+      ell[5] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      ell += 6;
+      dee += 6;
+    }
+    /* compute left-over iterations */
+    j += 6;
+    for (; j > 0; j--) {
+      p1 = ell[0];
+      dd = dee[0];
+      q1 = p1*dd;
+      ell[0] = q1;
+      m11 = p1*q1;
+      Z11 += m11;
+      ell++;
+      dee++;
+    }
+    /* solve for diagonal 1 x 1 block at A(i,i) */
+    Z11 = ell[0] - Z11;
+    dee = d + i;
+    /* factorize 1 x 1 block Z,dee */
+    /* factorize row 1 */
+    dee[0] = dRecip(Z11);
+    /* done factorizing 1 x 1 block */
+    break;
+    
+    default: *((char*)0)=0;  /* this should never happen! */
+  }
+}

src/source/ode/fastlsolve.c

@@ -0,0 +1,298 @@
+/* generated code, do not edit. */
+
+#include "ode/matrix.h"
+
+/* solve L*X=B, with B containing 1 right hand sides.
+ * L is an n*n lower triangular matrix with ones on the diagonal.
+ * L is stored by rows and its leading dimension is lskip.
+ * B is an n*1 matrix that contains the right hand sides.
+ * B is stored by columns and its leading dimension is also lskip.
+ * B is overwritten with X.
+ * this processes blocks of 4*4.
+ * if this is in the factorizer source file, n must be a multiple of 4.
+ */
+
+void dSolveL1 (const dReal *L, dReal *B, int n, int lskip1)
+{  
+  /* declare variables - Z matrix, p and q vectors, etc */
+  dReal Z11,Z21,Z31,Z41,p1,q1,p2,p3,p4,*ex;
+  const dReal *ell;
+  int lskip2,lskip3,i,j;
+  /* compute lskip values */
+  lskip2 = 2*lskip1;
+  lskip3 = 3*lskip1;
+  /* compute all 4 x 1 blocks of X */
+  for (i=0; i <= n-4; i+=4) {
+    /* compute all 4 x 1 block of X, from rows i..i+4-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    Z21=0;
+    Z31=0;
+    Z41=0;
+    ell = L + i*lskip1;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-12; j >= 0; j -= 12) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      p2=ell[lskip1];
+      p3=ell[lskip2];
+      p4=ell[lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[1];
+      q1=ex[1];
+      p2=ell[1+lskip1];
+      p3=ell[1+lskip2];
+      p4=ell[1+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[2];
+      q1=ex[2];
+      p2=ell[2+lskip1];
+      p3=ell[2+lskip2];
+      p4=ell[2+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[3];
+      q1=ex[3];
+      p2=ell[3+lskip1];
+      p3=ell[3+lskip2];
+      p4=ell[3+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[4];
+      q1=ex[4];
+      p2=ell[4+lskip1];
+      p3=ell[4+lskip2];
+      p4=ell[4+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[5];
+      q1=ex[5];
+      p2=ell[5+lskip1];
+      p3=ell[5+lskip2];
+      p4=ell[5+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[6];
+      q1=ex[6];
+      p2=ell[6+lskip1];
+      p3=ell[6+lskip2];
+      p4=ell[6+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[7];
+      q1=ex[7];
+      p2=ell[7+lskip1];
+      p3=ell[7+lskip2];
+      p4=ell[7+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[8];
+      q1=ex[8];
+      p2=ell[8+lskip1];
+      p3=ell[8+lskip2];
+      p4=ell[8+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[9];
+      q1=ex[9];
+      p2=ell[9+lskip1];
+      p3=ell[9+lskip2];
+      p4=ell[9+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[10];
+      q1=ex[10];
+      p2=ell[10+lskip1];
+      p3=ell[10+lskip2];
+      p4=ell[10+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* load p and q values */
+      p1=ell[11];
+      q1=ex[11];
+      p2=ell[11+lskip1];
+      p3=ell[11+lskip2];
+      p4=ell[11+lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* advance pointers */
+      ell += 12;
+      ex += 12;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 12;
+    for (; j > 0; j--) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      p2=ell[lskip1];
+      p3=ell[lskip2];
+      p4=ell[lskip3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      Z21 += p2 * q1;
+      Z31 += p3 * q1;
+      Z41 += p4 * q1;
+      /* advance pointers */
+      ell += 1;
+      ex += 1;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+    p1 = ell[lskip1];
+    Z21 = ex[1] - Z21 - p1*Z11;
+    ex[1] = Z21;
+    p1 = ell[lskip2];
+    p2 = ell[1+lskip2];
+    Z31 = ex[2] - Z31 - p1*Z11 - p2*Z21;
+    ex[2] = Z31;
+    p1 = ell[lskip3];
+    p2 = ell[1+lskip3];
+    p3 = ell[2+lskip3];
+    Z41 = ex[3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
+    ex[3] = Z41;
+    /* end of outer loop */
+  }
+  /* compute rows at end that are not a multiple of block size */
+  for (; i < n; i++) {
+    /* compute all 1 x 1 block of X, from rows i..i+1-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    ell = L + i*lskip1;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-12; j >= 0; j -= 12) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[1];
+      q1=ex[1];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[2];
+      q1=ex[2];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[3];
+      q1=ex[3];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[4];
+      q1=ex[4];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[5];
+      q1=ex[5];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[6];
+      q1=ex[6];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[7];
+      q1=ex[7];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[8];
+      q1=ex[8];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[9];
+      q1=ex[9];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[10];
+      q1=ex[10];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* load p and q values */
+      p1=ell[11];
+      q1=ex[11];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* advance pointers */
+      ell += 12;
+      ex += 12;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 12;
+    for (; j > 0; j--) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      /* compute outer product and add it to the Z matrix */
+      Z11 += p1 * q1;
+      /* advance pointers */
+      ell += 1;
+      ex += 1;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+  }
+}

src/source/ode/fastltsolve.c

@@ -0,0 +1,199 @@
+/* generated code, do not edit. */
+
+#include "ode/matrix.h"
+
+/* solve L^T * x=b, with b containing 1 right hand side.
+ * L is an n*n lower triangular matrix with ones on the diagonal.
+ * L is stored by rows and its leading dimension is lskip.
+ * b is an n*1 matrix that contains the right hand side.
+ * b is overwritten with x.
+ * this processes blocks of 4.
+ */
+
+void dSolveL1T (const dReal *L, dReal *B, int n, int lskip1)
+{  
+  /* declare variables - Z matrix, p and q vectors, etc */
+  dReal Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
+  const dReal *ell;
+  int lskip2,lskip3,i,j;
+  /* special handling for L and B because we're solving L1 *transpose* */
+  L = L + (n-1)*(lskip1+1);
+  B = B + n-1;
+  lskip1 = -lskip1;
+  /* compute lskip values */
+  lskip2 = 2*lskip1;
+  lskip3 = 3*lskip1;
+  /* compute all 4 x 1 blocks of X */
+  for (i=0; i <= n-4; i+=4) {
+    /* compute all 4 x 1 block of X, from rows i..i+4-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    Z21=0;
+    Z31=0;
+    Z41=0;
+    ell = L - i;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-4; j >= 0; j -= 4) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-1];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-2];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-3];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      ex -= 4;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 4;
+    for (; j > 0; j--) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      p2=ell[-1];
+      p3=ell[-2];
+      p4=ell[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      m21 = p2 * q1;
+      m31 = p3 * q1;
+      m41 = p4 * q1;
+      ell += lskip1;
+      ex -= 1;
+      Z11 += m11;
+      Z21 += m21;
+      Z31 += m31;
+      Z41 += m41;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+    p1 = ell[-1];
+    Z21 = ex[-1] - Z21 - p1*Z11;
+    ex[-1] = Z21;
+    p1 = ell[-2];
+    p2 = ell[-2+lskip1];
+    Z31 = ex[-2] - Z31 - p1*Z11 - p2*Z21;
+    ex[-2] = Z31;
+    p1 = ell[-3];
+    p2 = ell[-3+lskip1];
+    p3 = ell[-3+lskip2];
+    Z41 = ex[-3] - Z41 - p1*Z11 - p2*Z21 - p3*Z31;
+    ex[-3] = Z41;
+    /* end of outer loop */
+  }
+  /* compute rows at end that are not a multiple of block size */
+  for (; i < n; i++) {
+    /* compute all 1 x 1 block of X, from rows i..i+1-1 */
+    /* set the Z matrix to 0 */
+    Z11=0;
+    ell = L - i;
+    ex = B;
+    /* the inner loop that computes outer products and adds them to Z */
+    for (j=i-4; j >= 0; j -= 4) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-1];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-2];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      Z11 += m11;
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[-3];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      ex -= 4;
+      Z11 += m11;
+      /* end of inner loop */
+    }
+    /* compute left-over iterations */
+    j += 4;
+    for (; j > 0; j--) {
+      /* load p and q values */
+      p1=ell[0];
+      q1=ex[0];
+      /* compute outer product and add it to the Z matrix */
+      m11 = p1 * q1;
+      ell += lskip1;
+      ex -= 1;
+      Z11 += m11;
+    }
+    /* finish computing the X(i) block */
+    Z11 = ex[0] - Z11;
+    ex[0] = Z11;
+  }
+}

src/source/ode/joint.h

@@ -0,0 +1,267 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_H_
+#define _ODE_JOINT_H_
+
+
+#include "objects.h"
+#include <ode/contact.h>
+#include "obstack.h"
+
+
+// joint flags
+enum {
+  // if this flag is set, the joint was allocated in a joint group
+  dJOINT_INGROUP = 1,
+
+  // if this flag is set, the joint was attached with arguments (0,body).
+  // our convention is to treat all attaches as (body,0), i.e. so node[0].body
+  // is always nonzero, so this flag records the fact that the arguments were
+  // swapped.
+  dJOINT_REVERSE = 2,
+
+  // if this flag is set, the joint can not have just one body attached to it,
+  // it must have either zero or two bodies attached.
+  dJOINT_TWOBODIES = 4
+};
+
+
+// there are two of these nodes in the joint, one for each connection to a
+// body. these are node of a linked list kept by each body of it's connecting
+// joints. but note that the body pointer in each node points to the body that
+// makes use of the *other* node, not this node. this trick makes it a bit
+// easier to traverse the body/joint graph.
+
+struct dxJointNode {
+  dxJoint *joint;		// pointer to enclosing dxJoint object
+  dxBody *body;			// *other* body this joint is connected to
+  dxJointNode *next;		// next node in body's list of connected joints
+};
+
+
+struct dxJoint : public dObject {
+  // naming convention: the "first" body this is connected to is node[0].body,
+  // and the "second" body is node[1].body. if this joint is only connected
+  // to one body then the second body is 0.
+
+  // info returned by getInfo1 function. the constraint dimension is m (<=6).
+  // i.e. that is the total number of rows in the jacobian. `nub' is the
+  // number of unbounded variables (which have lo,hi = -/+ infinity).
+
+  struct Info1 {
+    int m,nub;
+  };
+
+  // info returned by getInfo2 function
+
+  struct Info2 {
+    // integrator parameters: frames per second (1/stepsize), default error
+    // reduction parameter (0..1).
+    dReal fps,erp;
+
+    // for the first and second body, pointers to two (linear and angular)
+    // n*3 jacobian sub matrices, stored by rows. these matrices will have
+    // been initialized to 0 on entry. if the second body is zero then the
+    // J2xx pointers may be 0.
+    dReal *J1l,*J1a,*J2l,*J2a;
+
+    // elements to jump from one row to the next in J's
+    int rowskip;
+
+    // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+    // "constraint force mixing" vector. c is set to zero on entry, cfm is
+    // set to a constant value (typically very small or zero) value on entry.
+    dReal *c,*cfm;
+
+    // lo and hi limits for variables (set to -/+ infinity on entry).
+    dReal *lo,*hi;
+
+    // findex vector for variables. see the LCP solver interface for a
+    // description of what this does. this is set to -1 on entry.
+    // note that the returned indexes are relative to the first index of
+    // the constraint.
+    int *findex;
+  };
+
+  // virtual function table: size of the joint structure, function pointers.
+  // we do it this way instead of using C++ virtual functions because
+  // sometimes we need to allocate joints ourself within a memory pool.
+
+  typedef void init_fn (dxJoint *joint);
+  typedef void getInfo1_fn (dxJoint *joint, Info1 *info);
+  typedef void getInfo2_fn (dxJoint *joint, Info2 *info);
+  struct Vtable {
+    int size;
+    init_fn *init;
+    getInfo1_fn *getInfo1;
+    getInfo2_fn *getInfo2;
+    int typenum;		// a dJointTypeXXX type number
+  };
+
+  Vtable *vtable;		// virtual function table
+  int flags;			// dJOINT_xxx flags
+  dxJointNode node[2];		// connections to bodies. node[1].body can be 0
+  dJointFeedback *feedback;	// optional feedback structure
+  dReal lambda[6];		// lambda generated by last step
+};
+
+
+// joint group. NOTE: any joints in the group that have their world destroyed
+// will have their world pointer set to 0.
+
+struct dxJointGroup : public dBase {
+  int num;		// number of joints on the stack
+  dObStack stack;	// a stack of (possibly differently sized) dxJoint
+};			// objects.
+
+
+// common limit and motor information for a single joint axis of movement
+struct dxJointLimitMotor {
+  dReal vel,fmax;		// powered joint: velocity, max force
+  dReal lostop,histop;		// joint limits, relative to initial position
+  dReal fudge_factor;		// when powering away from joint limits
+  dReal normal_cfm;		// cfm to use when not at a stop
+  dReal stop_erp,stop_cfm;	// erp and cfm for when at joint limit
+  dReal bounce;			// restitution factor
+  // variables used between getInfo1() and getInfo2()
+  int limit;			// 0=free, 1=at lo limit, 2=at hi limit
+  dReal limit_err;		// if at limit, amount over limit
+
+  void init (dxWorld *);
+  void set (int num, dReal value);
+  dReal get (int num);
+  int testRotationalLimit (dReal angle);
+  int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row,
+		dVector3 ax1, int rotational);
+};
+
+
+// ball and socket
+
+struct dxJointBall : public dxJoint {
+  dVector3 anchor1;		// anchor w.r.t first body
+  dVector3 anchor2;		// anchor w.r.t second body
+};
+extern struct dxJoint::Vtable __dball_vtable;
+
+
+// hinge
+
+struct dxJointHinge : public dxJoint {
+  dVector3 anchor1;		// anchor w.r.t first body
+  dVector3 anchor2;		// anchor w.r.t second body
+  dVector3 axis1;		// axis w.r.t first body
+  dVector3 axis2;		// axis w.r.t second body
+  dQuaternion qrel;		// initial relative rotation body1 -> body2
+  dxJointLimitMotor limot;	// limit and motor information
+};
+extern struct dxJoint::Vtable __dhinge_vtable;
+
+
+// universal
+
+struct dxJointUniversal : public dxJoint {
+  dVector3 anchor1;		// anchor w.r.t first body
+  dVector3 anchor2;		// anchor w.r.t second body
+  dVector3 axis1;		// axis w.r.t first body
+  dVector3 axis2;		// axis w.r.t second body
+  dQuaternion qrel1;	// initial relative rotation body1 -> virtual cross piece
+  dQuaternion qrel2;    // initial relative rotation virtual cross piece -> body2
+  dxJointLimitMotor limot1;	// limit and motor information for axis1
+  dxJointLimitMotor limot2;	// limit and motor information for axis2
+};
+extern struct dxJoint::Vtable __duniversal_vtable;
+
+
+// slider. if body2 is 0 then qrel is the absolute rotation of body1 and
+// offset is the position of body1 center along axis1.
+
+struct dxJointSlider : public dxJoint {
+  dVector3 axis1;		// axis w.r.t first body
+  dQuaternion qrel;		// initial relative rotation body1 -> body2
+  dVector3 offset;		// point relative to body2 that should be
+				// aligned with body1 center along axis1
+  dxJointLimitMotor limot;	// limit and motor information
+};
+extern struct dxJoint::Vtable __dslider_vtable;
+
+
+// contact
+
+struct dxJointContact : public dxJoint {
+  int the_m;			// number of rows computed by getInfo1
+  dContact contact;
+};
+extern struct dxJoint::Vtable __dcontact_vtable;
+
+
+// hinge 2
+
+struct dxJointHinge2 : public dxJoint {
+  dVector3 anchor1;		// anchor w.r.t first body
+  dVector3 anchor2;		// anchor w.r.t second body
+  dVector3 axis1;		// axis 1 w.r.t first body
+  dVector3 axis2;		// axis 2 w.r.t second body
+  dReal c0,s0;			// cos,sin of desired angle between axis 1,2
+  dVector3 v1,v2;		// angle ref vectors embedded in first body
+  dxJointLimitMotor limot1;	// limit+motor info for axis 1
+  dxJointLimitMotor limot2;	// limit+motor info for axis 2
+  dReal susp_erp,susp_cfm;	// suspension parameters (erp,cfm)
+};
+extern struct dxJoint::Vtable __dhinge2_vtable;
+
+
+// angular motor
+
+struct dxJointAMotor : public dxJoint {
+  int num;			// number of axes (0..3)
+  int mode;			// a dAMotorXXX constant
+  int rel[3];			// what the axes are relative to (global,b1,b2)
+  dVector3 axis[3];		// three axes
+  dxJointLimitMotor limot[3];	// limit+motor info for axes
+  dReal angle[3];		// user-supplied angles for axes
+  // these vectors are used for calculating euler angles
+  dVector3 reference1;		// original axis[2], relative to body 1
+  dVector3 reference2;		// original axis[0], relative to body 2
+};
+extern struct dxJoint::Vtable __damotor_vtable;
+
+
+// fixed
+
+struct dxJointFixed : public dxJoint {
+  dQuaternion qrel;		// initial relative rotation body1 -> body2
+  dVector3 offset;		// relative offset between the bodies
+};
+extern struct dxJoint::Vtable __dfixed_vtable;
+
+
+// null joint, for testing only
+
+struct dxJointNull : public dxJoint {
+};
+extern struct dxJoint::Vtable __dnull_vtable;
+
+
+
+#endif

src/source/ode/lcp.h

@@ -0,0 +1,58 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i)
+satisfies one of
+	(1) x = lo, w >= 0
+	(2) x = hi, w <= 0
+	(3) lo < x < hi, w = 0
+A is a matrix of dimension n*n, everything else is a vector of size n*1.
+lo and hi can be +/- dInfinity as needed. the first `nub' variables are
+unbounded, i.e. hi and lo are assumed to be +/- dInfinity.
+
+we restrict lo(i) <= 0 and hi(i) >= 0.
+
+the original data (A,b) may be modified by this function.
+
+if the `findex' (friction index) parameter is nonzero, it points to an array
+of index values. in this case constraints that have findex[i] >= 0 are
+special. all non-special constraints are solved for, then the lo and hi values
+for the special constraints are set:
+  hi[i] = abs( hi[i] * x[findex[i]] )
+  lo[i] = -hi[i]
+and the solution continues. this mechanism allows a friction approximation
+to be implemented. the first `nub' variables are assumed to have findex < 0.
+
+*/
+
+
+#ifndef _ODE_LCP_H_
+#define _ODE_LCP_H_
+
+
+void dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *w,
+		int nub, dReal *lo, dReal *hi, int *findex);
+
+
+#endif

src/source/ode/mass.cpp

@@ -0,0 +1,313 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/config.h>
+#include <ode/mass.h>
+#include <ode/odemath.h>
+#include <ode/matrix.h>
+
+
+#define _I(i,j) I[(i)*4+(j)]
+
+
+// return 1 if ok, 0 if bad
+
+static int checkMass (dMass *m)
+{
+  int i;
+
+  if (m->mass <= 0) {
+    dDEBUGMSG ("mass must be > 0");
+    return 0;
+  }
+  if (!dIsPositiveDefinite (m->I,3)) {
+    dDEBUGMSG ("inertia must be positive definite");
+    return 0;
+  }
+
+  // verify that the center of mass position is consistent with the mass
+  // and inertia matrix. this is done by checking that the inertia around
+  // the center of mass is also positive definite. from the comment in
+  // dMassTranslate(), if the body is translated so that its center of mass
+  // is at the point of reference, then the new inertia is:
+  //   I + mass*crossmat(c)^2
+  // note that requiring this to be positive definite is exactly equivalent
+  // to requiring that the spatial inertia matrix
+  //   [ mass*eye(3,3)   M*crossmat(c)^T ]
+  //   [ M*crossmat(c)   I               ]
+  // is positive definite, given that I is PD and mass>0. see the theorem
+  // about partitioned PD matrices for proof.
+
+  dMatrix3 I2,chat;
+  dSetZero (chat,12);
+  dCROSSMAT (chat,m->c,4,+,-);
+  dMULTIPLY0_333 (I2,chat,chat);
+  for (i=0; i<3; i++) I2[i] = m->I[i] + m->mass*I2[i];
+  for (i=4; i<7; i++) I2[i] = m->I[i] + m->mass*I2[i];
+  for (i=8; i<11; i++) I2[i] = m->I[i] + m->mass*I2[i];
+  if (!dIsPositiveDefinite (I2,3)) {
+    dDEBUGMSG ("center of mass inconsistent with mass parameters");
+    return 0;
+  }
+  return 1;
+}
+
+
+void dMassSetZero (dMass *m)
+{
+  dAASSERT (m);
+  m->mass = REAL(0.0);
+  dSetZero (m->c,sizeof(m->c) / sizeof(dReal));
+  dSetZero (m->I,sizeof(m->I) / sizeof(dReal));
+}
+
+
+void dMassSetParameters (dMass *m, dReal themass,
+			 dReal cgx, dReal cgy, dReal cgz,
+			 dReal I11, dReal I22, dReal I33,
+			 dReal I12, dReal I13, dReal I23)
+{
+  dAASSERT (m);
+  dMassSetZero (m);
+  m->mass = themass;
+  m->c[0] = cgx;
+  m->c[1] = cgy;
+  m->c[2] = cgz;
+  m->_I(0,0) = I11;
+  m->_I(1,1) = I22;
+  m->_I(2,2) = I33;
+  m->_I(0,1) = I12;
+  m->_I(0,2) = I13;
+  m->_I(1,2) = I23;
+  m->_I(1,0) = I12;
+  m->_I(2,0) = I13;
+  m->_I(2,1) = I23;
+  checkMass (m);
+}
+
+
+void dMassSetSphere (dMass *m, dReal density, dReal radius)
+{
+  dMassSetSphereTotal (m, (REAL(4.0)/REAL(3.0)) * M_PI *
+			  radius*radius*radius * density, radius);
+}
+
+
+void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius)
+{
+  dAASSERT (m);
+  dMassSetZero (m);
+  m->mass = total_mass;
+  dReal II = REAL(0.4) * total_mass * radius*radius;
+  m->_I(0,0) = II;
+  m->_I(1,1) = II;
+  m->_I(2,2) = II;
+
+# ifndef dNODEBUG
+  checkMass (m);
+# endif
+}
+
+
+void dMassSetCappedCylinder (dMass *m, dReal density, int direction,
+			     dReal radius, dReal length)
+{
+  dReal M1,M2,Ia,Ib;
+  dAASSERT (m);
+  dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
+  dMassSetZero (m);
+  M1 = M_PI*radius*radius*length*density;			// cylinder mass
+  M2 = (REAL(4.0)/REAL(3.0))*M_PI*radius*radius*radius*density;	// total cap mass
+  m->mass = M1+M2;
+  Ia = M1*(REAL(0.25)*radius*radius + (REAL(1.0)/REAL(12.0))*length*length) +
+    M2*(REAL(0.4)*radius*radius + REAL(0.375)*radius*length + REAL(0.25)*length*length);
+  Ib = (M1*REAL(0.5) + M2*REAL(0.4))*radius*radius;
+  m->_I(0,0) = Ia;
+  m->_I(1,1) = Ia;
+  m->_I(2,2) = Ia;
+  m->_I(direction-1,direction-1) = Ib;
+
+# ifndef dNODEBUG
+  checkMass (m);
+# endif
+}
+
+
+void dMassSetCappedCylinderTotal (dMass *m, dReal total_mass, int direction,
+			     dReal a, dReal b)
+{
+  dMassSetCappedCylinder (m, 1.0, direction, a, b);
+  dMassAdjust (m, total_mass);
+}
+
+
+void dMassSetCylinder (dMass *m, dReal density, int direction,
+		       dReal radius, dReal length)
+{
+  dMassSetCylinderTotal (m, M_PI*radius*radius*length*density,
+			    direction, radius, length);
+}
+
+void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction,
+			    dReal radius, dReal length)
+{
+  dReal r2,I;
+  dAASSERT (m);
+  dMassSetZero (m);
+  r2 = radius*radius;
+  m->mass = total_mass;
+  I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length);
+  m->_I(0,0) = I;
+  m->_I(1,1) = I;
+  m->_I(2,2) = I;
+  m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2;
+
+# ifndef dNODEBUG
+  checkMass (m);
+# endif
+}
+
+
+void dMassSetBox (dMass *m, dReal density,
+		  dReal lx, dReal ly, dReal lz)
+{
+  dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz);
+}
+
+
+void dMassSetBoxTotal (dMass *m, dReal total_mass,
+		       dReal lx, dReal ly, dReal lz)
+{
+  dAASSERT (m);
+  dMassSetZero (m);
+  m->mass = total_mass;
+  m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz);
+  m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz);
+  m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly);
+
+# ifndef dNODEBUG
+  checkMass (m);
+# endif
+}
+
+
+void dMassAdjust (dMass *m, dReal newmass)
+{
+  dAASSERT (m);
+  dReal scale = newmass / m->mass;
+  m->mass = newmass;
+  for (int i=0; i<3; i++) for (int j=0; j<3; j++) m->_I(i,j) *= scale;
+
+# ifndef dNODEBUG
+  checkMass (m);
+# endif
+}
+
+
+void dMassTranslate (dMass *m, dReal x, dReal y, dReal z)
+{
+  // if the body is translated by `a' relative to its point of reference,
+  // the new inertia about the point of reference is:
+  //
+  //   I + mass*(crossmat(c)^2 - crossmat(c+a)^2)
+  //
+  // where c is the existing center of mass and I is the old inertia.
+
+  int i,j;
+  dMatrix3 ahat,chat,t1,t2;
+  dReal a[3];
+
+  dAASSERT (m);
+
+  // adjust inertia matrix
+  dSetZero (chat,12);
+  dCROSSMAT (chat,m->c,4,+,-);
+  a[0] = x + m->c[0];
+  a[1] = y + m->c[1];
+  a[2] = z + m->c[2];
+  dSetZero (ahat,12);
+  dCROSSMAT (ahat,a,4,+,-);
+  dMULTIPLY0_333 (t1,ahat,ahat);
+  dMULTIPLY0_333 (t2,chat,chat);
+  for (i=0; i<3; i++) for (j=0; j<3; j++)
+    m->_I(i,j) += m->mass * (t2[i*4+j]-t1[i*4+j]);
+
+  // ensure perfect symmetry
+  m->_I(1,0) = m->_I(0,1);
+  m->_I(2,0) = m->_I(0,2);
+  m->_I(2,1) = m->_I(1,2);
+
+  // adjust center of mass
+  m->c[0] += x;
+  m->c[1] += y;
+  m->c[2] += z;
+
+# ifndef dNODEBUG
+  checkMass (m);
+# endif
+}
+
+
+void dMassRotate (dMass *m, const dMatrix3 R)
+{
+  // if the body is rotated by `R' relative to its point of reference,
+  // the new inertia about the point of reference is:
+  //
+  //   R * I * R'
+  //
+  // where I is the old inertia.
+
+  dMatrix3 t1;
+  dReal t2[3];
+
+  dAASSERT (m);
+
+  // rotate inertia matrix
+  dMULTIPLY2_333 (t1,m->I,R);
+  dMULTIPLY0_333 (m->I,R,t1);
+
+  // ensure perfect symmetry
+  m->_I(1,0) = m->_I(0,1);
+  m->_I(2,0) = m->_I(0,2);
+  m->_I(2,1) = m->_I(1,2);
+
+  // rotate center of mass
+  dMULTIPLY0_331 (t2,R,m->c);
+  m->c[0] = t2[0];
+  m->c[1] = t2[1];
+  m->c[2] = t2[2];
+
+# ifndef dNODEBUG
+  checkMass (m);
+# endif
+}
+
+
+void dMassAdd (dMass *a, const dMass *b)
+{
+  int i;
+  dAASSERT (a && b);
+  dReal denom = dRecip (a->mass + b->mass);
+  for (i=0; i<3; i++) a->c[i] = (a->c[i]*a->mass + b->c[i]*b->mass)*denom;
+  a->mass += b->mass;
+  for (i=0; i<12; i++) a->I[i] += b->I[i];
+}

src/source/ode/mat.cpp

@@ -0,0 +1,230 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/config.h>
+#include <ode/misc.h>
+#include <ode/matrix.h>
+#include <ode/error.h>
+#include <ode/memory.h>
+#include "mat.h"
+
+
+dMatrix::dMatrix()
+{
+  n = 0;
+  m = 0;
+  data = 0;
+}
+
+
+dMatrix::dMatrix (int rows, int cols)
+{
+  if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
+  n = rows;
+  m = cols;
+  data = (dReal*) dAlloc (n*m*sizeof(dReal));
+  dSetZero (data,n*m);
+}
+
+
+dMatrix::dMatrix (const dMatrix &a)
+{
+  n = a.n;
+  m = a.m;
+  data = (dReal*) dAlloc (n*m*sizeof(dReal));
+  memcpy (data,a.data,n*m*sizeof(dReal));
+}
+
+
+dMatrix::dMatrix (int rows, int cols,
+		    dReal *_data, int rowskip, int colskip)
+{
+  if (rows < 1 || cols < 1) dDebug (0,"bad matrix size");
+  n = rows;
+  m = cols;
+  data = (dReal*) dAlloc (n*m*sizeof(dReal));
+  for (int i=0; i<n; i++) {
+    for (int j=0; j<m; j++) data[i*m+j] = _data[i*rowskip + j*colskip];
+  }
+}
+
+
+dMatrix::~dMatrix()
+{
+  if (data) dFree (data,n*m*sizeof(dReal));
+}
+
+
+dReal & dMatrix::operator () (int i, int j)
+{
+  if (i < 0 || i >= n || j < 0 || j >= m) dDebug (0,"bad matrix (i,j)");
+  return data [i*m+j];
+}
+
+
+void dMatrix::operator= (const dMatrix &a)
+{
+  if (data) dFree (data,n*m*sizeof(dReal));
+  n = a.n;
+  m = a.m;
+  if (n > 0 && m > 0) {
+    data = (dReal*) dAlloc (n*m*sizeof(dReal));
+    memcpy (data,a.data,n*m*sizeof(dReal));
+  }
+  else data = 0;
+}
+
+
+void dMatrix::operator= (dReal a)
+{
+  for (int i=0; i<n*m; i++) data[i] = a;
+}
+
+
+dMatrix dMatrix::transpose()
+{
+  dMatrix r (m,n);
+  for (int i=0; i<n; i++) {
+    for (int j=0; j<m; j++) r.data[j*n+i] = data[i*m+j];
+  }
+  return r;
+}
+
+
+dMatrix dMatrix::select (int np, int *p, int nq, int *q)
+{
+  if (np < 1 || nq < 1) dDebug (0,"Matrix select, bad index array sizes");
+  dMatrix r (np,nq);
+  for (int i=0; i<np; i++) {
+    for (int j=0; j<nq; j++) {
+      if (p[i] < 0 || p[i] >= n || q[i] < 0 || q[i] >= m)
+	dDebug (0,"Matrix select, bad index arrays");
+      r.data[i*nq+j] = data[p[i]*m+q[j]];
+    }
+  }
+  return r;
+}
+
+
+dMatrix dMatrix::operator + (const dMatrix &a)
+{
+  if (n != a.n || m != a.m) dDebug (0,"matrix +, mismatched sizes");
+  dMatrix r (n,m);
+  for (int i=0; i<n*m; i++) r.data[i] = data[i] + a.data[i];
+  return r;
+}
+
+
+dMatrix dMatrix::operator - (const dMatrix &a)
+{
+  if (n != a.n || m != a.m) dDebug (0,"matrix -, mismatched sizes");
+  dMatrix r (n,m);
+  for (int i=0; i<n*m; i++) r.data[i] = data[i] - a.data[i];
+  return r;
+}
+
+
+dMatrix dMatrix::operator - ()
+{
+  dMatrix r (n,m);
+  for (int i=0; i<n*m; i++) r.data[i] = -data[i];
+  return r;
+}
+
+
+dMatrix dMatrix::operator * (const dMatrix &a)
+{
+  if (m != a.n) dDebug (0,"matrix *, mismatched sizes");
+  dMatrix r (n,a.m);
+  for (int i=0; i<n; i++) {
+    for (int j=0; j<a.m; j++) {
+      dReal sum = 0;
+      for (int k=0; k<m; k++) sum += data[i*m+k] * a.data[k*a.m+j];
+      r.data [i*a.m+j] = sum;
+    }
+  }
+  return r;
+}
+
+
+void dMatrix::operator += (const dMatrix &a)
+{
+  if (n != a.n || m != a.m) dDebug (0,"matrix +=, mismatched sizes");
+  for (int i=0; i<n*m; i++) data[i] += a.data[i];
+}
+
+
+void dMatrix::operator -= (const dMatrix &a)
+{
+  if (n != a.n || m != a.m) dDebug (0,"matrix -=, mismatched sizes");
+  for (int i=0; i<n*m; i++) data[i] -= a.data[i];
+}
+
+
+void dMatrix::clearUpperTriangle()
+{
+  if (n != m) dDebug (0,"clearUpperTriangle() only works on square matrices");
+  for (int i=0; i<n; i++) {
+    for (int j=i+1; j<m; j++) data[i*m+j] = 0;
+  }
+}
+
+
+void dMatrix::clearLowerTriangle()
+{
+  if (n != m) dDebug (0,"clearLowerTriangle() only works on square matrices");
+  for (int i=0; i<n; i++) {
+    for (int j=0; j<i; j++) data[i*m+j] = 0;
+  }
+}
+
+
+void dMatrix::makeRandom (dReal range)
+{
+  for (int i=0; i<n; i++) {
+    for (int j=0; j<m; j++)
+      data[i*m+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
+  }
+}
+
+
+void dMatrix::print (char *fmt, FILE *f)
+{
+  for (int i=0; i<n; i++) {
+    for (int j=0; j<m; j++) fprintf (f,fmt,data[i*m+j]);
+    fprintf (f,"\n");
+  }
+}
+
+
+dReal dMatrix::maxDifference (const dMatrix &a)
+{
+  if (n != a.n || m != a.m) dDebug (0,"maxDifference(), mismatched sizes");
+  dReal max = 0;
+  for (int i=0; i<n; i++) {
+    for (int j=0; j<m; j++) {
+      dReal diff = dFabs(data[i*m+j] - a.data[i*m+j]);
+      if (diff > max) max = diff;
+    }
+  }
+  return max;
+}

src/source/ode/mat.h

@@ -0,0 +1,71 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// matrix class. this is mostly for convenience in the testing code, it is
+// not optimized at all. correctness is much more importance here.
+
+#ifndef _ODE_MAT_H_
+#define _ODE_MAT_H_
+
+#include <ode/common.h>
+
+
+class dMatrix {
+  int n,m;		// matrix dimension, n,m >= 0
+  dReal *data;		// if nonzero, n*m elements allocated on the heap
+
+public:
+  // constructors, destructors
+  dMatrix();				// make default 0x0 matrix
+  dMatrix (int rows, int cols);		// construct zero matrix of given size
+  dMatrix (const dMatrix &);		// construct copy of given matrix
+  // create copy of given data - element (i,j) is data[i*rowskip+j*colskip]
+  dMatrix (int rows, int cols, dReal *_data, int rowskip, int colskip);
+  ~dMatrix();				// destructor
+
+  // data movement
+  dReal & operator () (int i, int j);	// reference an element
+  void operator= (const dMatrix &);	// matrix = matrix
+  void operator= (dReal);		// matrix = scalar
+  dMatrix transpose();			// return transposed matrix
+  // return a permuted submatrix of this matrix, made up of the rows in p
+  // and the columns in q. p has np elements, q has nq elements.
+  dMatrix select (int np, int *p, int nq, int *q);
+
+  // operators
+  dMatrix operator + (const dMatrix &);
+  dMatrix operator - (const dMatrix &);
+  dMatrix operator - ();
+  dMatrix operator * (const dMatrix &);
+  void operator += (const dMatrix &);
+  void operator -= (const dMatrix &);
+
+  // utility
+  void clearUpperTriangle();
+  void clearLowerTriangle();
+  void makeRandom (dReal range);
+  void print (char *fmt = "%10.4f ", FILE *f=stdout);
+  dReal maxDifference (const dMatrix &);
+};
+
+
+#endif

src/source/ode/matrix.cpp

@@ -0,0 +1,358 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/common.h>
+#include <ode/matrix.h>
+
+// misc defines
+#define ALLOCA dALLOCA16
+
+
+void dSetZero (dReal *a, int n)
+{
+  dAASSERT (a && n >= 0);
+  while (n > 0) {
+    *(a++) = 0;
+    n--;
+  }
+}
+
+
+void dSetValue (dReal *a, int n, dReal value)
+{
+  dAASSERT (a && n >= 0);
+  while (n > 0) {
+    *(a++) = value;
+    n--;
+  }
+}
+
+
+void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
+{
+  int i,j,k,qskip,rskip,rpad;
+  dAASSERT (A && B && C && p>0 && q>0 && r>0);
+  qskip = dPAD(q);
+  rskip = dPAD(r);
+  rpad = rskip - r;
+  dReal sum;
+  const dReal *b,*c,*bb;
+  bb = B;
+  for (i=p; i; i--) {
+    for (j=0 ; j<r; j++) {
+      c = C + j;
+      b = bb;
+      sum = 0;
+      for (k=q; k; k--, c+=rskip) sum += (*(b++))*(*c);
+      *(A++) = sum; 
+    }
+    A += rpad;
+    bb += qskip;
+  }
+}
+
+
+void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
+{
+  int i,j,k,pskip,rskip;
+  dReal sum;
+  dAASSERT (A && B && C && p>0 && q>0 && r>0);
+  pskip = dPAD(p);
+  rskip = dPAD(r);
+  for (i=0; i<p; i++) {
+    for (j=0; j<r; j++) {
+      sum = 0;
+      for (k=0; k<q; k++) sum += B[i+k*pskip] * C[j+k*rskip];
+      A[i*rskip+j] = sum;
+    }
+  }
+}
+
+
+void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)
+{
+  int i,j,k,z,rpad,qskip;
+  dReal sum;
+  const dReal *bb,*cc;
+  dAASSERT (A && B && C && p>0 && q>0 && r>0);
+  rpad = dPAD(r) - r;
+  qskip = dPAD(q);
+  bb = B;
+  for (i=p; i; i--) {
+    cc = C;
+    for (j=r; j; j--) {
+      z = 0;
+      sum = 0;
+      for (k=q; k; k--,z++) sum += bb[z] * cc[z];
+      *(A++) = sum; 
+      cc += qskip;
+    }
+    A += rpad;
+    bb += qskip;
+  }
+}
+
+
+int dFactorCholesky (dReal *A, int n)
+{
+  int i,j,k,nskip;
+  dReal sum,*a,*b,*aa,*bb,*cc,*recip;
+  dAASSERT (n > 0 && A);
+  nskip = dPAD (n);
+  recip = (dReal*) ALLOCA (n * sizeof(dReal));
+  aa = A;
+  for (i=0; i<n; i++) {
+    bb = A;
+    cc = A + i*nskip;
+    for (j=0; j<i; j++) {
+      sum = *cc;
+      a = aa;
+      b = bb;
+      for (k=j; k; k--) sum -= (*(a++))*(*(b++));
+      *cc = sum * recip[j];
+      bb += nskip;
+      cc++;
+    }
+    sum = *cc;
+    a = aa;
+    for (k=i; k; k--, a++) sum -= (*a)*(*a);
+    if (sum <= REAL(0.0)) return 0;
+    *cc = dSqrt(sum);
+    recip[i] = dRecip (*cc);
+    aa += nskip;
+  }
+  return 1;
+}
+
+
+void dSolveCholesky (const dReal *L, dReal *b, int n)
+{
+  int i,k,nskip;
+  dReal sum,*y;
+  dAASSERT (n > 0 && L && b);
+  nskip = dPAD (n);
+  y = (dReal*) ALLOCA (n*sizeof(dReal));
+  for (i=0; i<n; i++) {
+    sum = 0;
+    for (k=0; k < i; k++) sum += L[i*nskip+k]*y[k];
+    y[i] = (b[i]-sum)/L[i*nskip+i];
+  }
+  for (i=n-1; i >= 0; i--) {
+    sum = 0;
+    for (k=i+1; k < n; k++) sum += L[k*nskip+i]*b[k];
+    b[i] = (y[i]-sum)/L[i*nskip+i];
+  }
+}
+
+
+int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n)
+{
+  int i,j,nskip;
+  dReal *L,*x;
+  dAASSERT (n > 0 && A && Ainv);
+  nskip = dPAD (n);
+  L = (dReal*) ALLOCA (nskip*n*sizeof(dReal));
+  memcpy (L,A,nskip*n*sizeof(dReal));
+  x = (dReal*) ALLOCA (n*sizeof(dReal));
+  if (dFactorCholesky (L,n)==0) return 0;
+  dSetZero (Ainv,n*nskip);	// make sure all padding elements set to 0
+  for (i=0; i<n; i++) {
+    for (j=0; j<n; j++) x[j] = 0;
+    x[i] = 1;
+    dSolveCholesky (L,x,n);
+    for (j=0; j<n; j++) Ainv[j*nskip+i] = x[j];
+  }
+  return 1;  
+}
+
+
+int dIsPositiveDefinite (const dReal *A, int n)
+{
+  dReal *Acopy;
+  dAASSERT (n > 0 && A);
+  int nskip = dPAD (n);
+  Acopy = (dReal*) ALLOCA (nskip*n * sizeof(dReal));
+  memcpy (Acopy,A,nskip*n * sizeof(dReal));
+  return dFactorCholesky (Acopy,n);
+}
+
+
+/***** this has been replaced by a faster version
+void dSolveL1T (const dReal *L, dReal *b, int n, int nskip)
+{
+  int i,j;
+  dAASSERT (L && b && n >= 0 && nskip >= n);
+  dReal sum;
+  for (i=n-2; i>=0; i--) {
+    sum = 0;
+    for (j=i+1; j<n; j++) sum += L[j*nskip+i]*b[j];
+    b[i] -= sum;
+  }
+}
+*/
+
+
+void dVectorScale (dReal *a, const dReal *d, int n)
+{
+  dAASSERT (a && d && n >= 0);
+  for (int i=0; i<n; i++) a[i] *= d[i];
+}
+
+
+void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip)
+{
+  dAASSERT (L && d && b && n > 0 && nskip >= n);
+  dSolveL1 (L,b,n,nskip);
+  dVectorScale (b,d,n);
+  dSolveL1T (L,b,n,nskip);
+}
+
+
+void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip)
+{
+  int j,p;
+  dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee;
+  dAASSERT (L && d && a && n > 0 && nskip >= n);
+
+  if (n < 2) return;
+  W1 = (dReal*) ALLOCA (n*sizeof(dReal));
+  W2 = (dReal*) ALLOCA (n*sizeof(dReal));
+
+  W1[0] = 0;
+  W2[0] = 0;
+  for (j=1; j<n; j++) W1[j] = W2[j] = a[j] * M_SQRT1_2;
+  W11 = (REAL(0.5)*a[0]+1)*M_SQRT1_2;
+  W21 = (REAL(0.5)*a[0]-1)*M_SQRT1_2;
+
+  alpha1=1;
+  alpha2=1;
+
+  dee = d[0];
+  alphanew = alpha1 + (W11*W11)*dee;
+  dee /= alphanew;
+  gamma1 = W11 * dee;
+  dee *= alpha1;
+  alpha1 = alphanew;
+  alphanew = alpha2 - (W21*W21)*dee;
+  dee /= alphanew;
+  gamma2 = W21 * dee;
+  alpha2 = alphanew;
+  k1 = REAL(1.0) - W21*gamma1;
+  k2 = W21*gamma1*W11 - W21;
+  for (p=1; p<n; p++) {
+    Wp = W1[p];
+    ell = L[p*nskip];
+    W1[p] =    Wp - W11*ell;
+    W2[p] = k1*Wp +  k2*ell;
+  }
+
+  for (j=1; j<n; j++) {
+    dee = d[j];
+    alphanew = alpha1 + (W1[j]*W1[j])*dee;
+    dee /= alphanew;
+    gamma1 = W1[j] * dee;
+    dee *= alpha1;
+    alpha1 = alphanew;
+    alphanew = alpha2 - (W2[j]*W2[j])*dee;
+    dee /= alphanew;
+    gamma2 = W2[j] * dee;
+    dee *= alpha2;
+    d[j] = dee;
+    alpha2 = alphanew;
+
+    k1 = W1[j];
+    k2 = W2[j];
+    for (p=j+1; p<n; p++) {
+      ell = L[p*nskip+j];
+      Wp = W1[p] - k1 * ell;
+      ell += gamma1 * Wp;
+      W1[p] = Wp;
+      Wp = W2[p] - k2 * ell;
+      ell -= gamma2 * Wp;
+      W2[p] = Wp;
+      L[p*nskip+j] = ell;
+    }
+  }
+}
+
+
+// macros for dLDLTRemove() for accessing A - either access the matrix
+// directly or access it via row pointers. we are only supposed to reference
+// the lower triangle of A (it is symmetric), but indexes i and j come from
+// permutation vectors so they are not predictable. so do a test on the
+// indexes - this should not slow things down too much, as we don't do this
+// in an inner loop.
+
+#define _GETA(i,j) (A[i][j])
+//#define _GETA(i,j) (A[(i)*nskip+(j)])
+#define GETA(i,j) ((i > j) ? _GETA(i,j) : _GETA(j,i))
+
+
+void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
+		  int n1, int n2, int r, int nskip)
+{
+  int i;
+  dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
+	   n1 >= n2 && nskip >= n1);
+  #ifndef dNODEBUG
+  for (i=0; i<n2; i++) dIASSERT(p[i] >= 0 && p[i] < n1);
+  #endif
+
+  if (r==n2-1) {
+    return;		// deleting last row/col is easy
+  }
+  else if (r==0) {
+    dReal *a = (dReal*) ALLOCA (n2 * sizeof(dReal));
+    for (i=0; i<n2; i++) a[i] = -GETA(p[i],p[0]);
+    a[0] += REAL(1.0);
+    dLDLTAddTL (L,d,a,n2,nskip);
+  }
+  else {
+    dReal *t = (dReal*) ALLOCA (r * sizeof(dReal));
+    dReal *a = (dReal*) ALLOCA ((n2-r) * sizeof(dReal));
+    for (i=0; i<r; i++) t[i] = L[r*nskip+i] / d[i];
+    for (i=0; i<(n2-r); i++)
+      a[i] = dDot(L+(r+i)*nskip,t,r) - GETA(p[r+i],p[r]);
+    a[0] += REAL(1.0);
+    dLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip);
+  }
+
+  // snip out row/column r from L and d
+  dRemoveRowCol (L,n2,nskip,r);
+  if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(dReal));
+}
+
+
+void dRemoveRowCol (dReal *A, int n, int nskip, int r)
+{
+  int i;
+  dAASSERT(A && n > 0 && nskip >= n && r >= 0 && r < n);
+  if (r >= n-1) return;
+  if (r > 0) {
+    for (i=0; i<r; i++)
+      memmove (A+i*nskip+r,A+i*nskip+r+1,(n-r-1)*sizeof(dReal));
+    for (i=r; i<(n-1); i++)
+      memcpy (A+i*nskip,A+i*nskip+nskip,r*sizeof(dReal));
+  }
+  for (i=r; i<(n-1); i++)
+    memcpy (A+i*nskip+r,A+i*nskip+nskip+r+1,(n-r-1)*sizeof(dReal));
+}

src/source/ode/memory.cpp

@@ -0,0 +1,87 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/config.h>
+#include <ode/memory.h>
+#include <ode/error.h>
+
+
+static dAllocFunction *allocfn = 0;
+static dReallocFunction *reallocfn = 0;
+static dFreeFunction *freefn = 0;
+
+
+
+void dSetAllocHandler (dAllocFunction *fn)
+{
+  allocfn = fn;
+}
+
+
+void dSetReallocHandler (dReallocFunction *fn)
+{
+  reallocfn = fn;
+}
+
+
+void dSetFreeHandler (dFreeFunction *fn)
+{
+  freefn = fn;
+}
+
+
+dAllocFunction *dGetAllocHandler()
+{
+  return allocfn;
+}
+
+
+dReallocFunction *dGetReallocHandler()
+{
+  return reallocfn;
+}
+
+
+dFreeFunction *dGetFreeHandler()
+{
+  return freefn;
+}
+
+
+void * dAlloc (size_t size)
+{
+  if (allocfn) return allocfn (size); else return malloc (size);
+}
+
+
+void * dRealloc (void *ptr, size_t oldsize, size_t newsize)
+{
+  if (reallocfn) return reallocfn (ptr,oldsize,newsize);
+  else return realloc (ptr,newsize);
+}
+
+
+void dFree (void *ptr, size_t size)
+{
+  if (!ptr) return;
+  if (freefn) freefn (ptr,size); else free (ptr);
+}

src/source/ode/misc.cpp

@@ -0,0 +1,147 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/config.h>
+#include <ode/misc.h>
+#include <ode/matrix.h>
+
+//****************************************************************************
+// random numbers
+
+static unsigned long seed = 0;
+
+unsigned long dRand()
+{
+  seed = (1664525L*seed + 1013904223L) & 0xffffffff;
+  return seed;
+}
+
+
+unsigned long  dRandGetSeed()
+{
+  return seed;
+}
+
+
+void dRandSetSeed (unsigned long s)
+{
+  seed = s;
+}
+
+
+int dTestRand()
+{
+  unsigned long oldseed = seed;
+  int ret = 1;
+  seed = 0;
+  if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||
+      dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||
+      dRand() != 0x6252e503) ret = 0;
+  seed = oldseed;
+  return ret;
+}
+
+
+int dRandInt (int n)
+{
+  double a = double(n) / 4294967296.0;
+  return (int) (double(dRand()) * a);
+}
+
+
+dReal dRandReal()
+{
+  return ((dReal) dRand()) / ((dReal) 0xffffffff);
+}
+
+//****************************************************************************
+// matrix utility stuff
+
+void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f)
+{
+  int i,j;
+  int skip = dPAD(m);
+  for (i=0; i<n; i++) {
+    for (j=0; j<m; j++) fprintf (f,fmt,A[i*skip+j]);
+    fprintf (f,"\n");
+  }
+}
+
+
+void dMakeRandomVector (dReal *A, int n, dReal range)
+{
+  int i;
+  for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
+}
+
+
+void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)
+{
+  int i,j;
+  int skip = dPAD(m);
+  dSetZero (A,n*skip);
+  for (i=0; i<n; i++) {
+    for (j=0; j<m; j++) A[i*skip+j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
+  }
+}
+
+
+void dClearUpperTriangle (dReal *A, int n)
+{
+  int i,j;
+  int skip = dPAD(n);
+  for (i=0; i<n; i++) {
+    for (j=i+1; j<n; j++) A[i*skip+j] = 0;
+  }
+}
+
+
+dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)
+{
+  int i,j;
+  int skip = dPAD(m);
+  dReal diff,max;
+  max = 0;
+  for (i=0; i<n; i++) {
+    for (j=0; j<m; j++) {
+      diff = dFabs(A[i*skip+j] - B[i*skip+j]);
+      if (diff > max) max = diff;
+    }
+  }
+  return max;
+}
+
+
+dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)
+{
+  int i,j;
+  int skip = dPAD(n);
+  dReal diff,max;
+  max = 0;
+  for (i=0; i<n; i++) {
+    for (j=0; j<=i; j++) {
+      diff = dFabs(A[i*skip+j] - B[i*skip+j]);
+      if (diff > max) max = diff;
+    }
+  }
+  return max;
+}

src/source/ode/objects.h

@@ -0,0 +1,125 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// object, body, and world structs.
+
+
+#ifndef _ODE_OBJECT_H_
+#define _ODE_OBJECT_H_
+
+#include <ode/common.h>
+#include <ode/memory.h>
+#include <ode/mass.h>
+#include "array.h"
+
+
+// some body flags
+
+enum {
+  dxBodyFlagFiniteRotation = 1,		// use finite rotations
+  dxBodyFlagFiniteRotationAxis = 2,	// use finite rotations only along axis
+  dxBodyDisabled = 4,			// body is disabled
+  dxBodyNoGravity = 8,			// body is not influenced by gravity
+  dxBodyAutoDisable = 16		// enable auto-disable on body
+};
+
+
+// base class that does correct object allocation / deallocation
+
+struct dBase {
+  void *operator new (size_t size) { return dAlloc (size); }
+  void operator delete (void *ptr, size_t size) { dFree (ptr,size); }
+  void *operator new[] (size_t size) { return dAlloc (size); }
+  void operator delete[] (void *ptr, size_t size) { dFree (ptr,size); }
+};
+
+
+// base class for bodies and joints
+
+struct dObject : public dBase {
+  dxWorld *world;		// world this object is in
+  dObject *next;		// next object of this type in list
+  dObject **tome;		// pointer to previous object's next ptr
+  void *userdata;		// user settable data
+  int tag;			// used by dynamics algorithms
+};
+
+
+// auto disable parameters
+struct dxAutoDisable {
+  dReal linear_threshold;	// linear (squared) velocity treshold
+  dReal angular_threshold;	// angular (squared) velocity treshold
+  dReal idle_time;		// time the body needs to be idle to auto-disable it
+  int idle_steps;		// steps the body needs to be idle to auto-disable it
+};
+
+
+// quick-step parameters
+struct dxQuickStepParameters {
+  int num_iterations;		// number of SOR iterations to perform
+  dReal w;			// the SOR over-relaxation parameter
+};
+
+
+// contact generation parameters
+struct dxContactParameters {
+  dReal max_vel;		// maximum correcting velocity
+  dReal min_depth;		// thickness of 'surface layer'
+};
+
+
+struct dxBody : public dObject {
+  dxJointNode *firstjoint;	// list of attached joints
+  int flags;			// some dxBodyFlagXXX flags
+  dGeomID geom;			// first collision geom associated with body
+  dMass mass;			// mass parameters about POR
+  dMatrix3 invI;		// inverse of mass.I
+  dReal invMass;		// 1 / mass.mass
+  dVector3 pos;			// position of POR (point of reference)
+  dQuaternion q;		// orientation quaternion
+  dMatrix3 R;			// rotation matrix, always corresponds to q
+  dVector3 lvel,avel;		// linear and angular velocity of POR
+  dVector3 facc,tacc;		// force and torque accumulators
+  dVector3 finite_rot_axis;	// finite rotation axis, unit length or 0=none
+
+  // auto-disable information
+  dxAutoDisable adis;		// auto-disable parameters
+  dReal adis_timeleft;		// time left to be idle
+  int adis_stepsleft;		// steps left to be idle
+};
+
+
+struct dxWorld : public dBase {
+  dxBody *firstbody;		// body linked list
+  dxJoint *firstjoint;		// joint linked list
+  int nb,nj;			// number of bodies and joints in lists
+  dVector3 gravity;		// gravity vector (m/s/s)
+  dReal global_erp;		// global error reduction parameter
+  dReal global_cfm;		// global costraint force mixing parameter
+  dxAutoDisable adis;		// auto-disable parameters
+  int adis_flag;		// auto-disable flag for new bodies
+  dxQuickStepParameters qs;
+  dxContactParameters contactp;
+};
+
+
+#endif

src/source/ode/obstack.cpp

@@ -0,0 +1,130 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/common.h>
+#include <ode/error.h>
+#include <ode/memory.h>
+#include "obstack.h"
+
+//****************************************************************************
+// macros and constants
+
+#define ROUND_UP_OFFSET_TO_EFFICIENT_SIZE(arena,ofs) \
+  ofs = (size_t) (dEFFICIENT_SIZE( ((intP)(arena)) + ofs ) - ((intP)(arena)) );
+
+#define MAX_ALLOC_SIZE \
+  ((size_t)(dOBSTACK_ARENA_SIZE - sizeof (Arena) - EFFICIENT_ALIGNMENT + 1))
+
+//****************************************************************************
+// dObStack
+
+dObStack::dObStack()
+{
+  first = 0;
+  last = 0;
+  current_arena = 0;
+  current_ofs = 0;
+}
+
+
+dObStack::~dObStack()
+{
+  // free all arenas
+  Arena *a,*nexta;
+  a = first;
+  while (a) {
+    nexta = a->next;
+    dFree (a,dOBSTACK_ARENA_SIZE);
+    a = nexta;
+  }
+}
+
+
+void *dObStack::alloc (int num_bytes)
+{
+  if ((size_t)num_bytes > MAX_ALLOC_SIZE) dDebug (0,"num_bytes too large");
+
+  // allocate or move to a new arena if necessary
+  if (!first) {
+    // allocate the first arena if necessary
+    first = last = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
+    first->next = 0;
+    first->used = sizeof (Arena);
+    ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
+  }
+  else {
+    // we already have one or more arenas, see if a new arena must be used
+    if ((last->used + num_bytes) > dOBSTACK_ARENA_SIZE) {
+      if (!last->next) {
+	last->next = (Arena *) dAlloc (dOBSTACK_ARENA_SIZE);
+	last->next->next = 0;
+      }
+      last = last->next;
+      last->used = sizeof (Arena);
+      ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
+    }
+  }
+
+  // allocate an area in the arena
+  char *c = ((char*) last) + last->used;
+  last->used += num_bytes;
+  ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (last,last->used);
+  return c;
+}
+
+
+void dObStack::freeAll()
+{
+  last = first;
+  if (first) {
+    first->used = sizeof(Arena);
+    ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (first,first->used);
+  }
+}
+
+
+void *dObStack::rewind()
+{
+  current_arena = first;
+  current_ofs = sizeof (Arena);
+  if (current_arena) {
+    ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs)
+    return ((char*) current_arena) + current_ofs;
+  }
+  else return 0;
+}
+
+
+void *dObStack::next (int num_bytes)
+{
+  // this functions like alloc, except that no new storage is ever allocated
+  if (!current_arena) return 0;
+  current_ofs += num_bytes;
+  ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs);
+  if (current_ofs >= current_arena->used) {
+    current_arena = current_arena->next;
+    if (!current_arena) return 0;
+    current_ofs = sizeof (Arena);
+    ROUND_UP_OFFSET_TO_EFFICIENT_SIZE (current_arena,current_ofs);
+  }
+  return ((char*) current_arena) + current_ofs;
+}

src/source/ode/obstack.h

@@ -0,0 +1,68 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_OBSTACK_H_
+#define _ODE_OBSTACK_H_
+
+#include "objects.h" 
+
+// each obstack Arena pointer points to a block of this many bytes
+#define dOBSTACK_ARENA_SIZE 16384
+
+
+struct dObStack : public dBase {
+  struct Arena {
+    Arena *next;	// next arena in linked list
+    int used;		// total number of bytes used in this arena, counting
+  };			//   this header
+
+  Arena *first;		// head of the arena linked list. 0 if no arenas yet
+  Arena *last;		// arena where blocks are currently being allocated
+
+  // used for iterator
+  Arena *current_arena;
+  int current_ofs;
+
+  dObStack();
+  ~dObStack();
+
+  void *alloc (int num_bytes);
+  // allocate a block in the last arena, allocating a new arena if necessary.
+  // it is a runtime error if num_bytes is larger than the arena size.
+
+  void freeAll();
+  // free all blocks in all arenas. this does not deallocate the arenas
+  // themselves, so future alloc()s will reuse them.
+
+  void *rewind();
+  // rewind the obstack iterator, and return the address of the first
+  // allocated block. return 0 if there are no allocated blocks.
+
+  void *next (int num_bytes);
+  // return the address of the next allocated block. 'num_bytes' is the size
+  // of the previous block. this returns null if there are no more arenas.
+  // the sequence of 'num_bytes' parameters passed to next() during a
+  // traversal of the list must exactly match the parameters passed to alloc().
+};
+
+
+#endif

src/source/ode/odemath.cpp

@@ -0,0 +1,156 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/common.h>
+#include <ode/odemath.h>
+
+
+// this may be called for vectors `a' with extremely small magnitude, for
+// example the result of a cross product on two nearly perpendicular vectors.
+// we must be robust to these small vectors. to prevent numerical error,
+// first find the component a[i] with the largest magnitude and then scale
+// all the components by 1/a[i]. then we can compute the length of `a' and
+// scale the components by 1/l. this has been verified to work with vectors
+// containing the smallest representable numbers.
+
+void dNormalize3 (dVector3 a)
+{
+  dReal a0,a1,a2,aa0,aa1,aa2,l;
+  dAASSERT (a);
+  a0 = a[0];
+  a1 = a[1];
+  a2 = a[2];
+  aa0 = dFabs(a0);
+  aa1 = dFabs(a1);
+  aa2 = dFabs(a2);
+  if (aa1 > aa0) {
+    if (aa2 > aa1) {
+      goto aa2_largest;
+    }
+    else {		// aa1 is largest
+      a0 /= aa1;
+      a2 /= aa1;
+      l = dRecipSqrt (a0*a0 + a2*a2 + 1);
+      a[0] = a0*l;
+      a[1] = dCopySign(l,a1);
+      a[2] = a2*l;
+    }
+  }
+  else {
+    if (aa2 > aa0) {
+      aa2_largest:	// aa2 is largest
+      a0 /= aa2;
+      a1 /= aa2;
+      l = dRecipSqrt (a0*a0 + a1*a1 + 1);
+      a[0] = a0*l;
+      a[1] = a1*l;
+      a[2] = dCopySign(l,a2);
+    }
+    else {		// aa0 is largest
+      if (aa0 <= 0) {
+	// dDEBUGMSG ("vector has zero size"); ... this messace is annoying
+	a[0] = 1;	// if all a's are zero, this is where we'll end up.
+	a[1] = 0;	// return a default unit length vector.
+	a[2] = 0;
+	return;
+      }
+      a1 /= aa0;
+      a2 /= aa0;
+      l = dRecipSqrt (a1*a1 + a2*a2 + 1);
+      a[0] = dCopySign(l,a0);
+      a[1] = a1*l;
+      a[2] = a2*l;
+    }
+  }
+}
+
+
+/* OLD VERSION */
+/*
+void dNormalize3 (dVector3 a)
+{
+  dASSERT (a);
+  dReal l = dDOT(a,a);
+  if (l > 0) {
+    l = dRecipSqrt(l);
+    a[0] *= l;
+    a[1] *= l;
+    a[2] *= l;
+  }
+  else {
+    a[0] = 1;
+    a[1] = 0;
+    a[2] = 0;
+  }
+}
+*/
+
+
+void dNormalize4 (dVector4 a)
+{
+  dAASSERT (a);
+  dReal l = dDOT(a,a)+a[3]*a[3];
+  if (l > 0) {
+    l = dRecipSqrt(l);
+    a[0] *= l;
+    a[1] *= l;
+    a[2] *= l;
+    a[3] *= l;
+  }
+  else {
+    dDEBUGMSG ("vector has zero size");
+    a[0] = 1;
+    a[1] = 0;
+    a[2] = 0;
+    a[3] = 0;
+  }
+}
+
+
+void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
+{
+  dAASSERT (n && p && q);
+  if (dFabs(n[2]) > M_SQRT1_2) {
+    // choose p in y-z plane
+    dReal a = n[1]*n[1] + n[2]*n[2];
+    dReal k = dRecipSqrt (a);
+    p[0] = 0;
+    p[1] = -n[2]*k;
+    p[2] = n[1]*k;
+    // set q = n x p
+    q[0] = a*k;
+    q[1] = -n[0]*p[2];
+    q[2] = n[0]*p[1];
+  }
+  else {
+    // choose p in x-y plane
+    dReal a = n[0]*n[0] + n[1]*n[1];
+    dReal k = dRecipSqrt (a);
+    p[0] = -n[1]*k;
+    p[1] = n[0]*k;
+    p[2] = 0;
+    // set q = n x p
+    q[0] = -n[2]*p[1];
+    q[1] = n[2]*p[0];
+    q[2] = a*k;
+  }
+}

src/source/ode/quickstep.cpp

@@ -0,0 +1,787 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include "objects.h"
+#include "joint.h"
+#include <ode/config.h>
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/matrix.h>
+#include <ode/misc.h>
+#include "lcp.h"
+#include "util.h"
+
+#define ALLOCA dALLOCA16
+
+typedef const dReal *dRealPtr;
+typedef dReal *dRealMutablePtr;
+#define dRealArray(name,n) dReal name[n];
+#define dRealAllocaArray(name,n) dReal *name = (dReal*) ALLOCA ((n)*sizeof(dReal));
+
+//***************************************************************************
+// configuration
+
+// for the SOR and CG methods:
+// uncomment the following line to use warm starting. this definitely
+// help for motor-driven joints. unfortunately it appears to hurt
+// with high-friction contacts using the SOR method. use with care
+
+#define WARM_STARTING 1
+
+
+// for the SOR method:
+// uncomment the following line to determine a new constraint-solving
+// order for each iteration. however, the qsort per iteration is expensive,
+// and the optimal order is somewhat problem dependent. 
+// @@@ try the leaf->root ordering.
+
+//#define REORDER_CONSTRAINTS 1
+
+
+// for the SOR method:
+// uncomment the following line to randomly reorder constraint rows
+// during the solution. depending on the situation, this can help a lot
+// or hardly at all, but it doesn't seem to hurt.
+
+#define RANDOMLY_REORDER_CONSTRAINTS 1
+
+//***************************************************************************
+// testing stuff
+
+#ifdef TIMING
+#define IFTIMING(x) x
+#else
+#define IFTIMING(x) /* */
+#endif
+
+//***************************************************************************
+// various common computations involving the matrix J
+
+// compute iMJ = inv(M)*J'
+
+static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
+	dxBody * const *body, dRealPtr invI)
+{
+	int i,j;
+	dRealMutablePtr iMJ_ptr = iMJ;
+	dRealMutablePtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];	
+		int b2 = jb[i*2+1];
+		dReal k = body[b1]->invMass;
+		for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
+		dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
+		if (b2 >= 0) {
+			k = body[b2]->invMass;
+			for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
+			dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
+		}
+		J_ptr += 12;
+		iMJ_ptr += 12;
+	}
+}
+
+
+// compute out = inv(M)*J'*in.
+
+static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
+	dRealMutablePtr in, dRealMutablePtr out)
+{
+	int i,j;
+	dSetZero (out,6*nb);
+	dRealPtr iMJ_ptr = iMJ;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];	
+		int b2 = jb[i*2+1];
+		dRealMutablePtr out_ptr = out + b1*6;
+		for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
+		iMJ_ptr += 6;
+		if (b2 >= 0) {
+			out_ptr = out + b2*6;
+			for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
+		}
+		iMJ_ptr += 6;
+	}
+}
+
+
+// compute out = J*in.
+
+static void multiply_J (int m, dRealMutablePtr J, int *jb,
+	dRealMutablePtr in, dRealMutablePtr out)
+{
+	int i,j;
+	dRealPtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];	
+		int b2 = jb[i*2+1];
+		dReal sum = 0;
+		dRealMutablePtr in_ptr = in + b1*6;
+		for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];				
+		J_ptr += 6;
+		if (b2 >= 0) {
+			in_ptr = in + b2*6;
+			for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];				
+		}
+		J_ptr += 6;
+		out[i] = sum;
+	}
+}
+
+
+// compute out = (J*inv(M)*J' + cfm)*in.
+// use z as an nb*6 temporary.
+
+static void multiply_J_invM_JT (int m, int nb, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
+	dRealPtr cfm, dRealMutablePtr z, dRealMutablePtr in, dRealMutablePtr out)
+{
+	multiply_invM_JT (m,nb,iMJ,jb,in,z);
+	multiply_J (m,J,jb,z,out);
+
+	// add cfm
+	for (int i=0; i<m; i++) out[i] += cfm[i] * in[i];
+}
+
+//***************************************************************************
+// conjugate gradient method with jacobi preconditioner
+// THIS IS EXPERIMENTAL CODE that doesn't work too well, so it is ifdefed out.
+//
+// adding CFM seems to be critically important to this method.
+
+#if 0
+
+static inline dReal dot (int n, dRealPtr x, dRealPtr y)
+{
+	dReal sum=0;
+	for (int i=0; i<n; i++) sum += x[i]*y[i];
+	return sum;
+}
+
+
+// x = y + z*alpha
+
+static inline void add (int n, dRealMutablePtr x, dRealPtr y, dRealPtr z, dReal alpha)
+{
+	for (int i=0; i<n; i++) x[i] = y[i] + z[i]*alpha;
+}
+
+
+static void CG_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
+	dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
+	dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
+	dxQuickStepParameters *qs)
+{
+	int i,j;
+	const int num_iterations = qs->num_iterations;
+
+	// precompute iMJ = inv(M)*J'
+	dRealAllocaArray (iMJ,m*12);
+	compute_invM_JT (m,J,iMJ,jb,body,invI);
+
+	dReal last_rho = 0;
+	dRealAllocaArray (r,m);
+	dRealAllocaArray (z,m);
+	dRealAllocaArray (p,m);
+	dRealAllocaArray (q,m);
+
+	// precompute 1 / diagonals of A
+	dRealAllocaArray (Ad,m);
+	dRealPtr iMJ_ptr = iMJ;
+	dRealPtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		dReal sum = 0;
+		for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
+		if (jb[i*2+1] >= 0) {
+			for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
+		}
+		iMJ_ptr += 12;
+		J_ptr += 12;
+		Ad[i] = REAL(1.0) / (sum + cfm[i]);
+	}
+
+#ifdef WARM_STARTING
+	// compute residual r = b - A*lambda
+	multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r);
+	for (i=0; i<m; i++) r[i] = b[i] - r[i];
+#else
+	dSetZero (lambda,m);
+	memcpy (r,b,m*sizeof(dReal));		// residual r = b - A*lambda
+#endif
+	
+	for (int iteration=0; iteration < num_iterations; iteration++) {
+		for (i=0; i<m; i++) z[i] = r[i]*Ad[i];	// z = inv(M)*r
+		dReal rho = dot (m,r,z);		// rho = r'*z
+		
+		// @@@
+		// we must check for convergence, otherwise rho will go to 0 if
+		// we get an exact solution, which will introduce NaNs into the equations.
+		if (rho < 1e-10) {
+			printf ("CG returned at iteration %d\n",iteration);
+			break;
+		}
+		
+		if (iteration==0) {
+			memcpy (p,z,m*sizeof(dReal));	// p = z
+		}
+		else {
+			add (m,p,z,p,rho/last_rho);	// p = z + (rho/last_rho)*p
+		}
+		
+		// compute q = (J*inv(M)*J')*p
+		multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,p,q);
+	
+		dReal alpha = rho/dot (m,p,q);		// alpha = rho/(p'*q)
+		add (m,lambda,lambda,p,alpha);		// lambda = lambda + alpha*p
+		add (m,r,r,q,-alpha);			// r = r - alpha*q
+		last_rho = rho;
+	}
+
+	// compute fc = inv(M)*J'*lambda
+	multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
+
+#if 0
+	// measure solution error
+	multiply_J_invM_JT (m,nb,J,iMJ,jb,cfm,fc,lambda,r);
+	dReal error = 0;
+	for (i=0; i<m; i++) error += dFabs(r[i] - b[i]);
+	printf ("lambda error = %10.6e\n",error);
+#endif
+}
+
+#endif
+
+//***************************************************************************
+// SOR-LCP method
+
+// nb is the number of bodies in the body array.
+// J is an m*12 matrix of constraint rows
+// jb is an array of first and second body numbers for each constraint row
+// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
+//
+// this returns lambda and fc (the constraint force).
+// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
+//
+// b, lo and hi are modified on exit
+
+
+struct IndexError {
+	dReal error;		// error to sort on
+	int findex;
+	int index;		// row index
+};
+
+
+#ifdef REORDER_CONSTRAINTS
+
+static int compare_index_error (const void *a, const void *b)
+{
+	const IndexError *i1 = (IndexError*) a;
+	const IndexError *i2 = (IndexError*) b;
+	if (i1->findex < 0 && i2->findex >= 0) return -1;
+	if (i1->findex >= 0 && i2->findex < 0) return 1;
+	if (i1->error < i2->error) return -1;
+	if (i1->error > i2->error) return 1;
+	return 0;
+}
+
+#endif
+
+
+static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
+	dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
+	dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
+	dxQuickStepParameters *qs)
+{
+	const int num_iterations = qs->num_iterations;
+	const dReal sor_w = qs->w;		// SOR over-relaxation parameter
+
+	int i,j;
+
+#ifdef WARM_STARTING
+	// for warm starting, this seems to be necessary to prevent
+	// jerkiness in motor-driven joints. i have no idea why this works.
+	for (i=0; i<m; i++) lambda[i] *= 0.9;
+#else
+	dSetZero (lambda,m);
+#endif
+
+	// the lambda computed at the previous iteration.
+	// this is used to measure error for when we are reordering the indexes.
+	dRealAllocaArray (last_lambda,m);
+
+	// a copy of the 'hi' vector in case findex[] is being used
+	dRealAllocaArray (hicopy,m);
+	memcpy (hicopy,hi,m*sizeof(dReal));
+
+	// precompute iMJ = inv(M)*J'
+	dRealAllocaArray (iMJ,m*12);
+	compute_invM_JT (m,J,iMJ,jb,body,invI);
+
+	// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
+	// as we change lambda.
+#ifdef WARM_STARTING
+	multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
+#else
+	dSetZero (fc,nb*6);
+#endif
+
+	// precompute 1 / diagonals of A
+	dRealAllocaArray (Ad,m);
+	dRealPtr iMJ_ptr = iMJ;
+	dRealMutablePtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		dReal sum = 0;
+		for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
+		if (jb[i*2+1] >= 0) {
+			for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
+		}
+		iMJ_ptr += 12;
+		J_ptr += 12;
+		Ad[i] = sor_w / (sum + cfm[i]);
+	}
+
+	// scale J and b by Ad
+	J_ptr = J;
+	for (i=0; i<m; i++) {
+		for (j=0; j<12; j++) {
+			J_ptr[0] *= Ad[i];
+			J_ptr++;
+		}
+		b[i] *= Ad[i];
+	}
+
+	// scale Ad by CFM
+	for (i=0; i<m; i++) Ad[i] *= cfm[i];
+
+	// order to solve constraint rows in
+	IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
+
+#ifndef REORDER_CONSTRAINTS
+	// make sure constraints with findex < 0 come first.
+	j=0;
+	for (i=0; i<m; i++) if (findex[i] < 0) order[j++].index = i;
+	for (i=0; i<m; i++) if (findex[i] >= 0) order[j++].index = i;
+	dIASSERT (j==m);
+#endif
+
+	for (int iteration=0; iteration < num_iterations; iteration++) {
+
+#ifdef REORDER_CONSTRAINTS
+		// constraints with findex < 0 always come first.
+		if (iteration < 2) {
+			// for the first two iterations, solve the constraints in
+			// the given order
+			for (i=0; i<m; i++) {
+				order[i].error = i;
+				order[i].findex = findex[i];
+				order[i].index = i;
+			}
+		}
+		else {
+			// sort the constraints so that the ones converging slowest
+			// get solved last. use the absolute (not relative) error.
+			for (i=0; i<m; i++) {
+				dReal v1 = dFabs (lambda[i]);
+				dReal v2 = dFabs (last_lambda[i]);
+				dReal max = (v1 > v2) ? v1 : v2;
+				if (max > 0) {
+					//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
+					order[i].error = dFabs(lambda[i]-last_lambda[i]);
+				}
+				else {
+					order[i].error = dInfinity;
+				}
+				order[i].findex = findex[i];
+				order[i].index = i;
+			}
+		}
+		qsort (order,m,sizeof(IndexError),&compare_index_error);
+#endif
+#ifdef RANDOMLY_REORDER_CONSTRAINTS
+                if ((iteration & 7) == 0) {
+			for (i=1; i<m; ++i) {
+				IndexError tmp = order[i];
+				int swapi = dRandInt(i+1);
+				order[i] = order[swapi];
+				order[swapi] = tmp;
+			}
+                }
+#endif
+
+		//@@@ potential optimization: swap lambda and last_lambda pointers rather
+		//    than copying the data. we must make sure lambda is properly
+		//    returned to the caller
+		memcpy (last_lambda,lambda,m*sizeof(dReal));
+
+		for (int i=0; i<m; i++) {
+			// @@@ potential optimization: we could pre-sort J and iMJ, thereby
+			//     linearizing access to those arrays. hmmm, this does not seem
+			//     like a win, but we should think carefully about our memory
+			//     access pattern.
+		
+			int index = order[i].index;
+			J_ptr = J + index*12;
+			iMJ_ptr = iMJ + index*12;
+		
+			// set the limits for this constraint. note that 'hicopy' is used.
+			// this is the place where the QuickStep method differs from the
+			// direct LCP solving method, since that method only performs this
+			// limit adjustment once per time step, whereas this method performs
+			// once per iteration per constraint row.
+			// the constraints are ordered so that all lambda[] values needed have
+			// already been computed.
+			if (findex[index] >= 0) {
+				hi[index] = dFabs (hicopy[index] * lambda[findex[index]]);
+				lo[index] = -hi[index];
+			}
+
+			int b1 = jb[index*2];
+			int b2 = jb[index*2+1];
+			dReal delta = b[index] - lambda[index]*Ad[index];
+			dRealMutablePtr fc_ptr = fc + 6*b1;
+			
+			// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
+			delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
+				fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
+				fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
+			// @@@ potential optimization: handle 1-body constraints in a separate
+			//     loop to avoid the cost of test & jump?
+			if (b2 >= 0) {
+				fc_ptr = fc + 6*b2;
+				delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
+					fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
+					fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
+			}
+
+			// compute lambda and clamp it to [lo,hi].
+			// @@@ potential optimization: does SSE have clamping instructions
+			//     to save test+jump penalties here?
+			dReal new_lambda = lambda[index] + delta;
+			if (new_lambda < lo[index]) {
+				delta = lo[index]-lambda[index];
+				lambda[index] = lo[index];
+			}
+			else if (new_lambda > hi[index]) {
+				delta = hi[index]-lambda[index];
+				lambda[index] = hi[index];
+			}
+			else {
+				lambda[index] = new_lambda;
+			}
+
+			//@@@ a trick that may or may not help
+			//dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations));
+			//delta *= ramp;
+		
+			// update fc.
+			// @@@ potential optimization: SIMD for this and the b2 >= 0 case
+			fc_ptr = fc + 6*b1;
+			fc_ptr[0] += delta * iMJ_ptr[0];
+			fc_ptr[1] += delta * iMJ_ptr[1];
+			fc_ptr[2] += delta * iMJ_ptr[2];
+			fc_ptr[3] += delta * iMJ_ptr[3];
+			fc_ptr[4] += delta * iMJ_ptr[4];
+			fc_ptr[5] += delta * iMJ_ptr[5];
+			// @@@ potential optimization: handle 1-body constraints in a separate
+			//     loop to avoid the cost of test & jump?
+			if (b2 >= 0) {
+				fc_ptr = fc + 6*b2;
+				fc_ptr[0] += delta * iMJ_ptr[6];
+				fc_ptr[1] += delta * iMJ_ptr[7];
+				fc_ptr[2] += delta * iMJ_ptr[8];
+				fc_ptr[3] += delta * iMJ_ptr[9];
+				fc_ptr[4] += delta * iMJ_ptr[10];
+				fc_ptr[5] += delta * iMJ_ptr[11];
+			}
+		}
+	}
+}
+
+
+void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
+		     dxJoint * const *_joint, int nj, dReal stepsize)
+{
+	int i,j;
+	IFTIMING(dTimerStart("preprocessing");)
+
+	dReal stepsize1 = dRecip(stepsize);
+
+	// number all bodies in the body list - set their tag values
+	for (i=0; i<nb; i++) body[i]->tag = i;
+	
+	// make a local copy of the joint array, because we might want to modify it.
+	// (the "dxJoint *const*" declaration says we're allowed to modify the joints
+	// but not the joint array, because the caller might need it unchanged).
+	//@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
+	dxJoint **joint = (dxJoint**) alloca (nj * sizeof(dxJoint*));
+	memcpy (joint,_joint,nj * sizeof(dxJoint*));
+	
+	// for all bodies, compute the inertia tensor and its inverse in the global
+	// frame, and compute the rotational force and add it to the torque
+	// accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
+	dRealAllocaArray (I,3*4*nb);	// need to remember all I's for feedback purposes only
+	dRealAllocaArray (invI,3*4*nb);
+	for (i=0; i<nb; i++) {
+		dMatrix3 tmp;
+		// compute inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
+		dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
+		// compute inverse inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
+		dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
+		// compute rotational force
+		dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
+		dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
+	}
+
+	// add the gravity force to all bodies
+	for (i=0; i<nb; i++) {
+		if ((body[i]->flags & dxBodyNoGravity)==0) {
+			body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
+			body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
+			body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
+		}
+	}
+
+	// get joint information (m = total constraint dimension, nub = number of unbounded variables).
+	// joints with m=0 are inactive and are removed from the joints array
+	// entirely, so that the code that follows does not consider them.
+	//@@@ do we really need to save all the info1's
+	dxJoint::Info1 *info = (dxJoint::Info1*) alloca (nj*sizeof(dxJoint::Info1));
+	for (i=0, j=0; j<nj; j++) {	// i=dest, j=src
+		joint[j]->vtable->getInfo1 (joint[j],info+i);
+		dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
+		if (info[i].m > 0) {
+			joint[i] = joint[j];
+			i++;
+		}
+	}
+	nj = i;
+
+	// create the row offset array
+	int m = 0;
+	int *ofs = (int*) alloca (nj*sizeof(int));
+	for (i=0; i<nj; i++) {
+		ofs[i] = m;
+		m += info[i].m;
+	}
+
+	// if there are constraints, compute the constraint force
+	dRealAllocaArray (J,m*12);
+	int *jb = (int*) alloca (m*2*sizeof(int));
+	if (m > 0) {
+		// create a constraint equation right hand side vector `c', a constraint
+		// force mixing vector `cfm', and LCP low and high bound vectors, and an
+		// 'findex' vector.
+		dRealAllocaArray (c,m);
+		dRealAllocaArray (cfm,m);
+		dRealAllocaArray (lo,m);
+		dRealAllocaArray (hi,m);
+		int *findex = (int*) alloca (m*sizeof(int));
+		dSetZero (c,m);
+		dSetValue (cfm,m,world->global_cfm);
+		dSetValue (lo,m,-dInfinity);
+		dSetValue (hi,m, dInfinity);
+		for (i=0; i<m; i++) findex[i] = -1;
+		
+		// get jacobian data from constraints. an m*12 matrix will be created
+		// to store the two jacobian blocks from each constraint. it has this
+		// format:
+		//
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \    .
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2  }-- jacobian for joint 0, body 1 and body 2 (3 rows)
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
+		//   etc...
+		//
+		//   (lll) = linear jacobian data
+		//   (aaa) = angular jacobian data
+		//
+		IFTIMING (dTimerNow ("create J");)
+		dSetZero (J,m*12);
+		dxJoint::Info2 Jinfo;
+		Jinfo.rowskip = 12;
+		Jinfo.fps = stepsize1;
+		Jinfo.erp = world->global_erp;
+		for (i=0; i<nj; i++) {
+			Jinfo.J1l = J + ofs[i]*12;
+			Jinfo.J1a = Jinfo.J1l + 3;
+			Jinfo.J2l = Jinfo.J1l + 6;
+			Jinfo.J2a = Jinfo.J1l + 9;
+			Jinfo.c = c + ofs[i];
+			Jinfo.cfm = cfm + ofs[i];
+			Jinfo.lo = lo + ofs[i];
+			Jinfo.hi = hi + ofs[i];
+			Jinfo.findex = findex + ofs[i];
+			joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
+			// adjust returned findex values for global index numbering
+			for (j=0; j<info[i].m; j++) {
+				if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
+			}
+		}
+
+		// create an array of body numbers for each joint row
+		int *jb_ptr = jb;
+		for (i=0; i<nj; i++) {
+			int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->tag) : -1;
+			int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->tag) : -1;
+			for (j=0; j<info[i].m; j++) {
+				jb_ptr[0] = b1;
+				jb_ptr[1] = b2;
+				jb_ptr += 2;
+			}
+		}
+		dIASSERT (jb_ptr == jb+2*m);
+
+		// compute the right hand side `rhs'
+		IFTIMING (dTimerNow ("compute rhs");)
+		dRealAllocaArray (tmp1,nb*6);
+		// put v/h + invM*fe into tmp1
+		for (i=0; i<nb; i++) {
+			dReal body_invMass = body[i]->invMass;
+			for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1;
+			dMULTIPLY0_331 (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc);
+			for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1;
+		}
+
+		// put J*tmp1 into rhs
+		dRealAllocaArray (rhs,m);
+		multiply_J (m,J,jb,tmp1,rhs);
+
+		// complete rhs
+		for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
+
+		// scale CFM
+		for (i=0; i<m; i++) cfm[i] *= stepsize1;
+
+		// load lambda from the value saved on the previous iteration
+		dRealAllocaArray (lambda,m);
+#ifdef WARM_STARTING
+		dSetZero (lambda,m);	//@@@ shouldn't be necessary
+		for (i=0; i<nj; i++) {
+			memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(dReal));
+		}
+#endif
+
+		// solve the LCP problem and get lambda and invM*constraint_force
+		IFTIMING (dTimerNow ("solving LCP problem");)
+		dRealAllocaArray (cforce,nb*6);
+		SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,&world->qs);
+
+#ifdef WARM_STARTING
+		// save lambda for the next iteration
+		//@@@ note that this doesn't work for contact joints yet, as they are
+		// recreated every iteration
+		for (i=0; i<nj; i++) {
+			memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(dReal));
+		}
+#endif
+
+		// note that the SOR method overwrites rhs and J at this point, so
+		// they should not be used again.
+		
+		// add stepsize * cforce to the body velocity
+		for (i=0; i<nb; i++) {
+			for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * cforce[i*6+j];
+			for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j];
+		}
+
+		// if joint feedback is requested, compute the constraint force.
+		// BUT: cforce is inv(M)*J'*lambda, whereas we want just J'*lambda,
+		// so we must compute M*cforce.
+		// @@@ if any joint has a feedback request we compute the entire
+		//     adjusted cforce, which is not the most efficient way to do it.
+		for (j=0; j<nj; j++) {
+			if (joint[j]->feedback) {
+				// compute adjusted cforce
+				for (i=0; i<nb; i++) {
+					dReal k = body[i]->mass.mass;
+					cforce [i*6+0] *= k;
+					cforce [i*6+1] *= k;
+					cforce [i*6+2] *= k;
+					dVector3 tmp;
+					dMULTIPLY0_331 (tmp, I + 12*i, cforce + i*6 + 3);
+					cforce [i*6+3] = tmp[0];
+					cforce [i*6+4] = tmp[1];
+					cforce [i*6+5] = tmp[2];
+				}
+				// compute feedback for this and all remaining joints
+				for (; j<nj; j++) {
+					dJointFeedback *fb = joint[j]->feedback;
+					if (fb) {
+						int b1 = joint[j]->node[0].body->tag;
+						memcpy (fb->f1,cforce+b1*6,3*sizeof(dReal));
+						memcpy (fb->t1,cforce+b1*6+3,3*sizeof(dReal));
+						if (joint[j]->node[1].body) {
+							int b2 = joint[j]->node[1].body->tag;
+							memcpy (fb->f2,cforce+b2*6,3*sizeof(dReal));
+							memcpy (fb->t2,cforce+b2*6+3,3*sizeof(dReal));
+						}
+					}
+				}
+			}
+		}
+	}
+
+	// compute the velocity update:
+	// add stepsize * invM * fe to the body velocity
+
+	IFTIMING (dTimerNow ("compute velocity update");)
+	for (i=0; i<nb; i++) {
+		dReal body_invMass = body[i]->invMass;
+		for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j];
+		for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize;	
+		dMULTIPLYADD0_331 (body[i]->avel,invI + i*12,body[i]->tacc);
+	}
+
+#if 0
+	// check that the updated velocity obeys the constraint (this check needs unmodified J)
+	dRealAllocaArray (vel,nb*6);
+	for (i=0; i<nb; i++) {
+		for (j=0; j<3; j++) vel[i*6+j] = body[i]->lvel[j];
+		for (j=0; j<3; j++) vel[i*6+3+j] = body[i]->avel[j];
+	}
+	dRealAllocaArray (tmp,m);
+	multiply_J (m,J,jb,vel,tmp);
+	dReal error = 0;
+	for (i=0; i<m; i++) error += dFabs(tmp[i]);
+	printf ("velocity error = %10.6e\n",error);
+#endif
+
+	// update the position and orientation from the new linear/angular velocity
+	// (over the given timestep)
+	IFTIMING (dTimerNow ("update position");)
+	for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
+
+	IFTIMING (dTimerNow ("tidy up");)
+
+	// zero all force accumulators
+	for (i=0; i<nb; i++) {
+		dSetZero (body[i]->facc,3);
+		dSetZero (body[i]->tacc,3);
+	}
+
+	IFTIMING (dTimerEnd();)
+	IFTIMING (if (m > 0) dTimerReport (stdout,1);)
+}

src/source/ode/quickstep.h

@@ -0,0 +1,33 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_QUICK_STEP_H_
+#define _ODE_QUICK_STEP_H_
+
+#include <ode/common.h>
+
+
+void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
+		     dxJoint * const *_joint, int nj, dReal stepsize);
+
+
+#endif

src/source/ode/rotation.cpp

@@ -0,0 +1,304 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+quaternions have the format: (s,vx,vy,vz) where (vx,vy,vz) is the
+"rotation axis" and s is the "rotation angle".
+
+*/
+
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+
+
+#define _R(i,j) R[(i)*4+(j)]
+
+#define SET_3x3_IDENTITY \
+  _R(0,0) = REAL(1.0); \
+  _R(0,1) = REAL(0.0); \
+  _R(0,2) = REAL(0.0); \
+  _R(0,3) = REAL(0.0); \
+  _R(1,0) = REAL(0.0); \
+  _R(1,1) = REAL(1.0); \
+  _R(1,2) = REAL(0.0); \
+  _R(1,3) = REAL(0.0); \
+  _R(2,0) = REAL(0.0); \
+  _R(2,1) = REAL(0.0); \
+  _R(2,2) = REAL(1.0); \
+  _R(2,3) = REAL(0.0);
+
+
+void dRSetIdentity (dMatrix3 R)
+{
+  dAASSERT (R);
+  SET_3x3_IDENTITY;
+}
+
+
+void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az,
+			 dReal angle)
+{
+  dAASSERT (R);
+  dQuaternion q;
+  dQFromAxisAndAngle (q,ax,ay,az,angle);
+  dQtoR (q,R);
+}
+
+
+void dRFromEulerAngles (dMatrix3 R, dReal phi, dReal theta, dReal psi)
+{
+  dReal sphi,cphi,stheta,ctheta,spsi,cpsi;
+  dAASSERT (R);
+  sphi = dSin(phi);
+  cphi = dCos(phi);
+  stheta = dSin(theta);
+  ctheta = dCos(theta);
+  spsi = dSin(psi);
+  cpsi = dCos(psi);
+  _R(0,0) = cpsi*ctheta;
+  _R(0,1) = spsi*ctheta;
+  _R(0,2) =-stheta;
+  _R(1,0) = cpsi*stheta*sphi - spsi*cphi;
+  _R(1,1) = spsi*stheta*sphi + cpsi*cphi;
+  _R(1,2) = ctheta*sphi;
+  _R(2,0) = cpsi*stheta*cphi + spsi*sphi;
+  _R(2,1) = spsi*stheta*cphi - cpsi*sphi;
+  _R(2,2) = ctheta*cphi;
+}
+
+
+void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
+		  dReal bx, dReal by, dReal bz)
+{
+  dReal l,k;
+  dAASSERT (R);
+  l = dSqrt (ax*ax + ay*ay + az*az);
+  if (l <= REAL(0.0)) {
+    dDEBUGMSG ("zero length vector");
+    return;
+  }
+  l = dRecip(l);
+  ax *= l;
+  ay *= l;
+  az *= l;
+  k = ax*bx + ay*by + az*bz;
+  bx -= k*ax;
+  by -= k*ay;
+  bz -= k*az;
+  l = dSqrt (bx*bx + by*by + bz*bz);
+  if (l <= REAL(0.0)) {
+    dDEBUGMSG ("zero length vector");
+    return;
+  }
+  l = dRecip(l);
+  bx *= l;
+  by *= l;
+  bz *= l;
+  _R(0,0) = ax;
+  _R(1,0) = ay;
+  _R(2,0) = az;
+  _R(0,1) = bx;
+  _R(1,1) = by;
+  _R(2,1) = bz;
+  _R(0,2) = - by*az + ay*bz;
+  _R(1,2) = - bz*ax + az*bx;
+  _R(2,2) = - bx*ay + ax*by;
+}
+
+
+void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az)
+{
+  dVector3 n,p,q;
+  n[0] = ax;
+  n[1] = ay;
+  n[2] = az;
+  dNormalize3 (n);
+  dPlaneSpace (n,p,q);
+  _R(0,0) = p[0];
+  _R(1,0) = p[1];
+  _R(2,0) = p[2];
+  _R(0,1) = q[0];
+  _R(1,1) = q[1];
+  _R(2,1) = q[2];
+  _R(0,2) = n[0];
+  _R(1,2) = n[1];
+  _R(2,2) = n[2];
+}
+
+
+void dQSetIdentity (dQuaternion q)
+{
+  dAASSERT (q);
+  q[0] = 1;
+  q[1] = 0;
+  q[2] = 0;
+  q[3] = 0;
+}
+
+
+void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
+			 dReal angle)
+{
+  dAASSERT (q);
+  dReal l = ax*ax + ay*ay + az*az;
+  if (l > REAL(0.0)) {
+    angle *= REAL(0.5);
+    q[0] = dCos (angle);
+    l = dSin(angle) * dRecipSqrt(l);
+    q[1] = ax*l;
+    q[2] = ay*l;
+    q[3] = az*l;
+  }
+  else {
+    q[0] = 1;
+    q[1] = 0;
+    q[2] = 0;
+    q[3] = 0;
+  }
+}
+
+
+void dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
+{
+  dAASSERT (qa && qb && qc);
+  qa[0] = qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
+  qa[1] = qb[0]*qc[1] + qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
+  qa[2] = qb[0]*qc[2] + qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
+  qa[3] = qb[0]*qc[3] + qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
+}
+
+
+void dQMultiply1 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
+{
+  dAASSERT (qa && qb && qc);
+  qa[0] = qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
+  qa[1] = qb[0]*qc[1] - qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
+  qa[2] = qb[0]*qc[2] - qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
+  qa[3] = qb[0]*qc[3] - qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
+}
+
+
+void dQMultiply2 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
+{
+  dAASSERT (qa && qb && qc);
+  qa[0] =  qb[0]*qc[0] + qb[1]*qc[1] + qb[2]*qc[2] + qb[3]*qc[3];
+  qa[1] = -qb[0]*qc[1] + qb[1]*qc[0] - qb[2]*qc[3] + qb[3]*qc[2];
+  qa[2] = -qb[0]*qc[2] + qb[2]*qc[0] - qb[3]*qc[1] + qb[1]*qc[3];
+  qa[3] = -qb[0]*qc[3] + qb[3]*qc[0] - qb[1]*qc[2] + qb[2]*qc[1];
+}
+
+
+void dQMultiply3 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc)
+{
+  dAASSERT (qa && qb && qc);
+  qa[0] =  qb[0]*qc[0] - qb[1]*qc[1] - qb[2]*qc[2] - qb[3]*qc[3];
+  qa[1] = -qb[0]*qc[1] - qb[1]*qc[0] + qb[2]*qc[3] - qb[3]*qc[2];
+  qa[2] = -qb[0]*qc[2] - qb[2]*qc[0] + qb[3]*qc[1] - qb[1]*qc[3];
+  qa[3] = -qb[0]*qc[3] - qb[3]*qc[0] + qb[1]*qc[2] - qb[2]*qc[1];
+}
+
+
+// dRfromQ(), dQfromR() and dDQfromW() are derived from equations in "An Introduction
+// to Physically Based Modeling: Rigid Body Simulation - 1: Unconstrained
+// Rigid Body Dynamics" by David Baraff, Robotics Institute, Carnegie Mellon
+// University, 1997.
+
+void dRfromQ (dMatrix3 R, const dQuaternion q)
+{
+  dAASSERT (q && R);
+  // q = (s,vx,vy,vz)
+  dReal qq1 = 2*q[1]*q[1];
+  dReal qq2 = 2*q[2]*q[2];
+  dReal qq3 = 2*q[3]*q[3];
+  _R(0,0) = 1 - qq2 - qq3;
+  _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
+  _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
+  _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
+  _R(1,1) = 1 - qq1 - qq3;
+  _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
+  _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
+  _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
+  _R(2,2) = 1 - qq1 - qq2;
+}
+
+
+void dQfromR (dQuaternion q, const dMatrix3 R)
+{
+  dAASSERT (q && R);
+  dReal tr,s;
+  tr = _R(0,0) + _R(1,1) + _R(2,2);
+  if (tr >= 0) {
+    s = dSqrt (tr + 1);
+    q[0] = REAL(0.5) * s;
+    s = REAL(0.5) * dRecip(s);
+    q[1] = (_R(2,1) - _R(1,2)) * s;
+    q[2] = (_R(0,2) - _R(2,0)) * s;
+    q[3] = (_R(1,0) - _R(0,1)) * s;
+  }
+  else {
+    // find the largest diagonal element and jump to the appropriate case
+    if (_R(1,1) > _R(0,0)) {
+      if (_R(2,2) > _R(1,1)) goto case_2;
+      goto case_1;
+    }
+    if (_R(2,2) > _R(0,0)) goto case_2;
+    goto case_0;
+
+    case_0:
+    s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
+    q[1] = REAL(0.5) * s;
+    s = REAL(0.5) * dRecip(s);
+    q[2] = (_R(0,1) + _R(1,0)) * s;
+    q[3] = (_R(2,0) + _R(0,2)) * s;
+    q[0] = (_R(2,1) - _R(1,2)) * s;
+    return;
+
+    case_1:
+    s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
+    q[2] = REAL(0.5) * s;
+    s = REAL(0.5) * dRecip(s);
+    q[3] = (_R(1,2) + _R(2,1)) * s;
+    q[1] = (_R(0,1) + _R(1,0)) * s;
+    q[0] = (_R(0,2) - _R(2,0)) * s;
+    return;
+
+    case_2:
+    s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
+    q[3] = REAL(0.5) * s;
+    s = REAL(0.5) * dRecip(s);
+    q[1] = (_R(2,0) + _R(0,2)) * s;
+    q[2] = (_R(1,2) + _R(2,1)) * s;
+    q[0] = (_R(1,0) - _R(0,1)) * s;
+    return;
+  }
+}
+
+
+void dDQfromW (dReal dq[4], const dVector3 w, const dQuaternion q)
+{
+  dAASSERT (w && q && dq);
+  dq[0] = REAL(0.5)*(- w[0]*q[1] - w[1]*q[2] - w[2]*q[3]);
+  dq[1] = REAL(0.5)*(  w[0]*q[0] + w[1]*q[3] - w[2]*q[2]);
+  dq[2] = REAL(0.5)*(- w[0]*q[3] + w[1]*q[0] + w[2]*q[1]);
+  dq[3] = REAL(0.5)*(  w[0]*q[2] - w[1]*q[1] + w[2]*q[0]);
+}

src/source/ode/scrapbook.cpp

@@ -0,0 +1,485 @@
+
+/*
+
+this is code that was once useful but has now been obseleted.
+
+this file should not be compiled as part of ODE!
+
+*/
+
+//***************************************************************************
+// intersect a line segment with a plane
+
+extern "C" int dClipLineToBox (const dVector3 p1, const dVector3 p2,
+			       const dVector3 p, const dMatrix3 R,
+			       const dVector3 side)
+{
+  // compute the start and end of the line (p1 and p2) relative to the box.
+  // we will do all subsequent computations in this box-relative coordinate
+  // system. we have to do a translation and rotation for each point.
+  dVector3 tmp,s,e;
+  tmp[0] = p1[0] - p[0];
+  tmp[1] = p1[1] - p[1];
+  tmp[2] = p1[2] - p[2];
+  dMULTIPLY1_331 (s,R,tmp);
+  tmp[0] = p2[0] - p[0];
+  tmp[1] = p2[1] - p[1];
+  tmp[2] = p2[2] - p[2];
+  dMULTIPLY1_331 (e,R,tmp);
+
+  // compute the vector 'v' from the start point to the end point
+  dVector3 v;
+  v[0] = e[0] - s[0];
+  v[1] = e[1] - s[1];
+  v[2] = e[2] - s[2];
+
+  // a point on the line is defined by the parameter 't'. t=0 corresponds
+  // to the start of the line, t=1 corresponds to the end of the line.
+  // we will clip the line to the box by finding the range of t where a
+  // point on the line is inside the box. the currently known bounds for
+  // t and tlo..thi.
+  dReal tlo=0,thi=1;
+
+  // clip in the X/Y/Z direction
+  for (int i=0; i<3; i++) {
+    // first adjust s,e for the current t range. this is redundant for the
+    // first iteration, but never mind.
+    e[i] = s[i] + thi*v[i];
+    s[i] = s[i] + tlo*v[i];
+    // compute where t intersects the positive and negative sides.
+    dReal tp = ( side[i] - s[i])/v[i];	// @@@ handle case where denom=0
+    dReal tm = (-side[i] - s[i])/v[i];
+    // handle 9 intersection cases
+    if (s[i] <= -side[i]) {
+      tlo = tm;
+      if (e[i] <= -side[i]) return 0;
+      else if (e[i] >= side[i]) thi = tp;
+    }
+    else if (s[i] <= side[i]) {
+      if (e[i] <= -side[i]) thi = tm;
+      else if (e[i] >= side[i]) thi = tp;
+    }
+    else {
+      tlo = tp;
+      if (e[i] <= -side[i]) thi = tm;
+      else if (e[i] >= side[i]) return 0;
+    }
+  }
+
+  //... @@@ AT HERE @@@
+
+  return 1;
+}
+
+
+//***************************************************************************
+// a nice try at C-B collision. unfortunately it doesn't work. the logic
+// for testing for line-box intersection is correct, but unfortunately the
+// closest-point distance estimates are often too large. as a result contact
+// points are placed incorrectly.
+
+
+int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags,
+		dContactGeom *contact, int skip)
+{
+  int i;
+
+  dIASSERT (skip >= (int)sizeof(dContactGeom));
+  dIASSERT (o1->_class->num == dCCylinderClass);
+  dIASSERT (o2->_class->num == dBoxClass);
+  contact->g1 = const_cast<dxGeom*> (o1);
+  contact->g2 = const_cast<dxGeom*> (o2);
+  dxCCylinder *cyl = (dxCCylinder*) CLASSDATA(o1);
+  dxBox *box = (dxBox*) CLASSDATA(o2);
+
+  // get p1,p2 = cylinder axis endpoints, get radius
+  dVector3 p1,p2;
+  dReal clen = cyl->lz * REAL(0.5);
+  p1[0] = o1->pos[0] + clen * o1->R[2];
+  p1[1] = o1->pos[1] + clen * o1->R[6];
+  p1[2] = o1->pos[2] + clen * o1->R[10];
+  p2[0] = o1->pos[0] - clen * o1->R[2];
+  p2[1] = o1->pos[1] - clen * o1->R[6];
+  p2[2] = o1->pos[2] - clen * o1->R[10];
+  dReal radius = cyl->radius;
+
+  // copy out box center, rotation matrix, and side array
+  dReal *c = o2->pos;
+  dReal *R = o2->R;
+  dReal *side = box->side;
+
+  // compute the start and end of the line (p1 and p2) relative to the box.
+  // we will do all subsequent computations in this box-relative coordinate
+  // system. we have to do a translation and rotation for each point.
+  dVector3 tmp3,s,e;
+  tmp3[0] = p1[0] - c[0];
+  tmp3[1] = p1[1] - c[1];
+  tmp3[2] = p1[2] - c[2];
+  dMULTIPLY1_331 (s,R,tmp3);
+  tmp3[0] = p2[0] - c[0];
+  tmp3[1] = p2[1] - c[1];
+  tmp3[2] = p2[2] - c[2];
+  dMULTIPLY1_331 (e,R,tmp3);
+
+  // compute the vector 'v' from the start point to the end point
+  dVector3 v;
+  v[0] = e[0] - s[0];
+  v[1] = e[1] - s[1];
+  v[2] = e[2] - s[2];
+
+  // compute the half-sides of the box
+  dReal S0 = side[0] * REAL(0.5);
+  dReal S1 = side[1] * REAL(0.5);
+  dReal S2 = side[2] * REAL(0.5);
+
+  // compute the size of the bounding box around the line segment
+  dReal B0 = dFabs (v[0]);
+  dReal B1 = dFabs (v[1]);
+  dReal B2 = dFabs (v[2]);
+
+  // for all 6 separation axes, measure the penetration depth. if any depth is
+  // less than 0 then the objects don't penetrate at all so we can just
+  // return 0. find the axis with the smallest depth, and record its normal.
+
+  // note: normalR is set to point to a column of R if that is the smallest
+  // depth normal so far. otherwise normalR is 0 and normalC is set to a
+  // vector relative to the box. invert_normal is 1 if the sign of the normal
+  // should be flipped.
+
+  dReal depth,trial_depth,tmp,length;
+  const dReal *normalR=0;
+  dVector3 normalC;
+  int invert_normal = 0;
+  int code = 0;		// 0=no contact, 1-3=face contact, 4-6=edge contact
+
+  depth = dInfinity;
+
+  // look at face-normal axes
+
+#undef TEST
+#define TEST(center,depth_expr,norm,contact_code) \
+  tmp = (center); \
+  trial_depth = radius + REAL(0.5) * ((depth_expr) - dFabs(tmp)); \
+  if (trial_depth < 0) return 0; \
+  if (trial_depth < depth) { \
+    depth = trial_depth; \
+    normalR = (norm); \
+    invert_normal = (tmp < 0); \
+    code = contact_code; \
+  }
+
+  TEST (s[0]+e[0], side[0] + B0, R+0, 1);
+  TEST (s[1]+e[1], side[1] + B1, R+1, 2);
+  TEST (s[2]+e[2], side[2] + B2, R+2, 3);
+
+  // look at v x box-edge axes
+
+#undef TEST
+#define TEST(box_radius,line_offset,nx,ny,nz,contact_code) \
+  tmp = (line_offset); \
+  trial_depth = (box_radius) - dFabs(tmp); \
+  length = dSqrt ((nx)*(nx) + (ny)*(ny) + (nz)*(nz)); \
+  if (length > 0) { \
+    length = dRecip(length); \
+    trial_depth = trial_depth * length + radius; \
+    if (trial_depth < 0) return 0; \
+    if (trial_depth < depth) { \
+      depth = trial_depth; \
+      normalR = 0; \
+      normalC[0] = (nx)*length; \
+      normalC[1] = (ny)*length; \
+      normalC[2] = (nz)*length; \
+      invert_normal = (tmp < 0); \
+      code = contact_code; \
+    } \
+  }
+
+  TEST (B2*S1+B1*S2,v[1]*s[2]-v[2]*s[1], 0,-v[2],v[1], 4);
+  TEST (B2*S0+B0*S2,v[2]*s[0]-v[0]*s[2], v[2],0,-v[0], 5);
+  TEST (B1*S0+B0*S1,v[0]*s[1]-v[1]*s[0], -v[1],v[0],0, 6);
+
+#undef TEST
+
+  // if we get to this point, the box and ccylinder interpenetrate.
+  // compute the normal in global coordinates.
+  dReal *normal = contact[0].normal;
+  if (normalR) {
+    normal[0] = normalR[0];
+    normal[1] = normalR[4];
+    normal[2] = normalR[8];
+  }
+  else {
+    dMULTIPLY0_331 (normal,R,normalC);
+  }
+  if (invert_normal) {
+    normal[0] = -normal[0];
+    normal[1] = -normal[1];
+    normal[2] = -normal[2];
+  }
+
+  // set the depth
+  contact[0].depth = depth;
+
+  if (code == 0) {
+    return 0;		// should never get here
+  }
+  else if (code >= 4) {
+    // handle edge contacts
+    // find an endpoint q1 on the intersecting edge of the box
+    dVector3 q1;
+    dReal sign[3];
+    for (i=0; i<3; i++) q1[i] = c[i];
+    sign[0] = (dDOT14(normal,R+0) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) q1[i] += sign[0] * S0 * R[i*4];
+    sign[1] = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) q1[i] += sign[1] * S1 * R[i*4+1];
+    sign[2] = (dDOT14(normal,R+2) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) q1[i] += sign[2] * S2 * R[i*4+2];
+
+    // find the other endpoint q2 of the intersecting edge
+    dVector3 q2;
+    for (i=0; i<3; i++)
+      q2[i] = q1[i] - R[code-4 + i*4] * (sign[code-4] * side[code-4]);
+
+    // determine the closest point between the box edge and the line segment
+    dVector3 cp1,cp2;
+    dClosestLineSegmentPoints (q1,q2, p1,p2, cp1,cp2);
+    for (i=0; i<3; i++) contact[0].pos[i] = cp1[i] - REAL(0.5)*normal[i]*depth;
+    return 1;
+  }
+  else {
+    // handle face contacts.
+    // @@@ temporary: make deepest vertex on the line the contact point.
+    // @@@ this kind of works, but we sometimes need two contact points for
+    // @@@ stability.
+
+    // compute 'v' in global coordinates
+    dVector3 gv;
+    for (i=0; i<3; i++) gv[i] = p2[i] - p1[i];
+
+    if (dDOT (normal,gv) > 0) {
+      for (i=0; i<3; i++)
+	contact[0].pos[i] = p1[i] + (depth*REAL(0.5)-radius)*normal[i];
+    }
+    else {
+      for (i=0; i<3; i++)
+	contact[0].pos[i] = p2[i] + (depth*REAL(0.5)-radius)*normal[i];
+    }
+    return 1;
+  }
+}
+
+//***************************************************************************
+// this function works, it's just not being used for anything at the moment:
+
+// given a box (R,side), `R' is the rotation matrix for the box, and `side'
+// is a vector of x/y/z side lengths, return the size of the interval of the
+// box projected along the given axis. if the axis has unit length then the
+// return value will be the actual diameter, otherwise the result will be
+// scaled by the axis length.
+
+static inline dReal boxDiameter (const dMatrix3 R, const dVector3 side,
+				 const dVector3 axis)
+{
+  dVector3 q;
+  dMULTIPLY1_331 (q,R,axis);	// transform axis to body-relative
+  return dFabs(q[0])*side[0] + dFabs(q[1])*side[1] + dFabs(q[2])*side[2];
+}
+
+//***************************************************************************
+// the old capped cylinder to capped cylinder collision code. this fails to
+// detect cap-to-cap contact points when the cylinder axis are aligned, but
+// other that that it is pretty robust.
+
+// this returns at most one contact point when the two cylinder's axes are not
+// aligned, and at most two (for stability) when they are aligned.
+// the algorithm minimizes the distance between two "sample spheres" that are
+// positioned along the cylinder axes according to:
+//    sphere1 = pos1 + alpha1 * axis1
+//    sphere2 = pos2 + alpha2 * axis2
+// alpha1 and alpha2 are limited to +/- half the length of the cylinders.
+// the algorithm works by finding a solution that has both alphas free, or
+// a solution that has one or both alphas fixed to the ends of the cylinder.
+
+int dCollideCCylinderCCylinder (dxGeom *o1, dxGeom *o2,
+				int flags, dContactGeom *contact, int skip)
+{
+  int i;
+  const dReal tolerance = REAL(1e-5);
+
+  dIASSERT (skip >= (int)sizeof(dContactGeom));
+  dIASSERT (o1->type == dCCylinderClass);
+  dIASSERT (o2->type == dCCylinderClass);
+  dxCCylinder *cyl1 = (dxCCylinder*) o1;
+  dxCCylinder *cyl2 = (dxCCylinder*) o2;
+
+  contact->g1 = o1;
+  contact->g2 = o2;
+
+  // copy out some variables, for convenience
+  dReal lz1 = cyl1->lz * REAL(0.5);
+  dReal lz2 = cyl2->lz * REAL(0.5);
+  dReal *pos1 = o1->pos;
+  dReal *pos2 = o2->pos;
+  dReal axis1[3],axis2[3];
+  axis1[0] = o1->R[2];
+  axis1[1] = o1->R[6];
+  axis1[2] = o1->R[10];
+  axis2[0] = o2->R[2];
+  axis2[1] = o2->R[6];
+  axis2[2] = o2->R[10];
+
+  dReal alpha1,alpha2,sphere1[3],sphere2[3];
+  int fix1 = 0;		// 0 if alpha1 is free, +/-1 to fix at +/- lz1
+  int fix2 = 0;		// 0 if alpha2 is free, +/-1 to fix at +/- lz2
+
+  for (int count=0; count<9; count++) {
+    // find a trial solution by fixing or not fixing the alphas
+    if (fix1) {
+      if (fix2) {
+	// alpha1 and alpha2 are fixed, so the solution is easy
+	if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1;
+	if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2;
+	for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
+	for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
+      }
+      else {
+	// fix alpha1 but let alpha2 be free
+	if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1;
+	for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
+	alpha2 = (axis2[0]*(sphere1[0]-pos2[0]) +
+		  axis2[1]*(sphere1[1]-pos2[1]) +
+		  axis2[2]*(sphere1[2]-pos2[2]));
+	for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
+      }
+    }
+    else {
+      if (fix2) {
+	// fix alpha2 but let alpha1 be free
+	if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2;
+	for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
+	alpha1 = (axis1[0]*(sphere2[0]-pos1[0]) +
+		  axis1[1]*(sphere2[1]-pos1[1]) +
+		  axis1[2]*(sphere2[2]-pos1[2]));
+	for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
+      }
+      else {
+	// let alpha1 and alpha2 be free
+	// compute determinant of d(d^2)\d(alpha) jacobian
+	dReal a1a2 = dDOT (axis1,axis2);
+	dReal det = REAL(1.0)-a1a2*a1a2;
+	if (det < tolerance) {
+	  // the cylinder axes (almost) parallel, so we will generate up to two
+	  // contacts. the solution matrix is rank deficient so alpha1 and
+	  // alpha2 are related by:
+	  //       alpha2 =   alpha1 + (pos1-pos2)'*axis1   (if axis1==axis2)
+	  //    or alpha2 = -(alpha1 + (pos1-pos2)'*axis1)  (if axis1==-axis2)
+	  // first compute where the two cylinders overlap in alpha1 space:
+	  if (a1a2 < 0) {
+	    axis2[0] = -axis2[0];
+	    axis2[1] = -axis2[1];
+	    axis2[2] = -axis2[2];
+	  }
+	  dReal q[3];
+	  for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];
+	  dReal k = dDOT (axis1,q);
+	  dReal a1lo = -lz1;
+	  dReal a1hi = lz1;
+	  dReal a2lo = -lz2 - k;
+	  dReal a2hi = lz2 - k;
+	  dReal lo = (a1lo > a2lo) ? a1lo : a2lo;
+	  dReal hi = (a1hi < a2hi) ? a1hi : a2hi;
+	  if (lo <= hi) {
+	    int num_contacts = flags & NUMC_MASK;
+	    if (num_contacts >= 2 && lo < hi) {
+	      // generate up to two contacts. if one of those contacts is
+	      // not made, fall back on the one-contact strategy.
+	      for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i];
+	      for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i];
+	      int n1 = dCollideSpheres (sphere1,cyl1->radius,
+					sphere2,cyl2->radius,contact);
+	      if (n1) {
+		for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i];
+		for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i];
+		dContactGeom *c2 = CONTACT(contact,skip);
+		int n2 = dCollideSpheres (sphere1,cyl1->radius,
+					  sphere2,cyl2->radius, c2);
+		if (n2) {
+		  c2->g1 = o1;
+		  c2->g2 = o2;
+		  return 2;
+		}
+	      }
+	    }
+
+	    // just one contact to generate, so put it in the middle of
+	    // the range
+	    alpha1 = (lo + hi) * REAL(0.5);
+	    alpha2 = alpha1 + k;
+	    for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
+	    for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
+	    return dCollideSpheres (sphere1,cyl1->radius,
+				    sphere2,cyl2->radius,contact);
+	  }
+	  else return 0;
+	}
+	det = REAL(1.0)/det;
+	dReal delta[3];
+	for (i=0; i<3; i++) delta[i] = pos1[i] - pos2[i];
+	dReal q1 = dDOT (delta,axis1);
+	dReal q2 = dDOT (delta,axis2);
+	alpha1 = det*(a1a2*q2-q1);
+	alpha2 = det*(q2-a1a2*q1);
+	for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i];
+	for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i];
+      }
+    }
+
+    // if the alphas are outside their allowed ranges then fix them and
+    // try again
+    if (fix1==0) {
+      if (alpha1 < -lz1) {
+	fix1 = -1;
+	continue;
+      }
+      if (alpha1 > lz1) {
+	fix1 = 1;
+	continue;
+      }
+    }
+    if (fix2==0) {
+      if (alpha2 < -lz2) {
+	fix2 = -1;
+	continue;
+      }
+      if (alpha2 > lz2) {
+	fix2 = 1;
+	continue;
+      }
+    }
+
+    // unfix the alpha variables if the local distance gradient indicates
+    // that we are not yet at the minimum
+    dReal tmp[3];
+    for (i=0; i<3; i++) tmp[i] = sphere1[i] - sphere2[i];
+    if (fix1) {
+      dReal gradient = dDOT (tmp,axis1);
+      if ((fix1 > 0 && gradient > 0) || (fix1 < 0 && gradient < 0)) {
+	fix1 = 0;
+	continue;
+      }
+    }
+    if (fix2) {
+      dReal gradient = -dDOT (tmp,axis2);
+      if ((fix2 > 0 && gradient > 0) || (fix2 < 0 && gradient < 0)) {
+	fix2 = 0;
+	continue;
+      }
+    }
+    return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);
+  }
+  // if we go through the loop too much, then give up. we should NEVER get to
+  // this point (i hope).
+  dMessage (0,"dCollideCC(): too many iterations");
+  return 0;
+}

src/source/ode/stack.cpp

@@ -0,0 +1,114 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+@@@ this file should not be compiled any more @@@
+
+#include <string.h>
+#include <errno.h>
+#include "stack.h"
+#include "ode/error.h"
+#include "ode/config.h"
+
+//****************************************************************************
+// unix version that uses mmap(). some systems have anonymous mmaps and some
+// need to mmap /dev/zero.
+
+#ifndef WIN32
+
+#include <unistd.h>
+#include <sys/mman.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+
+void dStack::init (int max_size)
+{
+  if (sizeof(long int) != sizeof(char*)) dDebug (0,"internal");
+  if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0");
+
+#ifndef MMAP_ANONYMOUS
+  static int dev_zero_fd = -1;	// cached file descriptor for /dev/zero
+  if (dev_zero_fd < 0) dev_zero_fd = open ("/dev/zero", O_RDWR);
+  if (dev_zero_fd < 0) dError (0,"can't open /dev/zero (%s)",strerror(errno));
+  base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE, MAP_PRIVATE,
+		       dev_zero_fd,0);
+#else
+  base = (char*) mmap (0,max_size, PROT_READ | PROT_WRITE,
+		       MAP_PRIVATE | MAP_ANON,0,0);
+#endif
+
+  if (int(base) == -1) dError (0,"Stack::init(), mmap() failed, "
+    "max_size=%d (%s)",max_size,strerror(errno));
+  size = max_size;
+  pointer = base;
+  frame = 0;
+}
+
+
+void dStack::destroy()
+{
+  munmap (base,size);
+  base = 0;
+  size = 0;
+  pointer = 0;
+  frame = 0;
+}
+
+#endif
+
+//****************************************************************************
+
+#ifdef WIN32
+
+#include "windows.h"
+
+
+void dStack::init (int max_size)
+{
+  if (sizeof(LPVOID) != sizeof(char*)) dDebug (0,"internal");
+  if (max_size <= 0) dDebug (0,"Stack::init() given size <= 0");
+  base = (char*) VirtualAlloc (NULL,max_size,MEM_RESERVE,PAGE_READWRITE);
+  if (base == 0) dError (0,"Stack::init(), VirtualAlloc() failed, "
+    "max_size=%d",max_size);
+  size = max_size;
+  pointer = base;
+  frame = 0;
+  committed = 0;
+
+  // get page size
+  SYSTEM_INFO info;
+  GetSystemInfo (&info);
+  pagesize = info.dwPageSize;
+}
+
+
+void dStack::destroy()
+{
+  VirtualFree (base,0,MEM_RELEASE);
+  base = 0;
+  size = 0;
+  pointer = 0;
+  frame = 0;
+}
+
+#endif

src/source/ode/stack.h

@@ -0,0 +1,138 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* this comes from the `reuse' library. copy any changes back to the source.
+
+these stack allocation functions are a replacement for alloca(), except that
+they allocate memory from a separate pool.
+
+advantages over alloca():
+  - consecutive allocations are guaranteed to be contiguous with increasing
+    address.
+  - functions can allocate stack memory that is returned to the caller,
+    in other words pushing and popping stack frames is optional.
+
+disadvantages compared to alloca():
+  - less portable
+  - slightly slower, although still orders of magnitude faster than malloc().
+  - longjmp() and exceptions do not deallocate stack memory (but who cares?).
+
+just like alloca():
+  - using too much stack memory does not fail gracefully, it fails with a
+    segfault.
+
+*/
+
+
+#ifndef _ODE_STACK_H_
+#define _ODE_STACK_H_
+
+
+#ifdef WIN32
+#include "windows.h"
+#endif
+
+
+struct dStack {
+  char *base;		// bottom of the stack
+  int size;		// maximum size of the stack
+  char *pointer;	// current top of the stack
+  char *frame;		// linked list of stack frame ptrs
+# ifdef WIN32		// stuff for windows:
+  int pagesize;		//   - page size - this is ASSUMED to be a power of 2
+  int committed;	//   - bytes committed in allocated region
+#endif
+
+  // initialize the stack. `max_size' is the maximum size that the stack can
+  // reach. on unix and windows a `virtual' memory block of this size is
+  // mapped into the address space but does not actually consume physical
+  // memory until it is referenced - so it is safe to set this to a high value.
+
+  void init (int max_size);
+
+
+  // destroy the stack. this unmaps any virtual memory that was allocated.
+
+  void destroy();
+
+
+  // allocate `size' bytes from the stack and return a pointer to the allocated
+  // memory. `size' must be >= 0. the returned pointer will be aligned to the
+  // size of a long int.
+
+  char * alloc (int size)
+  {
+    char *ret = pointer;
+    pointer += ((size-1) | (sizeof(long int)-1) )+1;
+#   ifdef WIN32
+    // for windows we need to commit pages as they are required
+    if ((pointer-base) > committed) {
+      committed = ((pointer-base-1) | (pagesize-1))+1;	// round up to pgsize
+      VirtualAlloc (base,committed,MEM_COMMIT,PAGE_READWRITE);
+    }
+#   endif
+    return ret;
+  }
+
+
+  // return the address that will be returned by the next call to alloc()
+
+  char *nextAlloc()
+  {
+    return pointer;
+  }
+
+
+  // push and pop the current size of the stack. pushFrame() saves the current
+  // frame pointer on the stack, and popFrame() retrieves it. a typical
+  // stack-using function will bracket alloc() calls with pushFrame() and
+  // popFrame(). both functions return the current stack pointer - this should
+  // be the same value for the two bracketing calls. calling popFrame() too
+  // many times will result in a segfault.
+
+  char * pushFrame()
+  {
+    char *newframe = pointer;
+    char **addr = (char**) alloc (sizeof(char*));
+    *addr = frame;
+    frame = newframe;
+    return newframe;
+
+    /* OLD CODE
+	*((char**)pointer) = frame;
+	frame = pointer;
+	char *ret = pointer;
+	pointer += sizeof(char*);
+	return ret;
+    */
+  }
+
+  char * popFrame()
+  {
+    pointer = frame;
+    frame = *((char**)pointer);
+    return pointer;
+  }
+};
+
+
+#endif

src/source/ode/step.cpp

@@ -0,0 +1,999 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include "objects.h"
+#include "joint.h"
+#include <ode/config.h>
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/timer.h>
+#include <ode/error.h>
+#include <ode/matrix.h>
+#include "lcp.h"
+#include "util.h"
+
+//****************************************************************************
+// misc defines
+
+#define FAST_FACTOR
+//#define TIMING
+
+#define ALLOCA dALLOCA16
+
+//****************************************************************************
+// debugging - comparison of various vectors and matrices produced by the
+// slow and fast versions of the stepper.
+
+//#define COMPARE_METHODS
+
+#ifdef COMPARE_METHODS
+#include "testing.h"
+dMatrixComparison comparator;
+#endif
+
+//****************************************************************************
+// special matrix multipliers
+
+// this assumes the 4th and 8th rows of B and C are zero.
+
+static void Multiply2_p8r (dReal *A, dReal *B, dReal *C,
+			   int p, int r, int Askip)
+{
+  int i,j;
+  dReal sum,*bb,*cc;
+  dIASSERT (p>0 && r>0 && A && B && C);
+  bb = B;
+  for (i=p; i; i--) {
+    cc = C;
+    for (j=r; j; j--) {
+      sum = bb[0]*cc[0];
+      sum += bb[1]*cc[1];
+      sum += bb[2]*cc[2];
+      sum += bb[4]*cc[4];
+      sum += bb[5]*cc[5];
+      sum += bb[6]*cc[6];
+      *(A++) = sum; 
+      cc += 8;
+    }
+    A += Askip - r;
+    bb += 8;
+  }
+}
+
+
+// this assumes the 4th and 8th rows of B and C are zero.
+
+static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C,
+			      int p, int r, int Askip)
+{
+  int i,j;
+  dReal sum,*bb,*cc;
+  dIASSERT (p>0 && r>0 && A && B && C);
+  bb = B;
+  for (i=p; i; i--) {
+    cc = C;
+    for (j=r; j; j--) {
+      sum = bb[0]*cc[0];
+      sum += bb[1]*cc[1];
+      sum += bb[2]*cc[2];
+      sum += bb[4]*cc[4];
+      sum += bb[5]*cc[5];
+      sum += bb[6]*cc[6];
+      *(A++) += sum; 
+      cc += 8;
+    }
+    A += Askip - r;
+    bb += 8;
+  }
+}
+
+
+// this assumes the 4th and 8th rows of B are zero.
+
+static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p)
+{
+  int i;
+  dIASSERT (p>0 && A && B && C);
+  dReal sum;
+  for (i=p; i; i--) {
+    sum =  B[0]*C[0];
+    sum += B[1]*C[1];
+    sum += B[2]*C[2];
+    sum += B[4]*C[4];
+    sum += B[5]*C[5];
+    sum += B[6]*C[6];
+    *(A++) = sum;
+    B += 8;
+  }
+}
+
+
+// this assumes the 4th and 8th rows of B are zero.
+
+static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p)
+{
+  int i;
+  dIASSERT (p>0 && A && B && C);
+  dReal sum;
+  for (i=p; i; i--) {
+    sum =  B[0]*C[0];
+    sum += B[1]*C[1];
+    sum += B[2]*C[2];
+    sum += B[4]*C[4];
+    sum += B[5]*C[5];
+    sum += B[6]*C[6];
+    *(A++) += sum;
+    B += 8;
+  }
+}
+
+
+// this assumes the 4th and 8th rows of B are zero.
+
+static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q)
+{
+  int k;
+  dReal sum;
+  dIASSERT (q>0 && A && B && C);
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[k*8] * C[k];
+  A[0] += sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
+  A[1] += sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
+  A[2] += sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
+  A[4] += sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
+  A[5] += sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
+  A[6] += sum;
+}
+
+
+// this assumes the 4th and 8th rows of B are zero.
+
+static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)
+{
+  int k;
+  dReal sum;
+  dIASSERT (q>0 && A && B && C);
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[k*8] * C[k];
+  A[0] = sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[1+k*8] * C[k];
+  A[1] = sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[2+k*8] * C[k];
+  A[2] = sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[4+k*8] * C[k];
+  A[4] = sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[5+k*8] * C[k];
+  A[5] = sum;
+  sum = 0;
+  for (k=0; k<q; k++) sum += B[6+k*8] * C[k];
+  A[6] = sum;
+}
+
+//****************************************************************************
+// the slow, but sure way
+// note that this does not do any joint feedback!
+
+// given lists of bodies and joints that form an island, perform a first
+// order timestep.
+//
+// `body' is the body array, `nb' is the size of the array.
+// `_joint' is the body array, `nj' is the size of the array.
+
+void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,
+			     dxJoint * const *_joint, int nj, dReal stepsize)
+{
+  int i,j,k;
+  int n6 = 6*nb;
+
+# ifdef TIMING
+  dTimerStart("preprocessing");
+# endif
+
+  // number all bodies in the body list - set their tag values
+  for (i=0; i<nb; i++) body[i]->tag = i;
+
+  // make a local copy of the joint array, because we might want to modify it.
+  // (the "dxJoint *const*" declaration says we're allowed to modify the joints
+  // but not the joint array, because the caller might need it unchanged).
+  dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
+  memcpy (joint,_joint,nj * sizeof(dxJoint*));
+
+  // for all bodies, compute the inertia tensor and its inverse in the global
+  // frame, and compute the rotational force and add it to the torque
+  // accumulator.
+  // @@@ check computation of rotational force.
+  dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
+  dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
+
+  //dSetZero (I,3*nb*4);
+  //dSetZero (invI,3*nb*4);
+  for (i=0; i<nb; i++) {
+    dReal tmp[12];
+    // compute inertia tensor in global frame
+    dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
+    dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
+    // compute inverse inertia tensor in global frame
+    dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
+    dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
+    // compute rotational force
+    dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
+    dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
+  }
+
+  // add the gravity force to all bodies
+  for (i=0; i<nb; i++) {
+    if ((body[i]->flags & dxBodyNoGravity)==0) {
+      body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
+      body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
+      body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
+    }
+  }
+
+  // get m = total constraint dimension, nub = number of unbounded variables.
+  // create constraint offset array and number-of-rows array for all joints.
+  // the constraints are re-ordered as follows: the purely unbounded
+  // constraints, the mixed unbounded + LCP constraints, and last the purely
+  // LCP constraints.
+  //
+  // joints with m=0 are inactive and are removed from the joints array
+  // entirely, so that the code that follows does not consider them.
+  int m = 0;
+  dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
+  int *ofs = (int*) ALLOCA (nj*sizeof(int));
+  for (i=0, j=0; j<nj; j++) {	// i=dest, j=src
+    joint[j]->vtable->getInfo1 (joint[j],info+i);
+    dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
+	      info[i].nub >= 0 && info[i].nub <= info[i].m);
+    if (info[i].m > 0) {
+      joint[i] = joint[j];
+      i++;
+    }
+  }
+  nj = i;
+
+  // the purely unbounded constraints
+  for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
+    ofs[i] = m;
+    m += info[i].m;
+  }
+  int nub = m;
+  // the mixed unbounded + LCP constraints
+  for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
+    ofs[i] = m;
+    m += info[i].m;
+  }
+  // the purely LCP constraints
+  for (i=0; i<nj; i++) if (info[i].nub == 0) {
+    ofs[i] = m;
+    m += info[i].m;
+  }
+
+  // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass
+  // parameters
+# ifdef TIMING
+  dTimerNow ("create mass matrix");
+# endif
+  int nskip = dPAD (n6);
+  dReal *invM = (dReal*) ALLOCA (n6*nskip*sizeof(dReal));
+  dSetZero (invM,n6*nskip);
+  for (i=0; i<nb; i++) {
+    dReal *MM = invM+(i*6)*nskip+(i*6);
+    MM[0] = body[i]->invMass;
+    MM[nskip+1] = body[i]->invMass;
+    MM[2*nskip+2] = body[i]->invMass;
+    MM += 3*nskip+3;
+    for (j=0; j<3; j++) for (k=0; k<3; k++) {
+      MM[j*nskip+k] = invI[i*12+j*4+k];
+    }
+  }
+
+  // assemble some body vectors: fe = external forces, v = velocities
+  dReal *fe = (dReal*) ALLOCA (n6 * sizeof(dReal));
+  dReal *v = (dReal*) ALLOCA (n6 * sizeof(dReal));
+  //dSetZero (fe,n6);
+  //dSetZero (v,n6);
+  for (i=0; i<nb; i++) {
+    for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];
+    for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];
+    for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];
+    for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];
+  }
+
+  // this will be set to the velocity update
+  dReal *vnew = (dReal*) ALLOCA (n6 * sizeof(dReal));
+  dSetZero (vnew,n6);
+
+  // if there are constraints, compute cforce
+  if (m > 0) {
+    // create a constraint equation right hand side vector `c', a constraint
+    // force mixing vector `cfm', and LCP low and high bound vectors, and an
+    // 'findex' vector.
+    dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
+    dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
+    dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
+    dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
+    int *findex = (int*) alloca (m*sizeof(int));
+    dSetZero (c,m);
+    dSetValue (cfm,m,world->global_cfm);
+    dSetValue (lo,m,-dInfinity);
+    dSetValue (hi,m, dInfinity);
+    for (i=0; i<m; i++) findex[i] = -1;
+
+    // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint
+    // data. also fill the c vector.
+#   ifdef TIMING
+    dTimerNow ("create J");
+#   endif
+    dReal *J = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
+    dSetZero (J,m*nskip);
+    dxJoint::Info2 Jinfo;
+    Jinfo.rowskip = nskip;
+    Jinfo.fps = dRecip(stepsize);
+    Jinfo.erp = world->global_erp;
+    for (i=0; i<nj; i++) {
+      Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
+      Jinfo.J1a = Jinfo.J1l + 3;
+      if (joint[i]->node[1].body) {
+	Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
+	Jinfo.J2a = Jinfo.J2l + 3;
+      }
+      else {
+	Jinfo.J2l = 0;
+	Jinfo.J2a = 0;
+      }
+      Jinfo.c = c + ofs[i];
+      Jinfo.cfm = cfm + ofs[i];
+      Jinfo.lo = lo + ofs[i];
+      Jinfo.hi = hi + ofs[i];
+      Jinfo.findex = findex + ofs[i];
+      joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
+      // adjust returned findex values for global index numbering
+      for (j=0; j<info[i].m; j++) {
+	if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
+      }
+    }
+
+    // compute A = J*invM*J'
+#   ifdef TIMING
+    dTimerNow ("compute A");
+#   endif
+    dReal *JinvM = (dReal*) ALLOCA (m*nskip*sizeof(dReal));
+    //dSetZero (JinvM,m*nskip);
+    dMultiply0 (JinvM,J,invM,m,n6,n6);
+    int mskip = dPAD(m);
+    dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
+    //dSetZero (A,m*mskip);
+    dMultiply2 (A,JinvM,J,m,n6,m);
+
+    // add cfm to the diagonal of A
+    for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps;
+
+#   ifdef COMPARE_METHODS
+    comparator.nextMatrix (A,m,m,1,"A");
+#   endif
+
+    // compute `rhs', the right hand side of the equation J*a=c
+#   ifdef TIMING
+    dTimerNow ("compute rhs");
+#   endif
+    dReal *tmp1 = (dReal*) ALLOCA (n6 * sizeof(dReal));
+    //dSetZero (tmp1,n6);
+    dMultiply0 (tmp1,invM,fe,n6,n6,1);
+    for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize;
+    dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
+    //dSetZero (rhs,m);
+    dMultiply0 (rhs,J,tmp1,m,n6,1);
+    for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i];
+
+#   ifdef COMPARE_METHODS
+    comparator.nextMatrix (c,m,1,0,"c");
+    comparator.nextMatrix (rhs,m,1,0,"rhs");
+#   endif
+
+    // solve the LCP problem and get lambda.
+    // this will destroy A but that's okay
+#   ifdef TIMING
+    dTimerNow ("solving LCP problem");
+#   endif
+    dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
+    dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
+    dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
+
+//  OLD WAY - direct factor and solve
+//
+//    // factorize A (L*L'=A)
+//#   ifdef TIMING
+//    dTimerNow ("factorize A");
+//#   endif
+//    dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
+//    memcpy (L,A,m*mskip*sizeof(dReal));
+//    if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
+//
+//    // compute lambda
+//#   ifdef TIMING
+//    dTimerNow ("compute lambda");
+//#   endif
+//    dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
+//    memcpy (lambda,rhs,m * sizeof(dReal));
+//    dSolveCholesky (L,lambda,m);
+
+#   ifdef COMPARE_METHODS
+    comparator.nextMatrix (lambda,m,1,0,"lambda");
+#   endif
+
+    // compute the velocity update `vnew'
+#   ifdef TIMING
+    dTimerNow ("compute velocity update");
+#   endif
+    dMultiply1 (tmp1,J,lambda,n6,m,1);
+    for (i=0; i<n6; i++) tmp1[i] += fe[i];
+    dMultiply0 (vnew,invM,tmp1,n6,n6,1);
+    for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
+
+    // see if the constraint has worked: compute J*vnew and make sure it equals
+    // `c' (to within a certain tolerance).
+#   ifdef TIMING
+    dTimerNow ("verify constraint equation");
+#   endif
+    dMultiply0 (tmp1,J,vnew,m,n6,1);
+    dReal err = 0;
+    for (i=0; i<m; i++) err += dFabs(tmp1[i]-c[i]);
+    printf ("%.6e\n",err);
+  }
+  else {
+    // no constraints
+    dMultiply0 (vnew,invM,fe,n6,n6,1);
+    for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
+  }
+
+# ifdef COMPARE_METHODS
+  comparator.nextMatrix (vnew,n6,1,0,"vnew");
+# endif
+
+  // apply the velocity update to the bodies
+# ifdef TIMING
+  dTimerNow ("update velocity");
+# endif
+  for (i=0; i<nb; i++) {
+    for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j];
+    for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j];
+  }
+
+  // update the position and orientation from the new linear/angular velocity
+  // (over the given timestep)
+# ifdef TIMING
+  dTimerNow ("update position");
+# endif
+  for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
+
+# ifdef TIMING
+  dTimerNow ("tidy up");
+# endif
+
+  // zero all force accumulators
+  for (i=0; i<nb; i++) {
+    body[i]->facc[0] = 0;
+    body[i]->facc[1] = 0;
+    body[i]->facc[2] = 0;
+    body[i]->facc[3] = 0;
+    body[i]->tacc[0] = 0;
+    body[i]->tacc[1] = 0;
+    body[i]->tacc[2] = 0;
+    body[i]->tacc[3] = 0;
+  }
+
+# ifdef TIMING
+  dTimerEnd();
+  if (m > 0) dTimerReport (stdout,1);
+# endif
+}
+
+//****************************************************************************
+// an optimized version of dInternalStepIsland1()
+
+void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb,
+			     dxJoint * const *_joint, int nj, dReal stepsize)
+{
+  int i,j,k;
+# ifdef TIMING
+  dTimerStart("preprocessing");
+# endif
+
+  dReal stepsize1 = dRecip(stepsize);
+
+  // number all bodies in the body list - set their tag values
+  for (i=0; i<nb; i++) body[i]->tag = i;
+
+  // make a local copy of the joint array, because we might want to modify it.
+  // (the "dxJoint *const*" declaration says we're allowed to modify the joints
+  // but not the joint array, because the caller might need it unchanged).
+  dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
+  memcpy (joint,_joint,nj * sizeof(dxJoint*));
+
+  // for all bodies, compute the inertia tensor and its inverse in the global
+  // frame, and compute the rotational force and add it to the torque
+  // accumulator. I and invI are vertically stacked 3x4 matrices, one per body.
+  // @@@ check computation of rotational force.
+  dReal *I = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
+  dReal *invI = (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
+
+  //dSetZero (I,3*nb*4);
+  //dSetZero (invI,3*nb*4);
+  for (i=0; i<nb; i++) {
+    dReal tmp[12];
+    // compute inertia tensor in global frame
+    dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
+    dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
+    // compute inverse inertia tensor in global frame
+    dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
+    dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
+    // compute rotational force
+    dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
+    dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
+  }
+
+  // add the gravity force to all bodies
+  for (i=0; i<nb; i++) {
+    if ((body[i]->flags & dxBodyNoGravity)==0) {
+      body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
+      body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
+      body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
+    }
+  }
+
+  // get m = total constraint dimension, nub = number of unbounded variables.
+  // create constraint offset array and number-of-rows array for all joints.
+  // the constraints are re-ordered as follows: the purely unbounded
+  // constraints, the mixed unbounded + LCP constraints, and last the purely
+  // LCP constraints. this assists the LCP solver to put all unbounded
+  // variables at the start for a quick factorization.
+  //
+  // joints with m=0 are inactive and are removed from the joints array
+  // entirely, so that the code that follows does not consider them.
+  // also number all active joints in the joint list (set their tag values).
+  // inactive joints receive a tag value of -1.
+
+  int m = 0;
+  dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
+  int *ofs = (int*) ALLOCA (nj*sizeof(int));
+  for (i=0, j=0; j<nj; j++) {	// i=dest, j=src
+    joint[j]->vtable->getInfo1 (joint[j],info+i);
+    dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
+	      info[i].nub >= 0 && info[i].nub <= info[i].m);
+    if (info[i].m > 0) {
+      joint[i] = joint[j];
+      joint[i]->tag = i;
+      i++;
+    }
+    else {
+      joint[j]->tag = -1;
+    }
+  }
+  nj = i;
+
+  // the purely unbounded constraints
+  for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
+    ofs[i] = m;
+    m += info[i].m;
+  }
+  int nub = m;
+  // the mixed unbounded + LCP constraints
+  for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
+    ofs[i] = m;
+    m += info[i].m;
+  }
+  // the purely LCP constraints
+  for (i=0; i<nj; i++) if (info[i].nub == 0) {
+    ofs[i] = m;
+    m += info[i].m;
+  }
+
+  // this will be set to the force due to the constraints
+  dReal *cforce = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
+  dSetZero (cforce,nb*8);
+
+  // if there are constraints, compute cforce
+  if (m > 0) {
+    // create a constraint equation right hand side vector `c', a constraint
+    // force mixing vector `cfm', and LCP low and high bound vectors, and an
+    // 'findex' vector.
+    dReal *c = (dReal*) ALLOCA (m*sizeof(dReal));
+    dReal *cfm = (dReal*) ALLOCA (m*sizeof(dReal));
+    dReal *lo = (dReal*) ALLOCA (m*sizeof(dReal));
+    dReal *hi = (dReal*) ALLOCA (m*sizeof(dReal));
+    int *findex = (int*) alloca (m*sizeof(int));
+    dSetZero (c,m);
+    dSetValue (cfm,m,world->global_cfm);
+    dSetValue (lo,m,-dInfinity);
+    dSetValue (hi,m, dInfinity);
+    for (i=0; i<m; i++) findex[i] = -1;
+
+    // get jacobian data from constraints. a (2*m)x8 matrix will be created
+    // to store the two jacobian blocks from each constraint. it has this
+    // format:
+    //
+    //   l l l 0 a a a 0  \    .
+    //   l l l 0 a a a 0   }-- jacobian body 1 block for joint 0 (3 rows)
+    //   l l l 0 a a a 0  /
+    //   l l l 0 a a a 0  \    .
+    //   l l l 0 a a a 0   }-- jacobian body 2 block for joint 0 (3 rows)
+    //   l l l 0 a a a 0  /
+    //   l l l 0 a a a 0  }--- jacobian body 1 block for joint 1 (1 row)
+    //   l l l 0 a a a 0  }--- jacobian body 2 block for joint 1 (1 row)
+    //   etc...
+    //
+    //   (lll) = linear jacobian data
+    //   (aaa) = angular jacobian data
+    //
+#   ifdef TIMING
+    dTimerNow ("create J");
+#   endif
+    dReal *J = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
+    dSetZero (J,2*m*8);
+    dxJoint::Info2 Jinfo;
+    Jinfo.rowskip = 8;
+    Jinfo.fps = stepsize1;
+    Jinfo.erp = world->global_erp;
+    for (i=0; i<nj; i++) {
+      Jinfo.J1l = J + 2*8*ofs[i];
+      Jinfo.J1a = Jinfo.J1l + 4;
+      Jinfo.J2l = Jinfo.J1l + 8*info[i].m;
+      Jinfo.J2a = Jinfo.J2l + 4;
+      Jinfo.c = c + ofs[i];
+      Jinfo.cfm = cfm + ofs[i];
+      Jinfo.lo = lo + ofs[i];
+      Jinfo.hi = hi + ofs[i];
+      Jinfo.findex = findex + ofs[i];
+      joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
+      // adjust returned findex values for global index numbering
+      for (j=0; j<info[i].m; j++) {
+	if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
+      }
+    }
+
+    // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same
+    // format as J so we just go through the constraints in J multiplying by
+    // the appropriate scalars and matrices.
+#   ifdef TIMING
+    dTimerNow ("compute A");
+#   endif
+    dReal *JinvM = (dReal*) ALLOCA (2*m*8*sizeof(dReal));
+    dSetZero (JinvM,2*m*8);
+    for (i=0; i<nj; i++) {
+      int b = joint[i]->node[0].body->tag;
+      dReal body_invMass = body[b]->invMass;
+      dReal *body_invI = invI + b*12;
+      dReal *Jsrc = J + 2*8*ofs[i];
+      dReal *Jdst = JinvM + 2*8*ofs[i];
+      for (j=info[i].m-1; j>=0; j--) {
+	for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
+	dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
+	Jsrc += 8;
+	Jdst += 8;
+      }
+      if (joint[i]->node[1].body) {
+	b = joint[i]->node[1].body->tag;
+	body_invMass = body[b]->invMass;
+	body_invI = invI + b*12;
+	for (j=info[i].m-1; j>=0; j--) {
+	  for (k=0; k<3; k++) Jdst[k] = Jsrc[k] * body_invMass;
+	  dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
+	  Jsrc += 8;
+	  Jdst += 8;
+	}
+      }
+    }
+
+    // now compute A = JinvM * J'. A's rows and columns are grouped by joint,
+    // i.e. in the same way as the rows of J. block (i,j) of A is only nonzero
+    // if joints i and j have at least one body in common. this fact suggests
+    // the algorithm used to fill A:
+    //
+    //    for b = all bodies
+    //      n = number of joints attached to body b
+    //      for i = 1..n
+    //        for j = i+1..n
+    //          ii = actual joint number for i
+    //          jj = actual joint number for j
+    //          // (ii,jj) will be set to all pairs of joints around body b
+    //          compute blockwise: A(ii,jj) += JinvM(ii) * J(jj)'
+    //
+    // this algorithm catches all pairs of joints that have at least one body
+    // in common. it does not compute the diagonal blocks of A however -
+    // another similar algorithm does that.
+
+    int mskip = dPAD(m);
+    dReal *A = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
+    dSetZero (A,m*mskip);
+    for (i=0; i<nb; i++) {
+      for (dxJointNode *n1=body[i]->firstjoint; n1; n1=n1->next) {
+	for (dxJointNode *n2=n1->next; n2; n2=n2->next) {
+	  // get joint numbers and ensure ofs[j1] >= ofs[j2]
+	  int j1 = n1->joint->tag;
+	  int j2 = n2->joint->tag;
+	  if (ofs[j1] < ofs[j2]) {
+	    int tmp = j1;
+	    j1 = j2;
+	    j2 = tmp;
+	  }
+
+	  // if either joint was tagged as -1 then it is an inactive (m=0)
+	  // joint that should not be considered
+	  if (j1==-1 || j2==-1) continue;
+
+	  // determine if body i is the 1st or 2nd body of joints j1 and j2
+	  int jb1 = (joint[j1]->node[1].body == body[i]);
+	  int jb2 = (joint[j2]->node[1].body == body[i]);
+	  // jb1/jb2 must be 0 for joints with only one body
+	  dIASSERT(joint[j1]->node[1].body || jb1==0);
+	  dIASSERT(joint[j2]->node[1].body || jb2==0);
+
+	  // set block of A
+	  MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2],
+			    JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m,
+			    J     + 2*8*ofs[j2] + jb2*8*info[j2].m,
+			    info[j1].m,info[j2].m, mskip);
+	}
+      }
+    }
+    // compute diagonal blocks of A
+    for (i=0; i<nj; i++) {
+      Multiply2_p8r (A + ofs[i]*(mskip+1),
+		     JinvM + 2*8*ofs[i],
+		     J + 2*8*ofs[i],
+		     info[i].m,info[i].m, mskip);
+      if (joint[i]->node[1].body) {
+	MultiplyAdd2_p8r (A + ofs[i]*(mskip+1),
+			  JinvM + 2*8*ofs[i] + 8*info[i].m,
+			  J + 2*8*ofs[i] + 8*info[i].m,
+			  info[i].m,info[i].m, mskip);
+      }
+    }
+
+    // add cfm to the diagonal of A
+    for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * stepsize1;
+
+#   ifdef COMPARE_METHODS
+    comparator.nextMatrix (A,m,m,1,"A");
+#   endif
+
+    // compute the right hand side `rhs'
+#   ifdef TIMING
+    dTimerNow ("compute rhs");
+#   endif
+    dReal *tmp1 = (dReal*) ALLOCA (nb*8 * sizeof(dReal));
+    //dSetZero (tmp1,nb*8);
+    // put v/h + invM*fe into tmp1
+    for (i=0; i<nb; i++) {
+      dReal body_invMass = body[i]->invMass;
+      dReal *body_invI = invI + i*12;
+      for (j=0; j<3; j++) tmp1[i*8+j] = body[i]->facc[j] * body_invMass +
+			    body[i]->lvel[j] * stepsize1;
+      dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc);
+      for (j=0; j<3; j++) tmp1[i*8+4+j] += body[i]->avel[j] * stepsize1;
+    }
+    // put J*tmp1 into rhs
+    dReal *rhs = (dReal*) ALLOCA (m * sizeof(dReal));
+    //dSetZero (rhs,m);
+    for (i=0; i<nj; i++) {
+      dReal *JJ = J + 2*8*ofs[i];
+      Multiply0_p81 (rhs+ofs[i],JJ,
+		     tmp1 + 8*joint[i]->node[0].body->tag, info[i].m);
+      if (joint[i]->node[1].body) {
+	MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m,
+			  tmp1 + 8*joint[i]->node[1].body->tag, info[i].m);
+      }
+    }
+    // complete rhs
+    for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
+
+#   ifdef COMPARE_METHODS
+    comparator.nextMatrix (c,m,1,0,"c");
+    comparator.nextMatrix (rhs,m,1,0,"rhs");
+#   endif
+
+    // solve the LCP problem and get lambda.
+    // this will destroy A but that's okay
+#   ifdef TIMING
+    dTimerNow ("solving LCP problem");
+#   endif
+    dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
+    dReal *residual = (dReal*) ALLOCA (m * sizeof(dReal));
+    dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
+
+//  OLD WAY - direct factor and solve
+//
+//    // factorize A (L*L'=A)
+//#   ifdef TIMING
+//    dTimerNow ("factorize A");
+//#   endif
+//    dReal *L = (dReal*) ALLOCA (m*mskip*sizeof(dReal));
+//    memcpy (L,A,m*mskip*sizeof(dReal));
+//#   ifdef FAST_FACTOR
+//    dFastFactorCholesky (L,m);  // does not report non positive definiteness
+//#   else
+//    if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
+//#   endif
+//
+//    // compute lambda
+//#   ifdef TIMING
+//    dTimerNow ("compute lambda");
+//#   endif
+//    dReal *lambda = (dReal*) ALLOCA (m * sizeof(dReal));
+//    memcpy (lambda,rhs,m * sizeof(dReal));
+//    dSolveCholesky (L,lambda,m);
+
+#   ifdef COMPARE_METHODS
+    comparator.nextMatrix (lambda,m,1,0,"lambda");
+#   endif
+
+    // compute the constraint force `cforce'
+#   ifdef TIMING
+    dTimerNow ("compute constraint force");
+#   endif
+    // compute cforce = J'*lambda
+    for (i=0; i<nj; i++) {
+      dReal *JJ = J + 2*8*ofs[i];
+      dxBody* b1 = joint[i]->node[0].body;
+      dxBody* b2 = joint[i]->node[1].body;
+      dJointFeedback *fb = joint[i]->feedback;
+
+      if (fb) {
+	// the user has requested feedback on the amount of force that this
+	// joint is applying to the bodies. we use a slightly slower
+	// computation that splits out the force components and puts them
+	// in the feedback structure.
+	dReal data1[8],data2[8];
+	Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
+	dReal *cf1 = cforce + 8*b1->tag;
+	cf1[0] += (fb->f1[0] = data1[0]);
+	cf1[1] += (fb->f1[1] = data1[1]);
+	cf1[2] += (fb->f1[2] = data1[2]);
+	cf1[4] += (fb->t1[0] = data1[4]);
+	cf1[5] += (fb->t1[1] = data1[5]);
+	cf1[6] += (fb->t1[2] = data1[6]);
+	if (b2){
+	  Multiply1_8q1 (data2, JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
+	  dReal *cf2 = cforce + 8*b2->tag;
+	  cf2[0] += (fb->f2[0] = data2[0]);
+	  cf2[1] += (fb->f2[1] = data2[1]);
+	  cf2[2] += (fb->f2[2] = data2[2]);
+	  cf2[4] += (fb->t2[0] = data2[4]);
+	  cf2[5] += (fb->t2[1] = data2[5]);
+	  cf2[6] += (fb->t2[2] = data2[6]);
+	}
+      }
+      else {
+	// no feedback is required, let's compute cforce the faster way
+	MultiplyAdd1_8q1 (cforce + 8*b1->tag,JJ, lambda+ofs[i], info[i].m);
+	if (b2) {
+	  MultiplyAdd1_8q1 (cforce + 8*b2->tag,
+			    JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
+	}
+      }
+    }
+  }
+
+  // compute the velocity update
+# ifdef TIMING
+  dTimerNow ("compute velocity update");
+# endif
+
+  // add fe to cforce
+  for (i=0; i<nb; i++) {
+    for (j=0; j<3; j++) cforce[i*8+j] += body[i]->facc[j];
+    for (j=0; j<3; j++) cforce[i*8+4+j] += body[i]->tacc[j];
+  }
+  // multiply cforce by stepsize
+  for (i=0; i < nb*8; i++) cforce[i] *= stepsize;
+  // add invM * cforce to the body velocity
+  for (i=0; i<nb; i++) {
+    dReal body_invMass = body[i]->invMass;
+    dReal *body_invI = invI + i*12;
+    for (j=0; j<3; j++) body[i]->lvel[j] += body_invMass * cforce[i*8+j];
+    dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4);
+  }
+
+  // update the position and orientation from the new linear/angular velocity
+  // (over the given timestep)
+# ifdef TIMING
+  dTimerNow ("update position");
+# endif
+  for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
+
+# ifdef COMPARE_METHODS
+  dReal *tmp_vnew = (dReal*) ALLOCA (nb*6*sizeof(dReal));
+  for (i=0; i<nb; i++) {
+    for (j=0; j<3; j++) tmp_vnew[i*6+j] = body[i]->lvel[j];
+    for (j=0; j<3; j++) tmp_vnew[i*6+3+j] = body[i]->avel[j];
+  }
+  comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
+# endif
+
+# ifdef TIMING
+  dTimerNow ("tidy up");
+# endif
+
+  // zero all force accumulators
+  for (i=0; i<nb; i++) {
+    body[i]->facc[0] = 0;
+    body[i]->facc[1] = 0;
+    body[i]->facc[2] = 0;
+    body[i]->facc[3] = 0;
+    body[i]->tacc[0] = 0;
+    body[i]->tacc[1] = 0;
+    body[i]->tacc[2] = 0;
+    body[i]->tacc[3] = 0;
+  }
+
+# ifdef TIMING
+  dTimerEnd();
+  if (m > 0) dTimerReport (stdout,1);
+# endif
+}
+
+//****************************************************************************
+
+void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb,
+			  dxJoint * const *joint, int nj, dReal stepsize)
+{
+# ifndef COMPARE_METHODS
+  dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
+# endif
+
+# ifdef COMPARE_METHODS
+  int i;
+
+  // save body state
+  dxBody *state = (dxBody*) ALLOCA (nb*sizeof(dxBody));
+  for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
+
+  // take slow step
+  comparator.reset();
+  dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize);
+  comparator.end();
+
+  // restore state
+  for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
+
+  // take fast step
+  dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
+  comparator.end();
+
+  //comparator.dump();
+  //_exit (1);
+# endif
+}

src/source/ode/step.h

@@ -0,0 +1,36 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_STEP_H_
+#define _ODE_STEP_H_
+
+#include <ode/common.h>
+
+
+void dInternalStepIsland (dxWorld *world,
+			  dxBody * const *body, int nb,
+			  dxJoint * const *joint, int nj,
+			  dReal stepsize);
+
+
+
+#endif

src/source/ode/testing.cpp

@@ -0,0 +1,243 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/config.h>
+#include <ode/misc.h>
+#include <ode/memory.h>
+#include "testing.h"
+
+#ifdef dDOUBLE
+static const dReal tol = 1.0e-9;
+#else
+static const dReal tol = 1.0e-5f;
+#endif
+
+
+// matrix header on the stack
+
+struct dMatrixComparison::dMatInfo {
+  int n,m;		// size of matrix
+  char name[128];	// name of the matrix
+  dReal *data;		// matrix data
+  int size;		// size of `data'
+};
+
+
+
+dMatrixComparison::dMatrixComparison()
+{
+  afterfirst = 0;
+  index = 0;
+}
+
+
+dMatrixComparison::~dMatrixComparison()
+{
+  reset();
+}
+
+
+dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri,
+				     char *name, ...)
+{
+  if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix");
+  int num = n*dPAD(m);
+
+  if (afterfirst==0) {
+    dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo));
+    mi->n = n;
+    mi->m = m;
+    mi->size = num * sizeof(dReal);
+    mi->data = (dReal*) dAlloc (mi->size);
+    memcpy (mi->data,A,mi->size);
+
+    va_list ap;
+    va_start (ap,name);
+    vsprintf (mi->name,name,ap);
+    if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long");
+
+    mat.push (mi);
+    return 0;
+  }
+  else {
+    if (lower_tri && n != m)
+      dDebug (0,"dMatrixComparison, lower triangular matrix must be square");
+    if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices");
+    dMatInfo *mp = mat[index];
+    index++;
+
+    dMatInfo mi;
+    va_list ap;
+    va_start (ap,name);
+    vsprintf (mi.name,name,ap);
+    if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long");
+
+    if (strcmp(mp->name,mi.name) != 0)
+      dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")",
+	      mp->name,mi.name);
+    if (mp->n != n || mp->m != m)
+      dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)",
+	      mp->n,mp->m,n,m);
+
+    dReal maxdiff;
+    if (lower_tri) {
+      maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n);
+    }
+    else {
+      maxdiff = dMaxDifference (A,mp->data,n,m);
+    }
+    if (maxdiff > tol)
+      dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", "
+	      "error=%.4e)",n,m,mi.name,maxdiff);
+    return maxdiff;
+  }
+}
+
+
+void dMatrixComparison::end()
+{
+  if (mat.size() <= 0) dDebug (0,"no matrices in sequence");
+  afterfirst = 1;
+  index = 0;
+}
+
+
+void dMatrixComparison::reset()
+{
+  for (int i=0; i<mat.size(); i++) {
+    dFree (mat[i]->data,mat[i]->size);
+    dFree (mat[i],sizeof(dMatInfo));
+  }
+  mat.setSize (0);
+  afterfirst = 0;
+  index = 0;
+}
+
+
+void dMatrixComparison::dump()
+{
+  for (int i=0; i<mat.size(); i++)
+    printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m);
+}
+
+//****************************************************************************
+// unit test
+
+#include <setjmp.h>
+
+static jmp_buf jump_buffer;
+
+static void myDebug (int num, const char *msg, va_list ap)
+{
+  // printf ("(Error %d: ",num);
+  // vprintf (msg,ap);
+  // printf (")\n");
+  longjmp (jump_buffer,1);
+}
+
+
+extern "C" void dTestMatrixComparison()
+{
+  volatile int i;
+  printf ("dTestMatrixComparison()\n");
+  dMessageFunction *orig_debug = dGetDebugHandler();
+
+  dMatrixComparison mc;
+  dReal A[50*50];
+
+  // make first sequence
+  unsigned long seed = dRandGetSeed();
+  for (i=1; i<49; i++) {
+    dMakeRandomMatrix (A,i,i+1,1.0);
+    mc.nextMatrix (A,i,i+1,0,"A%d",i);
+  }
+  mc.end();
+
+  //mc.dump();
+
+  // test identical sequence
+  dSetDebugHandler (&myDebug);
+  dRandSetSeed (seed);
+  if (setjmp (jump_buffer)) {
+    printf ("\tFAILED (1)\n");
+  }
+  else {
+    for (i=1; i<49; i++) {
+      dMakeRandomMatrix (A,i,i+1,1.0);
+      mc.nextMatrix (A,i,i+1,0,"A%d",i);
+    }
+    mc.end();
+    printf ("\tpassed (1)\n");
+  }
+  dSetDebugHandler (orig_debug);
+
+  // test broken sequences (with matrix error)
+  dRandSetSeed (seed);
+  volatile int passcount = 0;
+  for (i=1; i<49; i++) {
+    if (setjmp (jump_buffer)) {
+      passcount++;
+    }
+    else {
+      dSetDebugHandler (&myDebug);
+      dMakeRandomMatrix (A,i,i+1,1.0);
+      A[(i-1)*dPAD(i+1)+i] += REAL(0.01);
+      mc.nextMatrix (A,i,i+1,0,"A%d",i);
+      dSetDebugHandler (orig_debug);
+    }
+  }
+  mc.end();
+  printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED");
+
+  // test broken sequences (with name error)
+  dRandSetSeed (seed);
+  passcount = 0;
+  for (i=1; i<49; i++) {
+    if (setjmp (jump_buffer)) {
+      passcount++;
+    }
+    else {
+      dSetDebugHandler (&myDebug);
+      dMakeRandomMatrix (A,i,i+1,1.0);
+      mc.nextMatrix (A,i,i+1,0,"B%d",i);
+      dSetDebugHandler (orig_debug);
+    }
+  }
+  mc.end();
+  printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED");
+
+  // test identical sequence again
+  dSetDebugHandler (&myDebug);
+  dRandSetSeed (seed);
+  if (setjmp (jump_buffer)) {
+    printf ("\tFAILED (4)\n");
+  }
+  else {
+    for (i=1; i<49; i++) {
+      dMakeRandomMatrix (A,i,i+1,1.0);
+      mc.nextMatrix (A,i,i+1,0,"A%d",i);
+    }
+    mc.end();
+    printf ("\tpassed (4)\n");
+  }
+  dSetDebugHandler (orig_debug);
+}

src/source/ode/testing.h

@@ -0,0 +1,65 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/* stuff used for testing */
+
+#ifndef _ODE_TESTING_H_
+#define _ODE_TESTING_H_
+
+#include <ode/common.h>
+#include "array.h"
+
+
+// compare a sequence of named matrices/vectors, i.e. to make sure that two
+// different pieces of code are giving the same results.
+
+class dMatrixComparison {
+  struct dMatInfo;
+  dArray<dMatInfo*> mat;
+  int afterfirst,index;
+
+public:
+  dMatrixComparison();
+  ~dMatrixComparison();
+
+  dReal nextMatrix (dReal *A, int n, int m, int lower_tri, char *name, ...);
+  // add a new n*m matrix A to the sequence. the name of the matrix is given
+  // by the printf-style arguments (name,...). if this is the first sequence
+  // then this object will simply record the matrices and return 0.
+  // if this the second or subsequent sequence then this object will compare
+  // the matrices with the first sequence, and report any differences.
+  // the matrix error will be returned. if `lower_tri' is 1 then only the
+  // lower triangle of the matrix (including the diagonal) will be compared
+  // (the matrix must be square).
+
+  void end();
+  // end a sequence.
+
+  void reset();
+  // restarts the object, so the next sequence will be the first sequence.
+
+  void dump();
+  // print out info about all the matrices in the sequence
+};
+
+
+#endif

src/source/ode/timer.cpp

@@ -0,0 +1,398 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+TODO
+----
+
+* gettimeofday() and the pentium time stamp counter return the real time,
+  not the process time. fix this somehow!
+
+*/
+
+#include <ode/common.h>
+#include <ode/timer.h>
+
+// misc defines
+#define ALLOCA dALLOCA16
+
+//****************************************************************************
+// implementation for windows based on the multimedia performance counter.
+
+#ifdef WIN32
+
+#include "windows.h"
+
+static inline void getClockCount (unsigned long cc[2])
+{
+  LARGE_INTEGER a;
+  QueryPerformanceCounter (&a);
+  cc[0] = a.LowPart;
+  cc[1] = a.HighPart;
+}
+
+
+static inline void serialize()
+{
+}
+
+
+static inline double loadClockCount (unsigned long cc[2])
+{
+  LARGE_INTEGER a;
+  a.LowPart = cc[0];
+  a.HighPart = cc[1];
+  return double(a.QuadPart);
+}
+
+
+double dTimerResolution()
+{
+  return 1.0/dTimerTicksPerSecond();
+}
+
+
+double dTimerTicksPerSecond()
+{
+  static int query=0;
+  static double hz=0.0;
+  if (!query) {
+    LARGE_INTEGER a;
+    QueryPerformanceFrequency (&a);
+    hz = double(a.QuadPart);
+    query = 1;
+  }
+  return hz;
+}
+
+#endif
+
+//****************************************************************************
+// implementation based on the pentium time stamp counter. the timer functions
+// can be serializing or non-serializing. serializing will ensure that all
+// instructions have executed and data has been written back before the cpu
+// time stamp counter is read. the CPUID instruction is used to serialize.
+
+#if defined(PENTIUM) && !defined(WIN32)
+
+// we need to know the clock rate so that the timing function can report
+// accurate times. this number only needs to be set accurately if we're
+// doing performance tests and care about real-world time numbers - otherwise,
+// just ignore this. i have not worked out how to determine this number
+// automatically yet.
+
+#define PENTIUM_HZ (500e6)
+
+
+static inline void getClockCount (unsigned long cc[2])
+{
+  asm volatile (
+	"rdtsc\n"
+	"movl %%eax,(%%esi)\n"
+	"movl %%edx,4(%%esi)\n"
+	: : "S" (cc) : "%eax","%edx","cc","memory");
+}
+
+
+static inline void serialize()
+{
+  asm volatile (
+	"mov $0,%%eax\n"
+	"cpuid\n"
+	: : : "%eax","%ebx","%ecx","%edx","cc","memory");
+}
+
+
+static inline double loadClockCount (unsigned long a[2])
+{
+  double ret;
+  asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) :
+		"cc","memory");
+  return ret;
+}
+
+
+double dTimerResolution()
+{
+  return 1.0/PENTIUM_HZ;
+}
+
+
+double dTimerTicksPerSecond()
+{
+  return PENTIUM_HZ;
+}
+
+#endif
+
+//****************************************************************************
+// otherwise, do the implementation based on gettimeofday().
+
+#if !defined(PENTIUM) && !defined(WIN32)
+
+#ifndef macintosh
+
+#include <sys/time.h>
+#include <unistd.h>
+
+
+static inline void getClockCount (unsigned long cc[2])
+{
+  struct timeval tv;
+  gettimeofday (&tv,0);
+  cc[0] = tv.tv_usec;
+  cc[1] = tv.tv_sec;
+}
+
+#else // macintosh
+
+#include <MacTypes.h>
+#include <Timer.h>
+
+static inline void getClockCount (unsigned long cc[2])
+{
+  UnsignedWide ms;
+  Microseconds (&ms);
+  cc[1] = ms.lo / 1000000;
+  cc[0] = ms.lo - ( cc[1] * 1000000 );
+}
+
+#endif
+
+
+static inline void serialize()
+{
+}
+
+
+static inline double loadClockCount (unsigned long a[2])
+{
+  return a[1]*1.0e6 + a[0];
+}
+
+
+double dTimerResolution()
+{
+  unsigned long cc1[2],cc2[2];
+  getClockCount (cc1);
+  do {
+    getClockCount (cc2);
+  }
+  while (cc1[0]==cc2[0] && cc1[1]==cc2[1]);
+  do {
+    getClockCount (cc1);
+  }
+  while (cc1[0]==cc2[0] && cc1[1]==cc2[1]);
+  double t1 = loadClockCount (cc1);
+  double t2 = loadClockCount (cc2);
+  return (t1-t2) / dTimerTicksPerSecond();
+}
+
+
+double dTimerTicksPerSecond()
+{
+  return 1000000;
+}
+
+#endif
+
+//****************************************************************************
+// stop watches
+
+void dStopwatchReset (dStopwatch *s)
+{
+  s->time = 0;
+  s->cc[0] = 0;
+  s->cc[1] = 0;
+}
+
+
+void dStopwatchStart (dStopwatch *s)
+{
+  serialize();
+  getClockCount (s->cc);
+}
+
+
+void dStopwatchStop  (dStopwatch *s)
+{
+  unsigned long cc[2];
+  serialize();
+  getClockCount (cc);
+  double t1 = loadClockCount (s->cc);
+  double t2 = loadClockCount (cc);
+  s->time += t2-t1;
+}
+
+
+double dStopwatchTime (dStopwatch *s)
+{
+  return s->time / dTimerTicksPerSecond();
+}
+
+//****************************************************************************
+// code timers
+
+// maximum number of events to record
+#define MAXNUM 100
+
+static int num = 0;		// number of entries used in event array
+static struct {
+  unsigned long cc[2];		// clock counts
+  double total_t;		// total clocks used in this slot.
+  double total_p;		// total percentage points used in this slot.
+  int count;			// number of times this slot has been updated.
+  char *description;		// pointer to static string
+} event[MAXNUM];
+
+
+// make sure all slot totals and counts reset to 0 at start
+
+static void initSlots()
+{
+  static int initialized=0;
+  if (!initialized) {
+    for (int i=0; i<MAXNUM; i++) {
+      event[i].count = 0;
+      event[i].total_t = 0;
+      event[i].total_p = 0;
+    }
+    initialized = 1;
+  }
+}
+
+
+void dTimerStart (const char *description)
+{
+  initSlots();
+  event[0].description = const_cast<char*> (description);
+  num = 1;
+  serialize();
+  getClockCount (event[0].cc);
+}
+
+
+void dTimerNow (const char *description)
+{
+  if (num < MAXNUM) {
+    // do not serialize
+    getClockCount (event[num].cc);
+    event[num].description = const_cast<char*> (description);
+    num++;
+  }
+}
+
+
+void dTimerEnd()
+{
+  if (num < MAXNUM) {
+    serialize();
+    getClockCount (event[num].cc);
+    event[num].description = "TOTAL";
+    num++;
+  }
+}
+
+//****************************************************************************
+// print report
+
+static void fprintDoubleWithPrefix (FILE *f, double a, char *fmt)
+{
+  if (a >= 0.999999) {
+    fprintf (f,fmt,a);
+    return;
+  }
+  a *= 1000.0;
+  if (a >= 0.999999) {
+    fprintf (f,fmt,a);
+    fprintf (f,"m");
+    return;
+  }
+  a *= 1000.0;
+  if (a >= 0.999999) {
+    fprintf (f,fmt,a);
+    fprintf (f,"u");
+    return;
+  }
+  a *= 1000.0;
+  fprintf (f,fmt,a);
+  fprintf (f,"n");
+}
+
+
+void dTimerReport (FILE *fout, int average)
+{
+  int i;
+  size_t maxl;
+  double ccunit = 1.0/dTimerTicksPerSecond();
+  fprintf (fout,"\nTimer Report (");
+  fprintDoubleWithPrefix (fout,ccunit,"%.2f ");
+  fprintf (fout,"s resolution)\n------------\n");
+  if (num < 1) return;
+
+  // get maximum description length
+  maxl = 0;
+  for (i=0; i<num; i++) {
+    size_t l = strlen (event[i].description);
+    if (l > maxl) maxl = l;
+  }
+
+  // calculate total time
+  double t1 = loadClockCount (event[0].cc);
+  double t2 = loadClockCount (event[num-1].cc);
+  double total = t2 - t1;
+  if (total <= 0) total = 1;
+
+  // compute time difference for all slots except the last one. update totals
+  double *times = (double*) ALLOCA (num * sizeof(double));
+  for (i=0; i < (num-1); i++) {
+    double t1 = loadClockCount (event[i].cc);
+    double t2 = loadClockCount (event[i+1].cc);
+    times[i] = t2 - t1;
+    event[i].count++;
+    event[i].total_t += times[i];
+    event[i].total_p += times[i]/total * 100.0;
+  }
+
+  // print report (with optional averages)
+  for (i=0; i<num; i++) {
+    double t,p;
+    if (i < (num-1)) {
+      t = times[i];
+      p = t/total * 100.0;
+    }
+    else {
+      t = total;
+      p = 100.0;
+    }
+    fprintf (fout,"%-*s %7.2fms %6.2f%%",maxl,event[i].description,
+	     t*ccunit * 1000.0, p);
+    if (average && i < (num-1)) {
+      fprintf (fout,"  (avg %7.2fms %6.2f%%)",
+	       (event[i].total_t / event[i].count)*ccunit * 1000.0,
+	       event[i].total_p / event[i].count);
+    }
+    fprintf (fout,"\n");
+  }
+  fprintf (fout,"\n");
+}

src/source/ode/util.cpp

@@ -0,0 +1,277 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include "ode/ode.h"
+#include "objects.h"
+#include "joint.h"
+#include "util.h"
+
+#define ALLOCA dALLOCA16
+
+//****************************************************************************
+// Auto disabling
+
+void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize)
+{
+	dxBody *bb;
+	for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
+		// nothing to do unless this body is currently enabled and has
+		// the auto-disable flag set
+		if ((bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable) continue;
+		
+		// see if the body is idle
+		int idle = 1;			// initial assumption
+		dReal lspeed2 = dDOT(bb->lvel,bb->lvel);
+		if (lspeed2 > bb->adis.linear_threshold) {
+			idle = 0;		// moving fast - not idle
+		}
+		else {
+			dReal aspeed = dDOT(bb->avel,bb->avel);
+			if (aspeed > bb->adis.angular_threshold) {
+				idle = 0;	// turning fast - not idle
+			}
+		}
+	
+		// if it's idle, accumulate steps and time.
+		// these counters won't overflow because this code doesn't run for disabled bodies.
+		if (idle) {
+			bb->adis_stepsleft--;
+			bb->adis_timeleft -= stepsize;
+		}
+		else {
+			bb->adis_stepsleft = bb->adis.idle_steps;
+			bb->adis_timeleft = bb->adis.idle_time;
+		}
+
+		// disable the body if it's idle for a long enough time
+		if (bb->adis_stepsleft < 0 && bb->adis_timeleft < 0) {
+			bb->flags |= dxBodyDisabled;
+		}
+	}
+}
+
+
+//****************************************************************************
+// body rotation
+
+// return sin(x)/x. this has a singularity at 0 so special handling is needed
+// for small arguments.
+
+static inline dReal sinc (dReal x)
+{
+  // if |x| < 1e-4 then use a taylor series expansion. this two term expansion
+  // is actually accurate to one LS bit within this range if double precision
+  // is being used - so don't worry!
+  if (dFabs(x) < 1.0e-4) return REAL(1.0) - x*x*REAL(0.166666666666666666667);
+  else return dSin(x)/x;
+}
+
+
+// given a body b, apply its linear and angular rotation over the time
+// interval h, thereby adjusting its position and orientation.
+
+void dxStepBody (dxBody *b, dReal h)
+{
+  int j;
+
+  // handle linear velocity
+  for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j];
+
+  if (b->flags & dxBodyFlagFiniteRotation) {
+    dVector3 irv;	// infitesimal rotation vector
+    dQuaternion q;	// quaternion for finite rotation
+
+    if (b->flags & dxBodyFlagFiniteRotationAxis) {
+      // split the angular velocity vector into a component along the finite
+      // rotation axis, and a component orthogonal to it.
+      dVector3 frv;		// finite rotation vector
+      dReal k = dDOT (b->finite_rot_axis,b->avel);
+      frv[0] = b->finite_rot_axis[0] * k;
+      frv[1] = b->finite_rot_axis[1] * k;
+      frv[2] = b->finite_rot_axis[2] * k;
+      irv[0] = b->avel[0] - frv[0];
+      irv[1] = b->avel[1] - frv[1];
+      irv[2] = b->avel[2] - frv[2];
+
+      // make a rotation quaternion q that corresponds to frv * h.
+      // compare this with the full-finite-rotation case below.
+      h *= REAL(0.5);
+      dReal theta = k * h;
+      q[0] = dCos(theta);
+      dReal s = sinc(theta) * h;
+      q[1] = frv[0] * s;
+      q[2] = frv[1] * s;
+      q[3] = frv[2] * s;
+    }
+    else {
+      // make a rotation quaternion q that corresponds to w * h
+      dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
+			  b->avel[2]*b->avel[2]);
+      h *= REAL(0.5);
+      dReal theta = wlen * h;
+      q[0] = dCos(theta);
+      dReal s = sinc(theta) * h;
+      q[1] = b->avel[0] * s;
+      q[2] = b->avel[1] * s;
+      q[3] = b->avel[2] * s;
+    }
+
+    // do the finite rotation
+    dQuaternion q2;
+    dQMultiply0 (q2,q,b->q);
+    for (j=0; j<4; j++) b->q[j] = q2[j];
+
+    // do the infitesimal rotation if required
+    if (b->flags & dxBodyFlagFiniteRotationAxis) {
+      dReal dq[4];
+      dWtoDQ (irv,b->q,dq);
+      for (j=0; j<4; j++) b->q[j] += h * dq[j];
+    }
+  }
+  else {
+    // the normal way - do an infitesimal rotation
+    dReal dq[4];
+    dWtoDQ (b->avel,b->q,dq);
+    for (j=0; j<4; j++) b->q[j] += h * dq[j];
+  }
+
+  // normalize the quaternion and convert it to a rotation matrix
+  dNormalize4 (b->q);
+  dQtoR (b->q,b->R);
+
+  // notify all attached geoms that this body has moved
+  for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
+    dGeomMoved (geom);
+}
+
+//****************************************************************************
+// island processing
+
+// this groups all joints and bodies in a world into islands. all objects
+// in an island are reachable by going through connected bodies and joints.
+// each island can be simulated separately.
+// note that joints that are not attached to anything will not be included
+// in any island, an so they do not affect the simulation.
+//
+// this function starts new island from unvisited bodies. however, it will
+// never start a new islands from a disabled body. thus islands of disabled
+// bodies will not be included in the simulation. disabled bodies are
+// re-enabled if they are found to be part of an active island.
+
+void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper)
+{
+  dxBody *b,*bb,**body;
+  dxJoint *j,**joint;
+
+  // nothing to do if no bodies
+  if (world->nb <= 0) return;
+
+  // handle auto-disabling of bodies
+  dInternalHandleAutoDisabling (world,stepsize);
+  
+  // make arrays for body and joint lists (for a single island) to go into
+  body = (dxBody**) ALLOCA (world->nb * sizeof(dxBody*));
+  joint = (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*));
+  int bcount = 0;	// number of bodies in `body'
+  int jcount = 0;	// number of joints in `joint'
+
+  // set all body/joint tags to 0
+  for (b=world->firstbody; b; b=(dxBody*)b->next) b->tag = 0;
+  for (j=world->firstjoint; j; j=(dxJoint*)j->next) j->tag = 0;
+
+  // allocate a stack of unvisited bodies in the island. the maximum size of
+  // the stack can be the lesser of the number of bodies or joints, because
+  // new bodies are only ever added to the stack by going through untagged
+  // joints. all the bodies in the stack must be tagged!
+  int stackalloc = (world->nj < world->nb) ? world->nj : world->nb;
+  dxBody **stack = (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*));
+
+  for (bb=world->firstbody; bb; bb=(dxBody*)bb->next) {
+    // get bb = the next enabled, untagged body, and tag it
+    if (bb->tag || (bb->flags & dxBodyDisabled)) continue;
+    bb->tag = 1;
+
+    // tag all bodies and joints starting from bb.
+    int stacksize = 0;
+    b = bb;
+    body[0] = bb;
+    bcount = 1;
+    jcount = 0;
+    goto quickstart;
+    while (stacksize > 0) {
+      b = stack[--stacksize];	// pop body off stack
+      body[bcount++] = b;	// put body on body list
+      quickstart:
+
+      // traverse and tag all body's joints, add untagged connected bodies
+      // to stack
+      for (dxJointNode *n=b->firstjoint; n; n=n->next) {
+	if (!n->joint->tag) {
+	  n->joint->tag = 1;
+	  joint[jcount++] = n->joint;
+	  if (n->body && !n->body->tag) {
+	    n->body->tag = 1;
+	    stack[stacksize++] = n->body;
+	  }
+	}
+      }
+      dIASSERT(stacksize <= world->nb);
+      dIASSERT(stacksize <= world->nj);
+    }
+
+    // now do something with body and joint lists
+    stepper (world,body,bcount,joint,jcount,stepsize);
+
+    // what we've just done may have altered the body/joint tag values.
+    // we must make sure that these tags are nonzero.
+    // also make sure all bodies are in the enabled state.
+    int i;
+    for (i=0; i<bcount; i++) {
+      body[i]->tag = 1;
+      body[i]->flags &= ~dxBodyDisabled;
+    }
+    for (i=0; i<jcount; i++) joint[i]->tag = 1;
+  }
+
+  // if debugging, check that all objects (except for disabled bodies,
+  // unconnected joints, and joints that are connected to disabled bodies)
+  // were tagged.
+# ifndef dNODEBUG
+  for (b=world->firstbody; b; b=(dxBody*)b->next) {
+    if (b->flags & dxBodyDisabled) {
+      if (b->tag) dDebug (0,"disabled body tagged");
+    }
+    else {
+      if (!b->tag) dDebug (0,"enabled body not tagged");
+    }
+  }
+  for (j=world->firstjoint; j; j=(dxJoint*)j->next) {
+    if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled)==0) ||
+	(j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)==0)) {
+      if (!j->tag) dDebug (0,"attached enabled joint not tagged");
+    }
+    else {
+      if (j->tag) dDebug (0,"unattached or disabled joint tagged");
+    }
+  }
+# endif
+}

src/source/ode/util.h

@@ -0,0 +1,38 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_UTIL_H_
+#define _ODE_UTIL_H_
+
+#include "objects.h"
+
+
+void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize);
+void dxStepBody (dxBody *b, dReal h);
+
+typedef void (*dstepper_fn_t) (dxWorld *world, dxBody * const *body, int nb,
+        dxJoint * const *_joint, int nj, dReal stepsize);
+
+void dxProcessIslands (dxWorld *world, dReal stepsize, dstepper_fn_t stepper);
+
+
+#endif