resubmitting ode

18 files changed, 8830 insertions, 0 deletions

diff --git a/libraries/ode-0.9/contrib/BreakableJoints/README.txt b/libraries/ode-0.9/contrib/BreakableJoints/README.txt
new file mode 100644
index 0000000..e996816
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/README.txt
@@ -0,0 +1,110 @@
+Breakable Joints
+
+================================================================================
+
+Description:
+This is a small addition to ODE that makes joints breakable. Breakable means
+that if a force on a joint is to high it wil break. I have included a modified
+version of test_buggy.cpp (test_breakable.cpp) so you can see it for your self.
+Just drive your buggy into an obstacle and enjoy!
+
+================================================================================
+
+Installation instructions:
+- copy joint.h, joint.cpp, ode.cpp and step.cpp to the ode/src/ directory
+- copy common.h and object.h to the include/ directory
+- copy test_breakable.cpp to the ode/test/ directory
+- add test_breakable.cpp to the ODE_TEST_SRC_CPP object in the makefile.
+- make ode-lib
+- make ode-test
+You can also use the diffs. The above files will quickly go out of sync with the
+rest of ODE but the diffs wil remain valid longer.
+
+================================================================================
+
+Functions:
+dJointSetBreakable (dJointID joint, int b)
+        If b is 1 the joint is made breakable. If b is 0 the joint is made
+        unbreakable.
+
+void dJointSetBreakCallback (dJointID joint, dJointBreakCallback *callbackFunc)
+        Sets the callback function for this joint. If a funtion is set it will be
+        called if the joint is broken but before it is actually detached or deleted.
+
+void dJointSetBreakMode (dJointID joint, int mode)
+        Use this functions to set some flags. These flags can be ORred ( | )
+        together; ie. dJointSetBreakMode (someJoint,
+dJOINT_BREAK_AT_B1_FORCE|dJOINT_DELETE_ON_BREAK)
+        dJOINT_DELETE_ON_BREAK    - If the joint breaks it wil be deleted.
+        dJOINT_BREAK_AT_B1_FORCE  - If the force on body 1 is to high the joint will
+                                                                break
+        dJOINT_BREAK_AT_B1_TORQUE - If the torque on body 1 is to high the joint will
+                                                                break
+        dJOINT_BREAK_AT_B2_FORCE  - If the force on body 2 is to high the joint will
+                                                                break
+        dJOINT_BREAK_AT_B2_TORQUE - If the torque on body 2 is to high the joint will
+                                                                break
+
+void dJointSetBreakForce (dJointID joint, int body, dReal x, dReal y, dReal z)
+        With this function you can set the maximum force for a body connected to this
+        joint. A value of 0 for body means body 1, 1 means body 2. The force is 
+        relative to the bodies rotation.
+
+void dJointSetBreakTorque (dJointID joint, int body, dReal x, dReal y, dReal z)
+        With this function you can set the maximum torque for a body connected to this
+        joint. A value of 0 for body means body 1, 1 means body 2. The torque is 
+        relative to the bodies rotation.
+
+int dJointIsBreakable (dJointID joint)
+        Returns 1 if this joint is breakable, 0 otherwise.
+        
+int dJointGetBreakMode (dJointID joint)
+        Returns the breakmode flag.
+
+void dJointGetBreakForce (dJointID joint, int body, dReal *force)
+        Returns the force at what this joint will break. A value of 0 for body means
+        body 1, 1 means body 2. force must have enough space for 3 dReal values.
+
+void dJointGetBreakTorque (dJointID joint, int body, dReal *torque)
+        Returns the torque at what this joint will break. A value of 0 for body 
+        means body 1, 1 means body 2. force must have enough space for 3 dReal 
+        values.
+
+================================================================================
+
+The callback function is defined like this (in common.h):
+void dJointBreakCallback (dJointID);
+
+================================================================================
+
+Problems, known bugs & other issues:
+- If the timestep is very small then joints get a lot weaker. They can even fall
+  apart!
+- I have tested all this with the latest checkout from CVS (at the time of 
+  writing ofcourse). I haven't tested it with earlier versions of ODE.
+- I have modified the code that fills the jointfeedback struct. I haven't tested
+  if it still works.
+- I'm not sure if the forces are really relative to the connected bodies.
+- There are some memory leaks in the test_breakable.cpp example.
+
+================================================================================
+
+Bugfixes and changes:
+09/08/2003
+- I fixed a bug when there where 0 joints in the simulation
+
+06/12/2003
+- dJointGetBreakMode() added, by vadim_mcagon@hotmail.com
+
+11/03/2004
+- Updated files to work with latest CVS checkout. 
+- Added support for dWorldStepFast1()
+- Added separate test_breakable.cpp example.
+- Updated the code that breaks and destroys a joint.
+
+================================================================================
+
+Send me an e-mail if you have any suggestions, ideas, bugs, bug-fixes, anything!
+e-mail: roelvandijk@home.nl
+
+Roel van Dijk - 11/03/2004
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/common.h b/libraries/ode-0.9/contrib/BreakableJoints/common.h
new file mode 100644
index 0000000..bc4272d
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/common.h
@@ -0,0 +1,337 @@
+/*************************************************************************
+ *                                                                       *
+ * 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)sqrt(x))               /* square root */
+#define dRecipSqrt(x) ((float)(1.0f/sqrt(x)))   /* reciprocal square root */
+#define dSin(x) ((float)sin(x))                 /* sine */
+#define dCos(x) ((float)cos(x))                 /* cosine */
+#define dFabs(x) ((float)fabs(x))               /* absolute value */
+#define dAtan2(y,x) ((float)atan2((y),(x)))     /* arc tangent with 2 args */
+
+#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))
+
+#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(((int)(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
+};
+
+/******************** breakable joint contribution ***********************/
+/* joint break callback function */
+typedef void dJointBreakCallback (dJointID joint);
+
+/* joint break modes */
+enum {
+  // if this flag is set, the joint wil break
+  dJOINT_BROKEN =             0x0001,
+  // if this flag is set, the joint wil be deleted when it breaks
+  dJOINT_DELETE_ON_BREAK =    0x0002,
+  // if this flag is set, the joint can break at a certain force on body 1
+  dJOINT_BREAK_AT_B1_FORCE =  0x0004,
+  // if this flag is set, the joint can break at a certain torque on body 1
+  dJOINT_BREAK_AT_B1_TORQUE = 0x0008,
+  // if this flag is set, the joint can break at a certain force on body 2
+  dJOINT_BREAK_AT_B2_FORCE =  0x0010,
+  // if this flag is set, the joint can break at a certain torque on body 2
+  dJOINT_BREAK_AT_B2_TORQUE = 0x0020
+};
+/*************************************************************************/
+
+/* 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
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff
new file mode 100644
index 0000000..24415a1
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/common.h.diff
@@ -0,0 +1,21 @@
+208,227d207
+< /******************** breakable joint contribution ***********************/
+< /* joint break callback function */
+< typedef void dJointBreakCallback (dJointID joint);
+< 
+< /* joint break modes */
+< enum {
+<   // if this flag is set, the joint wil break
+<   dJOINT_BROKEN =             0x0001,
+<   // if this flag is set, the joint wil be deleted when it breaks
+<   dJOINT_DELETE_ON_BREAK =    0x0002,
+<   // if this flag is set, the joint can break at a certain force on body 1
+<   dJOINT_BREAK_AT_B1_FORCE =  0x0004,
+<   // if this flag is set, the joint can break at a certain torque on body 1
+<   dJOINT_BREAK_AT_B1_TORQUE = 0x0008,
+<   // if this flag is set, the joint can break at a certain force on body 2
+<   dJOINT_BREAK_AT_B2_FORCE =  0x0010,
+<   // if this flag is set, the joint can break at a certain torque on body 2
+<   dJOINT_BREAK_AT_B2_TORQUE = 0x0020
+< };
+< /*************************************************************************/
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff
new file mode 100644
index 0000000..80397f0
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.cpp.diff
@@ -0,0 +1,148 @@
+2659,2804d2658
+< 
+< /******************** breakable joint contribution ***********************/
+< extern "C" void dJointSetBreakable (dxJoint *joint, int b) {
+<   dAASSERT(joint);
+<   if (b) {
+<     // we want this joint to be breakable but we must first check if it
+<     // was already breakable
+<     if (!joint->breakInfo) {
+<       // allocate a dxJointBreakInfo struct
+<       joint->breakInfo = new dxJointBreakInfo;
+<       joint->breakInfo->flags = 0;
+<       for (int i = 0; i < 3; i++) {
+<         joint->breakInfo->b1MaxF[0] = 0;
+<         joint->breakInfo->b1MaxT[0] = 0;
+<         joint->breakInfo->b2MaxF[0] = 0;
+<         joint->breakInfo->b2MaxT[0] = 0;
+<       }
+<         joint->breakInfo->callback = 0;
+<     }
+<     else {
+<       // the joint was already breakable
+<       return;
+<     }
+<   }
+<   else {
+<     // we want this joint to be unbreakable mut we must first check if
+<     // it is alreay unbreakable
+<     if (joint->breakInfo) {
+<       // deallocate the dxJointBreakInfo struct
+<       delete joint->breakInfo;
+<       joint->breakInfo = 0;
+<     }
+<     else {
+<       // the joint was already unbreakable
+<       return;
+<     }
+<   }
+< }
+< 
+< extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) {
+<   dAASSERT(joint);
+< # ifndef dNODEBUG
+<   // only works for a breakable joint
+<   if (!joint->breakInfo) {
+<     dDebug (0, "dJointSetBreakCallback called on unbreakable joint");
+<   }
+< # endif
+<   joint->breakInfo->callback = callbackFunc;
+< }
+< 
+< extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) {
+<   dAASSERT(joint);
+< # ifndef dNODEBUG
+<   // only works for a breakable joint
+<   if (!joint->breakInfo) {
+<     dDebug (0, "dJointSetBreakMode called on unbreakable joint");
+<   }
+< # endif
+<   joint->breakInfo->flags = mode;
+< }
+< 
+< extern "C" int dJointGetBreakMode (dxJoint *joint) {
+<   dAASSERT(joint);
+< # ifndef dNODEBUG
+<   // only works for a breakable joint
+<   if (!joint->breakInfo) {
+<     dDebug (0, "dJointGetBreakMode called on unbreakable joint");
+<   }
+< # endif
+<   return joint->breakInfo->flags;
+< }
+< 
+< extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) {
+<   dAASSERT(joint);
+< # ifndef dNODEBUG
+<   // only works for a breakable joint
+<   if (!joint->breakInfo) {
+<   dDebug (0, "dJointSetBreakForce called on unbreakable joint");
+<   }
+< # endif
+<   if (body) {
+<       joint->breakInfo->b2MaxF[0] = x;
+<       joint->breakInfo->b2MaxF[1] = y;
+<       joint->breakInfo->b2MaxF[2] = z;
+<   }
+<   else {
+<       joint->breakInfo->b1MaxF[0] = x;
+<       joint->breakInfo->b1MaxF[1] = y;
+<       joint->breakInfo->b1MaxF[2] = z;
+<   }
+< }
+< 
+< extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) {
+<   dAASSERT(joint);
+< # ifndef dNODEBUG
+<   // only works for a breakable joint
+<   if (!joint->breakInfo) {
+<   dDebug (0, "dJointSetBreakTorque called on unbreakable joint");
+<   }
+< # endif
+<   if (body) {
+<       joint->breakInfo->b2MaxT[0] = x;
+<       joint->breakInfo->b2MaxT[1] = y;
+<       joint->breakInfo->b2MaxT[2] = z;
+<   }
+<   else {
+<       joint->breakInfo->b1MaxT[0] = x;
+<       joint->breakInfo->b1MaxT[1] = y;
+<       joint->breakInfo->b1MaxT[2] = z;
+<   }
+< }
+< 
+< extern "C" int dJointIsBreakable (dxJoint *joint) {
+<   dAASSERT(joint);
+<   return joint->breakInfo != 0;
+< }
+< 
+< extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) {
+<   dAASSERT(joint);
+< # ifndef dNODEBUG
+<   // only works for a breakable joint
+<   if (!joint->breakInfo) {
+<     dDebug (0, "dJointGetBreakForce called on unbreakable joint");
+<   }
+< # endif
+<   if (body)
+<     for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i];
+<   else
+<     for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i];
+< }
+< 
+< extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) {
+<   dAASSERT(joint);
+< # ifndef dNODEBUG
+<   // only works for a breakable joint
+<   if (!joint->breakInfo) {
+<     dDebug (0, "dJointGetBreakTorque called on unbreakable joint");
+<   }
+< # endif
+<   if (body)
+<     for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i];
+<   else
+<     for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i];
+< }
+< /*************************************************************************/
+<   
+\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff
new file mode 100644
index 0000000..eed3c24
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/joint.h.diff
@@ -0,0 +1,18 @@
+61,70d60
+< /******************** breakable joint contribution ***********************/
+< struct dxJointBreakInfo : public dBase {
+<       int flags;
+<       dReal b1MaxF[3]; // maximum force on body 1
+<       dReal b1MaxT[3]; // maximum torque on body 1
+<       dReal b2MaxF[3]; // maximum force on body 2
+<       dReal b2MaxT[3]; // maximum torque on body 2
+<       dJointBreakCallback *callback; // function that is called when this joint breaks
+< };
+< /*************************************************************************/
+135,140d124
+<   
+<   /******************** breakable joint contribution ***********************/
+<   // optional break info structure. if this is not NULL the the joint is
+<   // breakable.
+<   dxJointBreakInfo *breakInfo;
+<   /*************************************************************************/
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff
new file mode 100644
index 0000000..fd2129e
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/objects.h.diff
@@ -0,0 +1,13 @@
+168,179d167
+< /******************** breakable joint contribution ***********************/
+< void dJointSetBreakable (dJointID, int b);
+< void dJointSetBreakCallback (dJointID, dJointBreakCallback *callbackFunc);
+< void dJointSetBreakMode (dJointID, int mode);
+< int dJointGetBreakMode (dJointID);
+< void dJointSetBreakForce (dJointID, int body, dReal x, dReal y, dReal z);
+< void dJointSetBreakTorque (dJointID, int body, dReal x, dReal y, dReal z);
+< int dJointIsBreakable (dJointID);
+< void dJointGetBreakForce (dJointID, int body, dReal *force);
+< void dJointGetBreakTorque (dJointID, int body, dReal *torque);
+< /*************************************************************************/
+< 
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff
new file mode 100644
index 0000000..761b7be
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/ode.cpp.diff
@@ -0,0 +1,28 @@
+212,230d211
+<   /******************** breakable joint contribution ***********************/
+<   dxJoint* nextJ;
+<   if (!world->firstjoint)
+<     nextJ = 0;
+<   else
+<     nextJ = (dxJoint*)world->firstjoint->next;
+<   for (j=world->firstjoint; j; j=nextJ) {
+<       nextJ = (dxJoint*)j->next;
+<       // check if joint is breakable and broken
+<     if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
+<               // detach (break) the joint
+<         dJointAttach (j, 0, 0);
+<               // call the callback function if it is set
+<               if (j->breakInfo->callback) j->breakInfo->callback (j);
+<               // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
+<               if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
+<       }
+<   }
+<   /*************************************************************************/
+931,933d911
+<   /******************** breakable joint contribution ***********************/
+<   j->breakInfo = 0;
+<   /*************************************************************************/
+1011,1013d988
+<   /******************** breakable joint contribution ***********************/
+<   if (j->breakInfo) delete j->breakInfo;
+<   /*************************************************************************/
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff
new file mode 100644
index 0000000..dfc8c2f
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/step.cpp.diff
@@ -0,0 +1,130 @@
+966,1066c966,989
+< /******************** breakable joint contribution ***********************/
+<     // this saves us a few dereferences
+<     dxJointBreakInfo *jBI = joint[i]->breakInfo;
+<     // we need joint feedback if the joint is breakable or if the user
+<     // requested feedback.
+<       if (jBI||fb) {
+<       // we need 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.
+<       dJointFeedback temp_fb; // temporary storage for joint feedback
+<         dReal data1[8],data2[8];
+<         Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
+<         dReal *cf1 = cforce + 8*b1->tag;
+<         cf1[0] += (temp_fb.f1[0] = data1[0]);
+<         cf1[1] += (temp_fb.f1[1] = data1[1]);
+<         cf1[2] += (temp_fb.f1[2] = data1[2]);
+<         cf1[4] += (temp_fb.t1[0] = data1[4]);
+<         cf1[5] += (temp_fb.t1[1] = data1[5]);
+<         cf1[6] += (temp_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] += (temp_fb.f2[0] = data2[0]);
+<           cf2[1] += (temp_fb.f2[1] = data2[1]);
+<           cf2[2] += (temp_fb.f2[2] = data2[2]);
+<           cf2[4] += (temp_fb.t2[0] = data2[4]);
+<           cf2[5] += (temp_fb.t2[1] = data2[5]);
+<           cf2[6] += (temp_fb.t2[2] = data2[6]);
+<         }
+<         // if the user requested so we must copy the feedback information to
+<         // the feedback struct that the user suplied.
+<         if (fb) {
+<           // copy temp_fb to fb
+<           fb->f1[0] = temp_fb.f1[0];
+<           fb->f1[1] = temp_fb.f1[1];
+<           fb->f1[2] = temp_fb.f1[2];
+<           fb->t1[0] = temp_fb.t1[0];
+<           fb->t1[1] = temp_fb.t1[1];
+<           fb->t1[2] = temp_fb.t1[2];
+<           if (b2) {
+<             fb->f2[0] = temp_fb.f2[0];
+<             fb->f2[1] = temp_fb.f2[1];
+<             fb->f2[2] = temp_fb.f2[2];
+<             fb->t2[0] = temp_fb.t2[0];
+<             fb->t2[1] = temp_fb.t2[1];
+<             fb->t2[2] = temp_fb.t2[2];
+<           }
+<         }
+<         // if the joint is breakable we need to check the breaking conditions
+<       if (jBI) {
+<         dReal relCF1[3];
+<               dReal relCT1[3];
+<               // multiply the force and torque vectors by the rotation matrix of body 1
+<               dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]);
+<               dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]);
+<               if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
+<                 // check if the force is to high
+<           for (int i = 0; i < 3; i++) {
+<             if (relCF1[i] > jBI->b1MaxF[i]) {
+<                     jBI->flags |= dJOINT_BROKEN;
+<                     goto doneCheckingBreaks;
+<                   }
+<           }
+<               }
+<               if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
+<                 // check if the torque is to high
+<           for (int i = 0; i < 3; i++) {
+<             if (relCT1[i] > jBI->b1MaxT[i]) {
+<                     jBI->flags |= dJOINT_BROKEN;
+<                     goto doneCheckingBreaks;
+<             }
+<           }
+<               }
+<         if (b2) {
+<           dReal relCF2[3];
+<           dReal relCT2[3];
+<           // multiply the force and torque vectors by the rotation matrix of body 2
+<           dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]);
+<           dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]);
+<                 if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
+<             // check if the force is to high
+<             for (int i = 0; i < 3; i++) {
+<               if (relCF2[i] > jBI->b2MaxF[i]) {
+<                 jBI->flags |= dJOINT_BROKEN;
+<                 goto doneCheckingBreaks;
+<               }
+<             }
+<                 }
+<                 if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
+<                 // check if the torque is to high
+<             for (int i = 0; i < 3; i++) {
+<               if (relCT2[i] > jBI->b2MaxT[i]) {
+<                 jBI->flags |= dJOINT_BROKEN;
+<                 goto doneCheckingBreaks;
+<               }
+<             }
+<                 }
+<         }
+<               doneCheckingBreaks:
+<               ;
+---
+>       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]);
+>       }
+1068,1069d990
+<     }
+< /*************************************************************************/
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff
new file mode 100644
index 0000000..ed64cba
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/stepfast.cpp.diff
@@ -0,0 +1,143 @@
+587,598c587,593
+< /******************** breakable joint contribution ***********************/
+<       // this saves us a few dereferences
+<     dxJointBreakInfo *jBI = joint->breakInfo;
+<     // we need joint feedback if the joint is breakable or if the user
+<     // requested feedback.
+<       if (jBI||fb) {
+<               // we need 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.
+<               dJointFeedback temp_fb; // temporary storage for joint feedback
+<               dReal data1[8],data2[8];
+---
+>       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];
+603,608c598,603
+<                       cf1[0] = (temp_fb.f1[0] = data1[0]);
+<                       cf1[1] = (temp_fb.f1[1] = data1[1]);
+<                       cf1[2] = (temp_fb.f1[2] = data1[2]);
+<                       cf1[4] = (temp_fb.t1[0] = data1[4]);
+<                       cf1[5] = (temp_fb.t1[1] = data1[5]);
+<                       cf1[6] = (temp_fb.t1[2] = data1[6]);
+---
+>                       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]);
+614,691c609,614
+<                       cf2[0] = (temp_fb.f2[0] = data2[0]);
+<                       cf2[1] = (temp_fb.f2[1] = data2[1]);
+<                       cf2[2] = (temp_fb.f2[2] = data2[2]);
+<                       cf2[4] = (temp_fb.t2[0] = data2[4]);
+<                       cf2[5] = (temp_fb.t2[1] = data2[5]);
+<                       cf2[6] = (temp_fb.t2[2] = data2[6]);
+<               }
+<               // if the user requested so we must copy the feedback information to
+<               // the feedback struct that the user suplied.
+<               if (fb) {
+<                       // copy temp_fb to fb
+<                       fb->f1[0] = temp_fb.f1[0];
+<                       fb->f1[1] = temp_fb.f1[1];
+<                       fb->f1[2] = temp_fb.f1[2];
+<                       fb->t1[0] = temp_fb.t1[0];
+<                       fb->t1[1] = temp_fb.t1[1];
+<                       fb->t1[2] = temp_fb.t1[2];
+<                       if (body[1]) {
+<                               fb->f2[0] = temp_fb.f2[0];
+<                               fb->f2[1] = temp_fb.f2[1];
+<                               fb->f2[2] = temp_fb.f2[2];
+<                               fb->t2[0] = temp_fb.t2[0];
+<                               fb->t2[1] = temp_fb.t2[1];
+<                               fb->t2[2] = temp_fb.t2[2];
+<                       }
+<               }
+<               // if the joint is breakable we need to check the breaking conditions
+<               if (jBI) {
+<                       dReal relCF1[3];
+<                       dReal relCT1[3];
+<                       // multiply the force and torque vectors by the rotation matrix of body 1
+<                       dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]);
+<                       dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]);
+<                       if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
+<                               // check if the force is to high
+<                               for (int i = 0; i < 3; i++) {
+<                                       if (relCF1[i] > jBI->b1MaxF[i]) {
+<                                               jBI->flags |= dJOINT_BROKEN;
+<                                               goto doneCheckingBreaks;
+<                                       }
+<                               }
+<                       }
+<                       if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
+<                               // check if the torque is to high
+<                               for (int i = 0; i < 3; i++) {
+<                                       if (relCT1[i] > jBI->b1MaxT[i]) {
+<                                               jBI->flags |= dJOINT_BROKEN;
+<                                               goto doneCheckingBreaks;
+<                                       }
+<                               }
+<                       }
+<                       if (body[1]) {
+<                               dReal relCF2[3];
+<                               dReal relCT2[3];
+<                               // multiply the force and torque vectors by the rotation matrix of body 2
+<                               dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]);
+<                               dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]);
+<                               if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
+<                                       // check if the force is to high
+<                                       for (int i = 0; i < 3; i++) {
+<                                               if (relCF2[i] > jBI->b2MaxF[i]) {
+<                                                       jBI->flags |= dJOINT_BROKEN;
+<                                                       goto doneCheckingBreaks;
+<                                               }
+<                                       }
+<                               }
+<                               if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
+<                                       // check if the torque is to high
+<                                       for (int i = 0; i < 3; i++) {
+<                                               if (relCT2[i] > jBI->b2MaxT[i]) {
+<                                                       jBI->flags |= dJOINT_BROKEN;
+<                                                       goto doneCheckingBreaks;
+<                                               }
+<                                       }
+<                               }
+<                       }
+<                       doneCheckingBreaks:
+<                       ;
+---
+>                       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]);
+694d616
+< /*************************************************************************/
+1178,1196d1099
+<   /******************** breakable joint contribution ***********************/
+<   dxJoint* nextJ;
+<   if (!world->firstjoint)
+<     nextJ = 0;
+<   else
+<     nextJ = (dxJoint*)world->firstjoint->next;
+<   for (j=world->firstjoint; j; j=nextJ) {
+<       nextJ = (dxJoint*)j->next;
+<       // check if joint is breakable and broken
+<     if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
+<               // detach (break) the joint
+<         dJointAttach (j, 0, 0);
+<               // call the callback function if it is set
+<               if (j->breakInfo->callback) j->breakInfo->callback (j);
+<               // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
+<               if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
+<       }
+<   }
+<   /*************************************************************************/
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff b/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff
new file mode 100644
index 0000000..65770da
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/diff/test_buggy.cpp.diff
@@ -0,0 +1,16 @@
+266,270d265
+< 
+<     // breakable joints contribution
+<               dJointSetBreakable (joint[i], 1);
+<               dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_FORCE);
+<               dJointSetBreakForce (joint[i], 0.5);
+298c293
+<   ground_box = dCreateBox (space,2,1.5,5);
+---
+>   ground_box = dCreateBox (space,2,1.5,1);
+300,301c295,296
+<   dRFromAxisAndAngle (R,0,1,0,-0.85);
+<   dGeomSetPosition (ground_box,5,0,-1);
+---
+>   dRFromAxisAndAngle (R,0,1,0,-0.15);
+>   dGeomSetPosition (ground_box,2,0,-0.34);
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp b/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp
new file mode 100644
index 0000000..2c724f8
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp
@@ -0,0 +1,2803 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+design note: the general principle for giving a joint the option of connecting
+to the static environment (i.e. the absolute frame) is to check the second
+body (joint->node[1].body), and if it is zero then behave as if its body
+transform is the identity.
+
+*/
+
+#include <ode/odemath.h>
+#include <ode/rotation.h>
+#include <ode/matrix.h>
+#include "joint.h"
+
+//****************************************************************************
+// externs
+
+extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
+extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
+
+//****************************************************************************
+// utility
+
+// set three "ball-and-socket" rows in the constraint equation, and the
+// corresponding right hand side.
+
+static inline void setBall (dxJoint *joint, dxJoint::Info2 *info,
+                            dVector3 anchor1, dVector3 anchor2)
+{
+  // anchor points in global coordinates with respect to body PORs.
+  dVector3 a1,a2;
+
+  int s = info->rowskip;
+
+  // set jacobian
+  info->J1l[0] = 1;
+  info->J1l[s+1] = 1;
+  info->J1l[2*s+2] = 1;
+  dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1);
+  dCROSSMAT (info->J1a,a1,s,-,+);
+  if (joint->node[1].body) {
+    info->J2l[0] = -1;
+    info->J2l[s+1] = -1;
+    info->J2l[2*s+2] = -1;
+    dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2);
+    dCROSSMAT (info->J2a,a2,s,+,-);
+  }
+
+  // set right hand side
+  dReal k = info->fps * info->erp;
+  if (joint->node[1].body) {
+    for (int j=0; j<3; j++) {
+      info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] -
+                        a1[j] - joint->node[0].body->pos[j]);
+    }
+  }
+  else {
+    for (int j=0; j<3; j++) {
+      info->c[j] = k * (anchor2[j] - a1[j] -
+                        joint->node[0].body->pos[j]);
+    }
+  }
+}
+
+
+// this is like setBall(), except that `axis' is a unit length vector
+// (in global coordinates) that should be used for the first jacobian
+// position row (the other two row vectors will be derived from this).
+// `erp1' is the erp value to use along the axis.
+
+static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info,
+                            dVector3 anchor1, dVector3 anchor2,
+                            dVector3 axis, dReal erp1)
+{
+  // anchor points in global coordinates with respect to body PORs.
+  dVector3 a1,a2;
+
+  int i,s = info->rowskip;
+
+  // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
+  // [0 1 0] and [0 0 1], which makes everything much easier.
+  dVector3 q1,q2;
+  dPlaneSpace (axis,q1,q2);
+
+  // set jacobian
+  for (i=0; i<3; i++) info->J1l[i] = axis[i];
+  for (i=0; i<3; i++) info->J1l[s+i] = q1[i];
+  for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i];
+  dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1);
+  dCROSS (info->J1a,=,a1,axis);
+  dCROSS (info->J1a+s,=,a1,q1);
+  dCROSS (info->J1a+2*s,=,a1,q2);
+  if (joint->node[1].body) {
+    for (i=0; i<3; i++) info->J2l[i] = -axis[i];
+    for (i=0; i<3; i++) info->J2l[s+i] = -q1[i];
+    for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i];
+    dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2);
+    dCROSS (info->J2a,= -,a2,axis);
+    dCROSS (info->J2a+s,= -,a2,q1);
+    dCROSS (info->J2a+2*s,= -,a2,q2);
+  }
+
+  // set right hand side - measure error along (axis,q1,q2)
+  dReal k1 = info->fps * erp1;
+  dReal k = info->fps * info->erp;
+
+  for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i];
+  if (joint->node[1].body) {
+    for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i];
+    info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1));
+    info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1));
+    info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1));
+  }
+  else {
+    info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1));
+    info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1));
+    info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1));
+  }
+}
+
+
+// set three orientation rows in the constraint equation, and the
+// corresponding right hand side.
+
+static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row)
+{
+  int s = info->rowskip;
+  int start_index = start_row * s;
+
+  // 3 rows to make body rotations equal
+  info->J1a[start_index] = 1;
+  info->J1a[start_index + s + 1] = 1;
+  info->J1a[start_index + s*2+2] = 1;
+  if (joint->node[1].body) {
+    info->J2a[start_index] = -1;
+    info->J2a[start_index + s+1] = -1;
+    info->J2a[start_index + s*2+2] = -1;
+  }
+
+  // compute the right hand side. the first three elements will result in
+  // relative angular velocity of the two bodies - this is set to bring them
+  // back into alignment. the correcting angular velocity is
+  //   |angular_velocity| = angle/time = erp*theta / stepsize
+  //                      = (erp*fps) * theta
+  //    angular_velocity  = |angular_velocity| * u
+  //                      = (erp*fps) * theta * u
+  // where rotation along unit length axis u by theta brings body 2's frame
+  // to qrel with respect to body 1's frame. using a small angle approximation
+  // for sin(), this gives
+  //    angular_velocity  = (erp*fps) * 2 * v
+  // where the quaternion of the relative rotation between the two bodies is
+  //    q = [cos(theta/2) sin(theta/2)*u] = [s v]
+
+  // get qerr = relative rotation (rotation error) between two bodies
+  dQuaternion qerr,e;
+  if (joint->node[1].body) {
+    dQuaternion qq;
+    dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q);
+    dQMultiply2 (qerr,qq,qrel);
+  }
+  else {
+    dQMultiply3 (qerr,joint->node[0].body->q,qrel);
+  }
+  if (qerr[0] < 0) {
+    qerr[1] = -qerr[1];         // adjust sign of qerr to make theta small
+    qerr[2] = -qerr[2];
+    qerr[3] = -qerr[3];
+  }
+  dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding!
+  dReal k = info->fps * info->erp;
+  info->c[start_row] = 2*k * e[0];
+  info->c[start_row+1] = 2*k * e[1];
+  info->c[start_row+2] = 2*k * e[2];
+}
+
+
+// compute anchor points relative to bodies
+
+static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z,
+                        dVector3 anchor1, dVector3 anchor2)
+{
+  if (j->node[0].body) {
+    dReal q[4];
+    q[0] = x - j->node[0].body->pos[0];
+    q[1] = y - j->node[0].body->pos[1];
+    q[2] = z - j->node[0].body->pos[2];
+    q[3] = 0;
+    dMULTIPLY1_331 (anchor1,j->node[0].body->R,q);
+    if (j->node[1].body) {
+      q[0] = x - j->node[1].body->pos[0];
+      q[1] = y - j->node[1].body->pos[1];
+      q[2] = z - j->node[1].body->pos[2];
+      q[3] = 0;
+      dMULTIPLY1_331 (anchor2,j->node[1].body->R,q);
+    }
+    else {
+      anchor2[0] = x;
+      anchor2[1] = y;
+      anchor2[2] = z;
+    }
+  }
+  anchor1[3] = 0;
+  anchor2[3] = 0;
+}
+
+
+// compute axes relative to bodies. either axis1 or axis2 can be 0.
+
+static void setAxes (dxJoint *j, dReal x, dReal y, dReal z,
+                     dVector3 axis1, dVector3 axis2)
+{
+  if (j->node[0].body) {
+    dReal q[4];
+    q[0] = x;
+    q[1] = y;
+    q[2] = z;
+    q[3] = 0;
+    dNormalize3 (q);
+    if (axis1) {
+      dMULTIPLY1_331 (axis1,j->node[0].body->R,q);
+      axis1[3] = 0;
+    }
+    if (axis2) {
+      if (j->node[1].body) {
+        dMULTIPLY1_331 (axis2,j->node[1].body->R,q);
+      }
+      else {
+        axis2[0] = x;
+        axis2[1] = y;
+        axis2[2] = z;
+      }
+      axis2[3] = 0;
+    }
+  }
+}
+
+
+static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1)
+{
+  if (j->node[0].body) {
+    dMULTIPLY0_331 (result,j->node[0].body->R,anchor1);
+    result[0] += j->node[0].body->pos[0];
+    result[1] += j->node[0].body->pos[1];
+    result[2] += j->node[0].body->pos[2];
+  }
+}
+
+
+static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2)
+{
+  if (j->node[1].body) {
+    dMULTIPLY0_331 (result,j->node[1].body->R,anchor2);
+    result[0] += j->node[1].body->pos[0];
+    result[1] += j->node[1].body->pos[1];
+    result[2] += j->node[1].body->pos[2];
+  }
+  else {
+    result[0] = anchor2[0];
+    result[1] = anchor2[1];
+    result[2] = anchor2[2];
+  }
+}
+
+
+static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1)
+{
+  if (j->node[0].body) {
+    dMULTIPLY0_331 (result,j->node[0].body->R,axis1);
+  }
+}
+
+
+static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2)
+{
+  if (j->node[1].body) {
+    dMULTIPLY0_331 (result,j->node[1].body->R,axis2);
+  }
+  else {
+    result[0] = axis2[0];
+    result[1] = axis2[1];
+    result[2] = axis2[2];
+  }
+}
+
+
+static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis)
+{
+  // the angle between the two bodies is extracted from the quaternion that
+  // represents the relative rotation between them. recall that a quaternion
+  // q is:
+  //    [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
+  // where s is a scalar and v is a 3-vector. u is a unit length axis and
+  // theta is a rotation along that axis. we can get theta/2 by:
+  //    theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
+  // but we can't get sin(theta/2) directly, only its absolute value, i.e.:
+  //    |v| = |sin(theta/2)| * |u|
+  //        = |sin(theta/2)|
+  // using this value will have a strange effect. recall that there are two
+  // quaternion representations of a given rotation, q and -q. typically as
+  // a body rotates along the axis it will go through a complete cycle using
+  // one representation and then the next cycle will use the other
+  // representation. this corresponds to u pointing in the direction of the
+  // hinge axis and then in the opposite direction. the result is that theta
+  // will appear to go "backwards" every other cycle. here is a fix: if u
+  // points "away" from the direction of the hinge (motor) axis (i.e. more
+  // than 90 degrees) then use -q instead of q. this represents the same
+  // rotation, but results in the cos(theta/2) value being sign inverted.
+
+  // extract the angle from the quaternion. cost2 = cos(theta/2),
+  // sint2 = |sin(theta/2)|
+  dReal cost2 = qrel[0];
+  dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]);
+  dReal theta = (dDOT(qrel+1,axis) >= 0) ?      // @@@ padding assumptions
+    (2 * dAtan2(sint2,cost2)) :         // if u points in direction of axis
+    (2 * dAtan2(sint2,-cost2));         // if u points in opposite direction
+
+  // the angle we get will be between 0..2*pi, but we want to return angles
+  // between -pi..pi
+  if (theta > M_PI) theta -= 2*M_PI;
+
+  // the angle we've just extracted has the wrong sign
+  theta = -theta;
+
+  return theta;
+}
+
+
+// given two bodies (body1,body2), the hinge axis that they are connected by
+// w.r.t. body1 (axis), and the initial relative orientation between them
+// (q_initial), return the relative rotation angle. the initial relative
+// orientation corresponds to an angle of zero. if body2 is 0 then measure the
+// angle between body1 and the static frame.
+//
+// this will not return the correct angle if the bodies rotate along any axis
+// other than the given hinge axis.
+
+static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis,
+                            dQuaternion q_initial)
+{
+  // get qrel = relative rotation between the two bodies
+  dQuaternion qrel;
+  if (body2) {
+    dQuaternion qq;
+    dQMultiply1 (qq,body1->q,body2->q);
+    dQMultiply2 (qrel,qq,q_initial);
+  }
+  else {
+    // pretend body2->q is the identity
+    dQMultiply3 (qrel,body1->q,q_initial);
+  }
+
+  return getHingeAngleFromRelativeQuat (qrel,axis);
+}
+
+//****************************************************************************
+// dxJointLimitMotor
+
+void dxJointLimitMotor::init (dxWorld *world)
+{
+  vel = 0;
+  fmax = 0;
+  lostop = -dInfinity;
+  histop = dInfinity;
+  fudge_factor = 1;
+  normal_cfm = world->global_cfm;
+  stop_erp = world->global_erp;
+  stop_cfm = world->global_cfm;
+  bounce = 0;
+  limit = 0;
+  limit_err = 0;
+}
+
+
+void dxJointLimitMotor::set (int num, dReal value)
+{
+  switch (num) {
+  case dParamLoStop:
+    if (value <= histop) lostop = value;
+    break;
+  case dParamHiStop:
+    if (value >= lostop) histop = value;
+    break;
+  case dParamVel:
+    vel = value;
+    break;
+  case dParamFMax:
+    if (value >= 0) fmax = value;
+    break;
+  case dParamFudgeFactor:
+    if (value >= 0 && value <= 1) fudge_factor = value;
+    break;
+  case dParamBounce:
+    bounce = value;
+    break;
+  case dParamCFM:
+    normal_cfm = value;
+    break;
+  case dParamStopERP:
+    stop_erp = value;
+    break;
+  case dParamStopCFM:
+    stop_cfm = value;
+    break;
+  }
+}
+
+
+dReal dxJointLimitMotor::get (int num)
+{
+  switch (num) {
+  case dParamLoStop: return lostop;
+  case dParamHiStop: return histop;
+  case dParamVel: return vel;
+  case dParamFMax: return fmax;
+  case dParamFudgeFactor: return fudge_factor;
+  case dParamBounce: return bounce;
+  case dParamCFM: return normal_cfm;
+  case dParamStopERP: return stop_erp;
+  case dParamStopCFM: return stop_cfm;
+  default: return 0;
+  }
+}
+
+
+int dxJointLimitMotor::testRotationalLimit (dReal angle)
+{
+  if (angle <= lostop) {
+    limit = 1;
+    limit_err = angle - lostop;
+    return 1;
+  }
+  else if (angle >= histop) {
+    limit = 2;
+    limit_err = angle - histop;
+    return 1;
+  }
+  else {
+    limit = 0;
+    return 0;
+  }
+}
+
+
+int dxJointLimitMotor::addLimot (dxJoint *joint,
+                                 dxJoint::Info2 *info, int row,
+                                 dVector3 ax1, int rotational)
+{
+  int srow = row * info->rowskip;
+
+  // if the joint is powered, or has joint limits, add in the extra row
+  int powered = fmax > 0;
+  if (powered || limit) {
+    dReal *J1 = rotational ? info->J1a : info->J1l;
+    dReal *J2 = rotational ? info->J2a : info->J2l;
+
+    J1[srow+0] = ax1[0];
+    J1[srow+1] = ax1[1];
+    J1[srow+2] = ax1[2];
+    if (joint->node[1].body) {
+      J2[srow+0] = -ax1[0];
+      J2[srow+1] = -ax1[1];
+      J2[srow+2] = -ax1[2];
+    }
+
+    // linear limot torque decoupling step:
+    //
+    // if this is a linear limot (e.g. from a slider), we have to be careful
+    // that the linear constraint forces (+/- ax1) applied to the two bodies
+    // do not create a torque couple. in other words, the points that the
+    // constraint force is applied at must lie along the same ax1 axis.
+    // a torque couple will result in powered or limited slider-jointed free
+    // bodies from gaining angular momentum.
+    // the solution used here is to apply the constraint forces at the point
+    // halfway between the body centers. there is no penalty (other than an
+    // extra tiny bit of computation) in doing this adjustment. note that we
+    // only need to do this if the constraint connects two bodies.
+
+    dVector3 ltd;       // Linear Torque Decoupling vector (a torque)
+    if (!rotational && joint->node[1].body) {
+      dVector3 c;
+      c[0]=REAL(0.5)*(joint->node[1].body->pos[0]-joint->node[0].body->pos[0]);
+      c[1]=REAL(0.5)*(joint->node[1].body->pos[1]-joint->node[0].body->pos[1]);
+      c[2]=REAL(0.5)*(joint->node[1].body->pos[2]-joint->node[0].body->pos[2]);
+      dCROSS (ltd,=,c,ax1);
+      info->J1a[srow+0] = ltd[0];
+      info->J1a[srow+1] = ltd[1];
+      info->J1a[srow+2] = ltd[2];
+      info->J2a[srow+0] = ltd[0];
+      info->J2a[srow+1] = ltd[1];
+      info->J2a[srow+2] = ltd[2];
+    }
+
+    // if we're limited low and high simultaneously, the joint motor is
+    // ineffective
+    if (limit && (lostop == histop)) powered = 0;
+
+    if (powered) {
+      info->cfm[row] = normal_cfm;
+      if (! limit) {
+        info->c[row] = vel;
+        info->lo[row] = -fmax;
+        info->hi[row] = fmax;
+      }
+      else {
+        // the joint is at a limit, AND is being powered. if the joint is
+        // being powered into the limit then we apply the maximum motor force
+        // in that direction, because the motor is working against the
+        // immovable limit. if the joint is being powered away from the limit
+        // then we have problems because actually we need *two* lcp
+        // constraints to handle this case. so we fake it and apply some
+        // fraction of the maximum force. the fraction to use can be set as
+        // a fudge factor.
+
+        dReal fm = fmax;
+        if (vel > 0) fm = -fm;
+
+        // if we're powering away from the limit, apply the fudge factor
+        if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= fudge_factor;
+
+        if (rotational) {
+          dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],
+                          -fm*ax1[2]);
+          if (joint->node[1].body)
+            dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
+        }
+        else {
+          dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]);
+          if (joint->node[1].body) {
+            dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
+
+            // linear limot torque decoupling step: refer to above discussion
+            dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1],
+                            -fm*ltd[2]);
+            dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1],
+                            -fm*ltd[2]);
+          }
+        }
+      }
+    }
+
+    if (limit) {
+      dReal k = info->fps * stop_erp;
+      info->c[row] = -k * limit_err;
+      info->cfm[row] = stop_cfm;
+
+      if (lostop == histop) {
+        // limited low and high simultaneously
+        info->lo[row] = -dInfinity;
+        info->hi[row] = dInfinity;
+      }
+      else {
+        if (limit == 1) {
+          // low limit
+          info->lo[row] = 0;
+          info->hi[row] = dInfinity;
+        }
+        else {
+          // high limit
+          info->lo[row] = -dInfinity;
+          info->hi[row] = 0;
+        }
+
+        // deal with bounce
+        if (bounce > 0) {
+          // calculate joint velocity
+          dReal vel;
+          if (rotational) {
+            vel = dDOT(joint->node[0].body->avel,ax1);
+            if (joint->node[1].body)
+              vel -= dDOT(joint->node[1].body->avel,ax1);
+          }
+          else {
+            vel = dDOT(joint->node[0].body->lvel,ax1);
+            if (joint->node[1].body)
+              vel -= dDOT(joint->node[1].body->lvel,ax1);
+          }
+
+          // only apply bounce if the velocity is incoming, and if the
+          // resulting c[] exceeds what we already have.
+          if (limit == 1) {
+            // low limit
+            if (vel < 0) {
+              dReal newc = -bounce * vel;
+              if (newc > info->c[row]) info->c[row] = newc;
+            }
+          }
+          else {
+            // high limit - all those computations are reversed
+            if (vel > 0) {
+              dReal newc = -bounce * vel;
+              if (newc < info->c[row]) info->c[row] = newc;
+            }
+          }
+        }
+      }
+    }
+    return 1;
+  }
+  else return 0;
+}
+
+//****************************************************************************
+// ball and socket
+
+static void ballInit (dxJointBall *j)
+{
+  dSetZero (j->anchor1,4);
+  dSetZero (j->anchor2,4);
+}
+
+
+static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info)
+{
+  info->m = 3;
+  info->nub = 3;
+}
+
+
+static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info)
+{
+  setBall (joint,info,joint->anchor1,joint->anchor2);
+}
+
+
+extern "C" void dJointSetBallAnchor (dxJointBall *joint,
+                                     dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
+  setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
+}
+
+
+extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
+  if (joint->flags & dJOINT_REVERSE)
+    getAnchor2 (joint,result,joint->anchor2);
+  else
+    getAnchor (joint,result,joint->anchor1);
+}
+
+
+extern "C" void dJointGetBallAnchor2 (dxJointBall *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball");
+  if (joint->flags & dJOINT_REVERSE)
+    getAnchor (joint,result,joint->anchor1);
+  else
+    getAnchor2 (joint,result,joint->anchor2);
+}
+
+
+dxJoint::Vtable __dball_vtable = {
+  sizeof(dxJointBall),
+  (dxJoint::init_fn*) ballInit,
+  (dxJoint::getInfo1_fn*) ballGetInfo1,
+  (dxJoint::getInfo2_fn*) ballGetInfo2,
+  dJointTypeBall};
+
+//****************************************************************************
+// hinge
+
+static void hingeInit (dxJointHinge *j)
+{
+  dSetZero (j->anchor1,4);
+  dSetZero (j->anchor2,4);
+  dSetZero (j->axis1,4);
+  j->axis1[0] = 1;
+  dSetZero (j->axis2,4);
+  j->axis2[0] = 1;
+  dSetZero (j->qrel,4);
+  j->limot.init (j->world);
+}
+
+
+static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info)
+{
+  info->nub = 5;
+
+  // see if joint is powered
+  if (j->limot.fmax > 0)
+    info->m = 6;        // powered hinge needs an extra constraint row
+  else info->m = 5;
+
+  // see if we're at a joint limit.
+  if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) &&
+       j->limot.lostop <= j->limot.histop) {
+    dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1,
+                                 j->qrel);
+    if (j->limot.testRotationalLimit (angle)) info->m = 6;
+  }
+}
+
+
+static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info)
+{
+  // set the three ball-and-socket rows
+  setBall (joint,info,joint->anchor1,joint->anchor2);
+
+  // set the two hinge rows. the hinge axis should be the only unconstrained
+  // rotational axis, the angular velocity of the two bodies perpendicular to
+  // the hinge axis should be equal. thus the constraint equations are
+  //    p*w1 - p*w2 = 0
+  //    q*w1 - q*w2 = 0
+  // where p and q are unit vectors normal to the hinge axis, and w1 and w2
+  // are the angular velocity vectors of the two bodies.
+
+  dVector3 ax1;  // length 1 joint axis in global coordinates, from 1st body
+  dVector3 p,q;  // plane space vectors for ax1
+  dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
+  dPlaneSpace (ax1,p,q);
+
+  int s3=3*info->rowskip;
+  int s4=4*info->rowskip;
+
+  info->J1a[s3+0] = p[0];
+  info->J1a[s3+1] = p[1];
+  info->J1a[s3+2] = p[2];
+  info->J1a[s4+0] = q[0];
+  info->J1a[s4+1] = q[1];
+  info->J1a[s4+2] = q[2];
+
+  if (joint->node[1].body) {
+    info->J2a[s3+0] = -p[0];
+    info->J2a[s3+1] = -p[1];
+    info->J2a[s3+2] = -p[2];
+    info->J2a[s4+0] = -q[0];
+    info->J2a[s4+1] = -q[1];
+    info->J2a[s4+2] = -q[2];
+  }
+
+  // compute the right hand side of the constraint equation. set relative
+  // body velocities along p and q to bring the hinge back into alignment.
+  // if ax1,ax2 are the unit length hinge axes as computed from body1 and
+  // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+  // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+  // along u to cover angle erp*theta in one step :
+  //   |angular_velocity| = angle/time = erp*theta / stepsize
+  //                      = (erp*fps) * theta
+  //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+  //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+  // ...as ax1 and ax2 are unit length. if theta is smallish,
+  // theta ~= sin(theta), so
+  //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+  // ax1 x ax2 is in the plane space of ax1, so we project the angular
+  // velocity to p and q to find the right hand side.
+
+  dVector3 ax2,b;
+  if (joint->node[1].body) {
+    dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
+  }
+  else {
+    ax2[0] = joint->axis2[0];
+    ax2[1] = joint->axis2[1];
+    ax2[2] = joint->axis2[2];
+  }
+  dCROSS (b,=,ax1,ax2);
+  dReal k = info->fps * info->erp;
+  info->c[3] = k * dDOT(b,p);
+  info->c[4] = k * dDOT(b,q);
+
+  // if the hinge is powered, or has joint limits, add in the stuff
+  joint->limot.addLimot (joint,info,5,ax1,1);
+}
+
+
+// compute initial relative rotation body1 -> body2, or env -> body1
+
+static void hingeComputeInitialRelativeRotation (dxJointHinge *joint)
+{
+  if (joint->node[0].body) {
+    if (joint->node[1].body) {
+      dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
+    }
+    else {
+      // set joint->qrel to the transpose of the first body q
+      joint->qrel[0] = joint->node[0].body->q[0];
+      for (int i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
+    }
+  }
+}
+
+
+extern "C" void dJointSetHingeAnchor (dxJointHinge *joint,
+                                      dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
+  setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
+  hingeComputeInitialRelativeRotation (joint);
+}
+
+
+extern "C" void dJointSetHingeAxis (dxJointHinge *joint,
+                                    dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
+  setAxes (joint,x,y,z,joint->axis1,joint->axis2);
+  hingeComputeInitialRelativeRotation (joint);
+}
+
+
+extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
+  if (joint->flags & dJOINT_REVERSE)
+    getAnchor2 (joint,result,joint->anchor2);
+  else
+    getAnchor (joint,result,joint->anchor1);
+}
+
+
+extern "C" void dJointGetHingeAnchor2 (dxJointHinge *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
+  if (joint->flags & dJOINT_REVERSE)
+    getAnchor (joint,result,joint->anchor1);
+  else
+    getAnchor2 (joint,result,joint->anchor2);
+}
+
+
+extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
+  getAxis (joint,result,joint->axis1);
+}
+
+
+extern "C" void dJointSetHingeParam (dxJointHinge *joint,
+                                     int parameter, dReal value)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
+  joint->limot.set (parameter,value);
+}
+
+
+extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int parameter)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
+  return joint->limot.get (parameter);
+}
+
+
+extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint)
+{
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge");
+  if (joint->node[0].body) {
+    dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1,
+                          joint->qrel);
+        if (joint->flags & dJOINT_REVERSE)
+           return -ang;
+        else
+           return ang;
+  }
+  else return 0;
+}
+
+
+extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint)
+{
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge");
+  if (joint->node[0].body) {
+    dVector3 axis;
+    dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
+    dReal rate = dDOT(axis,joint->node[0].body->avel);
+    if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
+    if (joint->flags & dJOINT_REVERSE) rate = - rate;
+    return rate;
+  }
+  else return 0;
+}
+
+
+extern "C" void dJointAddHingeTorque (dxJointHinge *joint, dReal torque)
+{
+  dVector3 axis;
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge");
+
+  if (joint->flags & dJOINT_REVERSE)
+    torque = -torque;
+
+  getAxis (joint,axis,joint->axis1);
+  axis[0] *= torque;
+  axis[1] *= torque;
+  axis[2] *= torque;
+
+  if (joint->node[0].body != 0)
+    dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]);
+  if (joint->node[1].body != 0)
+    dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]);
+}
+
+
+dxJoint::Vtable __dhinge_vtable = {
+  sizeof(dxJointHinge),
+  (dxJoint::init_fn*) hingeInit,
+  (dxJoint::getInfo1_fn*) hingeGetInfo1,
+  (dxJoint::getInfo2_fn*) hingeGetInfo2,
+  dJointTypeHinge};
+
+//****************************************************************************
+// slider
+
+static void sliderInit (dxJointSlider *j)
+{
+  dSetZero (j->axis1,4);
+  j->axis1[0] = 1;
+  dSetZero (j->qrel,4);
+  dSetZero (j->offset,4);
+  j->limot.init (j->world);
+}
+
+
+extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
+
+  // get axis1 in global coordinates
+  dVector3 ax1,q;
+  dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
+
+  if (joint->node[1].body) {
+    // get body2 + offset point in global coordinates
+    dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset);
+    for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] -
+                              joint->node[1].body->pos[i];
+  }
+  else {
+    for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] -
+                              joint->offset[i];
+
+  }
+  return dDOT(ax1,q);
+}
+
+
+extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
+
+  // get axis1 in global coordinates
+  dVector3 ax1;
+  dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
+
+  if (joint->node[1].body) {
+    return dDOT(ax1,joint->node[0].body->lvel) -
+      dDOT(ax1,joint->node[1].body->lvel);
+  }
+  else {
+    return dDOT(ax1,joint->node[0].body->lvel);
+  }
+}
+
+
+static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info)
+{
+  info->nub = 5;
+
+  // see if joint is powered
+  if (j->limot.fmax > 0)
+    info->m = 6;        // powered slider needs an extra constraint row
+  else info->m = 5;
+
+  // see if we're at a joint limit.
+  j->limot.limit = 0;
+  if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) &&
+      j->limot.lostop <= j->limot.histop) {
+    // measure joint position
+    dReal pos = dJointGetSliderPosition (j);
+    if (pos <= j->limot.lostop) {
+      j->limot.limit = 1;
+      j->limot.limit_err = pos - j->limot.lostop;
+      info->m = 6;
+    }
+    else if (pos >= j->limot.histop) {
+      j->limot.limit = 2;
+      j->limot.limit_err = pos - j->limot.histop;
+      info->m = 6;
+    }
+  }
+}
+
+
+static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info)
+{
+  int i,s = info->rowskip;
+  int s2=2*s,s3=3*s,s4=4*s;
+
+  // pull out pos and R for both bodies. also get the `connection'
+  // vector pos2-pos1.
+
+  dReal *pos1,*pos2,*R1,*R2;
+  dVector3 c;
+  pos1 = joint->node[0].body->pos;
+  R1 = joint->node[0].body->R;
+  if (joint->node[1].body) {
+    pos2 = joint->node[1].body->pos;
+    R2 = joint->node[1].body->R;
+    for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i];
+  }
+  else {
+    pos2 = 0;
+    R2 = 0;
+  }
+
+  // 3 rows to make body rotations equal
+  setFixedOrientation(joint, info, joint->qrel, 0);
+
+  // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
+  // result in three equations, so we project along the planespace vectors
+  // so that sliding along the slider axis is disregarded. for symmetry we
+  // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
+
+  dVector3 ax1; // joint axis in global coordinates (unit length)
+  dVector3 p,q; // plane space of ax1
+  dMULTIPLY0_331 (ax1,R1,joint->axis1);
+  dPlaneSpace (ax1,p,q);
+  if (joint->node[1].body) {
+    dVector3 tmp;
+    dCROSS (tmp, = REAL(0.5) * ,c,p);
+    for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
+    for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
+    dCROSS (tmp, = REAL(0.5) * ,c,q);
+    for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i];
+    for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i];
+    for (i=0; i<3; i++) info->J2l[s3+i] = -p[i];
+    for (i=0; i<3; i++) info->J2l[s4+i] = -q[i];
+  }
+  for (i=0; i<3; i++) info->J1l[s3+i] = p[i];
+  for (i=0; i<3; i++) info->J1l[s4+i] = q[i];
+
+  // compute last two elements of right hand side. we want to align the offset
+  // point (in body 2's frame) with the center of body 1.
+  dReal k = info->fps * info->erp;
+  if (joint->node[1].body) {
+    dVector3 ofs;               // offset point in global coordinates
+    dMULTIPLY0_331 (ofs,R2,joint->offset);
+    for (i=0; i<3; i++) c[i] += ofs[i];
+    info->c[3] = k * dDOT(p,c);
+    info->c[4] = k * dDOT(q,c);
+  }
+  else {
+    dVector3 ofs;               // offset point in global coordinates
+    for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i];
+    info->c[3] = k * dDOT(p,ofs);
+    info->c[4] = k * dDOT(q,ofs);
+  }
+
+  // if the slider is powered, or has joint limits, add in the extra row
+  joint->limot.addLimot (joint,info,5,ax1,0);
+}
+
+
+extern "C" void dJointSetSliderAxis (dxJointSlider *joint,
+                                     dReal x, dReal y, dReal z)
+{
+  int i;
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
+  setAxes (joint,x,y,z,joint->axis1,0);
+
+  // compute initial relative rotation body1 -> body2, or env -> body1
+  // also compute center of body1 w.r.t body 2
+  if (joint->node[1].body) {
+    dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
+    dVector3 c;
+    for (i=0; i<3; i++)
+      c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i];
+    dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c);
+  }
+  else {
+    // set joint->qrel to the transpose of the first body's q
+    joint->qrel[0] = joint->node[0].body->q[0];
+    for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
+    for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i];
+  }
+}
+
+
+extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
+  getAxis (joint,result,joint->axis1);
+}
+
+
+extern "C" void dJointSetSliderParam (dxJointSlider *joint,
+                                      int parameter, dReal value)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
+  joint->limot.set (parameter,value);
+}
+
+
+extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int parameter)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
+  return joint->limot.get (parameter);
+}
+
+
+extern "C" void dJointAddSliderForce (dxJointSlider *joint, dReal force)
+{
+  dVector3 axis;
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider");
+
+  if (joint->flags & dJOINT_REVERSE)
+    force -= force;
+
+  getAxis (joint,axis,joint->axis1);
+  axis[0] *= force;
+  axis[1] *= force;
+  axis[2] *= force;
+
+  if (joint->node[0].body != 0)
+    dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]);
+  if (joint->node[1].body != 0)
+    dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]);
+}
+
+
+dxJoint::Vtable __dslider_vtable = {
+  sizeof(dxJointSlider),
+  (dxJoint::init_fn*) sliderInit,
+  (dxJoint::getInfo1_fn*) sliderGetInfo1,
+  (dxJoint::getInfo2_fn*) sliderGetInfo2,
+  dJointTypeSlider};
+
+//****************************************************************************
+// contact
+
+static void contactInit (dxJointContact *j)
+{
+  // default frictionless contact. hmmm, this info gets overwritten straight
+  // away anyway, so why bother?
+#if 0 /* so don't bother ;) */
+  j->contact.surface.mode = 0;
+  j->contact.surface.mu = 0;
+  dSetZero (j->contact.geom.pos,4);
+  dSetZero (j->contact.geom.normal,4);
+  j->contact.geom.depth = 0;
+#endif
+}
+
+
+static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info)
+{
+  // make sure mu's >= 0, then calculate number of constraint rows and number
+  // of unbounded rows.
+  int m = 1, nub=0;
+  if (j->contact.surface.mu < 0) j->contact.surface.mu = 0;
+  if (j->contact.surface.mode & dContactMu2) {
+    if (j->contact.surface.mu > 0) m++;
+    if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0;
+    if (j->contact.surface.mu2 > 0) m++;
+    if (j->contact.surface.mu  == dInfinity) nub ++;
+    if (j->contact.surface.mu2 == dInfinity) nub ++;
+  }
+  else {
+    if (j->contact.surface.mu > 0) m += 2;
+    if (j->contact.surface.mu == dInfinity) nub += 2;
+  }
+
+  j->the_m = m;
+  info->m = m;
+  info->nub = nub;
+}
+
+
+static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info)
+{
+  int i,s = info->rowskip;
+  int s2 = 2*s;
+
+  // get normal, with sign adjusted for body1/body2 polarity
+  dVector3 normal;
+  if (j->flags & dJOINT_REVERSE) {
+    normal[0] = - j->contact.geom.normal[0];
+    normal[1] = - j->contact.geom.normal[1];
+    normal[2] = - j->contact.geom.normal[2];
+  }
+  else {
+    normal[0] = j->contact.geom.normal[0];
+    normal[1] = j->contact.geom.normal[1];
+    normal[2] = j->contact.geom.normal[2];
+  }
+  normal[3] = 0;        // @@@ hmmm
+
+  // c1,c2 = contact points with respect to body PORs
+  dVector3 c1,c2;
+  for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i];
+
+  // set jacobian for normal
+  info->J1l[0] = normal[0];
+  info->J1l[1] = normal[1];
+  info->J1l[2] = normal[2];
+  dCROSS (info->J1a,=,c1,normal);
+  if (j->node[1].body) {
+    for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
+                          j->node[1].body->pos[i];
+    info->J2l[0] = -normal[0];
+    info->J2l[1] = -normal[1];
+    info->J2l[2] = -normal[2];
+    dCROSS (info->J2a,= -,c2,normal);
+  }
+
+  // set right hand side and cfm value for normal
+  dReal erp = info->erp;
+  if (j->contact.surface.mode & dContactSoftERP)
+    erp = j->contact.surface.soft_erp;
+  dReal k = info->fps * erp;
+  info->c[0] = k*j->contact.geom.depth;
+  if (j->contact.surface.mode & dContactSoftCFM)
+    info->cfm[0] = j->contact.surface.soft_cfm;
+
+  // deal with bounce
+  if (j->contact.surface.mode & dContactBounce) {
+    // calculate outgoing velocity (-ve for incoming contact)
+    dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) +
+      dDOT(info->J1a,j->node[0].body->avel);
+    if (j->node[1].body) {
+      outgoing += dDOT(info->J2l,j->node[1].body->lvel) +
+        dDOT(info->J2a,j->node[1].body->avel);
+    }
+    // only apply bounce if the outgoing velocity is greater than the
+    // threshold, and if the resulting c[0] exceeds what we already have.
+    if (j->contact.surface.bounce_vel >= 0 &&
+        (-outgoing) > j->contact.surface.bounce_vel) {
+      dReal newc = - j->contact.surface.bounce * outgoing;
+      if (newc > info->c[0]) info->c[0] = newc;
+    }
+  }
+
+  // set LCP limits for normal
+  info->lo[0] = 0;
+  info->hi[0] = dInfinity;
+
+  // now do jacobian for tangential forces
+  dVector3 t1,t2;       // two vectors tangential to normal
+
+  // first friction direction
+  if (j->the_m >= 2) {
+    if (j->contact.surface.mode & dContactFDir1) {      // use fdir1 ?
+      t1[0] = j->contact.fdir1[0];
+      t1[1] = j->contact.fdir1[1];
+      t1[2] = j->contact.fdir1[2];
+      dCROSS (t2,=,normal,t1);
+    }
+    else {
+      dPlaneSpace (normal,t1,t2);
+    }
+    info->J1l[s+0] = t1[0];
+    info->J1l[s+1] = t1[1];
+    info->J1l[s+2] = t1[2];
+    dCROSS (info->J1a+s,=,c1,t1);
+    if (j->node[1].body) {
+      info->J2l[s+0] = -t1[0];
+      info->J2l[s+1] = -t1[1];
+      info->J2l[s+2] = -t1[2];
+      dCROSS (info->J2a+s,= -,c2,t1);
+    }
+    // set right hand side
+    if (j->contact.surface.mode & dContactMotion1) {
+      info->c[1] = j->contact.surface.motion1;
+    }
+    // set LCP bounds and friction index. this depends on the approximation
+    // mode
+    info->lo[1] = -j->contact.surface.mu;
+    info->hi[1] = j->contact.surface.mu;
+    if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0;
+
+    // set slip (constraint force mixing)
+    if (j->contact.surface.mode & dContactSlip1)
+      info->cfm[1] = j->contact.surface.slip1;
+  }
+
+  // second friction direction
+  if (j->the_m >= 3) {
+    info->J1l[s2+0] = t2[0];
+    info->J1l[s2+1] = t2[1];
+    info->J1l[s2+2] = t2[2];
+    dCROSS (info->J1a+s2,=,c1,t2);
+    if (j->node[1].body) {
+      info->J2l[s2+0] = -t2[0];
+      info->J2l[s2+1] = -t2[1];
+      info->J2l[s2+2] = -t2[2];
+      dCROSS (info->J2a+s2,= -,c2,t2);
+    }
+    // set right hand side
+    if (j->contact.surface.mode & dContactMotion2) {
+      info->c[2] = j->contact.surface.motion2;
+    }
+    // set LCP bounds and friction index. this depends on the approximation
+    // mode
+    if (j->contact.surface.mode & dContactMu2) {
+      info->lo[2] = -j->contact.surface.mu2;
+      info->hi[2] = j->contact.surface.mu2;
+    }
+    else {
+      info->lo[2] = -j->contact.surface.mu;
+      info->hi[2] = j->contact.surface.mu;
+    }
+    if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0;
+
+    // set slip (constraint force mixing)
+    if (j->contact.surface.mode & dContactSlip2)
+      info->cfm[2] = j->contact.surface.slip2;
+  }
+}
+
+
+dxJoint::Vtable __dcontact_vtable = {
+  sizeof(dxJointContact),
+  (dxJoint::init_fn*) contactInit,
+  (dxJoint::getInfo1_fn*) contactGetInfo1,
+  (dxJoint::getInfo2_fn*) contactGetInfo2,
+  dJointTypeContact};
+
+//****************************************************************************
+// hinge 2. note that this joint must be attached to two bodies for it to work
+
+static dReal measureHinge2Angle (dxJointHinge2 *joint)
+{
+  dVector3 a1,a2;
+  dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2);
+  dMULTIPLY1_331 (a2,joint->node[0].body->R,a1);
+  dReal x = dDOT(joint->v1,a2);
+  dReal y = dDOT(joint->v2,a2);
+  return -dAtan2 (y,x);
+}
+
+
+static void hinge2Init (dxJointHinge2 *j)
+{
+  dSetZero (j->anchor1,4);
+  dSetZero (j->anchor2,4);
+  dSetZero (j->axis1,4);
+  j->axis1[0] = 1;
+  dSetZero (j->axis2,4);
+  j->axis2[1] = 1;
+  j->c0 = 0;
+  j->s0 = 0;
+
+  dSetZero (j->v1,4);
+  j->v1[0] = 1;
+  dSetZero (j->v2,4);
+  j->v2[1] = 1;
+
+  j->limot1.init (j->world);
+  j->limot2.init (j->world);
+
+  j->susp_erp = j->world->global_erp;
+  j->susp_cfm = j->world->global_cfm;
+
+  j->flags |= dJOINT_TWOBODIES;
+}
+
+
+static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info)
+{
+  info->m = 4;
+  info->nub = 4;
+
+  // see if we're powered or at a joint limit for axis 1
+  int atlimit=0;
+  if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) &&
+      j->limot1.lostop <= j->limot1.histop) {
+    dReal angle = measureHinge2Angle (j);
+    if (j->limot1.testRotationalLimit (angle)) atlimit = 1;
+  }
+  if (atlimit || j->limot1.fmax > 0) info->m++;
+
+  // see if we're powering axis 2 (we currently never limit this axis)
+  j->limot2.limit = 0;
+  if (j->limot2.fmax > 0) info->m++;
+}
+
+
+// macro that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
+// relative to body 1 and 2 initially) and then computes the constrained
+// rotational axis as the cross product of ax1 and ax2.
+// the sin and cos of the angle between axis 1 and 2 is computed, this comes
+// from dot and cross product rules.
+
+#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \
+  dVector3 ax1,ax2; \
+  dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \
+  dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \
+  dCROSS (axis,=,ax1,ax2); \
+  sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \
+  cos_angle = dDOT (ax1,ax2);
+
+
+static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info)
+{
+  // get information we need to set the hinge row
+  dReal s,c;
+  dVector3 q;
+  HINGE2_GET_AXIS_INFO (q,s,c);
+  dNormalize3 (q);              // @@@ quicker: divide q by s ?
+
+  // set the three ball-and-socket rows (aligned to the suspension axis ax1)
+  setBall2 (joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp);
+
+  // set the hinge row
+  int s3=3*info->rowskip;
+  info->J1a[s3+0] = q[0];
+  info->J1a[s3+1] = q[1];
+  info->J1a[s3+2] = q[2];
+  if (joint->node[1].body) {
+    info->J2a[s3+0] = -q[0];
+    info->J2a[s3+1] = -q[1];
+    info->J2a[s3+2] = -q[2];
+  }
+
+  // compute the right hand side for the constrained rotational DOF.
+  // axis 1 and axis 2 are separated by an angle `theta'. the desired
+  // separation angle is theta0. sin(theta0) and cos(theta0) are recorded
+  // in the joint structure. the correcting angular velocity is:
+  //   |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
+  //                      = (erp*fps) * (theta0-theta)
+  // (theta0-theta) can be computed using the following small-angle-difference
+  // approximation:
+  //   theta0-theta ~= tan(theta0-theta)
+  //                 = sin(theta0-theta)/cos(theta0-theta)
+  //                 = (c*s0 - s*c0) / (c*c0 + s*s0)
+  //                 = c*s0 - s*c0         assuming c*c0 + s*s0 ~= 1
+  // where c = cos(theta), s = sin(theta)
+  //       c0 = cos(theta0), s0 = sin(theta0)
+
+  dReal k = info->fps * info->erp;
+  info->c[3] = k * (joint->c0 * s - joint->s0 * c);
+
+  // if the axis1 hinge is powered, or has joint limits, add in more stuff
+  int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1);
+
+  // if the axis2 hinge is powered, add in more stuff
+  joint->limot2.addLimot (joint,info,row,ax2,1);
+
+  // set parameter for the suspension
+  info->cfm[0] = joint->susp_cfm;
+}
+
+
+// compute vectors v1 and v2 (embedded in body1), used to measure angle
+// between body 1 and body 2
+
+static void makeHinge2V1andV2 (dxJointHinge2 *joint)
+{
+  if (joint->node[0].body) {
+    // get axis 1 and 2 in global coords
+    dVector3 ax1,ax2,v;
+    dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
+    dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
+
+    // don't do anything if the axis1 or axis2 vectors are zero or the same
+    if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) ||
+        (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) ||
+        (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return;
+
+    // modify axis 2 so it's perpendicular to axis 1
+    dReal k = dDOT(ax1,ax2);
+    for (int i=0; i<3; i++) ax2[i] -= k*ax1[i];
+    dNormalize3 (ax2);
+
+    // make v1 = modified axis2, v2 = axis1 x (modified axis2)
+    dCROSS (v,=,ax1,ax2);
+    dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2);
+    dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v);
+  }
+}
+
+
+extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint,
+                                       dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
+  makeHinge2V1andV2 (joint);
+}
+
+
+extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint,
+                                      dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->node[0].body) {
+    dReal q[4];
+    q[0] = x;
+    q[1] = y;
+    q[2] = z;
+    q[3] = 0;
+    dNormalize3 (q);
+    dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
+    joint->axis1[3] = 0;
+
+    // compute the sin and cos of the angle between axis 1 and axis 2
+    dVector3 ax;
+    HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
+  }
+  makeHinge2V1andV2 (joint);
+}
+
+
+extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint,
+                                      dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->node[1].body) {
+    dReal q[4];
+    q[0] = x;
+    q[1] = y;
+    q[2] = z;
+    q[3] = 0;
+    dNormalize3 (q);
+    dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
+    joint->axis1[3] = 0;
+
+    // compute the sin and cos of the angle between axis 1 and axis 2
+    dVector3 ax;
+    HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
+  }
+  makeHinge2V1andV2 (joint);
+}
+
+
+extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint,
+                                      int parameter, dReal value)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if ((parameter & 0xff00) == 0x100) {
+    joint->limot2.set (parameter & 0xff,value);
+  }
+  else {
+    if (parameter == dParamSuspensionERP) joint->susp_erp = value;
+    else if (parameter == dParamSuspensionCFM) joint->susp_cfm = value;
+    else joint->limot1.set (parameter,value);
+  }
+}
+
+
+extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->flags & dJOINT_REVERSE)
+    getAnchor2 (joint,result,joint->anchor2);
+  else
+    getAnchor (joint,result,joint->anchor1);
+}
+
+
+extern "C" void dJointGetHinge2Anchor2 (dxJointHinge2 *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->flags & dJOINT_REVERSE)
+    getAnchor (joint,result,joint->anchor1);
+  else
+    getAnchor2 (joint,result,joint->anchor2);
+}
+
+
+extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->node[0].body) {
+    dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1);
+  }
+}
+
+
+extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->node[1].body) {
+    dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2);
+  }
+}
+
+
+extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int parameter)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if ((parameter & 0xff00) == 0x100) {
+    return joint->limot2.get (parameter & 0xff);
+  }
+  else {
+    if (parameter == dParamSuspensionERP) return joint->susp_erp;
+    else if (parameter == dParamSuspensionCFM) return joint->susp_cfm;
+    else return joint->limot1.get (parameter);
+  }
+}
+
+
+extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->node[0].body) return measureHinge2Angle (joint);
+  else return 0;
+}
+
+
+extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->node[0].body) {
+    dVector3 axis;
+    dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
+    dReal rate = dDOT(axis,joint->node[0].body->avel);
+    if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
+    return rate;
+  }
+  else return 0;
+}
+
+
+extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+  if (joint->node[0].body && joint->node[1].body) {
+    dVector3 axis;
+    dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2);
+    dReal rate = dDOT(axis,joint->node[0].body->avel);
+    if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
+    return rate;
+  }
+  else return 0;
+}
+
+
+extern "C" void dJointAddHinge2Torques (dxJointHinge2 *joint, dReal torque1, dReal torque2)
+{
+  dVector3 axis1, axis2;
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2");
+
+  if (joint->node[0].body && joint->node[1].body) {
+    dMULTIPLY0_331 (axis1,joint->node[0].body->R,joint->axis1);
+    dMULTIPLY0_331 (axis2,joint->node[1].body->R,joint->axis2);
+    axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
+    axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
+    axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
+    dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]);
+    dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]);
+  }
+}
+
+
+dxJoint::Vtable __dhinge2_vtable = {
+  sizeof(dxJointHinge2),
+  (dxJoint::init_fn*) hinge2Init,
+  (dxJoint::getInfo1_fn*) hinge2GetInfo1,
+  (dxJoint::getInfo2_fn*) hinge2GetInfo2,
+  dJointTypeHinge2};
+
+//****************************************************************************
+// universal
+
+// I just realized that the universal joint is equivalent to a hinge 2 joint with
+// perfectly stiff suspension.  By comparing the hinge 2 implementation to
+// the universal implementation, you may be able to improve this
+// implementation (or, less likely, the hinge2 implementation).
+
+static void universalInit (dxJointUniversal *j)
+{
+  dSetZero (j->anchor1,4);
+  dSetZero (j->anchor2,4);
+  dSetZero (j->axis1,4);
+  j->axis1[0] = 1;
+  dSetZero (j->axis2,4);
+  j->axis2[1] = 1;
+  dSetZero(j->qrel1,4);
+  dSetZero(j->qrel2,4);
+  j->limot1.init (j->world);
+  j->limot2.init (j->world);
+}
+
+
+static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2)
+{
+  // This says "ax1 = joint->node[0].body->R * joint->axis1"
+  dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
+
+  if (joint->node[1].body) {
+    dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
+  }
+  else {
+    ax2[0] = joint->axis2[0];
+    ax2[1] = joint->axis2[1];
+    ax2[2] = joint->axis2[2];
+  }
+}
+
+
+static dReal getUniversalAngle1(dxJointUniversal *joint)
+{
+  if (joint->node[0].body) {
+    // length 1 joint axis in global coordinates, from each body
+    dVector3 ax1, ax2;
+    dMatrix3 R;
+    dQuaternion qcross, qq, qrel;
+
+    getUniversalAxes (joint,ax1,ax2);
+
+    // It should be possible to get both angles without explicitly
+    // constructing the rotation matrix of the cross.  Basically,
+    // orientation of the cross about axis1 comes from body 2,
+    // about axis 2 comes from body 1, and the perpendicular
+    // axis can come from the two bodies somehow.  (We don't really
+    // want to assume it's 90 degrees, because in general the
+    // constraints won't be perfectly satisfied, or even very well
+    // satisfied.)
+    //
+    // However, we'd need a version of getHingeAngleFromRElativeQuat()
+    // that CAN handle when its relative quat is rotated along a direction
+    // other than the given axis.  What I have here works,
+    // although it's probably much slower than need be.
+
+    dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
+    dRtoQ (R,qcross);
+
+    // This code is essential the same as getHingeAngle(), see the comments
+    // there for details.
+
+    // get qrel = relative rotation between node[0] and the cross
+    dQMultiply1 (qq,joint->node[0].body->q,qcross);
+    dQMultiply2 (qrel,qq,joint->qrel1);
+
+    return getHingeAngleFromRelativeQuat(qrel, joint->axis1);
+  }
+  return 0;
+}
+
+
+static dReal getUniversalAngle2(dxJointUniversal *joint)
+{
+  if (joint->node[0].body) {
+    // length 1 joint axis in global coordinates, from each body
+    dVector3 ax1, ax2;
+    dMatrix3 R;
+    dQuaternion qcross, qq, qrel;
+
+    getUniversalAxes (joint,ax1,ax2);
+
+    // It should be possible to get both angles without explicitly
+    // constructing the rotation matrix of the cross.  Basically,
+    // orientation of the cross about axis1 comes from body 2,
+    // about axis 2 comes from body 1, and the perpendicular
+    // axis can come from the two bodies somehow.  (We don't really
+    // want to assume it's 90 degrees, because in general the
+    // constraints won't be perfectly satisfied, or even very well
+    // satisfied.)
+    //
+    // However, we'd need a version of getHingeAngleFromRElativeQuat()
+    // that CAN handle when its relative quat is rotated along a direction
+    // other than the given axis.  What I have here works,
+    // although it's probably much slower than need be.
+
+    dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
+    dRtoQ(R, qcross);
+
+    if (joint->node[1].body) {
+      dQMultiply1 (qq, joint->node[1].body->q, qcross);
+      dQMultiply2 (qrel,qq,joint->qrel2);
+    }
+    else {
+      // pretend joint->node[1].body->q is the identity
+      dQMultiply2 (qrel,qcross, joint->qrel2);
+    }
+
+    return - getHingeAngleFromRelativeQuat(qrel, joint->axis2);
+  }
+  return 0;
+}
+
+
+static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info)
+{
+  info->nub = 4;
+  info->m = 4;
+
+  // see if we're powered or at a joint limit.
+  bool constraint1 = j->limot1.fmax > 0;
+  bool constraint2 = j->limot2.fmax > 0;
+
+  bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) &&
+       j->limot1.lostop <= j->limot1.histop;
+  bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) &&
+       j->limot2.lostop <= j->limot2.histop;
+
+  // We need to call testRotationLimit() even if we're motored, since it
+  // records the result.
+  if (limiting1 || limiting2) {
+    dReal angle1, angle2;
+    angle1 = getUniversalAngle1(j);
+    angle2 = getUniversalAngle2(j);
+    if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true;
+    if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true;
+  }
+  if (constraint1)
+    info->m++;
+  if (constraint2)
+    info->m++;
+}
+
+
+static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 *info)
+{
+  // set the three ball-and-socket rows
+  setBall (joint,info,joint->anchor1,joint->anchor2);
+
+  // set the universal joint row. the angular velocity about an axis
+  // perpendicular to both joint axes should be equal. thus the constraint
+  // equation is
+  //    p*w1 - p*w2 = 0
+  // where p is a vector normal to both joint axes, and w1 and w2
+  // are the angular velocity vectors of the two bodies.
+
+  // length 1 joint axis in global coordinates, from each body
+  dVector3 ax1, ax2;
+  dVector3 ax2_temp;
+  // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
+  // about this.
+  dVector3 p;
+  dReal k;
+
+  getUniversalAxes(joint, ax1, ax2);
+  k = dDOT(ax1, ax2);
+  ax2_temp[0] = ax2[0] - k*ax1[0];
+  ax2_temp[1] = ax2[1] - k*ax1[1];
+  ax2_temp[2] = ax2[2] - k*ax1[2];
+  dCROSS(p, =, ax1, ax2_temp);
+  dNormalize3(p);
+
+  int s3=3*info->rowskip;
+
+  info->J1a[s3+0] = p[0];
+  info->J1a[s3+1] = p[1];
+  info->J1a[s3+2] = p[2];
+
+  if (joint->node[1].body) {
+    info->J2a[s3+0] = -p[0];
+    info->J2a[s3+1] = -p[1];
+    info->J2a[s3+2] = -p[2];
+  }
+
+  // compute the right hand side of the constraint equation. set relative
+  // body velocities along p to bring the axes back to perpendicular.
+  // If ax1, ax2 are unit length joint axes as computed from body1 and
+  // body2, we need to rotate both bodies along the axis p.  If theta
+  // is the angle between ax1 and ax2, we need an angular velocity
+  // along p to cover the angle erp * (theta - Pi/2) in one step:
+  //
+  //   |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
+  //                      = (erp*fps) * (theta - Pi/2)
+  //
+  // if theta is close to Pi/2,
+  // theta - Pi/2 ~= cos(theta), so
+  //    |angular_velocity|  ~= (erp*fps) * (ax1 dot ax2)
+
+  info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2);
+
+  // if the first angle is powered, or has joint limits, add in the stuff
+  int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1);
+
+  // if the second angle is powered, or has joint limits, add in more stuff
+  joint->limot2.addLimot (joint,info,row,ax2,1);
+}
+
+
+static void universalComputeInitialRelativeRotations (dxJointUniversal *joint)
+{
+  if (joint->node[0].body) {
+    dVector3 ax1, ax2;
+    dMatrix3 R;
+    dQuaternion qcross;
+
+    getUniversalAxes(joint, ax1, ax2);
+
+    // Axis 1.
+    dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]);
+    dRtoQ(R, qcross);
+    dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross);
+
+    // Axis 2.
+    dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]);
+    dRtoQ(R, qcross);
+    if (joint->node[1].body) {
+      dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross);
+    }
+    else {
+      // set joint->qrel to qcross
+      for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i];
+    }
+  }
+}
+
+
+extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint,
+                                          dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
+  universalComputeInitialRelativeRotations(joint);
+}
+
+
+extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint,
+                                         dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if (joint->flags & dJOINT_REVERSE)
+    setAxes (joint,x,y,z,NULL,joint->axis2);
+  else
+    setAxes (joint,x,y,z,joint->axis1,NULL);
+  universalComputeInitialRelativeRotations(joint);
+}
+
+
+extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint,
+                                         dReal x, dReal y, dReal z)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if (joint->flags & dJOINT_REVERSE)
+    setAxes (joint,x,y,z,joint->axis1,NULL);
+  else
+    setAxes (joint,x,y,z,NULL,joint->axis2);
+  universalComputeInitialRelativeRotations(joint);
+}
+
+
+extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint,
+                                          dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if (joint->flags & dJOINT_REVERSE)
+    getAnchor2 (joint,result,joint->anchor2);
+  else
+    getAnchor (joint,result,joint->anchor1);
+}
+
+
+extern "C" void dJointGetUniversalAnchor2 (dxJointUniversal *joint,
+                                          dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if (joint->flags & dJOINT_REVERSE)
+    getAnchor (joint,result,joint->anchor1);
+  else
+    getAnchor2 (joint,result,joint->anchor2);
+}
+
+
+extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint,
+                                         dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if (joint->flags & dJOINT_REVERSE)
+    getAxis2 (joint,result,joint->axis2);
+  else
+    getAxis (joint,result,joint->axis1);
+}
+
+
+extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint,
+                                         dVector3 result)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(result,"bad result argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if (joint->flags & dJOINT_REVERSE)
+    getAxis (joint,result,joint->axis1);
+  else
+    getAxis2 (joint,result,joint->axis2);
+}
+
+
+extern "C" void dJointSetUniversalParam (dxJointUniversal *joint,
+                                     int parameter, dReal value)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if ((parameter & 0xff00) == 0x100) {
+    joint->limot2.set (parameter & 0xff,value);
+  }
+  else {
+    joint->limot1.set (parameter,value);
+  }
+}
+
+
+extern "C" dReal dJointGetUniversalParam (dxJointUniversal *joint, int parameter)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if ((parameter & 0xff00) == 0x100) {
+    return joint->limot2.get (parameter & 0xff);
+  }
+  else {
+    return joint->limot1.get (parameter);
+  }
+}
+
+
+extern "C" dReal dJointGetUniversalAngle1 (dxJointUniversal *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if (joint->flags & dJOINT_REVERSE)
+    return getUniversalAngle2 (joint);
+  else
+    return getUniversalAngle1 (joint);
+}
+
+
+extern "C" dReal dJointGetUniversalAngle2 (dxJointUniversal *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+  if (joint->flags & dJOINT_REVERSE)
+    return getUniversalAngle1 (joint);
+  else
+    return getUniversalAngle2 (joint);
+}
+
+
+extern "C" dReal dJointGetUniversalAngle1Rate (dxJointUniversal *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+
+  if (joint->node[0].body) {
+    dVector3 axis;
+
+    if (joint->flags & dJOINT_REVERSE)
+      getAxis2 (joint,axis,joint->axis2);
+    else
+      getAxis (joint,axis,joint->axis1);
+
+    dReal rate = dDOT(axis, joint->node[0].body->avel);
+    if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel);
+    return rate;
+  }
+  return 0;
+}
+
+
+extern "C" dReal dJointGetUniversalAngle2Rate (dxJointUniversal *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+
+  if (joint->node[0].body) {
+    dVector3 axis;
+
+    if (joint->flags & dJOINT_REVERSE)
+      getAxis (joint,axis,joint->axis1);
+    else
+      getAxis2 (joint,axis,joint->axis2);
+
+    dReal rate = dDOT(axis, joint->node[0].body->avel);
+    if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel);
+    return rate;
+  }
+  return 0;
+}
+
+
+extern "C" void dJointAddUniversalTorques (dxJointUniversal *joint, dReal torque1, dReal torque2)
+{
+  dVector3 axis1, axis2;
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal");
+
+  if (joint->flags & dJOINT_REVERSE) {
+    dReal temp = torque1;
+    torque1 = - torque2;
+    torque2 = - temp;
+  }
+
+  getAxis (joint,axis1,joint->axis1);
+  getAxis2 (joint,axis2,joint->axis2);
+  axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
+  axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
+  axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
+
+  if (joint->node[0].body != 0)
+    dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]);
+  if (joint->node[1].body != 0)
+    dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]);
+}
+
+
+
+
+
+dxJoint::Vtable __duniversal_vtable = {
+  sizeof(dxJointUniversal),
+  (dxJoint::init_fn*) universalInit,
+  (dxJoint::getInfo1_fn*) universalGetInfo1,
+  (dxJoint::getInfo2_fn*) universalGetInfo2,
+  dJointTypeUniversal};
+
+//****************************************************************************
+// angular motor
+
+static void amotorInit (dxJointAMotor *j)
+{
+  int i;
+  j->num = 0;
+  j->mode = dAMotorUser;
+  for (i=0; i<3; i++) {
+    j->rel[i] = 0;
+    dSetZero (j->axis[i],4);
+    j->limot[i].init (j->world);
+    j->angle[i] = 0;
+  }
+  dSetZero (j->reference1,4);
+  dSetZero (j->reference2,4);
+
+  j->flags |= dJOINT_TWOBODIES;
+}
+
+
+// compute the 3 axes in global coordinates
+
+static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3])
+{
+  if (joint->mode == dAMotorEuler) {
+    // special handling for euler mode
+    dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]);
+    dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]);
+    dCROSS (ax[1],=,ax[2],ax[0]);
+    dNormalize3 (ax[1]);
+  }
+  else {
+    for (int i=0; i < joint->num; i++) {
+      if (joint->rel[i] == 1) {
+        // relative to b1
+        dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]);
+      }
+      if (joint->rel[i] == 2) {
+        // relative to b2
+        dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]);
+      }
+      else {
+        // global - just copy it
+        ax[i][0] = joint->axis[i][0];
+        ax[i][1] = joint->axis[i][1];
+        ax[i][2] = joint->axis[i][2];
+      }
+    }
+  }
+}
+
+
+static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3])
+{
+  // assumptions:
+  //   global axes already calculated --> ax
+  //   axis[0] is relative to body 1 --> global ax[0]
+  //   axis[2] is relative to body 2 --> global ax[2]
+  //   ax[1] = ax[2] x ax[0]
+  //   original ax[0] and ax[2] are perpendicular
+  //   reference1 is perpendicular to ax[0] (in body 1 frame)
+  //   reference2 is perpendicular to ax[2] (in body 2 frame)
+  //   all ax[] and reference vectors are unit length
+
+  // calculate references in global frame
+  dVector3 ref1,ref2;
+  dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1);
+  dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2);
+
+  // get q perpendicular to both ax[0] and ref1, get first euler angle
+  dVector3 q;
+  dCROSS (q,=,ax[0],ref1);
+  joint->angle[0] = -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1));
+
+  // get q perpendicular to both ax[0] and ax[1], get second euler angle
+  dCROSS (q,=,ax[0],ax[1]);
+  joint->angle[1] = -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q));
+
+  // get q perpendicular to both ax[1] and ax[2], get third euler angle
+  dCROSS (q,=,ax[1],ax[2]);
+  joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q));
+}
+
+
+// set the reference vectors as follows:
+//   * reference1 = current axis[2] relative to body 1
+//   * reference2 = current axis[0] relative to body 2
+// this assumes that:
+//    * axis[0] is relative to body 1
+//    * axis[2] is relative to body 2
+
+static void amotorSetEulerReferenceVectors (dxJointAMotor *j)
+{
+  if (j->node[0].body && j->node[1].body) {
+    dVector3 r;         // axis[2] and axis[0] in global coordinates
+    dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]);
+    dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r);
+    dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]);
+    dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r);
+  }
+}
+
+
+static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info)
+{
+  info->m = 0;
+  info->nub = 0;
+
+  // compute the axes and angles, if in euler mode
+  if (j->mode == dAMotorEuler) {
+    dVector3 ax[3];
+    amotorComputeGlobalAxes (j,ax);
+    amotorComputeEulerAngles (j,ax);
+  }
+
+  // see if we're powered or at a joint limit for each axis
+  for (int i=0; i < j->num; i++) {
+    if (j->limot[i].testRotationalLimit (j->angle[i]) ||
+        j->limot[i].fmax > 0) {
+      info->m++;
+    }
+  }
+}
+
+
+static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info)
+{
+  int i;
+
+  // compute the axes (if not global)
+  dVector3 ax[3];
+  amotorComputeGlobalAxes (joint,ax);
+
+  // in euler angle mode we do not actually constrain the angular velocity
+  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
+  //
+  //    to get                  constrain w2-w1 along           ...not
+  //    ------                  ---------------------           ------
+  //    d(angle[0])/dt = 0      ax[1] x ax[2]                   ax[0]
+  //    d(angle[1])/dt = 0      ax[1]
+  //    d(angle[2])/dt = 0      ax[0] x ax[1]                   ax[2]
+  //
+  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+  // to prove the result for angle[0], write the expression for angle[0] from
+  // GetInfo1 then take the derivative. to prove this for angle[2] it is
+  // easier to take the euler rate expression for d(angle[2])/dt with respect
+  // to the components of w and set that to 0.
+
+  dVector3 *axptr[3];
+  axptr[0] = &ax[0];
+  axptr[1] = &ax[1];
+  axptr[2] = &ax[2];
+
+  dVector3 ax0_cross_ax1;
+  dVector3 ax1_cross_ax2;
+  if (joint->mode == dAMotorEuler) {
+    dCROSS (ax0_cross_ax1,=,ax[0],ax[1]);
+    axptr[2] = &ax0_cross_ax1;
+    dCROSS (ax1_cross_ax2,=,ax[1],ax[2]);
+    axptr[0] = &ax1_cross_ax2;
+  }
+
+  int row=0;
+  for (i=0; i < joint->num; i++) {
+    row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1);
+  }
+}
+
+
+extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num)
+{
+  dAASSERT(joint && num >= 0 && num <= 3);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  if (joint->mode == dAMotorEuler) {
+    joint->num = 3;
+  }
+  else {
+    if (num < 0) num = 0;
+    if (num > 3) num = 3;
+    joint->num = num;
+  }
+}
+
+
+extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel,
+                                     dReal x, dReal y, dReal z)
+{
+  dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  if (anum < 0) anum = 0;
+  if (anum > 2) anum = 2;
+  joint->rel[anum] = rel;
+
+  // x,y,z is always in global coordinates regardless of rel, so we may have
+  // to convert it to be relative to a body
+  dVector3 r;
+  r[0] = x;
+  r[1] = y;
+  r[2] = z;
+  r[3] = 0;
+  if (rel > 0) {
+    if (rel==1) {
+      dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r);
+    }
+    else {
+      dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r);
+    }
+  }
+  else {
+    joint->axis[anum][0] = r[0];
+    joint->axis[anum][1] = r[1];
+    joint->axis[anum][2] = r[2];
+  }
+  dNormalize3 (joint->axis[anum]);
+  if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint);
+}
+
+
+extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum,
+                                      dReal angle)
+{
+  dAASSERT(joint && anum >= 0 && anum < 3);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  if (joint->mode == dAMotorUser) {
+    if (anum < 0) anum = 0;
+    if (anum > 3) anum = 3;
+    joint->angle[anum] = angle;
+  }
+}
+
+
+extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter,
+                                      dReal value)
+{
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  int anum = parameter >> 8;
+  if (anum < 0) anum = 0;
+  if (anum > 2) anum = 2;
+  parameter &= 0xff;
+  joint->limot[anum].set (parameter, value);
+}
+
+
+extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode)
+{
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  joint->mode = mode;
+  if (joint->mode == dAMotorEuler) {
+    joint->num = 3;
+    amotorSetEulerReferenceVectors (joint);
+  }
+}
+
+
+extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint)
+{
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  return joint->num;
+}
+
+
+extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum,
+                                     dVector3 result)
+{
+  dAASSERT(joint && anum >= 0 && anum < 3);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  if (anum < 0) anum = 0;
+  if (anum > 2) anum = 2;
+  if (joint->rel[anum] > 0) {
+    if (joint->rel[anum]==1) {
+      dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]);
+    }
+    else {
+      dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]);
+    }
+  }
+  else {
+    result[0] = joint->axis[anum][0];
+    result[1] = joint->axis[anum][1];
+    result[2] = joint->axis[anum][2];
+  }
+}
+
+
+extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum)
+{
+  dAASSERT(joint && anum >= 0 && anum < 3);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  if (anum < 0) anum = 0;
+  if (anum > 2) anum = 2;
+  return joint->rel[anum];
+}
+
+
+extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum)
+{
+  dAASSERT(joint && anum >= 0 && anum < 3);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  if (anum < 0) anum = 0;
+  if (anum > 3) anum = 3;
+  return joint->angle[anum];
+}
+
+
+extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int anum)
+{
+  // @@@
+  dDebug (0,"not yet implemented");
+  return 0;
+}
+
+
+extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int parameter)
+{
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  int anum = parameter >> 8;
+  if (anum < 0) anum = 0;
+  if (anum > 2) anum = 2;
+  parameter &= 0xff;
+  return joint->limot[anum].get (parameter);
+}
+
+
+extern "C" int dJointGetAMotorMode (dxJointAMotor *joint)
+{
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+  return joint->mode;
+}
+
+
+extern "C" void dJointAddAMotorTorques (dxJointAMotor *joint, dReal torque1, dReal torque2, dReal torque3)
+{
+  dVector3 axes[3];
+  dAASSERT(joint);
+  dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor");
+
+  if (joint->num == 0)
+    return;
+  dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints");
+
+  amotorComputeGlobalAxes (joint,axes);
+  axes[0][0] *= torque1;
+  axes[0][1] *= torque1;
+  axes[0][2] *= torque1;
+  if (joint->num >= 2) {
+    axes[0][0] += axes[1][0] * torque2;
+    axes[0][1] += axes[1][0] * torque2;
+    axes[0][2] += axes[1][0] * torque2;
+    if (joint->num >= 3) {
+      axes[0][0] += axes[2][0] * torque3;
+      axes[0][1] += axes[2][0] * torque3;
+      axes[0][2] += axes[2][0] * torque3;
+    }
+  }
+
+  if (joint->node[0].body != 0)
+    dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]);
+  if (joint->node[1].body != 0)
+    dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]);
+}
+
+
+dxJoint::Vtable __damotor_vtable = {
+  sizeof(dxJointAMotor),
+  (dxJoint::init_fn*) amotorInit,
+  (dxJoint::getInfo1_fn*) amotorGetInfo1,
+  (dxJoint::getInfo2_fn*) amotorGetInfo2,
+  dJointTypeAMotor};
+
+//****************************************************************************
+// fixed joint
+
+static void fixedInit (dxJointFixed *j)
+{
+  dSetZero (j->offset,4);
+  dSetZero (j->qrel,4);
+}
+
+
+static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info)
+{
+  info->m = 6;
+  info->nub = 6;
+}
+
+
+static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info)
+{
+  int s = info->rowskip;
+
+  // Three rows for orientation
+  setFixedOrientation(joint, info, joint->qrel, 3);
+
+  // Three rows for position.
+  // set jacobian
+  info->J1l[0] = 1;
+  info->J1l[s+1] = 1;
+  info->J1l[2*s+2] = 1;
+
+  dVector3 ofs;
+  dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset);
+  if (joint->node[1].body) {
+    dCROSSMAT (info->J1a,ofs,s,+,-);
+    info->J2l[0] = -1;
+    info->J2l[s+1] = -1;
+    info->J2l[2*s+2] = -1;
+  }
+
+  // set right hand side for the first three rows (linear)
+  dReal k = info->fps * info->erp;
+  if (joint->node[1].body) {
+    for (int j=0; j<3; j++)
+      info->c[j] = k * (joint->node[1].body->pos[j] -
+                        joint->node[0].body->pos[j] + ofs[j]);
+  }
+  else {
+    for (int j=0; j<3; j++)
+      info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]);
+  }
+}
+
+
+extern "C" void dJointSetFixed (dxJointFixed *joint)
+{
+  dUASSERT(joint,"bad joint argument");
+  dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed");
+  int i;
+
+  // This code is taken from sJointSetSliderAxis(), we should really put the
+  // common code in its own function.
+  // compute the offset between the bodies
+  if (joint->node[0].body) {
+    if (joint->node[1].body) {
+      dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
+      dReal ofs[4];
+      for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i];
+      for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i];
+      dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs);
+    }
+    else {
+      // set joint->qrel to the transpose of the first body's q
+      joint->qrel[0] = joint->node[0].body->q[0];
+      for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i];
+      for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i];
+    }
+  }
+}
+
+
+dxJoint::Vtable __dfixed_vtable = {
+  sizeof(dxJointFixed),
+  (dxJoint::init_fn*) fixedInit,
+  (dxJoint::getInfo1_fn*) fixedGetInfo1,
+  (dxJoint::getInfo2_fn*) fixedGetInfo2,
+  dJointTypeFixed};
+
+//****************************************************************************
+// null joint
+
+static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info)
+{
+  info->m = 0;
+  info->nub = 0;
+}
+
+
+static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info)
+{
+  dDebug (0,"this should never get called");
+}
+
+
+dxJoint::Vtable __dnull_vtable = {
+  sizeof(dxJointNull),
+  (dxJoint::init_fn*) 0,
+  (dxJoint::getInfo1_fn*) nullGetInfo1,
+  (dxJoint::getInfo2_fn*) nullGetInfo2,
+  dJointTypeNull};
+
+/******************** breakable joint contribution ***********************/
+extern "C" void dJointSetBreakable (dxJoint *joint, int b) {
+  dAASSERT(joint);
+  if (b) {
+    // we want this joint to be breakable but we must first check if it
+    // was already breakable
+    if (!joint->breakInfo) {
+      // allocate a dxJointBreakInfo struct
+      joint->breakInfo = new dxJointBreakInfo;
+      joint->breakInfo->flags = 0;
+      for (int i = 0; i < 3; i++) {
+        joint->breakInfo->b1MaxF[0] = 0;
+        joint->breakInfo->b1MaxT[0] = 0;
+        joint->breakInfo->b2MaxF[0] = 0;
+        joint->breakInfo->b2MaxT[0] = 0;
+      }
+          joint->breakInfo->callback = 0;
+    }
+    else {
+      // the joint was already breakable
+      return;
+    }
+  }
+  else {
+    // we want this joint to be unbreakable mut we must first check if
+    // it is alreay unbreakable
+    if (joint->breakInfo) {
+      // deallocate the dxJointBreakInfo struct
+      delete joint->breakInfo;
+      joint->breakInfo = 0;
+    }
+    else {
+      // the joint was already unbreakable
+      return;
+    }
+  }
+}
+
+extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) {
+  dAASSERT(joint);
+# ifndef dNODEBUG
+  // only works for a breakable joint
+  if (!joint->breakInfo) {
+    dDebug (0, "dJointSetBreakCallback called on unbreakable joint");
+  }
+# endif
+  joint->breakInfo->callback = callbackFunc;
+}
+
+extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) {
+  dAASSERT(joint);
+# ifndef dNODEBUG
+  // only works for a breakable joint
+  if (!joint->breakInfo) {
+    dDebug (0, "dJointSetBreakMode called on unbreakable joint");
+  }
+# endif
+  joint->breakInfo->flags = mode;
+}
+
+extern "C" int dJointGetBreakMode (dxJoint *joint) {
+  dAASSERT(joint);
+# ifndef dNODEBUG
+  // only works for a breakable joint
+  if (!joint->breakInfo) {
+    dDebug (0, "dJointGetBreakMode called on unbreakable joint");
+  }
+# endif
+  return joint->breakInfo->flags;
+}
+
+extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) {
+  dAASSERT(joint);
+# ifndef dNODEBUG
+  // only works for a breakable joint
+  if (!joint->breakInfo) {
+  dDebug (0, "dJointSetBreakForce called on unbreakable joint");
+  }
+# endif
+  if (body) {
+        joint->breakInfo->b2MaxF[0] = x;
+        joint->breakInfo->b2MaxF[1] = y;
+        joint->breakInfo->b2MaxF[2] = z;
+  }
+  else {
+        joint->breakInfo->b1MaxF[0] = x;
+        joint->breakInfo->b1MaxF[1] = y;
+        joint->breakInfo->b1MaxF[2] = z;
+  }
+}
+
+extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) {
+  dAASSERT(joint);
+# ifndef dNODEBUG
+  // only works for a breakable joint
+  if (!joint->breakInfo) {
+  dDebug (0, "dJointSetBreakTorque called on unbreakable joint");
+  }
+# endif
+  if (body) {
+        joint->breakInfo->b2MaxT[0] = x;
+        joint->breakInfo->b2MaxT[1] = y;
+        joint->breakInfo->b2MaxT[2] = z;
+  }
+  else {
+        joint->breakInfo->b1MaxT[0] = x;
+        joint->breakInfo->b1MaxT[1] = y;
+        joint->breakInfo->b1MaxT[2] = z;
+  }
+}
+
+extern "C" int dJointIsBreakable (dxJoint *joint) {
+  dAASSERT(joint);
+  return joint->breakInfo != 0;
+}
+
+extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) {
+  dAASSERT(joint);
+# ifndef dNODEBUG
+  // only works for a breakable joint
+  if (!joint->breakInfo) {
+    dDebug (0, "dJointGetBreakForce called on unbreakable joint");
+  }
+# endif
+  if (body)
+    for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i];
+  else
+    for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i];
+}
+
+extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) {
+  dAASSERT(joint);
+# ifndef dNODEBUG
+  // only works for a breakable joint
+  if (!joint->breakInfo) {
+    dDebug (0, "dJointGetBreakTorque called on unbreakable joint");
+  }
+# endif
+  if (body)
+    for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i];
+  else
+    for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i];
+}
+/*************************************************************************/
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/joint.h b/libraries/ode-0.9/contrib/BreakableJoints/joint.h
new file mode 100644
index 0000000..0573119
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/joint.h
@@ -0,0 +1,282 @@
+/*************************************************************************
+ *                                                                       *
+ * 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
+};
+
+/******************** breakable joint contribution ***********************/
+struct dxJointBreakInfo : public dBase {
+        int flags;
+        dReal b1MaxF[3]; // maximum force on body 1
+        dReal b1MaxT[3]; // maximum torque on body 1
+        dReal b2MaxF[3]; // maximum force on body 2
+        dReal b2MaxT[3]; // maximum torque on body 2
+        dJointBreakCallback *callback; // function that is called when this joint breaks
+};
+/*************************************************************************/
+
+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
+  
+  /******************** breakable joint contribution ***********************/
+  // optional break info structure. if this is not NULL the the joint is
+  // breakable.
+  dxJointBreakInfo *breakInfo;
+  /*************************************************************************/
+};
+
+
+// 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
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/objects.h b/libraries/ode-0.9/contrib/BreakableJoints/objects.h
new file mode 100644
index 0000000..de08391
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/objects.h
@@ -0,0 +1,252 @@
+/*************************************************************************
+ *                                                                       *
+ * 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);
+
+/* StepFast1 functions */
+
+void dWorldStepFast1(dWorldID, dReal stepsize, int maxiterations);
+void dWorldSetAutoEnableDepthSF1(dWorldID, int autoEnableDepth);
+
+int dWorldGetAutoEnableDepthSF1(dWorldID);
+
+void dBodySetAutoDisableThresholdSF1(dBodyID, dReal autoDisableThreshold);
+
+/* These functions are not yet implemented by ODE. */
+/*
+dReal dBodyGetAutoDisableThresholdSF1(dBodyID);
+
+void dBodySetAutoDisableStepsSF1(dBodyID, int AutoDisableSteps);
+
+int dBodyGetAutoDisableStepsSF1(dBodyID);
+
+void dBodySetAutoDisableSF1(dBodyID, int doAutoDisable);
+
+int dBodyGetAutoDisableSF1(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);
+
+/******************** breakable joint contribution ***********************/
+void dJointSetBreakable (dJointID, int b);
+void dJointSetBreakCallback (dJointID, dJointBreakCallback *callbackFunc);
+void dJointSetBreakMode (dJointID, int mode);
+int dJointGetBreakMode (dJointID);
+void dJointSetBreakForce (dJointID, int body, dReal x, dReal y, dReal z);
+void dJointSetBreakTorque (dJointID, int body, dReal x, dReal y, dReal z);
+int dJointIsBreakable (dJointID);
+void dJointGetBreakForce (dJointID, int body, dReal *force);
+void dJointGetBreakTorque (dJointID, int body, dReal *torque);
+/*************************************************************************/
+
+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
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp b/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp
new file mode 100644
index 0000000..7137960
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/ode.cpp
@@ -0,0 +1,1404 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifdef _MSC_VER
+#pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
+#endif
+
+// this source file is mostly concerned with the data structures, not the
+// numerics.
+
+#include "objects.h"
+#include <ode/ode.h>
+#include "joint.h"
+#include <ode/odemath.h>
+#include <ode/matrix.h>
+#include "step.h"
+#include <ode/memory.h>
+#include <ode/error.h>
+
+// misc defines
+#define ALLOCA dALLOCA16
+
+//****************************************************************************
+// utility
+
+static inline void initObject (dObject *obj, dxWorld *w)
+{
+  obj->world = w;
+  obj->next = 0;
+  obj->tome = 0;
+  obj->userdata = 0;
+  obj->tag = 0;
+}
+
+
+// add an object `obj' to the list who's head pointer is pointed to by `first'.
+
+static inline void addObjectToList (dObject *obj, dObject **first)
+{
+  obj->next = *first;
+  obj->tome = first;
+  if (*first) (*first)->tome = &obj->next;
+  (*first) = obj;
+}
+
+
+// remove the object from the linked list
+
+static inline void removeObjectFromList (dObject *obj)
+{
+  if (obj->next) obj->next->tome = obj->tome;
+  *(obj->tome) = obj->next;
+  // safeguard
+  obj->next = 0;
+  obj->tome = 0;
+}
+
+
+// remove the joint from neighbour lists of all connected bodies
+
+static void removeJointReferencesFromAttachedBodies (dxJoint *j)
+{
+  for (int i=0; i<2; i++) {
+    dxBody *body = j->node[i].body;
+    if (body) {
+      dxJointNode *n = body->firstjoint;
+      dxJointNode *last = 0;
+      while (n) {
+        if (n->joint == j) {
+          if (last) last->next = n->next;
+          else body->firstjoint = n->next;
+          break;
+        }
+        last = n;
+        n = n->next;
+      }
+    }
+  }
+  j->node[0].body = 0;
+  j->node[0].next = 0;
+  j->node[1].body = 0;
+  j->node[1].next = 0;
+}
+
+//****************************************************************************
+// 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.
+
+static void processIslands (dxWorld *world, dReal stepsize)
+{
+  dxBody *b,*bb,**body;
+  dxJoint *j,**joint;
+
+  // nothing to do if no bodies
+  if (world->nb <= 0) return;
+
+  // 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
+    dInternalStepIsland (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
+  /******************** breakable joint contribution ***********************/
+  dxJoint* nextJ;
+  if (!world->firstjoint)
+    nextJ = 0;
+  else
+    nextJ = (dxJoint*)world->firstjoint->next;
+  for (j=world->firstjoint; j; j=nextJ) {
+        nextJ = (dxJoint*)j->next;
+        // check if joint is breakable and broken
+    if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
+                // detach (break) the joint
+        dJointAttach (j, 0, 0);
+                // call the callback function if it is set
+                if (j->breakInfo->callback) j->breakInfo->callback (j);
+                // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
+                if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
+      }
+  }
+  /*************************************************************************/
+}
+
+//****************************************************************************
+// debugging
+
+// see if an object list loops on itself (if so, it's bad).
+
+static int listHasLoops (dObject *first)
+{
+  if (first==0 || first->next==0) return 0;
+  dObject *a=first,*b=first->next;
+  int skip=0;
+  while (b) {
+    if (a==b) return 1;
+    b = b->next;
+    if (skip) a = a->next;
+    skip ^= 1;
+  }
+  return 0;
+}
+
+
+// check the validity of the world data structures
+
+static void checkWorld (dxWorld *w)
+{
+  dxBody *b;
+  dxJoint *j;
+
+  // check there are no loops
+  if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops");
+  if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops");
+
+  // check lists are well formed (check `tome' pointers)
+  for (b=w->firstbody; b; b=(dxBody*)b->next) {
+    if (b->next && b->next->tome != &b->next)
+      dDebug (0,"bad tome pointer in body list");
+  }
+  for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
+    if (j->next && j->next->tome != &j->next)
+      dDebug (0,"bad tome pointer in joint list");
+  }
+
+  // check counts
+  int n = 0;
+  for (b=w->firstbody; b; b=(dxBody*)b->next) n++;
+  if (w->nb != n) dDebug (0,"body count incorrect");
+  n = 0;
+  for (j=w->firstjoint; j; j=(dxJoint*)j->next) n++;
+  if (w->nj != n) dDebug (0,"joint count incorrect");
+
+  // set all tag values to a known value
+  static int count = 0;
+  count++;
+  for (b=w->firstbody; b; b=(dxBody*)b->next) b->tag = count;
+  for (j=w->firstjoint; j; j=(dxJoint*)j->next) j->tag = count;
+
+  // check all body/joint world pointers are ok
+  for (b=w->firstbody; b; b=(dxBody*)b->next) if (b->world != w)
+    dDebug (0,"bad world pointer in body list");
+  for (j=w->firstjoint; j; j=(dxJoint*)j->next) if (j->world != w)
+    dDebug (0,"bad world pointer in joint list");
+
+  /*
+  // check for half-connected joints - actually now these are valid
+  for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
+    if (j->node[0].body || j->node[1].body) {
+      if (!(j->node[0].body && j->node[1].body))
+        dDebug (0,"half connected joint found");
+    }
+  }
+  */
+
+  // check that every joint node appears in the joint lists of both bodies it
+  // attaches
+  for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
+    for (int i=0; i<2; i++) {
+      if (j->node[i].body) {
+        int ok = 0;
+        for (dxJointNode *n=j->node[i].body->firstjoint; n; n=n->next) {
+          if (n->joint == j) ok = 1;
+        }
+        if (ok==0) dDebug (0,"joint not in joint list of attached body");
+      }
+    }
+  }
+
+  // check all body joint lists (correct body ptrs)
+  for (b=w->firstbody; b; b=(dxBody*)b->next) {
+    for (dxJointNode *n=b->firstjoint; n; n=n->next) {
+      if (&n->joint->node[0] == n) {
+        if (n->joint->node[1].body != b)
+          dDebug (0,"bad body pointer in joint node of body list (1)");
+      }
+      else {
+        if (n->joint->node[0].body != b)
+          dDebug (0,"bad body pointer in joint node of body list (2)");
+      }
+      if (n->joint->tag != count) dDebug (0,"bad joint node pointer in body");
+    }
+  }
+
+  // check all body pointers in joints, check they are distinct
+  for (j=w->firstjoint; j; j=(dxJoint*)j->next) {
+    if (j->node[0].body && (j->node[0].body == j->node[1].body))
+      dDebug (0,"non-distinct body pointers in joint");
+    if ((j->node[0].body && j->node[0].body->tag != count) ||
+        (j->node[1].body && j->node[1].body->tag != count))
+      dDebug (0,"bad body pointer in joint");
+  }
+}
+
+
+void dWorldCheck (dxWorld *w)
+{
+  checkWorld (w);
+}
+
+//****************************************************************************
+// body
+
+dxBody *dBodyCreate (dxWorld *w)
+{
+  dAASSERT (w);
+  dxBody *b = new dxBody;
+  initObject (b,w);
+  b->firstjoint = 0;
+  b->flags = 0;
+  b->geom = 0;
+  dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0);
+  dSetZero (b->invI,4*3);
+  b->invI[0] = 1;
+  b->invI[5] = 1;
+  b->invI[10] = 1;
+  b->invMass = 1;
+  dSetZero (b->pos,4);
+  dSetZero (b->q,4);
+  b->q[0] = 1;
+  dRSetIdentity (b->R);
+  dSetZero (b->lvel,4);
+  dSetZero (b->avel,4);
+  dSetZero (b->facc,4);
+  dSetZero (b->tacc,4);
+  dSetZero (b->finite_rot_axis,4);
+  addObjectToList (b,(dObject **) &w->firstbody);
+  w->nb++;
+  return b;
+}
+
+
+void dBodyDestroy (dxBody *b)
+{
+  dAASSERT (b);
+
+  // all geoms that link to this body must be notified that the body is about
+  // to disappear. note that the call to dGeomSetBody(geom,0) will result in
+  // dGeomGetBodyNext() returning 0 for the body, so we must get the next body
+  // before setting the body to 0.
+  dxGeom *next_geom = 0;
+  for (dxGeom *geom = b->geom; geom; geom = next_geom) {
+    next_geom = dGeomGetBodyNext (geom);
+    dGeomSetBody (geom,0);
+  }
+
+  // detach all neighbouring joints, then delete this body.
+  dxJointNode *n = b->firstjoint;
+  while (n) {
+    // sneaky trick to speed up removal of joint references (black magic)
+    n->joint->node[(n == n->joint->node)].body = 0;
+
+    dxJointNode *next = n->next;
+    n->next = 0;
+    removeJointReferencesFromAttachedBodies (n->joint);
+    n = next;
+  }
+  removeObjectFromList (b);
+  b->world->nb--;
+  delete b;
+}
+
+
+void dBodySetData (dBodyID b, void *data)
+{
+  dAASSERT (b);
+  b->userdata = data;
+}
+
+
+void *dBodyGetData (dBodyID b)
+{
+  dAASSERT (b);
+  return b->userdata;
+}
+
+
+void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)
+{
+  dAASSERT (b);
+  b->pos[0] = x;
+  b->pos[1] = y;
+  b->pos[2] = z;
+
+  // notify all attached geoms that this body has moved
+  for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom))
+    dGeomMoved (geom);
+}
+
+
+void dBodySetRotation (dBodyID b, const dMatrix3 R)
+{
+  dAASSERT (b && R);
+  dQuaternion q;
+  dRtoQ (R,q);
+  dNormalize4 (q);
+  b->q[0] = q[0];
+  b->q[1] = q[1];
+  b->q[2] = q[2];
+  b->q[3] = q[3];
+  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);
+}
+
+
+void dBodySetQuaternion (dBodyID b, const dQuaternion q)
+{
+  dAASSERT (b && q);
+  b->q[0] = q[0];
+  b->q[1] = q[1];
+  b->q[2] = q[2];
+  b->q[3] = q[3];
+  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);
+}
+
+
+void dBodySetLinearVel  (dBodyID b, dReal x, dReal y, dReal z)
+{
+  dAASSERT (b);
+  b->lvel[0] = x;
+  b->lvel[1] = y;
+  b->lvel[2] = z;
+}
+
+
+void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)
+{
+  dAASSERT (b);
+  b->avel[0] = x;
+  b->avel[1] = y;
+  b->avel[2] = z;
+}
+
+
+const dReal * dBodyGetPosition (dBodyID b)
+{
+  dAASSERT (b);
+  return b->pos;
+}
+
+
+const dReal * dBodyGetRotation (dBodyID b)
+{
+  dAASSERT (b);
+  return b->R;
+}
+
+
+const dReal * dBodyGetQuaternion (dBodyID b)
+{
+  dAASSERT (b);
+  return b->q;
+}
+
+
+const dReal * dBodyGetLinearVel (dBodyID b)
+{
+  dAASSERT (b);
+  return b->lvel;
+}
+
+
+const dReal * dBodyGetAngularVel (dBodyID b)
+{
+  dAASSERT (b);
+  return b->avel;
+}
+
+
+void dBodySetMass (dBodyID b, const dMass *mass)
+{
+  dAASSERT (b && mass);
+  memcpy (&b->mass,mass,sizeof(dMass));
+  if (dInvertPDMatrix (b->mass.I,b->invI,3)==0) {
+    dDEBUGMSG ("inertia must be positive definite");
+    dRSetIdentity (b->invI);
+  }
+  b->invMass = dRecip(b->mass.mass);
+}
+
+
+void dBodyGetMass (dBodyID b, dMass *mass)
+{
+  dAASSERT (b && mass);
+  memcpy (mass,&b->mass,sizeof(dMass));
+}
+
+
+void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)
+{
+  dAASSERT (b);
+  b->facc[0] += fx;
+  b->facc[1] += fy;
+  b->facc[2] += fz;
+}
+
+
+void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
+{
+  dAASSERT (b);
+  b->tacc[0] += fx;
+  b->tacc[1] += fy;
+  b->tacc[2] += fz;
+}
+
+
+void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)
+{
+  dAASSERT (b);
+  dVector3 t1,t2;
+  t1[0] = fx;
+  t1[1] = fy;
+  t1[2] = fz;
+  t1[3] = 0;
+  dMULTIPLY0_331 (t2,b->R,t1);
+  b->facc[0] += t2[0];
+  b->facc[1] += t2[1];
+  b->facc[2] += t2[2];
+}
+
+
+void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
+{
+  dAASSERT (b);
+  dVector3 t1,t2;
+  t1[0] = fx;
+  t1[1] = fy;
+  t1[2] = fz;
+  t1[3] = 0;
+  dMULTIPLY0_331 (t2,b->R,t1);
+  b->tacc[0] += t2[0];
+  b->tacc[1] += t2[1];
+  b->tacc[2] += t2[2];
+}
+
+
+void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
+                         dReal px, dReal py, dReal pz)
+{
+  dAASSERT (b);
+  b->facc[0] += fx;
+  b->facc[1] += fy;
+  b->facc[2] += fz;
+  dVector3 f,q;
+  f[0] = fx;
+  f[1] = fy;
+  f[2] = fz;
+  q[0] = px - b->pos[0];
+  q[1] = py - b->pos[1];
+  q[2] = pz - b->pos[2];
+  dCROSS (b->tacc,+=,q,f);
+}
+
+
+void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
+                            dReal px, dReal py, dReal pz)
+{
+  dAASSERT (b);
+  dVector3 prel,f,p;
+  f[0] = fx;
+  f[1] = fy;
+  f[2] = fz;
+  f[3] = 0;
+  prel[0] = px;
+  prel[1] = py;
+  prel[2] = pz;
+  prel[3] = 0;
+  dMULTIPLY0_331 (p,b->R,prel);
+  b->facc[0] += f[0];
+  b->facc[1] += f[1];
+  b->facc[2] += f[2];
+  dCROSS (b->tacc,+=,p,f);
+}
+
+
+void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
+                            dReal px, dReal py, dReal pz)
+{
+  dAASSERT (b);
+  dVector3 frel,f;
+  frel[0] = fx;
+  frel[1] = fy;
+  frel[2] = fz;
+  frel[3] = 0;
+  dMULTIPLY0_331 (f,b->R,frel);
+  b->facc[0] += f[0];
+  b->facc[1] += f[1];
+  b->facc[2] += f[2];
+  dVector3 q;
+  q[0] = px - b->pos[0];
+  q[1] = py - b->pos[1];
+  q[2] = pz - b->pos[2];
+  dCROSS (b->tacc,+=,q,f);
+}
+
+
+void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
+                               dReal px, dReal py, dReal pz)
+{
+  dAASSERT (b);
+  dVector3 frel,prel,f,p;
+  frel[0] = fx;
+  frel[1] = fy;
+  frel[2] = fz;
+  frel[3] = 0;
+  prel[0] = px;
+  prel[1] = py;
+  prel[2] = pz;
+  prel[3] = 0;
+  dMULTIPLY0_331 (f,b->R,frel);
+  dMULTIPLY0_331 (p,b->R,prel);
+  b->facc[0] += f[0];
+  b->facc[1] += f[1];
+  b->facc[2] += f[2];
+  dCROSS (b->tacc,+=,p,f);
+}
+
+
+const dReal * dBodyGetForce (dBodyID b)
+{
+  dAASSERT (b);
+  return b->facc;
+}
+
+
+const dReal * dBodyGetTorque (dBodyID b)
+{
+  dAASSERT (b);
+  return b->tacc;
+}
+
+
+void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)
+{
+  dAASSERT (b);
+  b->facc[0] = x;
+  b->facc[1] = y;
+  b->facc[2] = z;
+}
+
+
+void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
+{
+  dAASSERT (b);
+  b->tacc[0] = x;
+  b->tacc[1] = y;
+  b->tacc[2] = z;
+}
+
+
+void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
+                          dVector3 result)
+{
+  dAASSERT (b);
+  dVector3 prel,p;
+  prel[0] = px;
+  prel[1] = py;
+  prel[2] = pz;
+  prel[3] = 0;
+  dMULTIPLY0_331 (p,b->R,prel);
+  result[0] = p[0] + b->pos[0];
+  result[1] = p[1] + b->pos[1];
+  result[2] = p[2] + b->pos[2];
+}
+
+
+void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
+                          dVector3 result)
+{
+  dAASSERT (b);
+  dVector3 prel,p;
+  prel[0] = px;
+  prel[1] = py;
+  prel[2] = pz;
+  prel[3] = 0;
+  dMULTIPLY0_331 (p,b->R,prel);
+  result[0] = b->lvel[0];
+  result[1] = b->lvel[1];
+  result[2] = b->lvel[2];
+  dCROSS (result,+=,b->avel,p);
+}
+
+
+void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
+                       dVector3 result)
+{
+  dAASSERT (b);
+  dVector3 p;
+  p[0] = px - b->pos[0];
+  p[1] = py - b->pos[1];
+  p[2] = pz - b->pos[2];
+  p[3] = 0;
+  result[0] = b->lvel[0];
+  result[1] = b->lvel[1];
+  result[2] = b->lvel[2];
+  dCROSS (result,+=,b->avel,p);
+}
+
+
+void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
+                          dVector3 result)
+{
+  dAASSERT (b);
+  dVector3 prel;
+  prel[0] = px - b->pos[0];
+  prel[1] = py - b->pos[1];
+  prel[2] = pz - b->pos[2];
+  prel[3] = 0;
+  dMULTIPLY1_331 (result,b->R,prel);
+}
+
+
+void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
+                         dVector3 result)
+{
+  dAASSERT (b);
+  dVector3 p;
+  p[0] = px;
+  p[1] = py;
+  p[2] = pz;
+  p[3] = 0;
+  dMULTIPLY0_331 (result,b->R,p);
+}
+
+
+void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
+                           dVector3 result)
+{
+  dAASSERT (b);
+  dVector3 p;
+  p[0] = px;
+  p[1] = py;
+  p[2] = pz;
+  p[3] = 0;
+  dMULTIPLY1_331 (result,b->R,p);
+}
+
+
+void dBodySetFiniteRotationMode (dBodyID b, int mode)
+{
+  dAASSERT (b);
+  b->flags &= ~(dxBodyFlagFiniteRotation | dxBodyFlagFiniteRotationAxis);
+  if (mode) {
+    b->flags |= dxBodyFlagFiniteRotation;
+    if (b->finite_rot_axis[0] != 0 || b->finite_rot_axis[1] != 0 ||
+        b->finite_rot_axis[2] != 0) {
+      b->flags |= dxBodyFlagFiniteRotationAxis;
+    }
+  }
+}
+
+
+void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)
+{
+  dAASSERT (b);
+  b->finite_rot_axis[0] = x;
+  b->finite_rot_axis[1] = y;
+  b->finite_rot_axis[2] = z;
+  if (x != 0 || y != 0 || z != 0) {
+    dNormalize3 (b->finite_rot_axis);
+    b->flags |= dxBodyFlagFiniteRotationAxis;
+  }
+  else {
+    b->flags &= ~dxBodyFlagFiniteRotationAxis;
+  }
+}
+
+
+int dBodyGetFiniteRotationMode (dBodyID b)
+{
+  dAASSERT (b);
+  return ((b->flags & dxBodyFlagFiniteRotation) != 0);
+}
+
+
+void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)
+{
+  dAASSERT (b);
+  result[0] = b->finite_rot_axis[0];
+  result[1] = b->finite_rot_axis[1];
+  result[2] = b->finite_rot_axis[2];
+}
+
+
+int dBodyGetNumJoints (dBodyID b)
+{
+  dAASSERT (b);
+  int count=0;
+  for (dxJointNode *n=b->firstjoint; n; n=n->next, count++);
+  return count;
+}
+
+
+dJointID dBodyGetJoint (dBodyID b, int index)
+{
+  dAASSERT (b);
+  int i=0;
+  for (dxJointNode *n=b->firstjoint; n; n=n->next, i++) {
+    if (i == index) return n->joint;
+  }
+  return 0;
+}
+
+
+void dBodyEnable (dBodyID b)
+{
+  dAASSERT (b);
+  b->flags &= ~dxBodyDisabled;
+}
+
+
+void dBodyDisable (dBodyID b)
+{
+  dAASSERT (b);
+  b->flags |= dxBodyDisabled;
+}
+
+
+int dBodyIsEnabled (dBodyID b)
+{
+  dAASSERT (b);
+  return ((b->flags & dxBodyDisabled) == 0);
+}
+
+
+void dBodySetGravityMode (dBodyID b, int mode)
+{
+  dAASSERT (b);
+  if (mode) b->flags &= ~dxBodyNoGravity;
+  else b->flags |= dxBodyNoGravity;
+}
+
+
+int dBodyGetGravityMode (dBodyID b)
+{
+  dAASSERT (b);
+  return ((b->flags & dxBodyNoGravity) == 0);
+}
+
+//****************************************************************************
+// joints
+
+static void dJointInit (dxWorld *w, dxJoint *j)
+{
+  dIASSERT (w && j);
+  initObject (j,w);
+  j->vtable = 0;
+  j->flags = 0;
+  j->node[0].joint = j;
+  j->node[0].body = 0;
+  j->node[0].next = 0;
+  j->node[1].joint = j;
+  j->node[1].body = 0;
+  j->node[1].next = 0;
+  addObjectToList (j,(dObject **) &w->firstjoint);
+  w->nj++;
+}
+
+
+static dxJoint *createJoint (dWorldID w, dJointGroupID group,
+                             dxJoint::Vtable *vtable)
+{
+  dIASSERT (w && vtable);
+  dxJoint *j;
+  if (group) {
+    j = (dxJoint*) group->stack.alloc (vtable->size);
+    group->num++;
+  }
+  else j = (dxJoint*) dAlloc (vtable->size);
+  dJointInit (w,j);
+  j->vtable = vtable;
+  if (group) j->flags |= dJOINT_INGROUP;
+  if (vtable->init) vtable->init (j);
+  j->feedback = 0;
+  /******************** breakable joint contribution ***********************/
+  j->breakInfo = 0;
+  /*************************************************************************/
+  return j;
+}
+
+
+dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)
+{
+  dAASSERT (w);
+  return createJoint (w,group,&__dball_vtable);
+}
+
+
+dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)
+{
+  dAASSERT (w);
+  return createJoint (w,group,&__dhinge_vtable);
+}
+
+
+dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)
+{
+  dAASSERT (w);
+  return createJoint (w,group,&__dslider_vtable);
+}
+
+
+dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
+                               const dContact *c)
+{
+  dAASSERT (w && c);
+  dxJointContact *j = (dxJointContact *)
+    createJoint (w,group,&__dcontact_vtable);
+  j->contact = *c;
+  return j;
+}
+
+
+dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)
+{
+  dAASSERT (w);
+  return createJoint (w,group,&__dhinge2_vtable);
+}
+
+
+dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
+{
+  dAASSERT (w);
+  return createJoint (w,group,&__duniversal_vtable);
+}
+
+
+dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
+{
+  dAASSERT (w);
+  return createJoint (w,group,&__dfixed_vtable);
+}
+
+
+dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)
+{
+  dAASSERT (w);
+  return createJoint (w,group,&__dnull_vtable);
+}
+
+
+dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)
+{
+  dAASSERT (w);
+  return createJoint (w,group,&__damotor_vtable);
+}
+
+
+void dJointDestroy (dxJoint *j)
+{
+  dAASSERT (j);
+  if (j->flags & dJOINT_INGROUP) return;
+  removeJointReferencesFromAttachedBodies (j);
+  removeObjectFromList (j);
+  /******************** breakable joint contribution ***********************/
+  if (j->breakInfo) delete j->breakInfo;
+  /*************************************************************************/
+  j->world->nj--;
+  dFree (j,j->vtable->size);
+}
+
+
+dJointGroupID dJointGroupCreate (int max_size)
+{
+  // not any more ... dUASSERT (max_size > 0,"max size must be > 0");
+  dxJointGroup *group = new dxJointGroup;
+  group->num = 0;
+  return group;
+}
+
+
+void dJointGroupDestroy (dJointGroupID group)
+{
+  dAASSERT (group);
+  dJointGroupEmpty (group);
+  delete group;
+}
+
+
+void dJointGroupEmpty (dJointGroupID group)
+{
+  // the joints in this group are detached starting from the most recently
+  // added (at the top of the stack). this helps ensure that the various
+  // linked lists are not traversed too much, as the joints will hopefully
+  // be at the start of those lists.
+  // if any group joints have their world pointer set to 0, their world was
+  // previously destroyed. no special handling is required for these joints.
+
+  dAASSERT (group);
+  int i;
+  dxJoint **jlist = (dxJoint**) ALLOCA (group->num * sizeof(dxJoint*));
+  dxJoint *j = (dxJoint*) group->stack.rewind();
+  for (i=0; i < group->num; i++) {
+    jlist[i] = j;
+    j = (dxJoint*) (group->stack.next (j->vtable->size));
+  }
+  for (i=group->num-1; i >= 0; i--) {
+    if (jlist[i]->world) {
+      removeJointReferencesFromAttachedBodies (jlist[i]);
+      removeObjectFromList (jlist[i]);
+      jlist[i]->world->nj--;
+    }
+  }
+  group->num = 0;
+  group->stack.freeAll();
+}
+
+
+void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
+{
+  // check arguments
+  dUASSERT (joint,"bad joint argument");
+  dUASSERT (body1 == 0 || body1 != body2,"can't have body1==body2");
+  dxWorld *world = joint->world;
+  dUASSERT ( (!body1 || body1->world == world) &&
+             (!body2 || body2->world == world),
+             "joint and bodies must be in same world");
+
+  // check if the joint can not be attached to just one body
+  dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
+              ((body1 != 0) ^ (body2 != 0))),
+            "joint can not be attached to just one body");
+
+  // remove any existing body attachments
+  if (joint->node[0].body || joint->node[1].body) {
+    removeJointReferencesFromAttachedBodies (joint);
+  }
+
+  // if a body is zero, make sure that it is body2, so 0 --> node[1].body
+  if (body1==0) {
+    body1 = body2;
+    body2 = 0;
+    joint->flags |= dJOINT_REVERSE;
+  }
+  else {
+    joint->flags &= (~dJOINT_REVERSE);
+  }
+
+  // attach to new bodies
+  joint->node[0].body = body1;
+  joint->node[1].body = body2;
+  if (body1) {
+    joint->node[1].next = body1->firstjoint;
+    body1->firstjoint = &joint->node[1];
+  }
+  else joint->node[1].next = 0;
+  if (body2) {
+    joint->node[0].next = body2->firstjoint;
+    body2->firstjoint = &joint->node[0];
+  }
+  else {
+    joint->node[0].next = 0;
+  }
+}
+
+
+void dJointSetData (dxJoint *joint, void *data)
+{
+  dAASSERT (joint);
+  joint->userdata = data;
+}
+
+
+void *dJointGetData (dxJoint *joint)
+{
+  dAASSERT (joint);
+  return joint->userdata;
+}
+
+
+int dJointGetType (dxJoint *joint)
+{
+  dAASSERT (joint);
+  return joint->vtable->typenum;
+}
+
+
+dBodyID dJointGetBody (dxJoint *joint, int index)
+{
+  dAASSERT (joint);
+  if (index >= 0 && index < 2) return joint->node[index].body;
+  else return 0;
+}
+
+
+void dJointSetFeedback (dxJoint *joint, dJointFeedback *f)
+{
+  dAASSERT (joint);
+  joint->feedback = f;
+}
+
+
+dJointFeedback *dJointGetFeedback (dxJoint *joint)
+{
+  dAASSERT (joint);
+  return joint->feedback;
+}
+
+
+int dAreConnected (dBodyID b1, dBodyID b2)
+{
+  dAASSERT (b1 && b2);
+  // look through b1's neighbour list for b2
+  for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
+    if (n->body == b2) return 1;
+  }
+  return 0;
+}
+
+
+int dAreConnectedExcluding (dBodyID b1, dBodyID b2, int joint_type)
+{
+  dAASSERT (b1 && b2);
+  // look through b1's neighbour list for b2
+  for (dxJointNode *n=b1->firstjoint; n; n=n->next) {
+    if (dJointGetType (n->joint) != joint_type && n->body == b2) return 1;
+  }
+  return 0;
+}
+
+//****************************************************************************
+// world
+
+dxWorld * dWorldCreate()
+{
+  dxWorld *w = new dxWorld;
+  w->firstbody = 0;
+  w->firstjoint = 0;
+  w->nb = 0;
+  w->nj = 0;
+  dSetZero (w->gravity,4);
+  w->global_erp = REAL(0.2);
+#if defined(dSINGLE)
+  w->global_cfm = 1e-5f;
+#elif defined(dDOUBLE)
+  w->global_cfm = 1e-10;
+#else
+  #error dSINGLE or dDOUBLE must be defined
+#endif
+  return w;
+}
+
+
+void dWorldDestroy (dxWorld *w)
+{
+  // delete all bodies and joints
+  dAASSERT (w);
+  dxBody *nextb, *b = w->firstbody;
+  while (b) {
+    nextb = (dxBody*) b->next;
+    delete b;
+    b = nextb;
+  }
+  dxJoint *nextj, *j = w->firstjoint;
+  while (j) {
+    nextj = (dxJoint*)j->next;
+    if (j->flags & dJOINT_INGROUP) {
+      // the joint is part of a group, so "deactivate" it instead
+      j->world = 0;
+      j->node[0].body = 0;
+      j->node[0].next = 0;
+      j->node[1].body = 0;
+      j->node[1].next = 0;
+      dMessage (0,"warning: destroying world containing grouped joints");
+    }
+    else {
+      dFree (j,j->vtable->size);
+    }
+    j = nextj;
+  }
+  delete w;
+}
+
+
+void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
+{
+  dAASSERT (w);
+  w->gravity[0] = x;
+  w->gravity[1] = y;
+  w->gravity[2] = z;
+}
+
+
+void dWorldGetGravity (dWorldID w, dVector3 g)
+{
+  dAASSERT (w);
+  g[0] = w->gravity[0];
+  g[1] = w->gravity[1];
+  g[2] = w->gravity[2];
+}
+
+
+void dWorldSetERP (dWorldID w, dReal erp)
+{
+  dAASSERT (w);
+  w->global_erp = erp;
+}
+
+
+dReal dWorldGetERP (dWorldID w)
+{
+  dAASSERT (w);
+  return w->global_erp;
+}
+
+
+void dWorldSetCFM (dWorldID w, dReal cfm)
+{
+  dAASSERT (w);
+  w->global_cfm = cfm;
+}
+
+
+dReal dWorldGetCFM (dWorldID w)
+{
+  dAASSERT (w);
+  return w->global_cfm;
+}
+
+
+void dWorldStep (dWorldID w, dReal stepsize)
+{
+  dUASSERT (w,"bad world argument");
+  dUASSERT (stepsize > 0,"stepsize must be > 0");
+  processIslands (w,stepsize);
+}
+
+
+void dWorldImpulseToForce (dWorldID w, dReal stepsize,
+                           dReal ix, dReal iy, dReal iz,
+                           dVector3 force)
+{
+  dAASSERT (w);
+  stepsize = dRecip(stepsize);
+  force[0] = stepsize * ix;
+  force[1] = stepsize * iy;
+  force[2] = stepsize * iz;
+  // @@@ force[3] = 0;
+}
+
+//****************************************************************************
+// testing
+
+#define NUM 100
+
+#define DO(x)
+
+
+extern "C" void dTestDataStructures()
+{
+  int i;
+  DO(printf ("testDynamicsStuff()\n"));
+
+  dBodyID body [NUM];
+  int nb = 0;
+  dJointID joint [NUM];
+  int nj = 0;
+
+  for (i=0; i<NUM; i++) body[i] = 0;
+  for (i=0; i<NUM; i++) joint[i] = 0;
+
+  DO(printf ("creating world\n"));
+  dWorldID w = dWorldCreate();
+  checkWorld (w);
+
+  for (;;) {
+    if (nb < NUM && dRandReal() > 0.5) {
+      DO(printf ("creating body\n"));
+      body[nb] = dBodyCreate (w);
+      DO(printf ("\t--> %p\n",body[nb]));
+      nb++;
+      checkWorld (w);
+      DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
+    }
+    if (nj < NUM && nb > 2 && dRandReal() > 0.5) {
+      dBodyID b1 = body [dRand() % nb];
+      dBodyID b2 = body [dRand() % nb];
+      if (b1 != b2) {
+        DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
+        joint[nj] = dJointCreateBall (w,0);
+        DO(printf ("\t-->%p\n",joint[nj]));
+        checkWorld (w);
+        dJointAttach (joint[nj],b1,b2);
+        nj++;
+        checkWorld (w);
+        DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
+      }
+    }
+    if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
+      dBodyID b1 = body [dRand() % nb];
+      dBodyID b2 = body [dRand() % nb];
+      if (b1 != b2) {
+        int k = dRand() % nj;
+        DO(printf ("reattaching joint %p\n",joint[k]));
+        dJointAttach (joint[k],b1,b2);
+        checkWorld (w);
+        DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
+      }
+    }
+    if (nb > 0 && dRandReal() > 0.5) {
+      int k = dRand() % nb;
+      DO(printf ("destroying body %p\n",body[k]));
+      dBodyDestroy (body[k]);
+      checkWorld (w);
+      for (; k < (NUM-1); k++) body[k] = body[k+1];
+      nb--;
+      DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
+    }
+    if (nj > 0 && dRandReal() > 0.5) {
+      int k = dRand() % nj;
+      DO(printf ("destroying joint %p\n",joint[k]));
+      dJointDestroy (joint[k]);
+      checkWorld (w);
+      for (; k < (NUM-1); k++) joint[k] = joint[k+1];
+      nj--;
+      DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
+    }
+  }
+
+  /*
+  printf ("creating world\n");
+  dWorldID w = dWorldCreate();
+  checkWorld (w);
+  printf ("creating body\n");
+  dBodyID b1 = dBodyCreate (w);
+  checkWorld (w);
+  printf ("creating body\n");
+  dBodyID b2 = dBodyCreate (w);
+  checkWorld (w);
+  printf ("creating joint\n");
+  dJointID j = dJointCreateBall (w);
+  checkWorld (w);
+  printf ("attaching joint\n");
+  dJointAttach (j,b1,b2);
+  checkWorld (w);
+  printf ("destroying joint\n");
+  dJointDestroy (j);
+  checkWorld (w);
+  printf ("destroying body\n");
+  dBodyDestroy (b1);
+  checkWorld (w);
+  printf ("destroying body\n");
+  dBodyDestroy (b2);
+  checkWorld (w);
+  printf ("destroying world\n");
+  dWorldDestroy (w);
+  */
+}
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/step.cpp b/libraries/ode-0.9/contrib/BreakableJoints/step.cpp
new file mode 100644
index 0000000..38aed6c
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/step.cpp
@@ -0,0 +1,1170 @@
+/*************************************************************************
+ *                                                                       *
+ * 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"
+
+//****************************************************************************
+// 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;
+}
+
+//****************************************************************************
+// 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.
+
+static inline void moveAndRotateBody (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,irv;         // 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);
+}
+
+//****************************************************************************
+// 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++) moveAndRotateBody (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;
+
+/******************** breakable joint contribution ***********************/
+    // this saves us a few dereferences
+    dxJointBreakInfo *jBI = joint[i]->breakInfo;
+    // we need joint feedback if the joint is breakable or if the user
+    // requested feedback.
+        if (jBI||fb) {
+      // we need 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.
+      dJointFeedback temp_fb; // temporary storage for joint feedback
+          dReal data1[8],data2[8];
+          Multiply1_8q1 (data1, JJ, lambda+ofs[i], info[i].m);
+          dReal *cf1 = cforce + 8*b1->tag;
+          cf1[0] += (temp_fb.f1[0] = data1[0]);
+          cf1[1] += (temp_fb.f1[1] = data1[1]);
+          cf1[2] += (temp_fb.f1[2] = data1[2]);
+          cf1[4] += (temp_fb.t1[0] = data1[4]);
+          cf1[5] += (temp_fb.t1[1] = data1[5]);
+          cf1[6] += (temp_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] += (temp_fb.f2[0] = data2[0]);
+            cf2[1] += (temp_fb.f2[1] = data2[1]);
+            cf2[2] += (temp_fb.f2[2] = data2[2]);
+            cf2[4] += (temp_fb.t2[0] = data2[4]);
+            cf2[5] += (temp_fb.t2[1] = data2[5]);
+            cf2[6] += (temp_fb.t2[2] = data2[6]);
+          }
+          // if the user requested so we must copy the feedback information to
+          // the feedback struct that the user suplied.
+          if (fb) {
+            // copy temp_fb to fb
+            fb->f1[0] = temp_fb.f1[0];
+            fb->f1[1] = temp_fb.f1[1];
+            fb->f1[2] = temp_fb.f1[2];
+            fb->t1[0] = temp_fb.t1[0];
+            fb->t1[1] = temp_fb.t1[1];
+            fb->t1[2] = temp_fb.t1[2];
+            if (b2) {
+              fb->f2[0] = temp_fb.f2[0];
+              fb->f2[1] = temp_fb.f2[1];
+              fb->f2[2] = temp_fb.f2[2];
+              fb->t2[0] = temp_fb.t2[0];
+              fb->t2[1] = temp_fb.t2[1];
+              fb->t2[2] = temp_fb.t2[2];
+            }
+          }
+          // if the joint is breakable we need to check the breaking conditions
+      if (jBI) {
+        dReal relCF1[3];
+                dReal relCT1[3];
+                // multiply the force and torque vectors by the rotation matrix of body 1
+                dMULTIPLY1_331 (&relCF1[0],b1->R,&temp_fb.f1[0]);
+                dMULTIPLY1_331 (&relCT1[0],b1->R,&temp_fb.t1[0]);
+                if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
+                  // check if the force is to high
+          for (int i = 0; i < 3; i++) {
+            if (relCF1[i] > jBI->b1MaxF[i]) {
+                      jBI->flags |= dJOINT_BROKEN;
+                      goto doneCheckingBreaks;
+                    }
+          }
+                }
+                if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
+                  // check if the torque is to high
+          for (int i = 0; i < 3; i++) {
+            if (relCT1[i] > jBI->b1MaxT[i]) {
+                      jBI->flags |= dJOINT_BROKEN;
+                      goto doneCheckingBreaks;
+            }
+          }
+                }
+        if (b2) {
+          dReal relCF2[3];
+          dReal relCT2[3];
+          // multiply the force and torque vectors by the rotation matrix of body 2
+          dMULTIPLY1_331 (&relCF2[0],b2->R,&temp_fb.f2[0]);
+          dMULTIPLY1_331 (&relCT2[0],b2->R,&temp_fb.t2[0]);
+                  if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
+            // check if the force is to high
+            for (int i = 0; i < 3; i++) {
+              if (relCF2[i] > jBI->b2MaxF[i]) {
+                jBI->flags |= dJOINT_BROKEN;
+                goto doneCheckingBreaks;
+              }
+            }
+                  }
+                  if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
+                  // check if the torque is to high
+            for (int i = 0; i < 3; i++) {
+              if (relCT2[i] > jBI->b2MaxT[i]) {
+                jBI->flags |= dJOINT_BROKEN;
+                goto doneCheckingBreaks;
+              }
+            }
+                  }
+        }
+                doneCheckingBreaks:
+                ;
+      }
+    }
+/*************************************************************************/
+      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++) moveAndRotateBody (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
+}
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp b/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp
new file mode 100755
index 0000000..a0eb9ae
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/stepfast.cpp
@@ -0,0 +1,1212 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * Fast iterative solver, David Whittaker. Email: david@csworkbench.com  *
+ *                                                                       *
+ * 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 StepFast code by David Whittaker. This code is faster, but
+// sometimes less stable than, the original "big matrix" code.
+// Refer to the user's manual for more information.
+// Note that this source file duplicates a lot of stuff from step.cpp,
+// eventually we should move the common code to a third file.
+
+#include "objects.h"
+#include "joint.h"
+#include <ode/config.h>
+#include <ode/objects.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 "step.h"
+
+
+// misc defines
+
+#define ALLOCA dALLOCA16
+
+#define RANDOM_JOINT_ORDER
+//#define FAST_FACTOR   //use a factorization approximation to the LCP solver (fast, theoretically less accurate)
+#define SLOW_LCP      //use the old LCP solver
+//#define NO_ISLANDS    //does not perform island creation code (3~4% of simulation time), body disabling doesn't work
+//#define TIMING
+
+
+static int autoEnableDepth = 2;
+
+void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth)
+{
+        if (autodepth > 0)
+                autoEnableDepth = autodepth;
+        else
+                autoEnableDepth = 0;
+}
+
+int dWorldGetAutoEnableDepthSF1 (dxWorld *world)
+{
+        return autoEnableDepth;
+}
+
+//little bit of math.... the _sym_ functions assume the return matrix will be symmetric
+static void
+Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip)
+{
+        int i, j;
+        dReal sum, *aa, *ad, *bb, *cc;
+        dIASSERT (p > 0 && A && B && C);
+        bb = B;
+        for (i = 0; i < p; i++)
+        {
+                //aa is going accross the matrix, ad down
+                aa = ad = A;
+                cc = C;
+                for (j = i; j < p; 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];
+                        *(aa++) = *ad = sum;
+                        ad += Askip;
+                        cc += 8;
+                }
+                bb += 8;
+                A += Askip + 1;
+                C += 8;
+        }
+}
+
+static void
+MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip)
+{
+        int i, j;
+        dReal sum, *aa, *ad, *bb, *cc;
+        dIASSERT (p > 0 && A && B && C);
+        bb = B;
+        for (i = 0; i < p; i++)
+        {
+                //aa is going accross the matrix, ad down
+                aa = ad = A;
+                cc = C;
+                for (j = i; j < p; 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];
+                        *(aa++) += sum;
+                        *ad += sum;
+                        ad += Askip;
+                        cc += 8;
+                }
+                bb += 8;
+                A += Askip + 1;
+                C += 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
+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;
+}
+
+//****************************************************************************
+// 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.
+
+static inline void
+moveAndRotateBody (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, irv;      // 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);
+}
+
+//****************************************************************************
+//This is an implementation of the iterated/relaxation algorithm.
+//Here is a quick overview of the algorithm per Sergi Valverde's posts to the
+//mailing list:
+//
+//  for i=0..N-1 do
+//      for c = 0..C-1 do
+//          Solve constraint c-th
+//          Apply forces to constraint bodies
+//      next
+//  next
+//  Integrate bodies
+
+void
+dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize)
+{
+        int i, j, k;
+# ifdef TIMING
+        dTimerNow ("constraint preprocessing");
+# endif
+
+        dReal stepsize1 = dRecip (stepsize);
+
+        int m = info.m;
+        // nothing to do if no constraints.
+        if (m <= 0)
+                return;
+
+        int nub = 0;
+        if (info.nub == info.m)
+                nub = m;
+
+        // 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[2 * 6 * 8];
+        //dSetZero (JinvM, 2 * m * 8);
+
+        dReal *Jsrc = Jinfo.J1l;
+        dReal *Jdst = JinvM;
+        if (body[0])
+        {
+                for (j = m - 1; j >= 0; j--)
+                {
+                        for (k = 0; k < 3; k++)
+                                Jdst[k] = Jsrc[k] * body[0]->invMass;
+                        dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]);
+                        Jsrc += 8;
+                        Jdst += 8;
+                }
+        }
+        if (body[1])
+        {
+                Jsrc = Jinfo.J2l;
+                Jdst = JinvM + 8 * m;
+                for (j = m - 1; j >= 0; j--)
+                {
+                        for (k = 0; k < 3; k++)
+                                Jdst[k] = Jsrc[k] * body[1]->invMass;
+                        dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]);
+                        Jsrc += 8;
+                        Jdst += 8;
+                }
+        }
+
+
+        // now compute A = JinvM * J'.
+        int mskip = dPAD (m);
+        dReal A[6 * 8];
+        //dSetZero (A, 6 * 8);
+
+        if (body[0])
+                Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip);
+        if (body[1])
+                MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, m, mskip);
+
+        // add cfm to the diagonal of A
+        for (i = 0; i < m; i++)
+                A[i * mskip + i] += Jinfo.cfm[i] * stepsize1;
+
+        // compute the right hand side `rhs'
+#   ifdef TIMING
+        dTimerNow ("compute rhs");
+#   endif
+        dReal tmp1[16];
+        //dSetZero (tmp1, 16);
+        // put v/h + invM*fe into tmp1
+        for (i = 0; i < 2; i++)
+        {
+                if (!body[i])
+                        continue;
+                for (j = 0; j < 3; j++)
+                        tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1;
+                dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], 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[6];
+        //dSetZero (rhs, 6);
+
+        if (body[0])
+                Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m);
+        if (body[1])
+                MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m);
+
+        // complete rhs
+        for (i = 0; i < m; i++)
+                rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i];
+
+#ifdef SLOW_LCP
+        // 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));
+        dReal lo[6], hi[6];
+        memcpy (lo, Jinfo.lo, m * sizeof (dReal));
+        memcpy (hi, Jinfo.hi, m * sizeof (dReal));
+        dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex);
+#endif
+
+        // LCP Solver replacement:
+        // This algorithm goes like this:
+        // Do a straightforward LDLT factorization of the matrix A, solving for
+        // A*x = rhs
+        // For each x[i] that is outside of the bounds of lo[i] and hi[i],
+        //    clamp x[i] into that range.
+        //    Substitute into A the now known x's
+        //    subtract the residual away from the rhs.
+        //    Remove row and column i from L, updating the factorization
+        //    place the known x's at the end of the array, keeping up with location in p
+        // Repeat until all constraints have been clamped or all are within bounds
+        //
+        // This is probably only faster in the single joint case where only one repeat is
+        // the norm.
+
+#ifdef FAST_FACTOR
+        // factorize A (L*D*L'=A)
+#       ifdef TIMING
+        dTimerNow ("factorize A");
+#       endif
+        dReal d[6];
+        dReal L[6 * 8];
+        memcpy (L, A, m * mskip * sizeof (dReal));
+        dFactorLDLT (L, d, m, mskip);
+
+        // compute lambda
+#       ifdef TIMING
+        dTimerNow ("compute lambda");
+#       endif
+
+        int left = m;                           //constraints left to solve.
+        int remove[6];
+        dReal lambda[6];
+        dReal x[6];
+        int p[6];
+        for (i = 0; i < 6; i++)
+                p[i] = i;
+        while (true)
+        {
+                memcpy (x, rhs, left * sizeof (dReal));
+                dSolveLDLT (L, d, x, left, mskip);
+
+                int fixed = 0;
+                for (i = 0; i < left; i++)
+                {
+                        j = p[i];
+                        remove[i] = false;
+                        // This isn't the exact same use of findex as dSolveLCP.... since x[findex]
+                        // may change after I've already clamped x[i], but it should be close
+                        if (Jinfo.findex[j] > -1)
+                        {
+                                dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]);
+                                if (x[i] > f)
+                                        x[i] = f;
+                                else if (x[i] < -f)
+                                        x[i] = -f;
+                                else
+                                        continue;
+                        }
+                        else
+                        {
+                                if (x[i] > Jinfo.hi[j])
+                                        x[i] = Jinfo.hi[j];
+                                else if (x[i] < Jinfo.lo[j])
+                                        x[i] = Jinfo.lo[j];
+                                else
+                                        continue;
+                        }
+                        remove[i] = true;
+                        fixed++;
+                }
+                if (fixed == 0 || fixed == left)        //no change or all constraints solved
+                        break;
+
+                for (i = 0; i < left; i++)      //sub in to right hand side.
+                        if (remove[i])
+                                for (j = 0; j < left; j++)
+                                        if (!remove[j])
+                                                rhs[j] -= A[j * mskip + i] * x[i];
+
+                for (int r = left - 1; r >= 0; r--)     //eliminate row/col for fixed variables
+                {
+                        if (remove[r])
+                        {
+                                //dRemoveLDLT adapted for use without row pointers.
+                                if (r == left - 1)
+                                {
+                                        left--;
+                                        continue;       // deleting last row/col is easy
+                                }
+                                else if (r == 0)
+                                {
+                                        dReal a[6];
+                                        for (i = 0; i < left; i++)
+                                                a[i] = -A[i * mskip];
+                                        a[0] += REAL (1.0);
+                                        dLDLTAddTL (L, d, a, left, mskip);
+                                }
+                                else
+                                {
+                                        dReal t[6];
+                                        dReal a[6];
+                                        for (i = 0; i < r; i++)
+                                                t[i] = L[r * mskip + i] / d[i];
+                                        for (i = 0; i < left - r; i++)
+                                                a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r];
+                                        a[0] += REAL (1.0);
+                                        dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip);
+                                }
+
+                                dRemoveRowCol (L, left, mskip, r);
+                                //end dRemoveLDLT
+
+                                left--;
+                                if (r < (left - 1))
+                                {
+                                        dReal tx = x[r];
+                                        memmove (d + r, d + r + 1, (left - r) * sizeof (dReal));
+                                        memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal));
+                                        //x will get written over by rhs anyway, no need to move it around
+                                        //just store the fixed value we just discovered in it.
+                                        x[left] = tx;
+                                        for (i = 0; i < m; i++)
+                                                if (p[i] > r && p[i] <= left)
+                                                        p[i]--;
+                                        p[r] = left;
+                                }
+                        }
+                }
+        }
+
+        for (i = 0; i < m; i++)
+                lambda[i] = x[p[i]];
+#       endif
+        // compute the constraint force `cforce'
+#       ifdef TIMING
+        dTimerNow ("compute constraint force");
+#endif
+
+        // compute cforce = J'*lambda
+        dJointFeedback *fb = joint->feedback;
+        dReal cforce[16];
+        //dSetZero (cforce, 16);
+
+/******************** breakable joint contribution ***********************/
+        // this saves us a few dereferences
+    dxJointBreakInfo *jBI = joint->breakInfo;
+    // we need joint feedback if the joint is breakable or if the user
+    // requested feedback.
+        if (jBI||fb) {
+                // we need 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.
+                dJointFeedback temp_fb; // temporary storage for joint feedback
+                dReal data1[8],data2[8];
+                if (body[0])
+                {
+                        Multiply1_8q1 (data1, Jinfo.J1l, lambda, m);
+                        dReal *cf1 = cforce;
+                        cf1[0] = (temp_fb.f1[0] = data1[0]);
+                        cf1[1] = (temp_fb.f1[1] = data1[1]);
+                        cf1[2] = (temp_fb.f1[2] = data1[2]);
+                        cf1[4] = (temp_fb.t1[0] = data1[4]);
+                        cf1[5] = (temp_fb.t1[1] = data1[5]);
+                        cf1[6] = (temp_fb.t1[2] = data1[6]);
+                }
+                if (body[1])
+                {
+                        Multiply1_8q1 (data2, Jinfo.J2l, lambda, m);
+                        dReal *cf2 = cforce + 8;
+                        cf2[0] = (temp_fb.f2[0] = data2[0]);
+                        cf2[1] = (temp_fb.f2[1] = data2[1]);
+                        cf2[2] = (temp_fb.f2[2] = data2[2]);
+                        cf2[4] = (temp_fb.t2[0] = data2[4]);
+                        cf2[5] = (temp_fb.t2[1] = data2[5]);
+                        cf2[6] = (temp_fb.t2[2] = data2[6]);
+                }
+                // if the user requested so we must copy the feedback information to
+                // the feedback struct that the user suplied.
+                if (fb) {
+                        // copy temp_fb to fb
+                        fb->f1[0] = temp_fb.f1[0];
+                        fb->f1[1] = temp_fb.f1[1];
+                        fb->f1[2] = temp_fb.f1[2];
+                        fb->t1[0] = temp_fb.t1[0];
+                        fb->t1[1] = temp_fb.t1[1];
+                        fb->t1[2] = temp_fb.t1[2];
+                        if (body[1]) {
+                                fb->f2[0] = temp_fb.f2[0];
+                                fb->f2[1] = temp_fb.f2[1];
+                                fb->f2[2] = temp_fb.f2[2];
+                                fb->t2[0] = temp_fb.t2[0];
+                                fb->t2[1] = temp_fb.t2[1];
+                                fb->t2[2] = temp_fb.t2[2];
+                        }
+                }
+                // if the joint is breakable we need to check the breaking conditions
+                if (jBI) {
+                        dReal relCF1[3];
+                        dReal relCT1[3];
+                        // multiply the force and torque vectors by the rotation matrix of body 1
+                        dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]);
+                        dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]);
+                        if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) {
+                                // check if the force is to high
+                                for (int i = 0; i < 3; i++) {
+                                        if (relCF1[i] > jBI->b1MaxF[i]) {
+                                                jBI->flags |= dJOINT_BROKEN;
+                                                goto doneCheckingBreaks;
+                                        }
+                                }
+                        }
+                        if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) {
+                                // check if the torque is to high
+                                for (int i = 0; i < 3; i++) {
+                                        if (relCT1[i] > jBI->b1MaxT[i]) {
+                                                jBI->flags |= dJOINT_BROKEN;
+                                                goto doneCheckingBreaks;
+                                        }
+                                }
+                        }
+                        if (body[1]) {
+                                dReal relCF2[3];
+                                dReal relCT2[3];
+                                // multiply the force and torque vectors by the rotation matrix of body 2
+                                dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]);
+                                dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]);
+                                if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) {
+                                        // check if the force is to high
+                                        for (int i = 0; i < 3; i++) {
+                                                if (relCF2[i] > jBI->b2MaxF[i]) {
+                                                        jBI->flags |= dJOINT_BROKEN;
+                                                        goto doneCheckingBreaks;
+                                                }
+                                        }
+                                }
+                                if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) {
+                                        // check if the torque is to high
+                                        for (int i = 0; i < 3; i++) {
+                                                if (relCT2[i] > jBI->b2MaxT[i]) {
+                                                        jBI->flags |= dJOINT_BROKEN;
+                                                        goto doneCheckingBreaks;
+                                                }
+                                        }
+                                }
+                        }
+                        doneCheckingBreaks:
+                        ;
+                }
+        }
+/*************************************************************************/
+        else
+        {
+                // no feedback is required, let's compute cforce the faster way
+                if (body[0])
+                        Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m);
+                if (body[1])
+                        Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m);
+        }
+
+        for (i = 0; i < 2; i++)
+        {
+                if (!body[i])
+                        continue;
+                for (j = 0; j < 3; j++)
+                {
+                        body[i]->facc[j] += cforce[i * 8 + j];
+                        body[i]->tacc[j] += cforce[i * 8 + 4 + j];
+                }
+        }
+}
+
+void
+dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations)
+{
+#   ifdef TIMING
+        dTimerNow ("preprocessing");
+#   endif
+        dxBody *bodyPair[2], *body;
+        dReal *GIPair[2], *GinvIPair[2];
+        dxJoint *joint;
+        int iter, b, j, i;
+        dReal ministep = stepsize / maxiterations;
+
+        // 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 **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *));
+        memcpy (joints, _joints, nj * sizeof (dxJoint *));
+
+        // 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
+                joints[j]->vtable->getInfo1 (joints[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)
+                {
+                        joints[i] = joints[j];
+                        joints[i]->tag = i;
+                        i++;
+                }
+                else
+                {
+                        joints[j]->tag = -1;
+                }
+        }
+        nj = i;
+
+        // the purely unbounded constraints
+        for (i = 0; i < nj; i++)
+        {
+                ofs[i] = m;
+                m += info[i].m;
+        }
+        dReal *c = NULL;
+        dReal *cfm = NULL;
+        dReal *lo = NULL;
+        dReal *hi = NULL;
+        int *findex = NULL;
+
+        dReal *J = NULL;
+        dxJoint::Info2 * Jinfo = NULL;
+
+        if (m)
+        {
+        // 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.
+                c = (dReal *) ALLOCA (m * sizeof (dReal));
+                cfm = (dReal *) ALLOCA (m * sizeof (dReal));
+                lo = (dReal *) ALLOCA (m * sizeof (dReal));
+                hi = (dReal *) ALLOCA (m * sizeof (dReal));
+                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
+                J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal));
+                dSetZero (J, 2 * m * 8);
+                Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2));
+        for (i = 0; i < nj; i++)
+        {
+                Jinfo[i].rowskip = 8;
+                Jinfo[i].fps = dRecip (stepsize);
+                Jinfo[i].erp = world->global_erp;
+                Jinfo[i].J1l = J + 2 * 8 * ofs[i];
+                Jinfo[i].J1a = Jinfo[i].J1l + 4;
+                Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m;
+                Jinfo[i].J2a = Jinfo[i].J2l + 4;
+                Jinfo[i].c = c + ofs[i];
+                Jinfo[i].cfm = cfm + ofs[i];
+                Jinfo[i].lo = lo + ofs[i];
+                Jinfo[i].hi = hi + ofs[i];
+                Jinfo[i].findex = findex + ofs[i];
+                //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i);
+        }
+
+        }
+
+        dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
+        dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal));
+        dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
+        dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
+        for (b = 0; b < nb; b++)
+        {
+                for (i = 0; i < 4; i++)
+                {
+                        saveFacc[b * 4 + i] = bodies[b]->facc[i];
+                        saveTacc[b * 4 + i] = bodies[b]->tacc[i];
+                        bodies[b]->tag = b;
+                }
+        }
+
+        for (iter = 0; iter < maxiterations; iter++)
+        {
+#       ifdef TIMING
+                dTimerNow ("applying inertia and gravity");
+#       endif
+                dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+                for (b = 0; b < nb; b++)
+                {
+                        body = bodies[b];
+
+                        // 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.
+
+                        // compute inertia tensor in global frame
+                        dMULTIPLY2_333 (tmp, body->mass.I, body->R);
+                        dMULTIPLY0_333 (globalI + b * 12, body->R, tmp);
+                        // compute inverse inertia tensor in global frame
+                        dMULTIPLY2_333 (tmp, body->invI, body->R);
+                        dMULTIPLY0_333 (globalInvI + b * 12, body->R, tmp);
+
+                        for (i = 0; i < 4; i++)
+                                body->tacc[i] = saveTacc[b * 4 + i];
+                        // compute rotational force
+                        dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel);
+                        dCROSS (body->tacc, -=, body->avel, tmp);
+
+                        // add the gravity force to all bodies
+                        if ((body->flags & dxBodyNoGravity) == 0)
+                        {
+                                body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0];
+                                body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1];
+                                body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2];
+                                body->facc[3] = 0;
+                        }
+
+                }
+
+#ifdef RANDOM_JOINT_ORDER
+#ifdef TIMING
+                dTimerNow ("randomizing joint order");
+#endif
+                //randomize the order of the joints by looping through the array
+                //and swapping the current joint pointer with a random one before it.
+                for (j = 0; j < nj; j++)
+                {
+                        joint = joints[j];
+                        dxJoint::Info1 i1 = info[j];
+                        dxJoint::Info2 i2 = Jinfo[j];
+                        int r = rand () % (j + 1);
+                        joints[j] = joints[r];
+                        info[j] = info[r];
+                        Jinfo[j] = Jinfo[r];
+                        joints[r] = joint;
+                        info[r] = i1;
+                        Jinfo[r] = i2;
+                }
+#endif
+
+                //now iterate through the random ordered joint array we created.
+                for (j = 0; j < nj; j++)
+                {
+#ifdef TIMING
+                        dTimerNow ("setting up joint");
+#endif
+                        joint = joints[j];
+                        bodyPair[0] = joint->node[0].body;
+                        bodyPair[1] = joint->node[1].body;
+
+                        if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled))
+                                bodyPair[0] = 0;
+                        if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled))
+                                bodyPair[1] = 0;
+                        
+                        //if this joint is not connected to any enabled bodies, skip it.
+                        if (!bodyPair[0] && !bodyPair[1])
+                                continue;
+                        
+                        if (bodyPair[0])
+                        {
+                                GIPair[0] = globalI + bodyPair[0]->tag * 12;
+                                GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12;
+                        }
+                        if (bodyPair[1])
+                        {
+                                GIPair[1] = globalI + bodyPair[1]->tag * 12;
+                                GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12;
+                        }
+
+                        joints[j]->vtable->getInfo2 (joints[j], Jinfo + j);
+
+                        //dInternalStepIslandFast is an exact copy of the old routine with one
+                        //modification: the calculated forces are added back to the facc and tacc
+                        //vectors instead of applying them to the bodies and moving them.
+                        if (info[j].m > 0)
+                        {
+                        dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep);
+                        }               
+                }
+                //  }
+#       ifdef TIMING
+                dTimerNow ("moving bodies");
+#       endif
+                //Now we can simulate all the free floating bodies, and move them.
+                for (b = 0; b < nb; b++)
+                {
+                        body = bodies[b];
+
+                        for (i = 0; i < 4; i++)
+                        {
+                                body->facc[i] *= ministep;
+                                body->tacc[i] *= ministep;
+                        }
+
+                        //apply torque
+                        dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc);
+
+                        //apply force
+                        for (i = 0; i < 3; i++)
+                                body->lvel[i] += body->invMass * body->facc[i];
+
+                        //move It!
+                        moveAndRotateBody (body, ministep);
+                }
+        }
+        for (b = 0; b < nb; b++)
+                for (j = 0; j < 4; j++)
+                        bodies[b]->facc[j] = bodies[b]->tacc[j] = 0;
+}
+
+
+#ifdef NO_ISLANDS
+
+// Since the iterative algorithm doesn't care about islands of bodies, this is a
+// faster algorithm that just sends it all the joints and bodies in one array.
+// It's downfall is it's inability to handle disabled bodies as well as the old one.
+static void
+processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations)
+{
+        // nothing to do if no bodies
+        if (world->nb <= 0)
+                return;
+
+#       ifdef TIMING
+        dTimerStart ("creating joint and body arrays");
+#       endif
+        dxBody **bodies, *body;
+        dxJoint **joints, *joint;
+        joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *));
+        bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *));
+
+        int nj = 0;
+        for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next)
+                joints[nj++] = joint;
+
+        int nb = 0;
+        for (body = world->firstbody; body; body = (dxBody *) body->next)
+                bodies[nb++] = body;
+
+        dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations);
+#       ifdef TIMING
+        dTimerEnd ();
+        dTimerReport (stdout, 1);
+#       endif
+}
+
+#else
+
+//****************************************************************************
+// 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.
+
+static void
+processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations)
+{
+#ifdef TIMING
+        dTimerStart ("Island Setup");
+#endif
+        dxBody *b, *bb, **body;
+        dxJoint *j, **joint;
+
+        // nothing to do if no bodies
+        if (world->nb <= 0)
+                return;
+
+        // 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'
+        int tbcount = 0;
+        int tjcount = 0;
+        
+        // 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 *));
+        int *autostack = (int *) ALLOCA (stackalloc * sizeof (int));
+
+        for (bb = world->firstbody; bb; bb = (dxBody *) bb->next)
+        {
+#ifdef TIMING
+                dTimerNow ("Island Processing");
+#endif
+                // 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;
+                int autoDepth = autoEnableDepth;
+                b = bb;
+                body[0] = bb;
+                bcount = 1;
+                jcount = 0;
+                goto quickstart;
+                while (stacksize > 0)
+                {
+                        b = stack[--stacksize]; // pop body off stack
+                        autoDepth = autostack[stacksize];
+                        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)
+                                {
+                                        int thisDepth = autoEnableDepth;
+                                        n->joint->tag = 1;
+                                        joint[jcount++] = n->joint;
+                                        if (n->body && !n->body->tag)
+                                        {
+                                                if (n->body->flags & dxBodyDisabled)
+                                                        thisDepth = autoDepth - 1;
+                                                if (thisDepth < 0)
+                                                        continue;
+                                                n->body->flags &= ~dxBodyDisabled;
+                                                n->body->tag = 1;
+                                                autostack[stacksize] = thisDepth;
+                                                stack[stacksize++] = n->body;
+                                        }
+                                }
+                        }
+                        dIASSERT (stacksize <= world->nb);
+                        dIASSERT (stacksize <= world->nj);
+                }
+
+                // now do something with body and joint lists
+                dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations);
+
+                // 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;
+                
+                tbcount += bcount;
+                tjcount += jcount;
+        }
+        
+#ifdef TIMING
+        dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount);
+#endif
+
+        // 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
+  /******************** breakable joint contribution ***********************/
+  dxJoint* nextJ;
+  if (!world->firstjoint)
+    nextJ = 0;
+  else
+    nextJ = (dxJoint*)world->firstjoint->next;
+  for (j=world->firstjoint; j; j=nextJ) {
+        nextJ = (dxJoint*)j->next;
+        // check if joint is breakable and broken
+    if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) {
+                // detach (break) the joint
+        dJointAttach (j, 0, 0);
+                // call the callback function if it is set
+                if (j->breakInfo->callback) j->breakInfo->callback (j);
+                // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set
+                if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j);
+      }
+  }
+  /*************************************************************************/
+
+#       ifdef TIMING
+        dTimerEnd ();
+        dTimerReport (stdout, 1);
+#       endif
+}
+
+#endif
+
+
+void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations)
+{
+        dUASSERT (w, "bad world argument");
+        dUASSERT (stepsize > 0, "stepsize must be > 0");
+        processIslandsFast (w, stepsize, maxiterations);
+}
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp b/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp
new file mode 100644
index 0000000..bfed3a3
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/test_breakable.cpp
@@ -0,0 +1,416 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+buggy with suspension.
+this also shows you how to use geom groups.
+
+*/
+
+
+#include <stdlib.h>
+
+#include <ode/ode.h>
+#include <drawstuff/drawstuff.h>
+
+#ifdef _MSC_VER
+#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
+#endif
+
+// select correct drawing functions
+
+#ifdef dDOUBLE
+#define dsDrawBox dsDrawBoxD
+#define dsDrawSphere dsDrawSphereD
+#define dsDrawCylinder dsDrawCylinderD
+#define dsDrawCappedCylinder dsDrawCappedCylinderD
+#endif
+
+
+// some constants
+
+#define LENGTH 0.7      // chassis length
+#define WIDTH 0.4       // chassis width
+#define HEIGHT 0.2      // chassis height
+#define RADIUS 0.22     // wheel radius
+#define STARTZ 0.4      // starting height of chassis
+#define CMASS 1         // chassis mass
+#define WMASS 0.2       // wheel mass
+
+// dynamics and collision objects (chassis, 4 wheels, environment, obstacles, chain)
+static dWorldID world;
+static dSpaceID space;
+
+// chain stuff
+static const float chain_radius = 0.1;
+static const float chain_mass = 0.1;
+static const int chain_num = 10;
+static dBodyID chain_body[chain_num];
+static dGeomID chain_geom[chain_num];
+static dJointID chain_joint[chain_num-1]; 
+
+// 1 chasses, 4 wheels
+static dBodyID body[5];
+// joint[0] is left front wheel, joint[1] is right front wheel
+static dJointID joint[4]; 
+static int joint_exists[4];
+static dJointGroupID contactgroup;
+static dGeomID ground;
+static dSpaceID car_space;
+static dGeomID box[1];
+static dGeomID sphere[4];
+static dGeomID ground_box;
+static const int obstacle_num = 25;
+static dGeomID obstacle[obstacle_num];
+
+// things that the user controls
+
+static dReal speed=0,steer=0;   // user commands
+
+
+
+// this is called by dSpaceCollide when two objects in space are
+// potentially colliding.
+
+static void nearCallback (void *data, dGeomID o1, dGeomID o2)
+{
+  int i,n;
+
+//   // do not collide objects that are connected
+//   dBodyID b1 = dGeomGetBody (o1),
+//           b2 = dGeomGetBody (o2);
+//   if (b1 && b2 && dAreConnected(b1, b2)) return;
+  
+  const int N = 10;
+  dContact contact[N];
+  n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
+  if (n > 0) {
+    for (i=0; i<n; i++) {
+      contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
+        dContactSoftERP | dContactSoftCFM | dContactApprox1;
+      contact[i].surface.mu = dInfinity;
+      contact[i].surface.slip1 = 0.1;
+      contact[i].surface.slip2 = 0.1;
+      contact[i].surface.soft_erp = 0.5;
+      contact[i].surface.soft_cfm = 0.3;
+      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
+      dJointAttach (c,
+                    dGeomGetBody(contact[i].geom.g1),
+                    dGeomGetBody(contact[i].geom.g2));
+    }
+  }
+}
+
+// callback function for joints that break
+static void jointBreakCallback (dJointID j)
+{
+       if (j == joint[0]) joint_exists[0] = 0;
+  else if (j == joint[1]) joint_exists[1] = 0;
+  else if (j == joint[2]) joint_exists[2] = 0;
+  else if (j == joint[3]) joint_exists[3] = 0;
+  printf ("A joint just broke\n");
+}
+
+// start simulation - set viewpoint
+
+static void start()
+{
+  static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
+  static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
+  dsSetViewpoint (xyz,hpr);
+  printf ("Press:\t'a' to increase speed.\n"
+          "\t'z' to decrease speed.\n"
+          "\t',' to steer left.\n"
+          "\t'.' to steer right.\n"
+          "\t' ' to reset speed and steering.\n");
+}
+
+
+// called when a key pressed
+
+static void command (int cmd)
+{
+  switch (cmd) {
+  case 'a': case 'A':
+    speed += 0.3;
+    break;
+  case 'z': case 'Z':
+    speed -= 0.3;
+    break;
+  case ',':
+    steer -= 0.5;
+    break;
+  case '.':
+    steer += 0.5;
+    break;
+  case ' ':
+    speed = 0;
+    steer = 0;
+    break;
+  }
+}
+
+
+// simulation loop
+
+static void simLoop (int pause)
+{
+  int i;
+  if (!pause) {
+    for (i=0; i<2; i++) {
+          if (joint_exists[i]) {
+        // motor
+        dJointSetHinge2Param (joint[i],dParamVel2,-speed);
+        dJointSetHinge2Param (joint[i],dParamFMax2,0.1);
+
+        // steering
+        dReal v = steer - dJointGetHinge2Angle1 (joint[i]);
+        if (v > 0.1) v = 0.1;
+        if (v < -0.1) v = -0.1;
+        v *= 10.0;
+        dJointSetHinge2Param (joint[i],dParamVel,v);
+        dJointSetHinge2Param (joint[i],dParamFMax,0.2);
+        dJointSetHinge2Param (joint[i],dParamLoStop,-0.75);
+        dJointSetHinge2Param (joint[i],dParamHiStop,0.75);
+        dJointSetHinge2Param (joint[i],dParamFudgeFactor,0.1);
+          }
+        }
+        
+    dSpaceCollide (space,0,&nearCallback);
+    //dWorldStep (world,0.05);
+        dWorldStepFast1 (world,0.05,5);
+
+    // remove all contact joints
+    dJointGroupEmpty (contactgroup);
+  }
+
+  dsSetColor (0,1,1);
+  dsSetTexture (DS_WOOD);
+  dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
+  dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
+  dsSetColor (1,1,1);
+  for (i=1; i<=4; i++) 
+    dsDrawCylinder (dBodyGetPosition(body[i]),
+                    dBodyGetRotation(body[i]),
+                                        0.2,
+                    RADIUS);
+
+  dVector3 ss;
+  dGeomBoxGetLengths (ground_box,ss);
+  dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
+  
+  dsSetColor (1,0,0);
+  for (i=0; i<obstacle_num; i++) {
+    dVector3 ss;
+        dGeomBoxGetLengths (obstacle[i],ss);
+        dsDrawBox (dGeomGetPosition(obstacle[i]),dGeomGetRotation(obstacle[i]),ss);
+  }
+
+  dsSetColor (1,1,0);
+  for (i=0; i<chain_num; i++) {
+    dsDrawSphere (dGeomGetPosition(chain_geom[i]),dGeomGetRotation(chain_geom[i]),chain_radius);
+  }
+  
+  /*
+  printf ("%.10f %.10f %.10f %.10f\n",
+          dJointGetHingeAngle (joint[1]),
+          dJointGetHingeAngle (joint[2]),
+          dJointGetHingeAngleRate (joint[1]),
+          dJointGetHingeAngleRate (joint[2]));
+  */
+}
+
+int main (int argc, char **argv)
+{
+  int i;
+  dMass m;
+
+  // setup pointers to drawstuff callback functions
+  dsFunctions fn;
+  fn.version = DS_VERSION;
+  fn.start = &start;
+  fn.step = &simLoop;
+  fn.command = &command;
+  fn.stop = 0;
+  fn.path_to_textures = "../../drawstuff/textures";
+  if(argc==2)
+    {
+        fn.path_to_textures = argv[1];
+    }
+  // create world
+
+  world = dWorldCreate();
+  space = dHashSpaceCreate (0);
+  contactgroup = dJointGroupCreate (0);
+  dWorldSetGravity (world,0,0,-0.5);
+  ground = dCreatePlane (space,0,0,1,0);
+
+  // chassis body
+  body[0] = dBodyCreate (world);
+  dBodySetPosition (body[0],0,0,STARTZ);
+  dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
+  dMassAdjust (&m,CMASS);
+  dBodySetMass (body[0],&m);
+  box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
+  dGeomSetBody (box[0],body[0]);
+  
+  // a chain
+  for (i=0; i<chain_num; i++) {
+    chain_body[i] = dBodyCreate (world);
+        dBodySetPosition (chain_body[i],-LENGTH-(i*2*chain_radius),0,STARTZ-HEIGHT*0.5);
+    dMassSetSphere (&m,1,chain_radius);
+    dMassAdjust (&m,chain_mass);
+    dBodySetMass (chain_body[i],&m);
+    chain_geom[i] = dCreateSphere (space,chain_radius);
+    dGeomSetBody (chain_geom[i],chain_body[i]);
+  }
+  
+  // wheel bodies
+  for (i=1; i<=4; i++) {
+    body[i] = dBodyCreate (world);
+    dQuaternion q;
+    dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
+    dBodySetQuaternion (body[i],q);
+    dMassSetSphere (&m,1,RADIUS);
+    dMassAdjust (&m,WMASS);
+    dBodySetMass (body[i],&m);
+    sphere[i-1] = dCreateSphere (0,RADIUS);
+    dGeomSetBody (sphere[i-1],body[i]);
+  }
+  dBodySetPosition (body[1],  0.5*LENGTH,  WIDTH*1.0, STARTZ-HEIGHT*0.5);
+  dBodySetPosition (body[2],  0.5*LENGTH, -WIDTH*1.0, STARTZ-HEIGHT*0.5);
+  dBodySetPosition (body[3], -0.5*LENGTH,  WIDTH*1.0, STARTZ-HEIGHT*0.5);
+  dBodySetPosition (body[4], -0.5*LENGTH, -WIDTH*1.0, STARTZ-HEIGHT*0.5);
+
+  // front wheel hinge
+  /*
+  joint[0] = dJointCreateHinge2 (world,0);
+  dJointAttach (joint[0],body[0],body[1]);
+  const dReal *a = dBodyGetPosition (body[1]);
+  dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
+  dJointSetHinge2Axis1 (joint[0],0,0,1);
+  dJointSetHinge2Axis2 (joint[0],0,1,0);
+  */
+
+  // front and back wheel hinges
+  for (i=0; i<4; i++) {
+    joint[i] = dJointCreateHinge2 (world,0);
+        joint_exists[i] = 1;
+    dJointAttach (joint[i],body[0],body[i+1]);
+    const dReal *a = dBodyGetPosition (body[i+1]);
+    dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
+    dJointSetHinge2Axis1 (joint[i],0,0,1);
+    dJointSetHinge2Axis2 (joint[i],0,1,0);
+
+    // the wheels can break
+    dJointSetBreakable (joint[i], 1);
+    // the wheels wil break at a specific force
+    dJointSetBreakMode (joint[i], 
+                        dJOINT_BREAK_AT_B1_FORCE |
+                        dJOINT_BREAK_AT_B2_FORCE |
+                        dJOINT_DELETE_ON_BREAK);
+    // specify the force for the first body connected to the joint ...
+    dJointSetBreakForce (joint[i], 0, 2.5, 2.5, 2.5);
+    // and for the second body
+    dJointSetBreakForce (joint[i], 1, 2.5, 2.5, 2.5);
+        // set the callback function
+        dJointSetBreakCallback (joint[i], &jointBreakCallback);
+  }
+  
+  // joints for the chain
+  for (i=0; i<chain_num-1; i++) {
+    chain_joint[i] = dJointCreateFixed (world,0);
+    dJointAttach (chain_joint[i],chain_body[i+1],chain_body[i]);
+        dJointSetFixed (chain_joint[i]);
+        // the chain can break
+    dJointSetBreakable (chain_joint[i], 1);
+    // the chain wil break at a specific force
+    dJointSetBreakMode (chain_joint[i], 
+      dJOINT_BREAK_AT_B1_FORCE |
+      dJOINT_BREAK_AT_B2_FORCE |
+      dJOINT_DELETE_ON_BREAK);
+    // specify the force for the first body connected to the joint ...
+    dJointSetBreakForce (chain_joint[i], 0, 0.5, 0.5, 0.5);
+    // and for the second body
+    dJointSetBreakForce (chain_joint[i], 1, 0.5, 0.5, 0.5);
+        // set the callback function
+        dJointSetBreakCallback (chain_joint[i], &jointBreakCallback);
+  }
+  
+  // set joint suspension
+  for (i=0; i<4; i++) {
+    dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
+    dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.1);
+  }
+
+  // lock back wheels along the steering axis
+  for (i=1; i<4; i++) {
+    // set stops to make sure wheels always stay in alignment
+    dJointSetHinge2Param (joint[i],dParamLoStop,0);
+    dJointSetHinge2Param (joint[i],dParamHiStop,0);
+    // the following alternative method is no good as the wheels may get out
+    // of alignment:
+    //   dJointSetHinge2Param (joint[i],dParamVel,0);
+    //   dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
+  }
+  
+  // create car space and add it to the top level space
+  car_space = dSimpleSpaceCreate (space);
+  dSpaceSetCleanup (car_space,0);
+  dSpaceAdd (car_space,box[0]);
+  dSpaceAdd (car_space,sphere[0]);
+  dSpaceAdd (car_space,sphere[1]);
+  dSpaceAdd (car_space,sphere[2]);
+
+  // environment
+  ground_box = dCreateBox (space,2,1.5,1);
+  dMatrix3 R;
+  dRFromAxisAndAngle (R,0,1,0,-0.15);
+  dGeomSetPosition (ground_box,2,0,-0.34);
+  dGeomSetRotation (ground_box,R);
+  
+  // obstacles
+  for (i=0; i<obstacle_num; i++) {
+    dReal height = 0.1+(dReal(rand()%10)/10.0);
+    obstacle[i] = dCreateBox (space,0.2,0.2,height);
+        dGeomSetPosition (
+          obstacle[i],
+          (rand()%20)-10,
+          (rand()%20)-10,
+          height/2.0);
+  }
+
+  // run simulation
+  dsSimulationLoop (argc,argv,352,288,&fn);
+
+  dJointGroupDestroy (contactgroup);
+  dSpaceDestroy (space);
+  dWorldDestroy (world);
+  dGeomDestroy (box[0]);
+  for (i=0; i<4; i++)
+    dGeomDestroy (sphere[i]);
+        
+  dCloseODE ();
+  
+  return 0;
+}
diff --git a/libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp b/libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp
new file mode 100644
index 0000000..1dc0ab3
--- /dev/null
+++ b/libraries/ode-0.9/contrib/BreakableJoints/test_buggy.cpp
@@ -0,0 +1,327 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+/*
+
+buggy with suspension.
+this also shows you how to use geom groups.
+
+*/
+
+
+#include <ode/ode.h>
+#include <drawstuff/drawstuff.h>
+
+#ifdef _MSC_VER
+#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
+#endif
+
+// select correct drawing functions
+
+#ifdef dDOUBLE
+#define dsDrawBox dsDrawBoxD
+#define dsDrawSphere dsDrawSphereD
+#define dsDrawCylinder dsDrawCylinderD
+#define dsDrawCappedCylinder dsDrawCappedCylinderD
+#endif
+
+
+// some constants
+
+#define LENGTH 0.7      // chassis length
+#define WIDTH 0.5       // chassis width
+#define HEIGHT 0.2      // chassis height
+#define RADIUS 0.18     // wheel radius
+#define STARTZ 0.5      // starting height of chassis
+#define CMASS 1         // chassis mass
+#define WMASS 0.2       // wheel mass
+
+
+// dynamics and collision objects (chassis, 3 wheels, environment)
+
+static dWorldID world;
+static dSpaceID space;
+static dBodyID body[4];
+static dJointID joint[3];       // joint[0] is the front wheel
+static dJointGroupID contactgroup;
+static dGeomID ground;
+static dSpaceID car_space;
+static dGeomID box[1];
+static dGeomID sphere[3];
+static dGeomID ground_box;
+
+
+// things that the user controls
+
+static dReal speed=0,steer=0;   // user commands
+
+
+
+// this is called by dSpaceCollide when two objects in space are
+// potentially colliding.
+
+static void nearCallback (void *data, dGeomID o1, dGeomID o2)
+{
+  int i,n;
+
+  // only collide things with the ground
+  int g1 = (o1 == ground || o1 == ground_box);
+  int g2 = (o2 == ground || o2 == ground_box);
+  if (!(g1 ^ g2)) return;
+
+  const int N = 10;
+  dContact contact[N];
+  n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
+  if (n > 0) {
+    for (i=0; i<n; i++) {
+      contact[i].surface.mode = dContactSlip1 | dContactSlip2 |
+        dContactSoftERP | dContactSoftCFM | dContactApprox1;
+      contact[i].surface.mu = dInfinity;
+      contact[i].surface.slip1 = 0.1;
+      contact[i].surface.slip2 = 0.1;
+      contact[i].surface.soft_erp = 0.5;
+      contact[i].surface.soft_cfm = 0.3;
+      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
+      dJointAttach (c,
+                    dGeomGetBody(contact[i].geom.g1),
+                    dGeomGetBody(contact[i].geom.g2));
+    }
+  }
+}
+
+
+// start simulation - set viewpoint
+
+static void start()
+{
+  static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
+  static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
+  dsSetViewpoint (xyz,hpr);
+  printf ("Press:\t'a' to increase speed.\n"
+          "\t'z' to decrease speed.\n"
+          "\t',' to steer left.\n"
+          "\t'.' to steer right.\n"
+          "\t' ' to reset speed and steering.\n");
+}
+
+
+// called when a key pressed
+
+static void command (int cmd)
+{
+  switch (cmd) {
+  case 'a': case 'A':
+    speed += 0.3;
+    break;
+  case 'z': case 'Z':
+    speed -= 0.3;
+    break;
+  case ',':
+    steer -= 0.5;
+    break;
+  case '.':
+    steer += 0.5;
+    break;
+  case ' ':
+    speed = 0;
+    steer = 0;
+    break;
+  }
+}
+
+
+// simulation loop
+
+static void simLoop (int pause)
+{
+  int i;
+  if (!pause) {
+    // motor
+    dJointSetHinge2Param (joint[0],dParamVel2,-speed);
+    dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
+
+    // steering
+    dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
+    if (v > 0.1) v = 0.1;
+    if (v < -0.1) v = -0.1;
+    v *= 10.0;
+    dJointSetHinge2Param (joint[0],dParamVel,v);
+    dJointSetHinge2Param (joint[0],dParamFMax,0.2);
+    dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
+    dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
+    dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
+
+    dSpaceCollide (space,0,&nearCallback);
+    dWorldStep (world,0.05);
+
+    // remove all contact joints
+    dJointGroupEmpty (contactgroup);
+  }
+
+  dsSetColor (0,1,1);
+  dsSetTexture (DS_WOOD);
+  dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
+  dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
+  dsSetColor (1,1,1);
+  for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
+                                       dBodyGetRotation(body[i]),0.02f,RADIUS);
+
+  dVector3 ss;
+  dGeomBoxGetLengths (ground_box,ss);
+  dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
+
+  /*
+  printf ("%.10f %.10f %.10f %.10f\n",
+          dJointGetHingeAngle (joint[1]),
+          dJointGetHingeAngle (joint[2]),
+          dJointGetHingeAngleRate (joint[1]),
+          dJointGetHingeAngleRate (joint[2]));
+  */
+}
+
+
+int main (int argc, char **argv)
+{
+  int i;
+  dMass m;
+
+  // setup pointers to drawstuff callback functions
+  dsFunctions fn;
+  fn.version = DS_VERSION;
+  fn.start = &start;
+  fn.step = &simLoop;
+  fn.command = &command;
+  fn.stop = 0;
+  fn.path_to_textures = "../../drawstuff/textures";
+  if(argc==2)
+    {
+        fn.path_to_textures = argv[1];
+    }
+
+  // create world
+
+  world = dWorldCreate();
+  space = dHashSpaceCreate (0);
+  contactgroup = dJointGroupCreate (0);
+  dWorldSetGravity (world,0,0,-0.5);
+  ground = dCreatePlane (space,0,0,1,0);
+
+  // chassis body
+  body[0] = dBodyCreate (world);
+  dBodySetPosition (body[0],0,0,STARTZ);
+  dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
+  dMassAdjust (&m,CMASS);
+  dBodySetMass (body[0],&m);
+  box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
+  dGeomSetBody (box[0],body[0]);
+
+  // wheel bodies
+  for (i=1; i<=3; i++) {
+    body[i] = dBodyCreate (world);
+    dQuaternion q;
+    dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
+    dBodySetQuaternion (body[i],q);
+    dMassSetSphere (&m,1,RADIUS);
+    dMassAdjust (&m,WMASS);
+    dBodySetMass (body[i],&m);
+    sphere[i-1] = dCreateSphere (0,RADIUS);
+    dGeomSetBody (sphere[i-1],body[i]);
+  }
+  dBodySetPosition (body[1],0.5*LENGTH,0,STARTZ-HEIGHT*0.5);
+  dBodySetPosition (body[2],-0.5*LENGTH, WIDTH*0.5,STARTZ-HEIGHT*0.5);
+  dBodySetPosition (body[3],-0.5*LENGTH,-WIDTH*0.5,STARTZ-HEIGHT*0.5);
+
+  // front wheel hinge
+  /*
+  joint[0] = dJointCreateHinge2 (world,0);
+  dJointAttach (joint[0],body[0],body[1]);
+  const dReal *a = dBodyGetPosition (body[1]);
+  dJointSetHinge2Anchor (joint[0],a[0],a[1],a[2]);
+  dJointSetHinge2Axis1 (joint[0],0,0,1);
+  dJointSetHinge2Axis2 (joint[0],0,1,0);
+  */
+
+  // front and back wheel hinges
+  for (i=0; i<3; i++) {
+    joint[i] = dJointCreateHinge2 (world,0);
+    dJointAttach (joint[i],body[0],body[i+1]);
+    const dReal *a = dBodyGetPosition (body[i+1]);
+    dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
+    dJointSetHinge2Axis1 (joint[i],0,0,1);
+    dJointSetHinge2Axis2 (joint[i],0,1,0);
+
+    // breakable joints contribution
+    // the wheels can break
+    dJointSetBreakable (joint[i], 1);
+    // the wheels wil break at a specific force
+    dJointSetBreakMode (joint[i], dJOINT_BREAK_AT_B1_FORCE|dJOINT_BREAK_AT_B2_FORCE);
+    // specify the force for the first body connected to the joint ...
+    dJointSetBreakForce (joint[i], 0, 1.5, 1.5, 1.5);
+    // and for the second body
+    dJointSetBreakForce (joint[i], 1, 1.5, 1.5, 1.5);
+  }
+
+  // set joint suspension
+  for (i=0; i<3; i++) {
+    dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
+    dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
+  }
+
+  // lock back wheels along the steering axis
+  for (i=1; i<3; i++) {
+    // set stops to make sure wheels always stay in alignment
+    dJointSetHinge2Param (joint[i],dParamLoStop,0);
+    dJointSetHinge2Param (joint[i],dParamHiStop,0);
+    // the following alternative method is no good as the wheels may get out
+    // of alignment:
+    //   dJointSetHinge2Param (joint[i],dParamVel,0);
+    //   dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
+  }
+
+  // create car space and add it to the top level space
+  car_space = dSimpleSpaceCreate (space);
+  dSpaceSetCleanup (car_space,0);
+  dSpaceAdd (car_space,box[0]);
+  dSpaceAdd (car_space,sphere[0]);
+  dSpaceAdd (car_space,sphere[1]);
+  dSpaceAdd (car_space,sphere[2]);
+
+  // environment
+  ground_box = dCreateBox (space,2,1.5,1);
+  dMatrix3 R;
+  dRFromAxisAndAngle (R,0,1,0,-0.15);
+  dGeomSetPosition (ground_box,2,0,-0.34);
+  dGeomSetRotation (ground_box,R);
+
+  // run simulation
+  dsSimulationLoop (argc,argv,352,288,&fn);
+
+  dJointGroupDestroy (contactgroup);
+  dSpaceDestroy (space);
+  dWorldDestroy (world);
+  dGeomDestroy (box[0]);
+  dGeomDestroy (sphere[0]);
+  dGeomDestroy (sphere[1]);
+  dGeomDestroy (sphere[2]);
+
+  return 0;
+}