105 files changed, 23566 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;
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp b/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp
new file mode 100644
index 0000000..e550a32
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/AssemblyInfo.cpp
@@ -0,0 +1,58 @@
+#include "stdafx.h"
+
+using namespace System::Reflection;
+using namespace System::Runtime::CompilerServices;
+
+//
+// General Information about an assembly is controlled through the following 
+// set of attributes. Change these attribute values to modify the information
+// associated with an assembly.
+//
+[assembly:AssemblyTitleAttribute("")];
+[assembly:AssemblyDescriptionAttribute("")];
+[assembly:AssemblyConfigurationAttribute("")];
+[assembly:AssemblyCompanyAttribute("")];
+[assembly:AssemblyProductAttribute("")];
+[assembly:AssemblyCopyrightAttribute("")];
+[assembly:AssemblyTrademarkAttribute("")];
+[assembly:AssemblyCultureAttribute("")];                
+
+//
+// Version information for an assembly consists of the following four values:
+//
+//      Major Version
+//      Minor Version 
+//      Build Number
+//      Revision
+//
+// You can specify all the value or you can default the Revision and Build Numbers 
+// by using the '*' as shown below:
+
+[assembly:AssemblyVersionAttribute("1.0.*")];
+
+//
+// In order to sign your assembly you must specify a key to use. Refer to the 
+// Microsoft .NET Framework documentation for more information on assembly signing.
+//
+// Use the attributes below to control which key is used for signing. 
+//
+// Notes: 
+//   (*) If no key is specified, the assembly is not signed.
+//   (*) KeyName refers to a key that has been installed in the Crypto Service
+//       Provider (CSP) on your machine. KeyFile refers to a file which contains
+//       a key.
+//   (*) If the KeyFile and the KeyName values are both specified, the 
+//       following processing occurs:
+//       (1) If the KeyName can be found in the CSP, that key is used.
+//       (2) If the KeyName does not exist and the KeyFile does exist, the key 
+//           in the KeyFile is installed into the CSP and used.
+//   (*) In order to create a KeyFile, you can use the sn.exe (Strong Name) utility.
+//        When specifying the KeyFile, the location of the KeyFile should be
+//        relative to the project directory.
+//   (*) Delay Signing is an advanced option - see the Microsoft .NET Framework
+//       documentation for more information on this.
+//
+[assembly:AssemblyDelaySignAttribute(false)];
+[assembly:AssemblyKeyFileAttribute("")];
+[assembly:AssemblyKeyNameAttribute("")];
+
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp
new file mode 100644
index 0000000..c95ae57
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Body.cpp
@@ -0,0 +1,322 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "Body.h"
+
+namespace ODEManaged
+{
+
+        //Constructors
+
+                Body::Body(void)
+                {
+                        _id = 0;
+                }
+
+                Body::Body(World &world)
+                {
+                        _id = dBodyCreate(world.Id());
+                }
+
+
+        //Destructor
+
+                Body::~Body(void)
+                {
+                        dBodyDestroy(this->_id);
+                }
+
+
+        //Methods
+
+                //Id
+                dBodyID Body::Id()
+                {
+                        return _id;
+                }
+
+
+                //SetData
+                void Body::SetData(void *data)
+                {
+                        dBodySetData(this->_id, data);
+                }
+
+                //GetData
+                void *Body::GetData(void)
+                {
+                        return dBodyGetData(this->_id);
+                }
+
+
+                //SetPosition
+                void Body::SetPosition (double x, double y, double z)
+                {
+                        dBodySetPosition(this->_id, x, y, z);
+                }
+
+
+        //Overloaded GetPosition
+                Vector3 Body::GetPosition(void)
+                {
+                        Vector3 retVal;
+                        const dReal *temp;
+                        temp = dBodyGetPosition(this->_id);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                };
+
+                void Body::GetPosition(double position __gc[])
+                {
+                        const dReal *temp;
+                        temp = dBodyGetPosition(this->_id);
+                        position[0] = temp[0];
+                        position[1] = temp[1];
+                        position[2] = temp[2];
+                }
+
+
+                //SetRotationIdentity
+                void Body::SetRotationIdentity(void)
+                {
+                        dMatrix3 temp;
+                        dRSetIdentity(temp);
+                        dBodySetRotation(this->_id, temp);      
+                }
+        
+
+                //SetRotation (left handed system=>transpose)
+                void Body::SetRotation(Matrix3 rotation)
+                {
+                        dMatrix3 temp;
+                        temp[0] = rotation.m11;  
+                        temp[4] = rotation.m12;  
+                        temp[8] = rotation.m13; 
+                        temp[1] = rotation.m21;
+                        temp[5] = rotation.m22; 
+                        temp[9] = rotation.m23; 
+                        temp[2] = rotation.m31; 
+                        temp[6] = rotation.m32; 
+                        temp[10] = rotation.m33; 
+                        dBodySetRotation(this->_id, temp);
+                }
+
+                //GetRotation (left handed system=>transpose)
+                Matrix3 Body::GetRotation(void)
+                {
+                        Matrix3 retVal;
+                        //const dMatrix3 *m;
+                        const dReal *temp;
+                        temp = dBodyGetRotation(this->_id);
+                        retVal.m11 = temp[0];
+                        retVal.m12 = temp[4];
+                        retVal.m13 = temp[8];
+                        retVal.m21 = temp[1];
+                        retVal.m22 = temp[5];
+                        retVal.m23 = temp[9];
+                        retVal.m31 = temp[2];
+                        retVal.m32 = temp[6];
+                        retVal.m33 = temp[10];
+                        return retVal;  
+                }
+
+
+                //Overloaded SetMass
+                void Body::SetMass(double mass, Vector3 centerOfGravity, Matrix3 inertia)
+                {
+                        dMass *temp = new dMass();
+                        dMassSetParameters(temp, mass, 
+                                                           centerOfGravity.x, 
+                                                           centerOfGravity.y, 
+                                                           centerOfGravity.z, 
+                                                           inertia.m11, inertia.m22, 
+                                                           inertia.m33, inertia.m12, 
+                                                           inertia.m13, inertia.m23);
+
+                        dBodySetMass(this->_id, temp);
+                }
+
+
+                //SetMassSphere
+                void Body::SetMassSphere(double density, double radius)
+                {
+                        dMass *temp = new dMass();
+                        dMassSetSphere(temp, density, radius);
+                        dBodySetMass(this->_id, temp);
+                }       
+
+
+                //SetMassBox
+                void Body::SetMassBox(double density, double sideX, double sideY, double sideZ)
+                {
+                        dMass *temp = new dMass();
+                        dMassSetBox(temp, density, sideX, sideY, sideZ);
+                        dBodySetMass(this->_id, temp);
+                }       
+
+
+                //SetMassCappedCylinder
+                void Body::SetMassCappedCylinder(double density, int axis, double cylinderRadius, double cylinderLength)
+                {
+                        dMass *temp = new dMass();
+                        dMassSetCappedCylinder(temp, density, axis,
+                                                                   cylinderRadius, 
+                                                                   cylinderLength);
+
+                        dBodySetMass(this->_id, temp);
+                }
+
+
+                //AddForce
+                void Body::AddForce(double fX, double fY, double fZ)
+                {
+                        dBodyAddForce(this->_id, fX, fY, fZ);
+                }
+
+
+                //AddRelForce
+                void Body::AddRelForce(double fX, double fY, double fZ)
+                {
+                        dBodyAddRelForce(this->_id, fX,fY,fZ);
+                }
+
+
+                //AddForceAtPos
+                void Body::AddForceAtPos(double fX, double fY, double fZ, double pX, double pY, double pZ)
+                {
+                        dBodyAddForceAtPos(this->_id, fX, fY, fZ, pX, pY, pZ);
+                }
+
+
+                //AddRelForceAtPos
+                void Body::AddRelForceAtPos(double fX, double fY, double fZ, double pX, double pY, double pZ)
+                {
+                        dBodyAddRelForceAtPos(this->_id, fX, fY, fZ, pX, pY, pZ);
+                }
+
+
+                //AddRelForceAtRelPos
+                void Body::AddRelForceAtRelPos(double fX, double fY, double fZ, double pX, double pY, double pZ)
+                {
+                        dBodyAddRelForceAtRelPos(this->_id, fX, fY, fZ, pX, pY, pZ);
+                }       
+
+
+                //ApplyLinearVelocityDrag
+                void Body::ApplyLinearVelocityDrag(double dragCoef)
+                {
+                        const dReal *temp;
+                        double fX;
+                        double fY;
+                        double fZ;              
+                        temp = dBodyGetLinearVel(this->_id);                    
+                        fX = temp[0]*dragCoef*-1;
+                        fY = temp[1]*dragCoef*-1;
+                        fZ = temp[2]*dragCoef*-1;
+                        dBodyAddForce(this->_id, fX, fY, fZ);
+                }
+
+
+                //ApplyAngularVelocityDrag
+                void Body::ApplyAngularVelocityDrag(double dragCoef)
+                {
+                        const dReal *temp;
+                        double fX;
+                        double fY;
+                        double fZ;
+                        temp = dBodyGetAngularVel(this->_id);
+                        fX = temp[0]*dragCoef*-1;
+                        fY = temp[1]*dragCoef*-1;
+                        fZ = temp[2]*dragCoef*-1;
+                        dBodyAddTorque(this->_id, fX, fY, fZ);
+                }
+
+
+                //AddTorque
+                void Body::AddTorque(double fX, double fY, double fZ)
+                {
+                        dBodyAddTorque(this->_id, fX, fY, fZ);
+                }
+
+
+                //AddRelTorque
+                void Body::AddRelTorque(double fX, double fY, double fZ)
+                {
+                        dBodyAddRelTorque(this->_id, fX,fY,fZ);
+                }
+
+
+                //SetLinearVelocity
+                void Body::SetLinearVelocity(double x, double y, double z)
+                {
+                        dBodySetLinearVel(this->_id, x, y, z);
+                }
+
+
+                //GetLinearVelocity
+                Vector3 Body::GetLinearVelocity(void)
+                {
+                        Vector3 retVal;
+                        const dReal *temp;
+                        temp = dBodyGetLinearVel(this->_id);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+
+                //SetAngularVelocity
+                void Body::SetAngularVelocity(double x, double y, double z)
+                {
+                        dBodySetAngularVel(this->_id, x, y, z);
+                }
+
+                //GetAngularVelocity
+                Vector3 Body::GetAngularVelocity(void)
+                {
+                        Vector3 retVal;
+                        const dReal *temp;
+                        temp = dBodyGetAngularVel(this->_id);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+                
+                //GetRelPointPos
+                Vector3 Body::GetRelPointPos(double pX, double pY, double pZ)
+                {
+                        Vector3 retVal;
+                        dVector3 temp;
+                        dBodyGetRelPointPos(this->_id, pX, pY, pZ, temp);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+
+                //GetRelPointVel
+                Vector3 Body::GetRelPointVel(double pX, double pY, double pZ)
+                {
+                        Vector3 retVal;
+                        dVector3 temp;
+                        dBodyGetRelPointVel(this->_id, pX, pY, pZ, temp);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+
+                //ConnectedTo
+                int Body::ConnectedTo(const Body &b)
+                { 
+                        return dAreConnected(this->_id, b._id);
+                }
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Body.h b/libraries/ode-0.9/contrib/DotNetManaged/Body.h
new file mode 100644
index 0000000..9347c17
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Body.h
@@ -0,0 +1,76 @@
+#pragma once
+
+#include "World.h"
+#include "CommonMgd.h"
+
+namespace ODEManaged
+{       
+        __gc public class Body
+        {
+                public:
+
+                        //Constructors and Destructors
+                
+                        Body(void);
+                        Body(World &world);
+                
+                        ~Body(void);
+
+
+                        //Public Methods
+
+                        dBodyID Id();
+                        void    SetData                         (void *data);
+                        void    *GetData                        (void);
+
+                        //POSITION
+                        void SetPosition(double x, double y, double z);
+                        Vector3 GetPosition(void);
+                        void GetPosition(double  position __gc[]);
+                        
+                        //ROTATION
+                        void SetRotationIdentity(void);
+                        void SetRotation(Matrix3 rotation);
+                        Matrix3 GetRotation(void);
+
+                        //MASS
+                        void SetMass(double mass, Vector3 centerOfGravity, Matrix3 inertia);
+                        void SetMassSphere(double density, double radius);
+                        void SetMassBox(double density, double sideX, double sideY, double sideZ);
+                        void SetMassCappedCylinder(double density, int axis, double cylinderRadius, double cylinderLength);
+
+                        //FORCE AND TORQUE
+                        void AddForce(double fX, double fY, double fZ);
+                        void AddRelForce(double fX, double fY, double fZ);
+                        void AddForceAtPos(double fX, double fY, double fZ,double pX, double pY, double pZ);
+                        void AddRelForceAtPos(double fX, double fY, double fZ,double pX, double pY, double pZ);
+                        void AddRelForceAtRelPos(double fX, double fY, double fZ,double pX, double pY, double pZ);
+                        void ApplyLinearVelocityDrag(double dragCoef);
+                        void ApplyAngularVelocityDrag(double dragCoef);
+
+                        
+                        void AddTorque(double fX, double fY, double fZ);
+                        void AddRelTorque(double fX, double fY, double fZ);
+
+                        //LINEAR VELOCITY
+                        void SetLinearVelocity (double x, double y, double z);
+                        Vector3 GetLinearVelocity(void);
+
+                        //ANGULAR VELOCITY
+                        void SetAngularVelocity (double x, double y, double z);
+                        Vector3 GetAngularVelocity(void);
+
+                        //POINT
+                        Vector3 GetRelPointPos(double pX, double pY, double pZ);
+                        Vector3 GetRelPointVel(double pX, double pY, double pZ);
+
+                        //CONNECTED TO
+                        int ConnectedTo (const Body &b);
+
+                private:
+                        
+                        dBodyID _id;
+
+        };
+}
+
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h b/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h
new file mode 100644
index 0000000..143397d
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/CommonMgd.h
@@ -0,0 +1,43 @@
+#pragma once
+
+namespace ODEManaged
+{
+
+        __value public struct Vector3
+        {
+                double x;
+                double y;
+                double z;
+        };
+
+
+        __value public struct Vector4
+        {
+                double W;
+                double x;
+                double y;
+                double z;
+        };
+
+
+        __value public struct Matrix3
+        {
+                double m11;
+                double m12;
+                double m13;
+                double m21;
+                double m22;
+                double m23;
+                double m31;
+                double m32;
+                double m33;
+        };
+
+        //__value public struct NearCallback
+        //{
+        //      void *data;
+        //      dGeomID o1;
+        //      dGeomID o2;
+        //};
+
+}
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln
new file mode 100644
index 0000000..2694a26
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.sln
@@ -0,0 +1,21 @@
+Microsoft Visual Studio Solution File, Format Version 7.00
+Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "DotNetManaged", "DotNetManaged.vcproj", "{4B75AC19-971A-4CC6-A4F5-0695C9F8562F}"
+EndProject
+Global
+        GlobalSection(SolutionConfiguration) = preSolution
+                ConfigName.0 = Debug
+                ConfigName.1 = Release
+        EndGlobalSection
+        GlobalSection(ProjectDependencies) = postSolution
+        EndGlobalSection
+        GlobalSection(ProjectConfiguration) = postSolution
+                {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Debug.ActiveCfg = Debug|Win32
+                {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Debug.Build.0 = Debug|Win32
+                {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Release.ActiveCfg = Release|Win32
+                {4B75AC19-971A-4CC6-A4F5-0695C9F8562F}.Release.Build.0 = Release|Win32
+        EndGlobalSection
+        GlobalSection(ExtensibilityGlobals) = postSolution
+        EndGlobalSection
+        GlobalSection(ExtensibilityAddIns) = postSolution
+        EndGlobalSection
+EndGlobal
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj
new file mode 100644
index 0000000..2f5bb6c
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/DotNetManaged.vcproj
@@ -0,0 +1,379 @@
+<?xml version="1.0" encoding = "Windows-1252"?>
+<VisualStudioProject
+        ProjectType="Visual C++"
+        Version="7.00"
+        Name="DotNetManaged"
+        ProjectGUID="{4B75AC19-971A-4CC6-A4F5-0695C9F8562F}"
+        Keyword="ManagedCProj">
+        <Platforms>
+                <Platform
+                        Name="Win32"/>
+        </Platforms>
+        <Configurations>
+                <Configuration
+                        Name="Debug|Win32"
+                        OutputDirectory="Debug"
+                        IntermediateDirectory="Debug"
+                        ConfigurationType="2"
+                        CharacterSet="2"
+                        ManagedExtensions="TRUE">
+                        <Tool
+                                Name="VCCLCompilerTool"
+                                Optimization="2"
+                                AdditionalIncludeDirectories="&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Contrib\tri-collider&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\ODE\Src&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\ODE&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Include&quot;"
+                                PreprocessorDefinitions="WIN32;_DEBUG"
+                                MinimalRebuild="FALSE"
+                                BasicRuntimeChecks="0"
+                                RuntimeLibrary="1"
+                                WarningLevel="3"
+                                DebugInformationFormat="3"/>
+                        <Tool
+                                Name="VCCustomBuildTool"/>
+                        <Tool
+                                Name="VCLinkerTool"
+                                OutputFile="$(OutDir)/DotNetManaged.exe"
+                                LinkIncremental="2"
+                                GenerateDebugInformation="TRUE"/>
+                        <Tool
+                                Name="VCMIDLTool"/>
+                        <Tool
+                                Name="VCPostBuildEventTool"/>
+                        <Tool
+                                Name="VCPreBuildEventTool"/>
+                        <Tool
+                                Name="VCPreLinkEventTool"/>
+                        <Tool
+                                Name="VCResourceCompilerTool"/>
+                        <Tool
+                                Name="VCWebServiceProxyGeneratorTool"/>
+                        <Tool
+                                Name="VCWebDeploymentTool"/>
+                </Configuration>
+                <Configuration
+                        Name="Release|Win32"
+                        OutputDirectory="Release"
+                        IntermediateDirectory="Release"
+                        ConfigurationType="2"
+                        CharacterSet="2"
+                        ManagedExtensions="TRUE"
+                        WholeProgramOptimization="FALSE">
+                        <Tool
+                                Name="VCCLCompilerTool"
+                                Optimization="2"
+                                InlineFunctionExpansion="0"
+                                OmitFramePointers="TRUE"
+                                AdditionalIncludeDirectories="&quot;F:\Documents and Settings\Visual Studio Projects\ODE&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Contrib\DotNetManaged\odeOLd&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Include&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Include\ODE&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\ODE\Src&quot;;&quot;F:\Documents and Settings\Visual Studio Projects\ODE\Contrib\tri-collider&quot;"
+                                PreprocessorDefinitions="WIN32;NDEBUG"
+                                MinimalRebuild="FALSE"
+                                WarningLevel="3"
+                                DebugInformationFormat="3"/>
+                        <Tool
+                                Name="VCCustomBuildTool"/>
+                        <Tool
+                                Name="VCLinkerTool"
+                                OutputFile="$(OutDir)/ode.dll"
+                                SuppressStartupBanner="TRUE"
+                                GenerateDebugInformation="FALSE"/>
+                        <Tool
+                                Name="VCMIDLTool"/>
+                        <Tool
+                                Name="VCPostBuildEventTool"/>
+                        <Tool
+                                Name="VCPreBuildEventTool"/>
+                        <Tool
+                                Name="VCPreLinkEventTool"/>
+                        <Tool
+                                Name="VCResourceCompilerTool"/>
+                        <Tool
+                                Name="VCWebServiceProxyGeneratorTool"/>
+                        <Tool
+                                Name="VCWebDeploymentTool"/>
+                </Configuration>
+        </Configurations>
+        <Files>
+                <Filter
+                        Name="Source Files"
+                        Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm">
+                        <File
+                                RelativePath="..\..\ODE\Src\array.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\error.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\fastdot.c">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\fastldlt.c">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\fastlsolve.c">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\fastltsolve.c">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\geom.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\joint.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\lcp.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\mass.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\mat.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\matrix.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\memory.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\misc.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\objects.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\obstack.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\ode.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\odemath.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\rotation.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\space.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\step.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\testing.cpp">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\timer.cpp">
+                        </File>
+                </Filter>
+                <Filter
+                        Name="Header Files"
+                        Filter="h;hpp;hxx;hm;inl;inc">
+                        <File
+                                RelativePath="..\..\ODE\Src\array.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\common.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\config.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\contact.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\error.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\geom.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\geom_internal.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\joint.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\lcp.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\mass.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\mat.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\matrix.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\memory.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\misc.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\objects.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\obstack.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\ode.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\odecpp.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\odemath.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\rotation.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\space.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\step.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\ODE\Src\testing.h">
+                        </File>
+                        <File
+                                RelativePath="..\..\Include\ODE\timer.h">
+                        </File>
+                </Filter>
+                <Filter
+                        Name="Resource Files"
+                        Filter="rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;r">
+                </Filter>
+                <Filter
+                        Name="Wrapper"
+                        Filter="">
+                        <Filter
+                                Name="Header Files"
+                                Filter="">
+                                <File
+                                        RelativePath="Body.h">
+                                </File>
+                                <File
+                                        RelativePath="CommonMgd.h">
+                                </File>
+                                <File
+                                        RelativePath="Geom.h">
+                                </File>
+                                <File
+                                        RelativePath="Joint.h">
+                                </File>
+                                <File
+                                        RelativePath="JointAMotor.h">
+                                </File>
+                                <File
+                                        RelativePath="JointBall.h">
+                                </File>
+                                <File
+                                        RelativePath="JointFixed.h">
+                                </File>
+                                <File
+                                        RelativePath="JointGroup.h">
+                                </File>
+                                <File
+                                        RelativePath="JointHinge.h">
+                                </File>
+                                <File
+                                        RelativePath="JointHinge2.h">
+                                </File>
+                                <File
+                                        RelativePath="JointSlider.h">
+                                </File>
+                                <File
+                                        RelativePath="Space.h">
+                                </File>
+                                <File
+                                        RelativePath="Stdafx.h">
+                                </File>
+                                <File
+                                        RelativePath="World.h">
+                                </File>
+                        </Filter>
+                        <Filter
+                                Name="Source Files"
+                                Filter="">
+                                <File
+                                        RelativePath="Body.cpp">
+                                </File>
+                                <File
+                                        RelativePath="Geom.cpp">
+                                        <FileConfiguration
+                                                Name="Debug|Win32">
+                                                <Tool
+                                                        Name="VCCLCompilerTool"
+                                                        ObjectFile="$(IntDir)/$(InputName)1.obj"/>
+                                        </FileConfiguration>
+                                        <FileConfiguration
+                                                Name="Release|Win32">
+                                                <Tool
+                                                        Name="VCCLCompilerTool"
+                                                        ObjectFile="$(IntDir)/$(InputName)1.obj"/>
+                                        </FileConfiguration>
+                                </File>
+                                <File
+                                        RelativePath="Joint.cpp">
+                                        <FileConfiguration
+                                                Name="Debug|Win32">
+                                                <Tool
+                                                        Name="VCCLCompilerTool"
+                                                        ObjectFile="$(IntDir)/$(InputName)1.obj"/>
+                                        </FileConfiguration>
+                                        <FileConfiguration
+                                                Name="Release|Win32">
+                                                <Tool
+                                                        Name="VCCLCompilerTool"
+                                                        ObjectFile="$(IntDir)/$(InputName)1.obj"/>
+                                        </FileConfiguration>
+                                </File>
+                                <File
+                                        RelativePath="JointAMotor.cpp">
+                                </File>
+                                <File
+                                        RelativePath="JointBall.cpp">
+                                </File>
+                                <File
+                                        RelativePath="JointFixed.cpp">
+                                </File>
+                                <File
+                                        RelativePath="JointGroup.cpp">
+                                </File>
+                                <File
+                                        RelativePath="JointHinge.cpp">
+                                </File>
+                                <File
+                                        RelativePath="JointHinge2.cpp">
+                                </File>
+                                <File
+                                        RelativePath="JointSlider.cpp">
+                                </File>
+                                <File
+                                        RelativePath="Space.cpp">
+                                        <FileConfiguration
+                                                Name="Debug|Win32">
+                                                <Tool
+                                                        Name="VCCLCompilerTool"
+                                                        ObjectFile="$(IntDir)/$(InputName)1.obj"/>
+                                        </FileConfiguration>
+                                        <FileConfiguration
+                                                Name="Release|Win32">
+                                                <Tool
+                                                        Name="VCCLCompilerTool"
+                                                        ObjectFile="$(IntDir)/$(InputName)1.obj"/>
+                                        </FileConfiguration>
+                                </File>
+                                <File
+                                        RelativePath="Stdafx.cpp">
+                                </File>
+                                <File
+                                        RelativePath="World.cpp">
+                                </File>
+                        </Filter>
+                </Filter>
+        </Files>
+        <Globals>
+        </Globals>
+</VisualStudioProject>
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp
new file mode 100644
index 0000000..3655466
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Geom.cpp
@@ -0,0 +1,219 @@
+
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "Geom.h"
+
+
+namespace ODEManaged
+{
+
+        //Constructors
+
+                Geom::Geom(void)
+                { 
+                        _id = 0;
+                }
+
+
+        //Destructor
+
+                Geom::~Geom(void)
+                {
+                        dGeomDestroy(this->_id);
+                }
+
+
+        //Methods
+                        
+                //Id
+                dGeomID Geom::Id(void)
+                {
+                        return _id;
+                }
+
+
+                //GetBody
+                dBodyID Geom::GetBody(void)
+                {
+                        return dGeomGetBody(this->_id);
+                }
+
+
+                //Overloaded SetBody
+                void Geom::SetBody(Body &body)
+                {
+                        dGeomSetBody(this->_id, body.Id());
+                }
+
+                //void Geom::SetBody(dBodyID b)
+                //{
+                //      dGeomSetBody(this->_id, b);
+                //}
+
+
+                //SetPosition
+                void Geom::SetPosition(double x, double y, double z)
+                {
+                        dGeomSetPosition(this->_id, x, y, z);
+                }
+
+
+                //SetRotation
+                void Geom::SetRotation(Matrix3 rotation)
+                {
+                        dMatrix3 temp;
+                        temp[0] = rotation.m11;  
+                        temp[4] = rotation.m12;  
+                        temp[8] = rotation.m13; 
+                        temp[1] = rotation.m21;
+                        temp[5] = rotation.m22; 
+                        temp[9] = rotation.m23; 
+                        temp[2] = rotation.m31; 
+                        temp[6] = rotation.m32; 
+                        temp[10] = rotation.m33;
+                        dGeomSetRotation(_id, temp);
+                }
+                
+                
+                //Destroy
+                void Geom::Destroy() 
+                {
+                        if(this->_id) dGeomDestroy(this->_id);
+                        _id = 0;
+                }
+
+
+                //SetData
+                void Geom::SetData(void *data)
+                {
+                        dGeomSetData(this->_id, data);
+                }
+
+
+                //GetData
+                void *Geom::GetData(void)
+                {
+                        return dGeomGetData(this->_id);
+                }
+
+
+                //GetPosition
+                Vector3 Geom::GetPosition(void)
+                {
+                        Vector3 retVal;
+                        const dReal *temp;
+                        temp = dGeomGetPosition(this->_id);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+
+                //GetRotation (left handed system=>transpose)
+                Matrix3 Geom::GetRotation(void)
+                {
+                        Matrix3 retVal;
+                        const dReal *temp;
+                        temp = dGeomGetRotation(this->_id);
+                        retVal.m11 = temp[0];
+                        retVal.m12 = temp[4];
+                        retVal.m13 = temp[8];
+                        retVal.m21 = temp[1];
+                        retVal.m22 = temp[5];
+                        retVal.m23 = temp[9];
+                        retVal.m31 = temp[2];
+                        retVal.m32 = temp[6];
+                        retVal.m33 = temp[10];
+                        return retVal;  
+                }
+
+
+                //CreateSphere
+                void Geom::CreateSphere(Space &space, double radius)
+                {
+                        if(this->_id) dGeomDestroy(this->_id);
+                        _id = dCreateSphere(space.Id(), radius);
+                }
+
+
+                //CreateBox
+                void Geom::CreateBox(Space &space, double lx, double ly, double lz)
+                {
+                        if(this->_id) dGeomDestroy(this->_id);
+                        _id = dCreateBox(space.Id(), lx, ly, lz);
+                }
+
+                
+                //CreatePlane
+                void Geom::CreatePlane(Space &space, double a, double b, double c, double d) 
+                {
+                        if(this->_id) dGeomDestroy(this->_id);
+                        _id = dCreatePlane(space.Id(), a, b, c, d);
+                }
+
+
+                //CreateCCylinder
+                void Geom::CreateCCylinder(Space &space, double radius, double length)
+                {
+                        if(this->_id) dGeomDestroy(this->_id);
+                        _id = dCreateCCylinder(space.Id(), radius, length);
+                }
+        
+
+                //SphereGetRadius
+                double Geom::SphereGetRadius(void)
+                {
+                        return dGeomSphereGetRadius(this->_id);
+                }
+                
+                
+                //BoxGetLengths
+                Vector3 Geom::BoxGetLengths(void)
+                {
+                        Vector3 retVal;
+                        dVector3 temp;
+                        dGeomBoxGetLengths(this->_id, temp);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+
+                //PlaneGetParams
+                Vector4 Geom::PlaneGetParams(void)
+                {
+                        Vector4 retVal;
+                        dVector4 temp;
+                        dGeomPlaneGetParams(this->_id, temp);
+                        retVal.W = temp[0];
+                        retVal.x = temp[1];
+                        retVal.y = temp[2];
+                        retVal.z = temp[3];
+                        return retVal;
+                }
+                
+                
+                //CCylinderGetParams
+                void Geom::CCylinderGetParams(double *radius, double *length)
+                {
+                        dGeomCCylinderGetParams(this->_id, radius, length);
+                }
+                
+
+                //GetClass
+                int Geom::GetClass(void)
+                {
+                        return dGeomGetClass(this->_id);
+                }
+                
+}
+
+
+
+
+
+
+
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Geom.h b/libraries/ode-0.9/contrib/DotNetManaged/Geom.h
new file mode 100644
index 0000000..83a6faf
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Geom.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#include "Body.h"
+#include "Space.h"
+#include "CommonMgd.h"
+
+namespace ODEManaged
+{
+        __gc public class Geom
+        {
+        public:
+
+                
+                //Constructor
+                        
+                        Geom                                    (void);
+
+                        
+                //Destructor
+                        
+                        ~Geom                                   (void);
+
+
+                //Methods
+
+                        //Basic Stuff
+
+                                dGeomID Id                                      (void);
+                                dBodyID GetBody                         (void);
+                                
+                                //Overloaded SetBody
+                                void    SetBody                         (Body &body);
+                                /*void  SetBody                         (dBodyID b);*/
+                                
+                                Vector3 GetPosition                     (void);
+                                void    SetPosition                     (double x, double y, double z);
+                                
+                                Matrix3 GetRotation                     (void);
+                                void    SetRotation                     (Matrix3 rotation);
+                                
+                                void    SetData                         (void *data);
+                                void    *GetData                        (void);
+
+
+                        //Create Objects
+
+                                void    CreateSphere            (Space &space, double radius);
+                                void    CreateBox                       (Space &space, double lx, double ly, double lz);
+                                void    CreatePlane                     (Space &space, double a, double b, double c, double d);
+                                void    CreateCCylinder         (Space &space, double radius, double length);
+                                
+
+                        //Destroy Objects
+
+                                void    Destroy                         (void);
+
+
+                        //Get Object's Parameters
+
+                                double  SphereGetRadius         (void);
+                                Vector3 BoxGetLengths           (void);
+                                Vector4 PlaneGetParams          (void);
+                                void    CCylinderGetParams      (double *radius, double *length);
+                                int             GetClass                        (void);
+
+
+                //Properties
+                
+                        private:
+                        
+                                dGeomID _id;
+        
+        };
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp
new file mode 100644
index 0000000..e2d8de6
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Joint.cpp
@@ -0,0 +1,35 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "joint.h"
+#include "CommonMgd.h"
+#include "world.h"
+
+namespace ODEManaged
+{
+        
+        //Constructor
+
+                Joint::Joint(void)
+                {
+                        _id=0;
+                }
+
+
+        //Destructor
+
+                Joint::~Joint(void)
+                {
+                        dJointDestroy(this->_id);
+                }
+
+
+        //Methods
+
+                //Id
+                dJointID Joint::Id(void)
+                {
+                        return _id;
+                }
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Joint.h b/libraries/ode-0.9/contrib/DotNetManaged/Joint.h
new file mode 100644
index 0000000..d9ab254
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Joint.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include "JointGroup.h"
+#include "World.h"
+#include "Body.h"
+
+namespace ODEManaged
+{       
+        __gc public class Joint
+        {
+        protected:
+                //Constructor and Destructor Defenition
+                Joint(void);
+                ~Joint(void);
+
+                //Public Methods
+                dJointID Id(void);
+
+                dJointID _id;   
+ };
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp
new file mode 100644
index 0000000..c5a4543
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.cpp
@@ -0,0 +1,162 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "JointAMotor.h"
+
+namespace ODEManaged
+{
+
+        //Constructors
+
+                JointAMotor::JointAMotor(void) : Joint(){}
+
+
+                JointAMotor::JointAMotor(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateAMotor(world.Id(), 0);
+                }
+
+                
+                JointAMotor::JointAMotor(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateAMotor(world.Id(), jointGroup.Id());
+                }
+
+
+        //Destructor
+
+                JointAMotor::~JointAMotor(void){}
+
+
+        //Methods
+
+                //Overloaded Create 
+                        void JointAMotor::Create(World &world, JointGroup &jointGroup)
+                        {
+                                if(this->_id) dJointDestroy(this->_id);
+                                _id = dJointCreateAMotor(world.Id(), jointGroup.Id());
+                        }
+
+                        void JointAMotor::Create(World &world)
+                        {
+                                if(this->_id) dJointDestroy(this->_id);
+                                _id = dJointCreateAMotor(world.Id(), 0);
+                        }
+
+
+                //Overloaded Attach
+                        void JointAMotor::Attach(Body &body1, Body &body2)
+                        {
+                                dJointAttach(this->_id, body1.Id(), body2.Id());
+                        }
+
+                        void JointAMotor::Attach(Body &body1)
+                        {
+                                dJointAttach(this->_id, body1.Id(), 0);
+                        }
+
+
+                //SetNumAxes
+
+                        void JointAMotor::SetNumAxes(int num)
+                        {
+                                dJointSetAMotorNumAxes(this->_id, num);
+                        }
+
+
+                //GetNumAxes
+
+                        int JointAMotor::GetNumAxes(void)
+                        {
+                                return dJointGetAMotorNumAxes(this->_id);
+                        }
+
+
+                //SetAxis
+
+                        void JointAMotor::SetAxis(int anum, int rel, double x, double y ,double z)
+                        {
+                                dJointSetAMotorAxis(this->_id, anum, rel, x, y, z);
+                        }
+
+
+                //GetAxis
+
+                        Vector3 JointAMotor::GetAxis(int anum)
+                        {
+                                Vector3 retVal;
+                                dVector3 temp;
+                                dJointGetAMotorAxis(this->_id, anum, temp);
+                                retVal.x = temp[0];
+                                retVal.y = temp[1];
+                                retVal.z = temp[2];
+                                return retVal;
+                        }
+
+
+                //SetAngle
+
+                        void JointAMotor::SetAngle(int anum, double angle)
+                        {
+                                dJointSetAMotorAngle(this->_id, anum, angle);
+                        }
+
+
+                //GetAngle
+
+                        double JointAMotor::GetAngle(int anum)
+                        {
+                                return dJointGetAMotorAngle(this->_id, anum);
+                        }
+
+
+                //SetParam
+
+                        void JointAMotor::SetParam(int parameter, double value)
+                        {
+                                dJointSetAMotorParam(this->_id, parameter, value);
+                        }
+
+
+                //GetParam
+
+                        double JointAMotor::GetParam(int parameter)
+                        {
+                                return dJointGetAMotorParam(this->_id, parameter);
+                        }
+
+
+                //SetMode
+
+                        void JointAMotor::SetMode(int mode)
+                        {
+                                dJointSetAMotorMode(this->_id, mode);
+                        }
+
+
+                //GetMode
+
+                        int JointAMotor::GetMode(void)
+                        {
+                                return dJointGetAMotorMode(this->_id);
+                        }
+
+
+                //GetAxisRel
+
+                        int JointAMotor::GetAxisRel(int anum)
+                        {
+                                return dJointGetAMotorAxisRel(this->_id, anum);
+                        }
+
+
+                //GetAngleRate
+
+                        double JointAMotor::GetAngleRate(int anum)
+                        {
+                                return dJointGetAMotorAngleRate(this->_id, anum);
+                        }
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h
new file mode 100644
index 0000000..aa3ca4b
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointAMotor.h
@@ -0,0 +1,62 @@
+#pragma once
+
+#include "Joint.h"
+
+namespace ODEManaged
+{
+        __gc public class JointAMotor : public Joint
+        {
+        public:
+
+
+                //Constructors
+
+                        JointAMotor                             (void);
+                        JointAMotor                             (World &world);
+                        JointAMotor                             (World &world, JointGroup &jointGroup);
+                        
+
+                //Destructor
+                        
+                        virtual ~JointAMotor    (void);
+
+
+                //Methods       
+                
+                        //Basic Stuff
+                        
+                                //Overloaded Create
+                                void    Create                  (World &world, JointGroup &jointGroup);
+                                void    Create                  (World &world);
+
+                                void    SetNumAxes              (int num);
+                                int             GetNumAxes              (void);
+
+                                void    SetAxis                 (int anum, int rel, double x, double y, double z);
+                                Vector3 GetAxis                 (int anum);
+                                
+                                void    SetAngle                (int anum, double angle);
+                                double  GetAngle                (int anum);
+
+                                void    SetMode                 (int mode);
+                                int             GetMode                 (void);
+
+                                int             GetAxisRel              (int anum);
+                                double  GetAngleRate    (int anum);
+
+                                //Overloaded Attach
+                                void    Attach                  (Body &body1, Body &body2);     
+                                void    Attach                  (Body &body1);
+
+
+                        //Movement Parameters
+
+                        void    SetParam                (int parameter, double value);
+                        double  GetParam                (int parameter);
+                        
+
+                        
+
+
+        };
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp
new file mode 100644
index 0000000..d9336c9
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.cpp
@@ -0,0 +1,79 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "jointball.h"
+
+namespace ODEManaged
+{
+        
+        //Constructors
+
+                JointBall::JointBall(void) : Joint(){}
+
+
+                JointBall::JointBall(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateBall(world.Id(), 0);
+                }
+
+                
+                JointBall::JointBall(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateBall(world.Id(), jointGroup.Id());
+                }
+
+
+        //Destructor
+
+                JointBall::~JointBall(void){}
+
+
+        //Methods
+
+                //Overloaded Create 
+                void JointBall::Create(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateBall(world.Id(), jointGroup.Id());
+                }
+
+                void JointBall::Create(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateBall(world.Id(), 0);
+                }
+
+
+                //Overloaded Attach
+                void JointBall::Attach(Body &body1, Body &body2)
+                {
+                        dJointAttach(this->_id, body1.Id(), body2.Id());
+                }
+
+                void JointBall::Attach(Body &body1)
+                {
+                        dJointAttach(this->_id, body1.Id(), 0);
+                }
+
+
+                //SetAnchor
+                void JointBall::SetAnchor(double x, double y ,double z)
+                {
+                        dJointSetBallAnchor(this->_id, x, y, z);
+                }
+
+                //GetAnchor
+                Vector3 JointBall::GetAnchor(void)
+                {
+                        Vector3 retVal;
+                        dVector3 temp;
+                        dJointGetBallAnchor(this->_id,temp);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h
new file mode 100644
index 0000000..2355bdd
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointBall.h
@@ -0,0 +1,38 @@
+#pragma once
+
+#include "Joint.h"
+
+namespace ODEManaged
+{
+        __gc public class JointBall : public Joint
+        {
+        public:
+
+                //Constructors
+
+                        JointBall(void);
+                        JointBall(World &world);
+                        JointBall(World &world, JointGroup &jointGroup);
+                
+
+                //Destructors
+
+                        virtual ~JointBall(void);
+
+
+                //Methods
+                
+                        //Overloaded Create
+                        void Create(World &world, JointGroup &jointGroup);
+                        void Create(World &world);
+                        
+                        //Overloaded Attach
+                        void Attach(Body &body1, Body &body2);  
+                        void Attach(Body &body1);
+
+                        void SetAnchor(double x, double y, double z);
+                        Vector3 GetAnchor(void);
+
+        };
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp
new file mode 100644
index 0000000..afe9222
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.cpp
@@ -0,0 +1,67 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "jointfixed.h"
+
+namespace ODEManaged
+{
+
+        //Constructors
+
+                JointFixed::JointFixed(void) : Joint(){}
+
+
+                JointFixed::JointFixed(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateFixed(world.Id(),0);
+                }
+                
+
+                JointFixed::JointFixed(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateFixed(world.Id(), jointGroup.Id());
+                }
+
+
+        //Destructor
+
+                JointFixed::~JointFixed(void){}
+
+
+        //Methods
+
+                //Overloaded Create 
+                void JointFixed::Create(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateFixed(world.Id(), jointGroup.Id());
+                }
+
+                void JointFixed::Create(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateFixed(world.Id(), 0);
+                }
+
+
+                //Overloaded Attach
+                void JointFixed::Attach(Body &body1, Body &body2)
+                {
+                        dJointAttach(this->_id, body1.Id(), body2.Id());
+                }
+
+                void JointFixed::Attach(Body &body1)
+                {
+                        dJointAttach(this->_id, body1.Id(), 0);
+                }
+
+
+                //Fixed
+                void JointFixed::SetFixed(void)
+                {
+                        dJointSetFixed(this->_id);
+                }
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h
new file mode 100644
index 0000000..5ca50dc
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointFixed.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include "Joint.h"
+
+namespace ODEManaged
+{
+        __gc public class JointFixed : public Joint
+        {
+        public:
+
+                //Constructors
+
+                        JointFixed(void);
+                        JointFixed(World &world);
+                        JointFixed(World &world, JointGroup &jointGroup);
+                
+
+                //Destructor
+
+                        virtual ~JointFixed(void);
+
+
+                //Methods
+                
+                        //Overloaded Create
+                        void Create(World &world, JointGroup &jointGroup);
+                        void Create(World &world);
+
+                        //Overloaded Attach 
+                        void Attach(Body &body1, Body &body2);  
+                        void Attach(Body &body1);
+                
+                        void SetFixed(void);
+
+        };
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp
new file mode 100644
index 0000000..925751f
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.cpp
@@ -0,0 +1,53 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "jointgroup.h"
+
+namespace ODEManaged
+{
+
+        //Constructors
+
+                JointGroup::JointGroup(void)
+                {       
+                        _id=0;
+                }
+
+                JointGroup::JointGroup (int maxSize)
+                {
+                        _id = dJointGroupCreate(maxSize);
+                }
+
+
+        //Destructor
+
+                JointGroup::~JointGroup(void)
+                {
+                        dJointGroupDestroy(this->_id);
+                }
+
+        
+        //Methods
+
+                //ID
+                dJointGroupID JointGroup::Id()
+                {
+                        return _id;
+                }
+
+
+                //Create
+                void JointGroup::Create (int maxSize)
+                {
+                        if(_id) dJointGroupDestroy(_id);
+                        _id = dJointGroupCreate(maxSize);
+                }
+
+
+                //Empty
+                void JointGroup::Empty (void)
+                {
+                        dJointGroupEmpty(this->_id);
+                }
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h
new file mode 100644
index 0000000..b62ced0
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointGroup.h
@@ -0,0 +1,33 @@
+#pragma once
+
+namespace ODEManaged
+{
+        __gc public class JointGroup
+        {
+        public:
+
+                //Constructors
+
+                        JointGroup(void);
+                        JointGroup(int maxSize);
+                
+
+                //Destructor
+
+                        ~JointGroup(void);
+
+
+                //Methods
+                
+                        dJointGroupID Id(void);
+                        void Create(int maxSize);
+                        void Empty(void);
+
+                
+                private:
+
+                        dJointGroupID _id;
+
+        };
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp
new file mode 100644
index 0000000..85d420b
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.cpp
@@ -0,0 +1,121 @@
+#include "stdafx.h"
+
+#include <ode/ode.h>
+#include "jointhinge.h"
+
+namespace ODEManaged
+{
+
+        //Constructors
+
+                JointHinge::JointHinge(void) : Joint(){}
+
+
+                JointHinge::JointHinge(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateHinge(world.Id(), 0);
+                }
+
+                
+                JointHinge::JointHinge(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateHinge(world.Id(), jointGroup.Id());
+                }
+
+
+        //Destructor
+
+                JointHinge::~JointHinge(void){}
+
+
+        //Methods
+
+                //Overloaded Create 
+                void JointHinge::Create(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateHinge(world.Id(), jointGroup.Id());
+                }
+
+                void JointHinge::Create(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateHinge(world.Id(), 0);
+                }
+
+
+                //Overloaded Attach 
+                void JointHinge::Attach(Body &body1, Body &body2)
+                {
+                        dJointAttach(this->_id, body1.Id(), body2.Id());
+                }
+
+                void JointHinge::Attach(Body &body1)
+                {
+                        dJointAttach(this->_id, body1.Id(), 0);
+                }
+
+
+                //SetAxis
+                void JointHinge::SetAxis(double x, double y, double z)
+                {
+                        dJointSetHingeAxis(this->_id, x, y, z);
+                }
+
+                //GetAxis
+                Vector3 JointHinge::GetAxis(void)
+                {
+                        Vector3 retVal;
+                        dVector3 temp;
+                        dJointGetHingeAxis(this->_id, temp);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+
+                //SetAnchor
+                void JointHinge::SetAnchor(double x, double y, double z)
+                {
+                        dJointSetHingeAnchor(this->_id, x, y, z);
+                }
+
+                //GetAnchor
+                Vector3 JointHinge::GetAnchor(void)
+                {
+                        Vector3 retVal;
+                        dVector3 temp;
+                        dJointGetHingeAnchor(this->_id, temp);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+
+        //Movement Parameters
+
+                //SetAllMovParams
+                void JointHinge::SetAllMovParams(double LoStop, double HiStop,
+                                                                                 double Velocity, double MaxForce,
+                                                                                 double FudgeFactor, double Bounce,
+                                                                                 double StopERP, double StopCFM)
+                {
+                        if (LoStop > -3.141592653 && LoStop <= 0) 
+                                dJointSetHingeParam(this->_id, dParamLoStop, LoStop);
+
+                        if (HiStop < 3.141592653 && HiStop >= 0)
+                                dJointSetHingeParam(this->_id, dParamHiStop, HiStop);
+
+                        dJointSetHingeParam(this->_id, dParamVel, Velocity);
+                        dJointSetHingeParam(this->_id, dParamFMax, MaxForce);
+                        dJointSetHingeParam(this->_id, dParamFudgeFactor, FudgeFactor);
+                        dJointSetHingeParam(this->_id, dParamBounce, Bounce);
+                        dJointSetHingeParam(this->_id, dParamStopERP, StopERP);
+                        dJointSetHingeParam(this->_id, dParamStopCFM, StopCFM);
+                }
+                                
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h
new file mode 100644
index 0000000..3115845
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge.h
@@ -0,0 +1,195 @@
+#pragma once
+
+#include "Joint.h"
+#include "CommonMgd.h"
+
+namespace ODEManaged
+{
+        __gc public class JointHinge : public Joint
+        {
+        public:
+
+                //Constructors
+
+                        JointHinge(void);
+                        JointHinge(World &world);
+                        JointHinge(World &world, JointGroup &jointGroup);
+                        
+
+                //Destructor
+
+                        virtual~JointHinge(void);
+
+                
+                //Methods       
+                        
+                        //Overloaded Create
+                        void Create(World &world, JointGroup &jointGroup);
+                        void Create(World &world);
+
+                        //Overloaded Attach
+                        void Attach(Body &body1, Body &body2);  
+                        void Attach(Body &body1);
+
+                        void SetAnchor(double x, double y, double z);
+                        Vector3 GetAnchor(void);
+
+                        void SetAxis(double x, double y, double z);
+                        Vector3 GetAxis(void);
+
+                        void SetAllMovParams(double LoStop, double HiStop,
+                                                                 double Velocity, double MaxForce,
+                                                                 double FudgeFactor, double Bounce,
+                                                                 double StopERP, double StopCFM);
+
+
+                //Properties
+
+                        //LoStop
+                        __property double get_LoStop(void)
+                                {
+                                        return dJointGetHingeParam(this->_id, dParamLoStop);
+                                }
+
+                        __property void set_LoStop(double value)
+                        {
+                                if (value > -3.141592653 && value <= 0)
+                                        dJointSetHingeParam(this->_id, dParamLoStop, value);
+                        }
+
+
+                        //HiStop
+                        __property double get_HiStop(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamHiStop);
+                        }
+
+                        __property void set_HiStop(double value)
+                        {
+                                if (value < 3.141592653 && value >= 0)
+                                        dJointSetHingeParam(this->_id, dParamHiStop, value);
+                        }
+                        
+
+                        //Velocity
+                        __property double get_Velocity(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamVel);
+                        }
+
+                        __property void set_Velocity(double value)
+                        {
+                                dJointSetHingeParam(this->_id, dParamVel, value);
+                        }
+
+
+                        //MaxForce
+                        __property double get_MaxForce(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamFMax);
+                        }
+
+                        __property void set_MaxForce(double value)
+                        {
+                                dJointSetHingeParam(this->_id, dParamFMax, value);
+                        }
+
+
+                        //FudgeFactor
+                        __property double get_FudgeFactor(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamFudgeFactor);
+                        }
+
+                        __property void set_FudgeFactor(double value)
+                        {
+                                dJointSetHingeParam(this->_id, dParamFudgeFactor, value);
+                        }
+
+
+                        //Bounce
+                        __property double get_Bounce(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamBounce);
+                        }
+
+                        __property void set_Bounce(double value)
+                        {
+                                dJointSetHingeParam(this->_id, dParamBounce, value);
+                        }
+
+
+                        //StopERP
+                        __property double get_StopERP(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamStopERP);
+                        }
+
+                        __property void set_StopERP(double value)
+                        {
+                                dJointSetHingeParam(this->_id, dParamStopERP, value);
+                        }
+
+
+                        //StopCFM
+                        __property double get_StopCFM(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamStopCFM);
+                        }
+
+                        __property void set_StopCFM(double value)
+                        {
+                                dJointSetHingeParam(this->_id, dParamStopCFM, value);
+                        }
+
+
+                        //GetAngle
+                        __property double get_Angle(void)
+                        {
+                                return dJointGetHingeAngle(this->_id);
+                        }
+
+
+                        //GetAngleRate
+                        __property double get_AngleRate(void)
+                        {
+                                return dJointGetHingeAngleRate(this->_id);
+                        }
+
+        };
+
+}
+
+//                              void    SetSuspensionERP(double value);
+//                              double  GetSuspensionERP(void);
+
+//                              void    SetSuspensionCFM(double value);
+//                              double  GetSuspensionCFM(void);
+
+/*                              
+                        //SetSuspensionERP
+                        void JointHinge::SetSuspensionERP(double value)
+                        {
+                                dJointSetHingeParam(this->_id, dParamSuspensionERP, value);
+                        }
+
+                        //GetSuspensionERP
+                        double JointHinge::GetSuspensionERP(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamSuspensionERP);
+                        }
+                                                        
+                                
+                        //SetSuspensionCFM
+                        void JointHinge::SetSuspensionCFM(double value)
+                        {
+                                dJointSetHingeParam(this->_id, dParamSuspensionCFM, value);
+                        }
+
+                        //GetSuspensionCFM
+                        double JointHinge::GetSuspensionCFM(void)
+                        {
+                                return dJointGetHingeParam(this->_id, dParamSuspensionCFM);
+                        }
+
+*/
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp
new file mode 100644
index 0000000..94fd7a7
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.cpp
@@ -0,0 +1,133 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "jointhinge2.h"
+
+namespace ODEManaged
+{
+        //Constructors
+        JointHinge2::JointHinge2(void) : Joint(){}
+
+        JointHinge2::JointHinge2(World &world)
+        {
+                if(this->_id) dJointDestroy(this->_id);
+                _id = dJointCreateHinge2(world.Id(),0);
+        }
+        
+        JointHinge2::JointHinge2(World &world, JointGroup &jointGroup)
+        {
+                if(this->_id) dJointDestroy(this->_id);
+                _id = dJointCreateHinge2(world.Id(), jointGroup.Id());
+        }
+
+        //Destructor
+        JointHinge2::~JointHinge2(void){}
+
+        //CreateHinge2 (overload 1)
+        void JointHinge2::Create(World &world, JointGroup &jointGroup)
+        {
+                if(this->_id) dJointDestroy(this->_id);
+                _id = dJointCreateHinge2(world.Id(), jointGroup.Id());
+        }
+
+        //CreateHinge2 (overload 2)
+        void JointHinge2::Create(World &world)
+        {
+                if(this->_id) dJointDestroy(this->_id);
+                _id = dJointCreateHinge2(world.Id(),0);
+        }
+
+        //SetAnchor1
+        void JointHinge2::SetAnchor (double x, double y ,double z)
+        {
+                dJointSetHinge2Anchor(_id, x,y,z);
+        }
+
+        //GetAnchor1
+        Vector3 JointHinge2::GetAnchor()
+        {
+                Vector3 retVal;
+                dVector3 temp;
+                dJointGetHinge2Anchor(_id,temp);
+                retVal.x = temp[0];
+                retVal.y = temp[1];
+                retVal.z = temp[2];
+                return retVal;
+        }
+
+        //SetAxis1
+        void JointHinge2::SetAxis1 (double x, double y ,double z)
+        {
+                dJointSetHinge2Axis1(_id, x,y,z);
+        }
+
+        //GetAxis1
+        Vector3 JointHinge2::GetAxis1()
+        {
+                Vector3 retVal;
+                dVector3 temp;
+                dJointGetHinge2Axis1(_id,temp);
+                retVal.x = temp[0];
+                retVal.y = temp[1];
+                retVal.z = temp[2];
+                return retVal;
+        }
+
+        //SetAxis2
+        void JointHinge2::SetAxis2 (double x, double y ,double z)
+        {
+                dJointSetHinge2Axis2(_id, x,y,z);
+        }
+
+        //GetAxis2
+        Vector3 JointHinge2::GetAxis2()
+        {
+                Vector3 retVal;
+                dVector3 temp;
+                dJointGetHinge2Axis2(_id,temp);
+                retVal.x = temp[0];
+                retVal.y = temp[1];
+                retVal.z = temp[2];
+                return retVal;
+        }
+
+        //GetAngle1
+        double JointHinge2::GetAngle1 ()
+        {
+                return dJointGetHinge2Angle1(this->_id);
+        }
+
+        //GetAngle1Rate
+        double JointHinge2::GetAngle1Rate ()
+        {
+                return dJointGetHinge2Angle1Rate(this->_id);
+        }
+
+        ////GetAngle hmm, this doesn't exist
+        //double JointHinge2::GetAngle2 ()
+        //{
+        //      return dJointGetHinge2Angle2(this->_id);
+        //}
+
+        //GetAngle2Rate
+        double JointHinge2::GetAngle2Rate ()
+        {
+                return dJointGetHinge2Angle2Rate(this->_id);
+        }
+
+
+        //Attach (overload 1)
+        void JointHinge2::Attach (Body &body1, Body &body2)
+        {
+                dJointAttach(_id, body1.Id(),body2.Id());
+        }
+
+        //Attach (overload 2) 
+        //TODO: possibly add an overload that takes anchor as a param also.
+        void JointHinge2::Attach (Body &body1)
+        {
+                dJointAttach(_id, body1.Id(),0);
+        }
+
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h
new file mode 100644
index 0000000..e883ea8
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointHinge2.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include "Joint.h"
+#include "CommonMgd.h"
+
+namespace ODEManaged
+{
+        __gc public class JointHinge2 : public Joint
+        {
+                public:
+
+
+                //Constructors
+
+                        JointHinge2                             (void);
+                        JointHinge2                             (World &world);
+                        JointHinge2                             (World &world, JointGroup &jointGroup);
+                        
+                //Destructors
+
+                        virtual ~JointHinge2    (void);
+                        
+
+                //Methods
+
+                        //Overloaded Hinge.Create
+                        void    Create                  (World &world, JointGroup &jointGroup);
+                        void    Create                  (World &world);
+                        
+                        void    SetAnchor               (double x, double y, double z);
+                        Vector3 GetAnchor               (void);
+
+                        void    SetAxis1                (double x, double y, double z);
+                        Vector3 GetAxis1                (void);
+
+                        void    SetAxis2                (double x, double y, double z);
+                        Vector3 GetAxis2                (void);
+
+                        double  GetAngle1               (void);
+                        double  GetAngle1Rate   (void);
+
+                        //double GetAngle2 (void);
+                        double  GetAngle2Rate   (void);
+
+                        void    Attach                  (Body &body1, Body &body2);     
+                        void    Attach(                 Body &body1);
+        };
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp
new file mode 100644
index 0000000..ab7ebd6
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.cpp
@@ -0,0 +1,102 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "jointslider.h"
+
+namespace ODEManaged
+{
+        
+        //Constructors
+        
+                JointSlider::JointSlider(void) : Joint(){}
+
+
+                JointSlider::JointSlider(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateSlider(world.Id(), 0);
+                }
+
+
+                JointSlider::JointSlider(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateSlider(world.Id(), jointGroup.Id());
+                }
+
+
+        //Destructor
+
+                JointSlider::~JointSlider(void){}
+
+
+        //Methods
+
+                //Overloaded Create
+                void JointSlider::Create(World &world, JointGroup &jointGroup)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateSlider(world.Id(), jointGroup.Id());
+                }
+
+                void JointSlider::Create(World &world)
+                {
+                        if(this->_id) dJointDestroy(this->_id);
+                        _id = dJointCreateSlider(world.Id(), 0);
+                }
+
+                
+                //Overloaded Attach 
+                void JointSlider::Attach(Body &body1, Body &body2)
+                {
+                        dJointAttach(this->_id, body1.Id(), body2.Id());
+                }
+
+                void JointSlider::Attach(Body &body1)
+                {
+                        dJointAttach(this->_id, body1.Id(), 0);
+                }
+
+
+                //SetAxis
+                void JointSlider::SetAxis(double x, double y, double z)
+                {
+                        dJointSetSliderAxis(this->_id, x, y, z);
+                }
+
+                //GetAxis
+                Vector3 JointSlider::GetAxis(void)
+                {
+                        Vector3 retVal;
+                        dVector3 temp;
+                        dJointGetSliderAxis(this->_id, temp);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+
+        //Movement Parameters
+
+                //SetAllMovParams
+                void JointSlider::SetAllMovParams(double LoStop, double HiStop,
+                                                                                  double Velocity, double MaxForce,
+                                                                                  double FudgeFactor, double Bounce,
+                                                                                  double StopERP, double StopCFM)
+                {
+                        if (LoStop <= 0) 
+                                dJointSetHingeParam(this->_id, dParamLoStop, LoStop);
+
+                        if (HiStop >= 0)
+                                dJointSetHingeParam(this->_id, dParamHiStop, HiStop);
+
+                        dJointSetSliderParam(this->_id, dParamVel, Velocity);
+                        dJointSetSliderParam(this->_id, dParamFMax, MaxForce);
+                        dJointSetSliderParam(this->_id, dParamFudgeFactor, FudgeFactor);
+                        dJointSetSliderParam(this->_id, dParamBounce, Bounce);
+                        dJointSetSliderParam(this->_id, dParamStopERP, StopERP);
+                        dJointSetSliderParam(this->_id, dParamStopCFM, StopCFM);
+                }
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h
new file mode 100644
index 0000000..7e96e59
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/JointSlider.h
@@ -0,0 +1,158 @@
+#pragma once
+
+#include "Joint.h"
+#include "CommonMgd.h"
+
+namespace ODEManaged
+{
+        __gc public class JointSlider : public Joint
+        {
+        public:
+                        
+                
+                //Constructors
+
+                        JointSlider(void);
+                        JointSlider(World &world);
+                        JointSlider(World &world, JointGroup &jointGroup);
+                        
+
+                //Destructors
+
+                        virtual ~JointSlider(void);
+
+                        
+                //Methods       
+                        
+                        //Overloaded Create
+                        void Create(World &world, JointGroup &jointGroup);
+                        void Create(World &world);
+                        
+                        //Overloaded Attach
+                        void Attach(Body &body1, Body &body2);  
+                        void Attach(Body &body1);
+                        
+                        void SetAxis(double x, double y, double z);
+                        Vector3 GetAxis(void);
+
+                        void SetAllMovParams(double LoStop, double HiStop,
+                                                                 double Velocity, double MaxForce,
+                                                                 double FudgeFactor, double Bounce,
+                                                                 double StopERP, double StopCFM);
+
+
+                //Properties
+
+                        //LoStop
+                        __property double get_LoStop(void)
+                                {
+                                        return dJointGetSliderParam(this->_id, dParamLoStop);
+                                }
+
+                        __property void set_LoStop(double value)
+                        {
+                                if (value <=0)
+                                        dJointSetSliderParam(this->_id, dParamLoStop, value);
+                        }
+
+
+                        //HiStop
+                        __property double get_HiStop(void)
+                        {
+                                return dJointGetSliderParam(this->_id, dParamHiStop);
+                        }
+
+                        __property void set_HiStop(double value)
+                        {
+                                if (value >= 0)
+                                        dJointSetSliderParam(this->_id, dParamHiStop, value);
+                        }
+                        
+
+                        //Velocity
+                        __property double get_Velocity(void)
+                        {
+                                return dJointGetSliderParam(this->_id, dParamVel);
+                        }
+
+                        __property void set_Velocity(double value)
+                        {
+                                dJointSetSliderParam(this->_id, dParamVel, value);
+                        }
+
+
+                        //MaxForce
+                        __property double get_MaxForce(void)
+                        {
+                                return dJointGetSliderParam(this->_id, dParamFMax);
+                        }
+
+                        __property void set_MaxForce(double value)
+                        {
+                                dJointSetSliderParam(this->_id, dParamFMax, value);
+                        }
+
+
+                        //FudgeFactor
+                        __property double get_FudgeFactor(void)
+                        {
+                                return dJointGetSliderParam(this->_id, dParamFudgeFactor);
+                        }
+
+                        __property void set_FudgeFactor(double value)
+                        {
+                                dJointSetSliderParam(this->_id, dParamFudgeFactor, value);
+                        }
+
+
+                        //Bounce
+                        __property double get_Bounce(void)
+                        {
+                                return dJointGetSliderParam(this->_id, dParamBounce);
+                        }
+
+                        __property void set_Bounce(double value)
+                        {
+                                dJointSetSliderParam(this->_id, dParamBounce, value);
+                        }
+
+
+                        //StopERP
+                        __property double get_StopERP(void)
+                        {
+                                return dJointGetSliderParam(this->_id, dParamStopERP);
+                        }
+
+                        __property void set_StopERP(double value)
+                        {
+                                dJointSetSliderParam(this->_id, dParamStopERP, value);
+                        }
+
+
+                        //StopCFM
+                        __property double get_StopCFM(void)
+                        {
+                                return dJointGetSliderParam(this->_id, dParamStopCFM);
+                        }
+
+                        __property void set_StopCFM(double value)
+                        {
+                                dJointSetSliderParam(this->_id, dParamStopCFM, value);
+                        }
+
+
+                        //GetAngle
+                        __property double get_Position(void)
+                        {
+                                return dJointGetSliderPosition(this->_id);
+                        }
+
+
+                        //GetAngleRate
+                        __property double get_PositionRate(void)
+                        {
+                                return dJointGetSliderPositionRate(this->_id);
+                        }
+
+        };
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll b/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll
new file mode 100755
index 0000000..ccf2a41
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Release/ode.dll
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp
new file mode 100644
index 0000000..c9a7e19
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Space.cpp
@@ -0,0 +1,53 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "Space.h"
+#include "TEST.h"
+
+namespace ODEManaged
+{
+
+        //Constructor
+
+                Space::Space(void)
+                { 
+                        _id = dSimpleSpaceCreate();
+                }
+
+                Space::Space(int minlevel, int maxlevel)
+                { 
+                        _id = dHashSpaceCreate();
+                        dHashSpaceSetLevels(this->_id, minlevel, maxlevel);
+                }
+
+        
+        //Destructor
+
+                Space::~Space(void)
+                {
+                        dSpaceDestroy(this->_id);
+                }
+
+
+        //Methods
+
+                //Id
+                dSpaceID Space::Id()
+                {
+                        return _id;
+                }
+
+
+                //Collide
+                void Space::Collide(void *data, dNearCallback *callback)
+                {               
+                        dSpaceCollide(this->_id, data, callback);
+                }
+
+
+
+
+
+
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Space.h b/libraries/ode-0.9/contrib/DotNetManaged/Space.h
new file mode 100644
index 0000000..78e81ad
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Space.h
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "CommonMgd.h"
+
+namespace ODEManaged
+{
+        __gc public class Space
+        {
+        public:
+
+                //Constructor
+                        
+                        Space(void);
+                        Space(int minlevel, int maxlevel);
+                        
+                //Destructor
+                        
+                        ~Space(void);
+
+
+                //Methods
+
+                        dSpaceID Id(void);
+                        void Collide(void *data, dNearCallback *callback);
+
+
+                private:
+                        
+                        dSpaceID _id;
+
+        };
+
+}
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp
new file mode 100644
index 0000000..b6c9d98
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.cpp
@@ -0,0 +1,5 @@
+// stdafx.cpp : source file that includes just the standard includes
+// ODEManaged.pch will be the pre-compiled header
+// stdafx.obj will contain the pre-compiled type information
+
+#include "stdafx.h"
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h
new file mode 100644
index 0000000..2222759
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/Stdafx.h
@@ -0,0 +1,12 @@
+// stdafx.h : include file for standard system include files,
+// or project specific include files that are used frequently,
+// but are changed infrequently
+
+#pragma once
+
+#using <mscorlib.dll>
+
+
+
+
+
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/TEST.h b/libraries/ode-0.9/contrib/DotNetManaged/TEST.h
new file mode 100644
index 0000000..e2cdbc3
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/TEST.h
@@ -0,0 +1,17 @@
+                
+#pragma once
+
+#include "CommonMgd.h"
+
+namespace ODEManaged
+{       
+                
+void RnearCallback(void *data, dGeomID o1, dGeomID o2)
+                {
+                }
+
+}
+
+
+
+
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/World.cpp b/libraries/ode-0.9/contrib/DotNetManaged/World.cpp
new file mode 100644
index 0000000..beab21a
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/World.cpp
@@ -0,0 +1,74 @@
+#include "StdAfx.h"
+
+#include <ode/ode.h>
+#include "World.h"
+
+namespace ODEManaged
+{
+        
+        //Constructor
+        
+                World::World(void)
+                {
+                        /*dWorldID _temp = dWorldCreate();
+                        _id = _temp;*/ 
+                        _id = dWorldCreate();
+                }
+
+
+        //Destructor
+
+                World::~World(void)
+                {
+                        dWorldDestroy(this->_id);
+                }
+
+
+        //Methods
+
+                //Id
+                dWorldID World::Id()
+                {
+                        return _id;
+                }
+
+
+                //SetGravity
+                void World::SetGravity(double x, double y, double z)
+                { 
+                        dWorldSetGravity(this->_id, x, y, z); 
+                }
+
+
+                //Overloaded GetGravity
+                Vector3 World::GetGravity(void)
+                {
+                        Vector3 retVal;
+                        dVector3 temp;
+                        dWorldGetGravity(this->_id, temp);
+                        retVal.x = temp[0];
+                        retVal.y = temp[1];
+                        retVal.z = temp[2];
+                        return retVal;
+                }
+
+                void World::GetGravity(double gravity __gc[])
+                {
+                        dVector3 temp;
+                        dWorldGetGravity(this->_id, temp);
+                        gravity[0] = temp[0];
+                        gravity[1] = temp[1];
+                        gravity[2] = temp[2];
+                }
+
+
+                //Step
+                void World::Step(double stepSize)
+                {
+                        dWorldStep(this->_id, stepSize);
+                }
+
+}
+
+
+
diff --git a/libraries/ode-0.9/contrib/DotNetManaged/World.h b/libraries/ode-0.9/contrib/DotNetManaged/World.h
new file mode 100644
index 0000000..c4c60e5
--- /dev/null
+++ b/libraries/ode-0.9/contrib/DotNetManaged/World.h
@@ -0,0 +1,67 @@
+#pragma once
+
+#include "CommonMgd.h"
+
+namespace ODEManaged
+{       
+        __gc public class World
+        {       
+        public:
+
+                //Constructor
+                
+                        World(void);
+
+
+                //Destructor
+                        
+                        ~World(void);
+
+                        
+                // Methods
+
+                        dWorldID Id(void);
+                        
+                        void SetGravity(double x, double y, double z);
+
+                        //Overloaded GetGravity
+                        Vector3 GetGravity(void);               
+                        void GetGravity(double gravity __gc[]);
+
+                        void Step(double stepSize);
+
+
+                //Properties
+
+                        //Constraint Force Mixing
+                        __property void set_CFM(double cfm)
+                        {
+                                dWorldSetCFM(this->_id,cfm);
+                        }
+
+                        __property double get_CFM(void)
+                        {
+                                return dWorldGetCFM(this->_id); 
+                        }
+
+
+                        //Error Reduction Parameter
+                        __property void set_ERP(double erp)
+                        {
+                                dWorldSetERP(this->_id,erp);
+                        }
+
+                        __property double get_ERP(void)
+                        {
+                                return dWorldGetERP(this->_id); 
+                        }
+
+
+                private:
+
+                        dWorldID _id;
+
+        };
+
+}
+
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
new file mode 100644
index 0000000..26b77b0
--- /dev/null
+++ b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.cpp
@@ -0,0 +1,218 @@
+
+/* ************************************************************************ */
+/* 
+   grouped and transformed geometry functions 
+   author: Tim Schmidt tisch@uni-paderborn.de
+*/
+
+
+#include <ode/common.h>
+#include <ode/geom.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include <ode/memory.h>
+#include <ode/misc.h>
+#include <ode/objects.h>
+#include <ode/matrix.h>
+#include <ode/GeomTransformGroup.h>
+#include "objects.h"
+#include "array.h"
+#include "geom_internal.h"
+
+// given a pointer `p' to a dContactGeom, return the dContactGeom at
+// p + skip bytes.
+
+#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
+
+
+// ############################################################################
+
+int dGeomTransformGroupClass = -1;
+// ############################################################################
+
+struct dxGeomTransformGroup {
+  dArray<dxGeom*> parts;        // all the geoms that make up the group
+  dVector3 relativePosition;
+  dMatrix3 relativeRotation;
+};
+// ############################################################################
+
+void dGeomTransformGroupSetRelativePosition (dxGeom *g, dReal x, dReal y, dReal z)
+{
+  dAASSERT (g);
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  transformGroup->relativePosition[0] = x;
+  transformGroup->relativePosition[1] = y;
+  transformGroup->relativePosition[2] = z;
+}
+// ############################################################################
+
+void dGeomTransformGroupSetRelativeRotation (dxGeom *g, const dMatrix3 R)
+{
+  dAASSERT (g);
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  memcpy (transformGroup->relativeRotation,R,sizeof(dMatrix3));
+}
+// ############################################################################
+
+const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g)
+{
+  dAASSERT (g);
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  return transformGroup->relativePosition;
+}
+// ############################################################################
+
+const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g)
+{
+  dAASSERT (g);
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  return transformGroup->relativeRotation;
+}
+// ############################################################################
+
+static void computeFinalTransformation (const dxGeom *tg, const dxGeom *part)
+{
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(tg);
+  dMULTIPLY0_331 (part->pos,tg->R,transformGroup->relativePosition);
+  part->pos[0] += tg->pos[0];
+  part->pos[1] += tg->pos[1];
+  part->pos[2] += tg->pos[2];
+  dMULTIPLY0_333 (part->R,tg->R,transformGroup->relativeRotation);
+}
+// ############################################################################
+
+int dCollideTransformGroup (const dxGeom *o1, const dxGeom *o2, int flags,
+               dContactGeom *contact, int skip)
+{
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(o1);
+  if (transformGroup->parts.size() == 0)
+  {
+    return 0;
+  }
+  int numleft = flags & NUMC_MASK;
+  if (numleft == 0) numleft = 1;
+  flags &= ~NUMC_MASK;
+  int num=0, i=0;
+  while (i < transformGroup->parts.size() && numleft > 0) 
+  {
+    dUASSERT (transformGroup->parts[i]->spaceid==0,
+              "GeomTransformGroup encapsulated object must not be in a space");
+    dUASSERT (transformGroup->parts[i]->body==0,
+              "GeomTransformGroup encapsulated object must not be attached to a body");
+    if (!o1->space_aabb) 
+    {
+      computeFinalTransformation (o1, transformGroup->parts[i]);
+    }
+    dxBody *bodyBackup = transformGroup->parts[i]->body;
+    transformGroup->parts[i]->body = o1->body;
+    int n = dCollide (transformGroup->parts[i],const_cast<dxGeom*>(o2),
+                      flags | numleft,contact,skip);
+    transformGroup->parts[i]->body = bodyBackup;
+    contact = CONTACT (contact,skip*n);
+    numleft -= n;
+    num += n;
+    i++;
+  }
+  return num;
+}
+// ############################################################################
+
+static dColliderFn * dGeomTransformGroupColliderFn (int num)
+{
+  return (dColliderFn *) &dCollideTransformGroup;
+}
+// ############################################################################
+
+static void dGeomTransformGroupAABB (dxGeom *geom, dReal aabb[6])
+{
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom);
+  aabb[0] = dInfinity;
+  aabb[1] = -dInfinity;
+  aabb[2] = dInfinity;
+  aabb[3] = -dInfinity;
+  aabb[4] = dInfinity;
+  aabb[5] = -dInfinity;
+  int i,j;
+  for (i=0; i < transformGroup->parts.size(); i++)
+  {
+    computeFinalTransformation (geom, transformGroup->parts[i]);
+    dReal aabb2[6];
+    transformGroup->parts[i]->_class->aabb (transformGroup->parts[i],aabb2);
+    for (j=0; j<6; j += 2) if (aabb2[j] < aabb[j]) aabb[j] = aabb2[j];
+    for (j=1; j<6; j += 2) if (aabb2[j] > aabb[j]) aabb[j] = aabb2[j];
+  }
+}
+// ############################################################################
+
+static void dGeomTransformGroupDtor (dxGeom *geom)
+{
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(geom);
+  transformGroup->parts.~dArray();
+}
+// ############################################################################
+
+dxGeom *dCreateGeomTransformGroup (dSpaceID space)
+{
+  if (dGeomTransformGroupClass == -1) {
+    dGeomClass c;
+    c.bytes = sizeof (dxGeomTransformGroup);
+    c.collider = &dGeomTransformGroupColliderFn;
+    c.aabb = &dGeomTransformGroupAABB;
+    c.aabb_test = 0;
+    c.dtor = dGeomTransformGroupDtor;
+    dGeomTransformGroupClass = dCreateGeomClass (&c);
+  }
+  dxGeom *g = dCreateGeom (dGeomTransformGroupClass);
+  if (space)
+  {
+    dSpaceAdd (space,g);
+  }
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  transformGroup->parts.constructor();
+  dSetZero (transformGroup->relativePosition,4);
+  dRSetIdentity (transformGroup->relativeRotation);
+  return g;
+}
+// ############################################################################
+
+void dGeomTransformGroupAddGeom (dxGeom *g, dxGeom *obj)
+{
+  dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
+            "argument not a geom TransformGroup");
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  transformGroup->parts.push (obj);
+}
+// ############################################################################
+
+void dGeomTransformGroupRemoveGeom (dxGeom *g, dxGeom *obj)
+{
+  dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
+            "argument not a geom TransformGroup");
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  for (int i=0; i < transformGroup->parts.size(); i++) {
+    if (transformGroup->parts[i] == obj) {
+      transformGroup->parts.remove (i);
+      return;
+    }
+  }
+}
+// ############################################################################
+
+dxGeom * dGeomTransformGroupGetGeom (dxGeom *g, int i)
+{
+  dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
+            "argument not a geom TransformGroup");
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  dAASSERT (i >= 0 && i < transformGroup->parts.size());
+  return transformGroup->parts[i];
+}
+// ############################################################################
+
+int dGeomTransformGroupGetNumGeoms (dxGeom *g)
+{
+  dUASSERT (g && g->_class->num == dGeomTransformGroupClass,
+            "argument not a geom TransformGroup");
+  dxGeomTransformGroup *transformGroup = (dxGeomTransformGroup*) CLASSDATA(g);
+  return transformGroup->parts.size();
+}
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h
new file mode 100644
index 0000000..705fdb9
--- /dev/null
+++ b/libraries/ode-0.9/contrib/GeomTransformGroup/GeomTransformGroup.h
@@ -0,0 +1,29 @@
+ 
+/* ************************************************************************ */
+/* 
+   grouped and transformed geometry functions 
+   author: Tim Schmidt tisch@uni-paderborn.de
+*/
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+extern int dGeomTransformGroupClass;
+
+void dGeomTransformGroupSetRelativePosition (dGeomID g, dReal x, dReal y, dReal z);
+void dGeomTransformGroupSetRelativeRotation (dGeomID g, const dMatrix3 R);
+const dReal * dGeomTransformGroupGetRelativePosition (dxGeom *g);
+const dReal * dGeomTransformGroupGetRelativeRotation (dxGeom *g);
+dGeomID dCreateGeomTransformGroup (dSpaceID space);
+void dGeomTransformGroupAddGeom    (dGeomID tg, dGeomID obj);
+void dGeomTransformGroupRemoveGeom (dGeomID tg, dGeomID obj);
+dGeomID dGeomTransformGroupGetGeom (dGeomID tg, int i);
+int dGeomTransformGroupGetNumGeoms (dGeomID tg);
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt b/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt
new file mode 100644
index 0000000..bca0e66
--- /dev/null
+++ b/libraries/ode-0.9/contrib/GeomTransformGroup/README.txt
@@ -0,0 +1,148 @@
+README for GeomTransformGroup by Tim Schmidt.
+---------------------------------------------
+
+This is a patch to add the dGeomTransformGroup object to the list of geometry 
+objects.
+
+It should work with the cvs version of the ode library from 07/24/2002.
+
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+comment by russ smith: this code is easy to use with the rest of ODE.
+simply copy GeomTransformGroup.cpp to ode/src and copy GeomTransformGroup.h
+to include/ode. then add GeomTransformGroup.cpp to the ODE_SRC variable
+in the makefile. rebuild, and you're done! of course i could have done all
+this for you, but i prefer to keep GeomTransformGroup separated from the
+rest of ODE for now while other issues with the collision system are
+resolved.
+
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+
+Description:
+
+The dGeomTransformGroup is an adaption of the TransformGroup known from 
+Java3D (and maybe other libraries with a similar scene graph representation).
+It can be used to build an arbitrarily structured tree of objects that are 
+each positioned relative to the particular parent node.
+
+If you have a plane for example, there is one root node associated with the 
+plane's body and another three transformgroups placed 'under' this node. One 
+with the fuselage (cappedcylinder) under it and two with the underlying wings 
+(flat boxes). And if you want to add engines, simply put them 'next to' the 
+wings under another two transformgroups.
+
+bodyTG ---> associated with dBody
+    |
+    +--fuselageTG
+    |   |
+    |   +--fuselageCylinder
+    |
+    +--leftwingTG
+    |   |
+    |   +--wingBox
+    |   |
+    |   +--leftengineTG
+    |         |
+    |         +--leftengineCylinder
+    | 
+    +--rightwingTG
+        |
+        +--wingBox
+        |
+        +--rightengineTG
+              |
+              +--rightengineCylinder
+
+This is a method to easily compose objects without the necessity of always 
+calculating global coordinates. But apart from this there is something else 
+that makes dGeomTransformGroups very valuable.
+
+Maybe you remember that some users reported the problem of acquiring the 
+correct bodies to be attached by a contactjoint in the nearCallback when 
+using dGeomGroups and dGeomTransforms at the same time. This results from the 
+fact that dGeomGroups are not associated with bodies while all other 
+geometries are.
+
+So, as you can see in the nearCallback of the the test_buggy demo you have to 
+attach the contactjoint with the bodies that you get from the geometries that 
+are stored in the contact struct (-> dGeomGetBody(contacts[i].geom.g1)). 
+Normally you would do this by asking o1 and o2 directly with dGeomGetBody(o1) 
+and dGeomGetBody(o2) respectively.
+
+As a first approach you can overcome that problem by testing o1 and o2 if 
+they are groups or not to find out how to get the corresponding bodies.
+
+However this will fail if you want grouped transforms that are constructed 
+out of dGeomTransforms encapsulated in a dGeomGroup. According to the test 
+you use contacts[i].geom.g1 to get the right body. Unfortunately g1 is 
+encapsulated in a transform and therefore not attached to any body. In this 
+case the dGeomTransform 'in the middle' would have been the right object to 
+be asked for the body.
+
+You may now conclude that it is a good idea to unwrap the group encapsulated 
+geoms at the beginning of the nearcallback and use dGeomGetBody(o1) 
+consistently. But keep in mind that this also means not to invoke 
+dCollide(..) on groups at all and therefore not to expoit the capability of 
+dGeomGroups to speed up collision detection by the creation of bounding boxes 
+around the encapsulated geometry.
+
+Everything becomes even worse if you create a dGeomTransform that contains a 
+dGeomGroup of geoms. The function that cares about the collision of 
+transforms with other objects uses the position and rotation of the 
+respective encapsulated object to compute its final position and orientation. 
+Unfortunately dGeomGroups do not have a position and rotation, so the result 
+will not be what you have expected.
+
+Here the dGeomTransformGroups comes into operation, because it combines the 
+advantages and capabilities of the dGeomGroup and the dGeomTransform.
+And as an effect of synergy it is now even possible to set the position of a 
+group of geoms with one single command.
+Even nested encapsulations of dGeomTransformGroups in dGeomTransformGroups 
+should be possible (to be honest, I have not tried that so far ;-) ).
+
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+API:
+
+dGeomID dCreateGeomTransformGroup (dSpaceID space);
+  - create a GeomTransformGroup 
+    
+void dGeomTransformGroupAddGeom    (dGeomID tg, dGeomID obj);
+  - Comparable to dGeomTransformSetGeom or dGeomGroupAdd
+  - add objects to this group
+   
+void dGeomTransformGroupRemoveGeom (dGeomID tg, dGeomID obj);
+  - remove objects from this group
+
+void dGeomTransformGroupSetRelativePosition
+            (dGeomID g, dReal x, dReal y, dReal z);
+void dGeomTransformGroupSetRelativeRotation
+            (dGeomID g, const dMatrix3 R);
+  - Comparable to setting the position and rotation of all the
+    dGeomTransform encapsulated geometry. The difference 
+    is that it is global with respect to this group and therefore
+    affects all geoms in this group.
+  - The relative position and rotation are attributes of the 
+    transformgroup, so the position and rotation of the individual
+    geoms are not changed 
+
+const dReal * dGeomTransformGroupGetRelativePosition (dGeomID g);
+const dReal * dGeomTransformGroupGetRelativeRotation (dGeomID g);
+  - get the relative position and rotation
+  
+dGeomID dGeomTransformGroupGetGeom (dGeomID tg, int i);
+  - Comparable to dGeomGroupGetGeom
+  - get a specific geom of the group
+  
+int dGeomTransformGroupGetNumGeoms (dGeomID tg);
+  - Comparable to dGeomGroupGetNumGeoms
+  - get the number of geoms in the group
+  
+
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+
+Tim Schmidt
+student of computer science
+University of Paderborn, Germany
+tisch@uni-paderborn.de
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.bin b/libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.bin
new file mode 100644
index 0000000..c06768c
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/CW7_projects.sit.bin
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt b/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt
new file mode 100644
index 0000000..7c1f8a4
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/README.txt
@@ -0,0 +1,95 @@
+-----------------------------
+ODE - Mac CFM Carbon Port
+(contact Frank Condello <pox@planetquake.com> with questions regarding this port)
+
+Although ODE contains a MacOSX makefile, and some individuals have implemented ODE in
+Cocoa, I opted to use (and prefer) CodeWarrior. This also opens up ODE to MacOS8 & 9
+users, without scarfing functionality in MacOSX (same binaries run on both platforms).
+
+The 'ode_CW7.mcp' project contains release and debug targets to create static ODE and
+DrawStuff libraries.
+
+'examples_CW7.mcp' contains targets for the entire ODE test suite, plus a couple other
+test programs which were posted to the ODE mailing list.
+
+
+-----------------------------
+Compiling Notes:
+
+You'll need to extract the CodeWarrior projects from the 'CW7_projects.sit.bin' archive
+(They're nearly a meg uncompressed so this was done to be bandwith friendly on the CVS).
+
+Projects require CodeWarrior 7 or above (recreating them with earlier versions shouldn't
+be too difficult). The projects use relative paths and are meant to be compiled from
+'contrib/Mac_CFMCarbon/'. Don't move them!
+
+All the libraries build into the 'lib/' directory, all test applications build into
+'contrib/Mac_CFMCarbon/mac_testbin/' (and must be run from that directory since the
+texture path is hard-coded).
+
+You'll need to compile the release ODE library, and the DrawStuff library before
+compiling the examples.
+
+The ODE 'configurator' has not been ported, but a Mac-friendly 'config.h' header has been
+manually hacked together (all PPC Macs should be fine with this header). Single or double
+precision can be defined in the 'CommonPrefix.h' header found in
+'contrib/Mac_CFMCarbon/mac_source/'.
+
+'contrib/Mac_CFMCarbon/mac_source/' also contains any mac specific additions to the main source.
+The directory structure here matches the main source tree, and I would recommend that this
+format is maintained when making additions, since the access paths are touchy (more below...)
+
+Some issues were encountered with duplicate header names. CodeWarrior tends to be
+unforgiving about this sort of thing but fudging with the access paths eventually
+cleared up the problem. If ODE fails to compile, make sure the <ode/objects.h> and
+"objects.h" or <timer.h> and <Timer.h> are actually pointing to the correct header.
+
+You'll need Apple's OpenGL SDK (with GLUT) in your compiler path to build DrawStuff. I've
+added redirection headers in 'contrib/Mac_CFMCarbon/mac_source/include/GL/' to properly
+link with the Apple headers (since the projects are set to follow DOS paths).
+
+The examples link against a crapload of static libraries, but my initial builds using
+ODE, MSL, GLUT, and DrawStuff shared/merged DLL's proved unstable (mostly problems with
+SIOUX spawning multiple sessions, and crashes in Classic). Static libs just worked better
+in the end, but the test apps are a little bloated as a result, and need to be re-linked
+whenever a change to a library is made.
+
+IMPORTANT: You must use the same 'CommonPrefix.h' settings for libraries, and test apps
+(i.e. double or single precision).
+
+
+-----------------------------
+Running the test apps:
+
+The test apps will show the SIOUX CLI prompt when run. Just hit OK to ignore it, or add any
+DrawStuff arguments. You'll want to log output to a file for 'test_ode'.
+
+There are two extra test programs in the 'mac_source' directory. Both were posted to the ODE
+mailing list by OSX users. 'test_stability1' visualizes some internal issues with ODE, and
+'test_stacktest' is a standalone GLUT program (doesn't use DrawStuff) that can be useful
+to stress test the library, and give you an idea of just how much stack memory you're
+going to need for large systems.
+
+ISSUES:
+
+The carbon DrawStuff lib uses GLUT to make life easy, but GLUT isn't exactly bug-free
+or stable on the Mac... Try moving the mouse around if a simulation is running slowly
+on OS9 (it's not ODE's fault, but rather a poor carbon GLUT implementation - seems GLUT stalls
+when it's not getting system events  - I haven't seen this problem on OSX).
+
+The 3D view may not update if typing in the SIOUX console window.
+
+You cannot pass startup args to GLUT due to the way the DrawStuff library initializes.
+
+'Write Frames' doesn't actually do anything at the moment.
+
+The 'test_joints' app seems broken (though I don't know what the intended effect should be)
+
+
+-----------------------------
+TODO:
+
+- Re-add shared library targets (if stability issues are resolved).
+- Implement 'Write Frames' in DrawStuff.
+- Write a Carbon compatible configurator
+- Create CodeWarrior 8 projects (once I scrounge up enough dough for the update).
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h
new file mode 100644
index 0000000..5948b3e
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/CommonPrefix.h
@@ -0,0 +1,6 @@
+#define TARGET_API_MAC_CARBON 1
+#define finite isfinite
+#define dNODEBUG 1
+
+// Comment out for single precision
+#define PRECISION_DOUBLE 1
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h
new file mode 100644
index 0000000..6122528
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DSPrefix.h
@@ -0,0 +1,6 @@
+#ifndef prefix_h
+#define prefix_h
+
+#include "CommonPrefix.h"
+
+#endif // prefix_h
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h
new file mode 100644
index 0000000..0e328a9
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/DebugPrefix.h
@@ -0,0 +1,10 @@
+#ifndef prefix_h
+#define prefix_h
+
+#include "CommonPrefix.h"
+
+#ifdef dNODEBUG
+#undef dNODEBUG
+#endif
+
+#endif // prefix_h
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h
new file mode 100644
index 0000000..1dedfc9
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ExamplesPrefix.h
@@ -0,0 +1,13 @@
+#ifndef prefix_h
+#define prefix_h
+
+#include "CommonPrefix.h"
+
+// Hack to automatically call SIOUX's CLI interface for the test apps
+#include <console.h>
+#include <SIOUX.h>
+int fmain (int argc, char **argv);
+int main (int argc, char **argv) { argc = ccommand(&argv); return fmain(argc, argv); }
+#define main(argc, argv) fmain(argc, argv)
+
+#endif // prefix_h
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h
new file mode 100644
index 0000000..f8f5022
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ODETestPrefix.h
@@ -0,0 +1,13 @@
+#ifndef prefix_h
+#define prefix_h
+
+#include "CommonPrefix.h"
+
+// Hack to automatically call SIOUX's CLI interface for the test apps
+#include <console.h>
+#include <SIOUX.h>
+int fmain ();
+int main (int argc, char **argv) { argc = ccommand(&argv); return fmain(); }
+#define main() fmain()
+
+#endif // prefix_h
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h
new file mode 100644
index 0000000..6122528
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ReleasePrefix.h
@@ -0,0 +1,6 @@
+#ifndef prefix_h
+#define prefix_h
+
+#include "CommonPrefix.h"
+
+#endif // prefix_h
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp
new file mode 100644
index 0000000..eb0b144
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/drawstuff/src/mac_glut_carbon.cpp
@@ -0,0 +1,281 @@
+/*************************************************************************
+ *                                                                       *
+ * DrawStuff Library, Copyright (C) 2001 Russell L. Smith.               *
+ *   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 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.    *
+ *                                                                       *
+ * 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 GNU      *
+ * Lesser General Public License for more details.                       *
+ *                                                                       *
+ * You should have received a copy of the GNU Lesser General Public      *
+ * License along with this library (see the file LICENSE.TXT); if not,   *
+ * write to the Free Software Foundation, Inc., 59 Temple Place,         *
+ * Suite 330, Boston, MA 02111-1307 USA.                                 *
+ *                                                                       *
+ *************************************************************************/
+
+// main window and event handling for Mac CFM Carbon
+
+#include <ode/config.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <glut.h>
+#include <SIOUX.h>
+
+#include <MacTypes.h>
+#include <Timer.h>
+
+#include <drawstuff/drawstuff.h>
+#include <drawstuff/version.h>
+#include "internal.h"
+
+
+//***************************************************************************
+// error handling for unix (works just fine with SIOUX)
+
+static void printMessage (char *msg1, char *msg2, va_list ap)
+{
+  fflush (stderr);
+  fflush (stdout);
+  fprintf (stderr,"\n%s: ",msg1);
+  vfprintf (stderr,msg2,ap);
+  fprintf (stderr,"\n");
+  fflush (stderr);
+}
+
+extern "C" void dsError (char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  printMessage ("Error",msg,ap);
+  exit (1);
+}
+
+
+extern "C" void dsDebug (char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  printMessage ("INTERNAL ERROR",msg,ap);
+  // *((char *)0) = 0;   ... commit SEGVicide ?
+  abort();
+}
+
+extern "C" void dsPrint (char *msg, ...)
+{
+  va_list ap;
+  va_start (ap,msg);
+  vprintf (msg,ap);
+}
+
+//***************************************************************************
+// openGL window
+
+// window and openGL
+static int width=0,height=0;            // window size
+static int last_key_pressed=0;          // last key pressed in the window
+static int pause=0;                                     // 1 if in `pause' mode
+static int singlestep=0;                        // 1 if single step key pressed
+static int writeframes=0;                       // 1 if frame files to be written
+static dsFunctions *gfn;
+static int frame = 1;
+
+float getTime (void)
+{
+        UnsignedWide ms;
+        
+        Microseconds(&ms);
+        return ms.lo / 1000000.0;
+}
+
+
+static void captureFrame (int num)
+{
+// TODO
+}
+
+static void reshape(int w, int h)
+{
+        width = w;
+        height = h;
+}
+
+static void draw(void)
+{
+        dsDrawFrame (width,height,gfn,pause && !singlestep);
+        singlestep = 0;
+        glutSwapBuffers();
+        
+        if (pause==0 && writeframes) {
+      captureFrame (frame);
+      frame++;
+    }
+}
+
+static void idle(void)
+{
+        static float lasttime=0;
+        float t;
+        
+        // Try to maintain a reasonable rate (good enough for testing anyway)
+        t = getTime();
+        if (lasttime < t) {
+                lasttime = t+0.005;
+                draw();
+        }
+}
+
+static void key(unsigned char key, int x, int y)
+{
+        if (!glutGetModifiers()) {
+                
+                if (key >= ' ' && key <= 126 && gfn->command) gfn->command (key);
+        
+        // GLUT_ACTIVE_CTRL doesn't seem to be working, so we use Alt
+        } else if (glutGetModifiers()&GLUT_ACTIVE_ALT) {
+                
+                switch (key) {
+                case 't': case 'T':
+                        dsSetTextures (dsGetTextures() ^ 1);
+                        break;
+                case 's': case 'S':
+                        dsSetShadows (dsGetShadows() ^ 1);
+                        break;
+                case 'p': case 'P':
+                        pause ^= 1;
+                        singlestep = 0;
+                        break;
+                case 'o': case 'O':
+                        if (pause) singlestep = 1;
+                        break;
+                case 'v': case 'V': {
+                        float xyz[3],hpr[3];
+                        dsGetViewpoint (xyz,hpr);
+                        printf ("Viewpoint = (%.4f,%.4f,%.4f,%.4f,%.4f,%.4f)\n",
+                                        xyz[0],xyz[1],xyz[2],hpr[0],hpr[1],hpr[2]);
+                        break;
+              }
+            // No case 'X' - Quit works through the Mac system menu, or cmd-q
+                case 'w': case 'W':
+                        writeframes ^= 1;
+                        if (writeframes) printf ("Write frames not done yet!\n");// TODO
+                                break;
+                }
+        }
+                
+    last_key_pressed = key;
+}
+
+static int mx=0,my=0;   // mouse position
+static int mode = 0;    // mouse button bits
+  
+static void MouseDown(int button, int state, int x, int y)
+{
+        if(button == GLUT_LEFT_BUTTON)
+        {
+                if(state == GLUT_DOWN)
+                        mode |= 1;
+                else if(state == GLUT_UP)
+                        mode &= (~1);
+        }
+        else if (button == GLUT_MIDDLE_BUTTON)
+        {
+                if(state == GLUT_DOWN)
+                        mode |= 3;
+                else if(state == GLUT_UP)
+                        mode &= (~3);
+        }
+        else if (button == GLUT_RIGHT_BUTTON)
+        {
+                if(state == GLUT_DOWN)
+                        mode |= 2;
+                else if(state == GLUT_UP)
+                        mode &= (~2);
+        }
+        
+        mx = x;
+        my = y;
+}
+
+static void MouseMove(int x, int y)
+{
+        dsMotion (mode, x - mx, y - my);
+        mx = x;
+        my = y; 
+}
+
+static void createMainWindow (int _width, int _height)
+{
+        // So GLUT doesn't complain
+        int argc = 0;
+        char **argv = NULL;
+        
+        // initialize variables
+        width = _width;
+        height = _height;
+        last_key_pressed = 0;
+
+        if (width < 1 || height < 1) dsDebug (0,"bad window width or height");
+        
+        glutInit(&argc, argv);
+        glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
+        glutInitWindowSize(_width, _height);
+        glutInitWindowPosition(100, 100);
+        glutCreateWindow("ODE Simulation");
+        
+        glutKeyboardFunc(key);
+        glutMotionFunc(MouseMove);
+        glutMouseFunc(MouseDown);
+        glutReshapeFunc(reshape);
+        glutDisplayFunc(idle);
+        glutIdleFunc(idle);
+}
+
+void dsPlatformSimLoop (int window_width, int window_height, dsFunctions *fn,
+                        int initial_pause)
+{       
+        SIOUXSettings.initializeTB     = false;
+        SIOUXSettings.standalone       = false;
+        SIOUXSettings.setupmenus       = false;
+        SIOUXSettings.autocloseonquit  = true;
+        SIOUXSettings.asktosaveonclose = false;
+                
+        gfn = fn;
+        pause = initial_pause;
+
+        printf (
+                        "\n"
+                        "Simulation test environment v%d.%02d\n"
+                        "   Option-P : pause / unpause (or say `-pause' on command line).\n"
+                        "   Option-O : single step when paused.\n"
+                        "   Option-T : toggle textures (or say `-notex' on command line).\n"
+                        "   Option-S : toggle shadows (or say `-noshadow' on command line).\n"
+                        "   Option-V : print current viewpoint coordinates (x,y,z,h,p,r).\n"
+                        "   Option-W : write frames to ppm files: frame/frameNNN.ppm\n"
+                        "\n"
+                        "Change the camera position by clicking + dragging in the window.\n"
+                        "   Left button - pan and tilt.\n"
+                        "   Right button - forward and sideways.\n"
+                        "   Left + Right button (or middle button) - sideways and up.\n"
+                        "\n",DS_VERSION >> 8,DS_VERSION & 0xff);
+        
+        createMainWindow (window_width, window_height);
+        dsStartGraphics (window_width,window_height,fn);
+        
+        if (fn->start) fn->start();
+        
+        glutMainLoop();
+        
+        if (fn->stop) fn->stop();
+        dsStopGraphics();
+}
+
+extern "C" void dsStop(){ }// GLUT/MSL hooks into the system to exit
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h
new file mode 100644
index 0000000..4acaeed
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/gl.h
@@ -0,0 +1,2 @@
+// A little hackaround (Apple use / in the FILENAME, which doesn't work when following DOS paths)
+#include <gl.h>
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h
new file mode 100644
index 0000000..5b4a791
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/GL/glu.h
@@ -0,0 +1,2 @@
+// A little hackaround (Apple use / in the FILENAME, which doesn't work when following DOS paths)
+#include <glu.h>
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h
new file mode 100644
index 0000000..bb889f9
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/include/ode/config.h
@@ -0,0 +1,52 @@
+/* This file has been manually hacked together for the Mac CFM Carbon build - Frank. */
+
+#ifndef _ODE_CONFIG_H_
+#define _ODE_CONFIG_H_
+
+/* standard system headers */
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+#include <stdarg.h>
+#include <malloc.h>
+#include <alloca.h>
+#include <float.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* #define PENTIUM 1 -- not a pentium */
+
+/* integer types (we assume int >= 32 bits) */
+typedef char int8;
+typedef unsigned char uint8;
+typedef int int32;
+typedef unsigned int uint32;
+
+/* an integer type that we can safely cast a pointer to and from without loss of bits. */
+typedef unsigned int intP;
+
+#ifdef PRECISION_DOUBLE
+
+ /*select the base floating point type*/
+ #define dDOUBLE 1
+
+ /* the floating point infinity */
+ #define dInfinity DBL_MAX
+
+#else
+
+ /* select the base floating point type */
+ #define dSINGLE 1
+
+ /* the floating point infinity */
+ #define dInfinity FLT_MAX
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+#endif
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp
new file mode 100644
index 0000000..79c066a
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stability1.cpp
@@ -0,0 +1,289 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#include <ode/ode.h>
+#include <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 DENSITY (5.0)           // density of all objects
+
+// dynamics and collision objects
+
+struct MyObject {
+  dBodyID body;         // the body
+  dGeomID geom;         // geometry representing this body
+};
+
+static dWorldID world;
+static dSpaceID space;
+static MyObject fallingObject;
+static dGeomID  box1, box2;
+static dJointGroupID contactgroup;
+
+
+// this is called by dSpaceCollide when two objects in space are
+// potentially colliding.
+
+static void nearCallback (void *data, dGeomID o1, dGeomID o2)
+{
+  int i;
+  // if (o1->body && o2->body) return;
+
+  // exit without doing anything if the two bodies are connected by a joint
+  dBodyID b1 = dGeomGetBody(o1);
+  dBodyID b2 = dGeomGetBody(o2);
+  if (b1 && b2 && dAreConnected (b1,b2)) return;
+
+  dContact contact[4];                  // up to 3 contacts per box
+  for (i=0; i<4; i++) {
+    contact[i].surface.mode = dContactBounce; //dContactMu2;
+    contact[i].surface.mu = dInfinity;
+    contact[i].surface.mu2 = 0;
+    contact[i].surface.bounce = 0.5;
+    contact[i].surface.bounce_vel = 0.1;
+  }
+  if (int numc = dCollide (o1,o2,4,&contact[0].geom,sizeof(dContact))) {
+    // dMatrix3 RI;
+    // dRSetIdentity (RI);
+    // const dReal ss[3] = {0.02,0.02,0.02};
+    for (i=0; i<numc; i++) {
+      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
+      dJointAttach (c,b1,b2);
+      // dsDrawBox (contact[i].geom.pos,RI,ss);
+    }
+  }
+}
+
+
+// start simulation - set viewpoint
+
+static void start()
+{
+  static float xyz[3] = {-4.0f, 0.0f, 3.0f};
+  static float hpr[3] = {0.0f,-15.0f,0.0f};
+  dsSetViewpoint (xyz,hpr);
+  printf ("To drop another object, press:\n");
+  printf ("   b for box.\n");
+  printf ("   s for sphere.\n");
+  printf ("   c for cylinder.\n");
+  printf ("To select an object, press space.\n");
+}
+
+
+char locase (char c)
+{
+  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
+  else return c;
+}
+
+
+// called when a key pressed
+
+static void command (int cmd)
+{
+    int i,k;
+    dReal sides[3];
+    dMass m;
+
+    cmd = locase (cmd);
+    if (cmd == 'b' || cmd == 's' || cmd == 'c') {
+        // Destroy the currently falling object and replace it by an instance of the requested type
+        if (fallingObject.body) {
+            dBodyDestroy (fallingObject.body);
+            dGeomDestroy (fallingObject.geom);
+            memset (&fallingObject, 0, sizeof(fallingObject));
+        }
+
+        fallingObject.body = dBodyCreate (world);
+        for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
+
+        // Start out centered above the V-gap
+        dBodySetPosition (fallingObject.body, 0,0,5);
+
+#if 0
+        dMatrix3 R;
+        dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+                            dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+        dBodySetRotation (fallingObject.body,R);
+        dBodySetData (fallingObject.body,(void*) i);
+#endif
+        
+        if (cmd == 'b') {
+            dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
+            fallingObject.geom = dCreateBox (space,sides[0],sides[1],sides[2]);
+        }
+        else if (cmd == 'c') {
+            sides[0] *= 0.5;
+            dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
+            fallingObject.geom = dCreateCCylinder (space,sides[0],sides[1]);
+        }
+        else if (cmd == 's') {
+            sides[0] *= 0.5;
+            dMassSetSphere (&m,DENSITY,sides[0]);
+            fallingObject.geom = dCreateSphere (space,sides[0]);
+        }
+
+        dGeomSetBody (fallingObject.geom,fallingObject.body);
+
+        dBodySetMass (fallingObject.body,&m);
+    }
+}
+
+
+// draw a geom
+
+void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
+{
+  if (!g) return;
+  if (!pos) pos = dGeomGetPosition (g);
+  if (!R) R = dGeomGetRotation (g);
+
+  int type = dGeomGetClass (g);
+  if (type == dBoxClass) {
+    dVector3 sides;
+    dGeomBoxGetLengths (g,sides);
+    dsDrawBox (pos,R,sides);
+  }
+  else if (type == dSphereClass) {
+    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
+  }
+  else if (type == dCCylinderClass) {
+    dReal radius,length;
+    dGeomCCylinderGetParams (g,&radius,&length);
+    dsDrawCappedCylinder (pos,R,length,radius);
+  }
+  /*
+  else if (type == dGeomTransformClass) {
+    dGeomID g2 = dGeomTransformGetGeom (g);
+    const dReal *pos2 = dGeomGetPosition (g2);
+    const dReal *R2 = dGeomGetRotation (g2);
+    dVector3 actual_pos;
+    dMatrix3 actual_R;
+    dMULTIPLY0_331 (actual_pos,R,pos2);
+    actual_pos[0] += pos[0];
+    actual_pos[1] += pos[1];
+    actual_pos[2] += pos[2];
+    dMULTIPLY0_333 (actual_R,R,R2);
+    drawGeom (g2,actual_pos,actual_R);
+  }
+   */
+}
+
+
+// simulation loop
+
+static void simLoop (int pause)
+{
+  dsSetColor (0,0,2);
+  dSpaceCollide (space,0,&nearCallback);
+  if (!pause) dWorldStep (world,0.0005);
+
+  // remove all contact joints
+  dJointGroupEmpty (contactgroup);
+
+  dsSetColor (1,1,0);
+  dsSetTexture (DS_WOOD);
+
+  // draw the falling object
+  dsSetColor (1,0,0);
+  drawGeom (fallingObject.geom,0,0);
+
+  // draw the constraining boxes
+  dsSetColor(0.8, 1, 0.8);
+  drawGeom (box1,0,0);
+  drawGeom (box2,0,0);
+}
+
+
+int main (int argc, char **argv)
+{
+  // 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();
+  contactgroup = dJointGroupCreate (0);
+  dWorldSetGravity (world,0,0,-0.5);
+  dWorldSetCFM (world,1e-5);
+  dCreatePlane (space,0,0,1,0);
+  memset (&fallingObject,0,sizeof(fallingObject));
+
+  // Create two flat boxes, just slightly off vertical and a bit apart for stuff to fall in between.
+  // Don't create bodies for these boxes -- they'll be immovable instead.
+  {
+      dReal sides[3];
+      dMatrix3 R;
+      
+      sides[0] = 4;
+      sides[1] = 0.2;
+      sides[2] = 3;
+
+      box1 = dCreateBox (space,sides[0],sides[1],sides[2]);
+      dGeomSetPosition (box1, 0, sides[1], sides[2]/2);
+      dRFromAxisAndAngle (R, 1, 0, 0, -0.1);
+      dGeomSetRotation (box1, R);
+      
+      box2 = dCreateBox (space,sides[0],sides[1],sides[2]);
+      dGeomSetPosition (box2, 0, -sides[1], sides[2]/2);
+      dRFromAxisAndAngle (R, 1, 0, 0, 0.1);
+      dGeomSetRotation (box2, R);
+  }
+  
+  // Pretend to drop a box to start
+  command('b');
+  
+  // run simulation
+  dsSimulationLoop (argc,argv,640,480,&fn);
+
+  dJointGroupDestroy (contactgroup);
+  dSpaceDestroy (space);
+  dWorldDestroy (world);
+
+  return 0;
+}
diff --git a/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c
new file mode 100644
index 0000000..e49fd73
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Mac_CFMCarbon/mac_source/ode/test/test_stacktest.c
@@ -0,0 +1,197 @@
+#include <stdio.h>
+#include <glut.h>
+#include "ode.h"
+
+#define NUMBODIES 80
+
+#define USE_SPHERE 0
+#define USE_HELIX 1
+#define USE_TORQUE 1
+#define USE_WEIRD_MATRIX_OPS 0
+
+#define CONTACTS 1
+
+dWorldID aWorld;
+dSpaceID aSpace;
+float cycle = 0, fade;
+dJointGroupID aContactGroup;
+dBodyID bodies[NUMBODIES];
+dGeomID geoms[NUMBODIES];
+GLfloat colors[NUMBODIES][4];
+unsigned int contactsThisFrame;
+
+void kglTransformByODEGeom(dGeomID geom) {
+  const dReal *p = dGeomGetPosition(geom);
+  const dReal *R = dGeomGetRotation(geom);
+  GLdouble glm[16];
+
+  glm[0]  = R[0]; glm[1]  = R[4]; glm[2]  = R[8]; glm[3]  = 0;
+  glm[4]  = R[1]; glm[5]  = R[5]; glm[6]  = R[9]; glm[7]  = 0;
+  glm[8]  = R[2]; glm[9]  = R[6]; glm[10] = R[10];glm[11] = 0;
+  glm[12] = p[0]; glm[13] = p[1]; glm[14] = p[2]; glm[15] = 1;
+   
+  glMultMatrixd(glm);
+}
+
+static void odeNearCallback(void *data, dGeomID g1, dGeomID g2) {
+  dBodyID b1 = dGeomGetBody(g1), 
+          b2 = dGeomGetBody(g2);
+  dContact contact[CONTACTS];
+  int contactsUsed, i;
+
+  if (b1 && b2 && dAreConnected(b1, b2)) return;
+
+  contactsUsed = dCollide(g1, g2, CONTACTS, &contact[0].geom,
+    sizeof(dContact));
+  if (contactsUsed > CONTACTS) contactsUsed = CONTACTS;
+
+  for (i = 0; i < contactsUsed; i++) {
+    contact[i].surface.mode = 0;
+    contact[i].surface.mu = 20.0;
+    
+    dJointAttach(dJointCreateContact(aWorld, aContactGroup,
+      &(contact[i])), b1, b2);
+    contactsThisFrame++;
+  }
+}
+
+void myGlutResize(int w, int h) {
+  glViewport(0, 0, w, h);
+  glMatrixMode(GL_PROJECTION);
+  glLoadIdentity();
+  gluPerspective(45.0, (GLfloat)w / h, 1.0, 120.0);
+  glMatrixMode(GL_MODELVIEW);
+  glLoadIdentity();
+  glTranslatef(0, -6, -20);
+}
+
+void myGlutIdle(void) {
+  const float step = 1.0/120;
+  int i;
+
+  cycle = fmod(cycle + step / 4, 1);
+  fade = fabs(cycle * 2 - 1);
+
+  contactsThisFrame = 0;
+  dSpaceCollide(aSpace, NULL, &odeNearCallback);
+  //printf("%u\n", contactsThisFrame);
+  dWorldStep(aWorld, step);
+  dJointGroupEmpty(aContactGroup);
+
+  for (i = 0; i < NUMBODIES; i++) {
+    const dReal *cvel = dBodyGetLinearVel(bodies[i]);
+    dBodyAddForce(bodies[i],
+      -cvel[0] * 0.5,
+      -cvel[1] * 0.5,
+      -cvel[2] * 0.5
+    );
+  }
+
+  glutPostRedisplay();
+}
+
+void myGlutDisplay(void) {
+  int i;
+
+  glClearColor(fade * 0.15, 0, 0, 1);
+  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+  
+  if (USE_WEIRD_MATRIX_OPS) glPushMatrix();
+  for (i = 0; i < NUMBODIES; i++) {
+    if (!USE_WEIRD_MATRIX_OPS) glPushMatrix();
+    kglTransformByODEGeom(geoms[i]);
+    glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, colors[i]);
+    glColor3f(fade * 1.5, 0, 0);
+#if USE_SPHERE
+    glRotatef(90, 1, 0, 0);
+    glutSolidSphere(0.5, 9, 6);
+    glDisable(GL_LIGHTING);
+    glutWireSphere(0.5, 9, 6);
+#else
+    glutSolidCube(1);
+    glDisable(GL_LIGHTING);
+    glutWireCube(1);
+#endif
+    glEnable(GL_LIGHTING);
+    if (!USE_WEIRD_MATRIX_OPS) glPopMatrix();
+  }
+  if (USE_WEIRD_MATRIX_OPS) glPopMatrix();
+  
+  glutSwapBuffers();
+}
+
+int main(int argc, char **argv) {
+        printf("Initializing GLUT\n");
+
+  glutInit(&argc, argv);                
+  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
+  glutInitWindowSize(400, 300);
+  glutInitWindowPosition(100, 100);
+  glutCreateWindow("ODE Crash Test");
+  
+  glutDisplayFunc(myGlutDisplay);
+  glutReshapeFunc(myGlutResize);
+  glutIdleFunc(myGlutIdle);
+
+  glPolygonOffset(1, 1);
+  glDepthFunc(GL_LEQUAL);
+  glEnable(GL_POLYGON_OFFSET_FILL);
+  glEnable(GL_DEPTH_TEST);
+  glEnable(GL_CULL_FACE);
+  glEnable(GL_LIGHTING);
+  glEnable(GL_LIGHT0);
+  myGlutResize(400, 300);
+
+  printf("Creating ODE world\n");
+  aWorld = dWorldCreate();
+  aSpace = dHashSpaceCreate();
+  aContactGroup = dJointGroupCreate(0);
+  dCreatePlane(aSpace, 0, 1, 0, 0);
+  dWorldSetGravity(aWorld, 0, -9.81, 0);
+  dWorldSetERP(aWorld, 0.4);
+  dWorldSetCFM(aWorld, 1e-10);
+  
+  printf("Creating objects\n");
+  {
+    int i;
+    dMass mass;
+    
+    dMassSetBox(&mass, 1.0, 1, 1, 1);
+
+    for (i = 0; i < NUMBODIES; i++) {
+      float fraction = (float)i / NUMBODIES;
+    
+      bodies[i] = dBodyCreate(aWorld);
+      dBodySetMass(bodies[i], &mass);
+#if USE_SPHERE
+      geoms[i] = dCreateSphere(aSpace, 0.5);
+#else
+      geoms[i] = dCreateBox(aSpace, 1, 1, 1);
+#endif
+      dGeomSetBody(geoms[i], bodies[i]);
+    
+      if (USE_HELIX) {
+        float r     = (i % 3 - 1) * (1.5+4*(1 - fraction)),
+              theta = (float)i / 4;
+        dBodySetPosition(bodies[i],
+          sin(theta) * r, 
+          (float)i + 1,
+          cos(theta) * r
+        );
+      } else {
+        dBodySetPosition(bodies[i], 0, (float)i * 2 + 1, 0);
+      }
+      if (USE_TORQUE) dBodyAddTorque(bodies[i], fraction*10, fraction*20, fraction*30);
+      
+      colors[i][0] = fraction;
+      colors[i][1] = 1 - fraction;
+      colors[i][2] = 1 - fabs(fraction * 2 - 1);
+      colors[i][3] = 1;
+    }
+  }
+
+  printf("Starting simulation\n");
+  glutMainLoop();
+        
+  return 0;
+}
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs
new file mode 100644
index 0000000..8d2b86a
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/AssemblyInfo.cs
@@ -0,0 +1,18 @@
+using System;

+using System.Reflection;

+using System.Runtime.CompilerServices;

+using System.Runtime.InteropServices;

+

+[assembly: AssemblyTitle("Drawstuff.NET")]

+[assembly: AssemblyDescription("")]

+[assembly: AssemblyConfiguration("")]

+[assembly: AssemblyCompany("")]

+[assembly: AssemblyProduct("Ode.NET")]

+[assembly: AssemblyCopyright("")]

+[assembly: AssemblyTrademark("")]

+[assembly: AssemblyCulture("")]

+[assembly: ComVisible(false)]

+[assembly: Guid("b2a39dd4-dd67-4e8a-af70-d3b412da8850")]

+[assembly: AssemblyVersion("0.7.0.0")]

+[assembly: AssemblyFileVersion("0.7.0.0")]

+[assembly: CLSCompliantAttribute(true)]

diff --git a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs
new file mode 100644
index 0000000..aa84966
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/Drawstuff.cs
@@ -0,0 +1,58 @@
+using System;

+using System.Runtime.InteropServices;

+using Ode.NET;

+

+namespace Drawstuff.NET

+{

+#if dDOUBLE

+        using dReal = System.Double;

+#else

+        using dReal = System.Single;

+#endif

+

+        public static class ds

+        {

+                public const int VERSION = 2;

+

+                public enum Texture

+                {

+                        None,

+                        Wood

+                }

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate void CallbackFunction(int arg);

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct Functions

+                {

+                        public int version;

+                        public CallbackFunction start;

+                        public CallbackFunction step;

+                        public CallbackFunction command;

+                        public CallbackFunction stop;

+                        public string path_to_textures;

+                }

+

+                [DllImport("drawstuff", EntryPoint="dsDrawBox")]

+                public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides);

+

+                [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")]

+                public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius);

+

+                [DllImport("drawstuff", EntryPoint = "dsDrawConvex")]

+                public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);

+

+                [DllImport("drawstuff", EntryPoint="dsSetColor")]

+                public static extern void SetColor(float red, float green, float blue);

+

+                [DllImport("drawstuff", EntryPoint="dsSetTexture")]

+                public static extern void SetTexture(Texture texture);

+

+                [DllImport("drawstuff", EntryPoint="dsSetViewpoint")]

+                public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr);

+

+                [DllImport("drawstuff", EntryPoint="dsSimulationLoop")]

+                public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn);

+        }

+}

diff --git a/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua
new file mode 100644
index 0000000..d777ffb
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/Drawstuff/premake.lua
@@ -0,0 +1,19 @@
+package.name = "Drawstuff.NET"

+package.kind = "dll"

+package.language = "c#"

+

+if (options["with-doubles"]) then

+  package.defines = { "dDOUBLE" }

+else

+  package.defines = { "dSINGLE " }

+end

+

+package.links = {

+  "System",

+  "Ode.NET"

+}

+

+package.files = {

+  "AssemblyInfo.cs",

+  "Drawstuff.cs"

+}

diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs b/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs
new file mode 100644
index 0000000..6a8c2b6
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/Ode/AssemblyInfo.cs
@@ -0,0 +1,18 @@
+using System;

+using System.Reflection;

+using System.Runtime.CompilerServices;

+using System.Runtime.InteropServices;

+

+[assembly: AssemblyTitle("Ode.NET")]

+[assembly: AssemblyDescription("")]

+[assembly: AssemblyConfiguration("")]

+[assembly: AssemblyCompany("")]

+[assembly: AssemblyProduct("Ode.NET")]

+[assembly: AssemblyCopyright("")]

+[assembly: AssemblyTrademark("")]

+[assembly: AssemblyCulture("")]

+[assembly: ComVisible(false)]

+[assembly: Guid("1347a35e-c32b-4ff6-8064-7d10b2cc113b")]

+[assembly: AssemblyVersion("0.7.0.0")]

+[assembly: AssemblyFileVersion("0.7.0.0")]

+[assembly: CLSCompliantAttribute(true)]

diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs b/libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs
new file mode 100644
index 0000000..0603810
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/Ode/Ode.cs
@@ -0,0 +1,1732 @@
+using System;

+using System.Runtime.InteropServices;

+using System.Security;

+

+namespace Ode.NET

+{

+#if dDOUBLE

+        using dReal = System.Double;

+#else

+        using dReal = System.Single;

+#endif

+

+        public static class d

+        {

+                public static dReal Infinity = dReal.MaxValue;

+

+                #region Flags and Enumerations

+

+                [Flags]

+                public enum ContactFlags : int

+                {

+                        Mu2 = 0x001,

+                        FDir1 = 0x002,

+                        Bounce = 0x004,

+                        SoftERP = 0x008,

+                        SoftCFM = 0x010,

+                        Motion1 = 0x020,

+                        Motion2 = 0x040,

+                        Slip1 = 0x080,

+                        Slip2 = 0x100,

+                        Approx0 = 0x0000,

+                        Approx1_1 = 0x1000,

+                        Approx1_2 = 0x2000,

+                        Approx1 = 0x3000

+                }

+

+                public enum GeomClassID : int

+                {

+                        SphereClass,

+                        BoxClass,

+                        CapsuleClass,

+                        CylinderClass,

+                        PlaneClass,

+                        RayClass,

+                        ConvexClass,

+                        GeomTransformClass,

+                        TriMeshClass,

+                        HeightfieldClass,

+                        FirstSpaceClass,

+                        SimpleSpaceClass = FirstSpaceClass,

+                        HashSpaceClass,

+                        QuadTreeSpaceClass,

+                        LastSpaceClass = QuadTreeSpaceClass,

+                        FirstUserClass,

+                        LastUserClass = FirstUserClass + MaxUserClasses - 1,

+                        NumClasses,

+                        MaxUserClasses = 4

+                }

+

+                public enum JointType : int

+                {

+                        None,

+                        Ball,

+                        Hinge,

+                        Slider,

+                        Contact,

+                        Universal,

+                        Hinge2,

+                        Fixed,

+                        Null,

+                        AMotor,

+                        LMotor,

+                        Plane2D

+                }

+

+                public enum JointParam : int

+                {

+                        LoStop,

+                        HiStop,

+                        Vel,

+                        FMax,

+                        FudgeFactor,

+                        Bounce,

+                        CFM,

+                        StopERP,

+                        StopCFM,

+                        SuspensionERP,

+                        SuspensionCFM,

+                        LoStop2 = 256,

+                        HiStop2,

+                        Vel2,

+                        FMax2,

+                        FudgeFactor2,

+                        Bounce2,

+                        CFM2,

+                        StopERP2,

+                        StopCFM2,

+                        SuspensionERP2,

+                        SuspensionCFM2,

+                        LoStop3 = 512,

+                        HiStop3,

+                        Vel3,

+                        FMax3,

+                        FudgeFactor3,

+                        Bounce3,

+                        CFM3,

+                        StopERP3,

+                        StopCFM3,

+                        SuspensionERP3,

+                        SuspensionCFM3

+                }

+

+                #endregion

+

+                #region Callbacks

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate void GetAABBFn(IntPtr geom, out AABB aabb);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate ColliderFn GetColliderFnFn(int num);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate void GeomDtorFn(IntPtr o);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);

+

+                [UnmanagedFunctionPointer(CallingConvention.Cdecl)]

+                public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);

+

+                #endregion

+

+                #region Structs

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct AABB

+                {

+                        public dReal MinX, MaxX;

+                        public dReal MinY, MaxY;

+                        public dReal MinZ, MaxZ;

+                }

+

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct Contact

+                {

+                        public SurfaceParameters surface;

+                        public ContactGeom geom;

+                        public Vector3 fdir1;

+                }

+

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct ContactGeom

+                {

+                        public static readonly int SizeOf = Marshal.SizeOf(typeof(ContactGeom));

+

+                        public Vector3 pos;

+                        public Vector3 normal;

+                        public dReal depth;

+                        public IntPtr g1;

+                        public IntPtr g2;

+                        public int side1;

+                        public int side2;

+                }

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct GeomClass

+                {

+                        public int bytes;

+                        public GetColliderFnFn collider;

+                        public GetAABBFn aabb;

+                        public AABBTestFn aabb_test;

+                        public GeomDtorFn dtor;

+                }

+

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct JointFeedback 

+                {

+                        public Vector3 f1;

+                        public Vector3 t1;

+                        public Vector3 f2;

+                        public Vector3 t2;

+                }

+

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct Mass

+                {

+                        public dReal mass;

+                        public Vector4 c;

+                        public Matrix3 I;

+                }

+

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct Matrix3

+                {

+                        public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)

+                        {

+                                M00 = m00;  M10 = m10;  M20 = m20;  _m30 = 0.0f;

+                                M01 = m01;  M11 = m11;  M21 = m21;  _m31 = 0.0f;

+                                M02 = m02;  M12 = m12;  M22 = m22;  _m32 = 0.0f;

+                        }

+                        public dReal M00, M10, M20;

+                        private dReal _m30;

+                        public dReal M01, M11, M21;

+                        private dReal _m31;

+                        public dReal M02, M12, M22;

+                        private dReal _m32;

+                }

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct Matrix4

+                {

+                        public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,

+                                dReal m01, dReal m11, dReal m21, dReal m31,

+                                dReal m02, dReal m12, dReal m22, dReal m32,

+                                dReal m03, dReal m13, dReal m23, dReal m33)

+                        {

+                                M00 = m00; M10 = m10; M20 = m20; M30 = m30;

+                                M01 = m01; M11 = m11; M21 = m21; M31 = m31;

+                                M02 = m02; M12 = m12; M22 = m22; M32 = m32;

+                                M03 = m03; M13 = m13; M23 = m23; M33 = m33;

+                        }

+                        public dReal M00, M10, M20, M30;

+                        public dReal M01, M11, M21, M31;

+                        public dReal M02, M12, M22, M32;

+                        public dReal M03, M13, M23, M33;

+                }

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct Quaternion

+                {

+                        public dReal W, X, Y, Z;

+                }

+

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct SurfaceParameters

+                {

+                        public ContactFlags mode;

+                        public dReal mu;

+                        public dReal mu2;

+                        public dReal bounce;

+                        public dReal bounce_vel;

+                        public dReal soft_erp;

+                        public dReal soft_cfm;

+                        public dReal motion1;

+                        public dReal motion2;

+                        public dReal slip1;

+                        public dReal slip2;

+                }

+

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct Vector3

+                {

+                        public Vector3(dReal x, dReal y, dReal z)

+                        {

+                                X = x;  Y = y;  Z = z;  _w = 0.0f;

+                        }

+                        public dReal X, Y, Z;

+                        private dReal _w;

+                }

+

+

+                [StructLayout(LayoutKind.Sequential)]

+                public struct Vector4

+                {

+                        public Vector4(dReal x, dReal y, dReal z, dReal w)

+                        {

+                                X = x;  Y = y;  Z = z;  W = w;

+                        }

+                        public dReal X, Y, Z, W;

+                }

+

+                #endregion

+

+                [DllImport("ode", EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]

+                public static extern bool AreConnected(IntPtr b1, IntPtr b2);

+

+                [DllImport("ode", EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]

+                public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);

+

+                [DllImport("ode", EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);

+

+                [DllImport("ode", EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);

+

+                [DllImport("ode", EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);

+

+                [DllImport("ode", EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);

+

+                [DllImport("ode", EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);

+

+                [DllImport("ode", EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);

+

+                [DllImport("ode", EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);

+

+                [DllImport("ode", EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);

+

+                [DllImport("ode", EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);

+

+                [DllImport("ode", EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyCopyPosition(IntPtr body, out dReal X);

+

+                [DllImport("ode", EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);

+

+                [DllImport("ode", EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);

+

+                [DllImport("ode", EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyCopyRotation(IntPtr body, out dReal M00);

+

+                [DllImport("ode", EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr BodyCreate(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyDestroy(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyDisable(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyEnable(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]

+                public static extern bool BodyGetAutoDisableFlag(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]

+                public static extern int BodyGetAutoDisableSteps(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal BodyGetAutoDisableTime(IntPtr body);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);

+                public static Vector3 BodyGetAngularVel(IntPtr body)

+                {

+                        unsafe { return *(BodyGetAngularVelUnsafe(body)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr BodyGetData(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]

+                public static extern int BodyGetFiniteRotationMode(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);

+                public static Vector3 BodyGetForce(IntPtr body)

+                {

+                        unsafe { return *(BodyGetForceUnsafe(body)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]

+                public static extern bool BodyGetGravityMode(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr BodyGetJoint(IntPtr body, int index);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);

+                public static Vector3 BodyGetLinearVel(IntPtr body)

+                {

+                        unsafe { return *(BodyGetLinearVelUnsafe(body)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyGetMass(IntPtr body, out Mass mass);

+

+                [DllImport("ode", EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]

+                public static extern int BodyGetNumJoints(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);

+                public static Vector3 BodyGetPosition(IntPtr body)

+                {

+                        unsafe { return *(BodyGetPositionUnsafe(body)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);

+                public static Quaternion BodyGetQuaternion(IntPtr body)

+                {

+                        unsafe { return *(BodyGetQuaternionUnsafe(body)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);

+                public static Matrix3 BodyGetRotation(IntPtr body)

+                {

+                        unsafe { return *(BodyGetRotationUnsafe(body)); }

+                }

+#endif

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);

+                public static Vector3 BodyGetTorque(IntPtr body)

+                {

+                        unsafe { return *(BodyGetTorqueUnsafe(body)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]

+                public static extern bool BodyIsEnabled(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);

+

+                [DllImport("ode", EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetAutoDisableDefaults(IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);

+

+                [DllImport("ode", EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);

+

+                [DllImport("ode", EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);

+

+                [DllImport("ode", EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);

+

+                [DllImport("ode", EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetData(IntPtr body, IntPtr data);

+

+                [DllImport("ode", EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);

+

+                [DllImport("ode", EntryPoint = "dBodySetFiniteRotationModeAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetFiniteRotationModeAxis(IntPtr body, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetGravityMode(IntPtr body, bool mode);

+

+                [DllImport("ode", EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetMass(IntPtr body, ref Mass mass);

+

+                [DllImport("ode", EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);

+

+                [DllImport("ode", EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetQuaternion(IntPtr body, ref dReal w);

+

+                [DllImport("ode", EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetRotation(IntPtr body, ref dReal M00);

+

+                [DllImport("ode", EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]

+                public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]

+                public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,

+                        ref Vector3 side1, ref Vector3 p2,

+                        ref Matrix3 R2, ref Vector3 side2,

+                        ref Vector3 normal, out dReal depth, out int return_code,

+                        int maxc, out ContactGeom contact, int skip);

+

+                [DllImport("ode", EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]

+                public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,

+                        ref Vector3 side1, ref Vector3 _p2,

+                        ref Matrix3 R2, ref Vector3 side2);

+

+                [DllImport("ode", EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]

+                public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2, 

+                        ref Vector3 b1, ref Vector3 b2, 

+                        ref Vector3 cp1, ref Vector3 cp2);

+

+                [DllImport("ode", EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]

+                public static extern void CloseODE();

+

+                [DllImport("ode", EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]

+                public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);

+

+                [DllImport("ode", EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);

+

+                [DllImport("ode", EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz);

+

+                [DllImport("ode", EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length);

+

+                [DllImport("ode", EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);

+

+                [DllImport("ode", EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length);

+

+                [DllImport("ode", EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable);

+

+                [DllImport("ode", EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateGeom(int classnum);

+

+                [DllImport("ode", EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]

+                public static extern int CreateGeomClass(ref GeomClass classptr);

+

+                [DllImport("ode", EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateGeomTransform(IntPtr space);

+

+                [DllImport("ode", EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);

+

+                [DllImport("ode", EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateRay(IntPtr space, dReal length);

+

+                [DllImport("ode", EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateSphere(IntPtr space, dReal radius);

+

+                [DllImport("ode", EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data, 

+                        TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);

+

+                [DllImport("ode", EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);

+

+                [DllImport("ode", EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]

+                public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);

+

+                [DllImport("ode", EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]

+                public static extern int FactorCholesky(ref dReal A00, int n);

+

+                [DllImport("ode", EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]

+                public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);

+

+                [DllImport("ode", EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);

+

+                [DllImport("ode", EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);

+

+                [DllImport("ode", EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);

+

+                [DllImport("ode", EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);

+

+                [DllImport("ode", EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomClearOffset(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);

+

+                [DllImport("ode", EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);

+

+                [DllImport("ode", EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);

+

+                [DllImport("ode", EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);

+

+                [DllImport("ode", EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);

+

+                [DllImport("ode", EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);

+

+                [DllImport("ode", EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCopyPosition(IntPtr geom, out dReal X);

+

+                [DllImport("ode", EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);

+

+                [DllImport("ode", EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);

+

+                [DllImport("ode", EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);

+

+                [DllImport("ode", EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomDestroy(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomDisable(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomEnable(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);

+

+                [DllImport("ode", EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomGetAABB(IntPtr geom, out dReal minX);

+

+                [DllImport("ode", EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomGetBody(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]

+                public static extern int GeomGetCategoryBits(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomGetClassData(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]

+                public static extern int GeomGetCollideBits(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]

+                public static extern GeomClassID GeomGetClass(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomGetData(IntPtr geom);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);

+                public static Vector3 GeomGetOffsetPosition(IntPtr geom)

+                {

+                        unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }

+                }

+#endif

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);

+                public static Matrix3 GeomGetOffsetRotation(IntPtr geom)

+                {

+                        unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }

+                }

+#endif

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);

+                public static Vector3 GeomGetPosition(IntPtr geom)

+                {

+                        unsafe { return *(GeomGetPositionUnsafe(geom)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);

+

+                [DllImport("ode", EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);

+                public static Matrix3 GeomGetRotation(IntPtr geom)

+                {

+                        unsafe { return *(GeomGetRotationUnsafe(geom)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomGetSpace(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness,     int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,

+                                dReal width, dReal depth, int widthSamples, int depthSamples,

+                                dReal scale, dReal offset, dReal thickness, int bWrap);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomHeightfieldDataCreate();

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataDestroy(IntPtr d);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);

+

+                [DllImport("ode", EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);

+

+                [DllImport("ode", EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]

+                public static extern bool GeomIsEnabled(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]

+                public static extern bool GeomIsOffset(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]

+                public static extern bool GeomIsSpace(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);

+

+                [DllImport("ode", EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);

+

+                [DllImport("ode", EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);

+

+                [DllImport("ode", EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);

+

+                [DllImport("ode", EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);

+

+                [DllImport("ode", EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]

+                public static extern int GeomRayGetClosestHit(IntPtr ray);

+

+                [DllImport("ode", EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal GeomRayGetLength(IntPtr ray);

+

+                [DllImport("ode", EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);

+

+                [DllImport("ode", EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);

+

+                [DllImport("ode", EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);

+

+                [DllImport("ode", EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomRaySetLength(IntPtr ray, dReal length);

+

+                [DllImport("ode", EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);

+

+                [DllImport("ode", EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetBody(IntPtr geom, IntPtr body);

+

+                [DllImport("ode", EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetCategoryBits(IntPtr geom, int bits);

+

+                [DllImport("ode", EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetCollideBits(IntPtr geom, int bits);

+

+                [DllImport("ode", EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);

+

+                [DllImport("ode", EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetData(IntPtr geom, IntPtr data);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);

+

+                [DllImport("ode", EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);

+

+                [DllImport("ode", EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);

+

+                [DllImport("ode", EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);

+

+                [DllImport("ode", EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal GeomSphereGetRadius(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);

+

+                [DllImport("ode", EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]

+                public static extern int GeomTransformGetCleanup(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomTransformGetGeom(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]

+                public static extern int GeomTransformGetInfo(IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);

+

+                [DllImport("ode", EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);

+

+                [DllImport("ode", EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTransformSetInfo(IntPtr geom, int info);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildDouble(IntPtr d,

+                        double[] vertices, int vertexStride, int vertexCount,

+                        int[] indices, int indexCount, int triStride);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildDouble(IntPtr d,

+                        IntPtr vertices, int vertexStride, int vertexCount,

+                        IntPtr indices, int indexCount, int triStride);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,

+                        double[] vertices, int vertexStride, int vertexCount,

+                        int[] indices, int indexCount, int triStride,

+                        double[] normals);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildDouble(IntPtr d,

+                        IntPtr vertices, int vertexStride, int vertexCount,

+                        IntPtr indices, int indexCount, int triStride,

+                        IntPtr normals);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildSingle(IntPtr d,

+                        dReal[] vertices, int vertexStride, int vertexCount,

+                        int[] indices, int indexCount, int triStride);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildSingle(IntPtr d,

+                        IntPtr vertices, int vertexStride, int vertexCount,

+                        IntPtr indices, int indexCount, int triStride);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,

+                        dReal[] vertices, int vertexStride, int vertexCount,

+                        int[] indices, int indexCount, int triStride,

+                        dReal[] normals);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,

+                        IntPtr vertices, int vertexStride, int vertexCount,

+                        IntPtr indices, int indexCount, int triStride,

+                        IntPtr normals);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildSimple(IntPtr d, 

+                        float[] vertices, int vertexStride, int vertexCount,

+                        int[] indices, int indexCount, int triStride);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildSimple(IntPtr d,

+                        IntPtr vertices, int vertexStride, int vertexCount,

+                        IntPtr indices, int indexCount, int triStride);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, 

+                        float[] vertices, int vertexStride, int vertexCount,

+                        int[] indices, int indexCount, int triStride,

+                        float[] normals);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,

+                        IntPtr vertices, int vertexStride, int vertexCount,

+                        IntPtr indices, int indexCount, int triStride,

+                        IntPtr normals);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshClearTCCache(IntPtr g);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomTriMeshDataCreate();

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataDestroy(IntPtr d);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataPreprocess(IntPtr d);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshDataUpdate(IntPtr d);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]

+                public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]

+                public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomTriMeshGetData(IntPtr g);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);

+                public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)

+                {

+                        unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]

+                public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]

+                public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]

+                public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]

+                public extern static int GeomTriMeshGetTriangleCount(IntPtr g);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]

+                public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);

+

+                [DllImport("ode", EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]

+                public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);

+

+                [DllImport("ode", EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr HashSpaceCreate(IntPtr space);

+

+                [DllImport("ode", EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]

+                public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);

+

+                [DllImport("ode", EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]

+                public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);

+

+                [DllImport("ode", EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]

+                public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);

+

+                [DllImport("ode", EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]

+                public static extern void InitODE();

+

+                [DllImport("ode", EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]

+                public static extern int IsPositiveDefinite(ref dReal A, int n);

+

+                [DllImport("ode", EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]

+                public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);

+

+                [DllImport("ode", EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);

+

+                [DllImport("ode", EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);

+

+                [DllImport("ode", EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);

+

+                [DllImport("ode", EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointAddPRTorque(IntPtr joint, dReal torque);

+

+                [DllImport("ode", EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);

+

+                [DllImport("ode", EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointAddSliderForce(IntPtr joint, dReal force);

+

+                [DllImport("ode", EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);

+

+                [DllImport("ode", EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);

+

+                [DllImport("ode", EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointDestroy(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);

+

+                [DllImport("ode", EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);

+

+                [DllImport("ode", EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]

+                public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);

+

+                [DllImport("ode", EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]

+                public static extern int JointGetAMotorMode(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]

+                public static extern int JointGetAMotorNumAxes(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);

+

+                [DllImport("ode", EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointGetBody(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointGetData(IntPtr j);

+

+#if !dNO_UNSAFE_CODE

+                [CLSCompliant(false)]

+                [DllImport("ode", EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]

+                public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);

+                public static JointFeedback JointGetFeedback(IntPtr j)

+                {

+                        unsafe { return *(JointGetFeedbackUnsafe(j)); }

+                }

+#endif

+

+                [DllImport("ode", EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetHingeAngle(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetHingeAngleRate(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetHingeParam(IntPtr j, int parameter);

+

+                [DllImport("ode", EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetHinge2Angle1(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);

+

+                [DllImport("ode", EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]

+                public static extern int JointGetLMotorNumAxes(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);

+

+                [DllImport("ode", EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetPRParam(IntPtr j, int parameter);

+

+                [DllImport("ode", EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetPRPosition(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetPRPositionRate(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetSliderParam(IntPtr j, int parameter);

+

+                [DllImport("ode", EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetSliderPosition(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetSliderPositionRate(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]

+                public static extern JointType JointGetType(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetUniversalAngle1(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetUniversalAngle2(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);

+

+                [DllImport("ode", EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);

+

+                [DllImport("ode", EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr JointGroupCreate(int max_size);

+

+                [DllImport("ode", EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGroupDestroy(IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointGroupEmpty(IntPtr group);

+

+                [DllImport("ode", EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);

+

+                [DllImport("ode", EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetAMotorMode(IntPtr j, int mode);

+

+                [DllImport("ode", EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetAMotorNumAxes(IntPtr group, int num);

+

+                [DllImport("ode", EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetData(IntPtr j, IntPtr data);

+

+                [DllImport("ode", EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);

+

+                [DllImport("ode", EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetFixed(IntPtr j);

+

+                [DllImport("ode", EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);

+

+                [DllImport("ode", EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetLMotorNumAxes(IntPtr j, int num);

+

+                [DllImport("ode", EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);

+

+                [DllImport("ode", EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]

+                public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);

+

+                [DllImport("ode", EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]

+                public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);

+

+                [DllImport("ode", EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassAdd(ref Mass a, ref Mass b);

+

+                [DllImport("ode", EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassAdjust(ref Mass m, dReal newmass);

+

+                [DllImport("ode", EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]

+                public static extern bool MassCheck(ref Mass m);

+

+                [DllImport("ode", EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassRotate(out Mass mass, ref Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassRotate(out Mass mass, ref dReal M00);

+

+                [DllImport("ode", EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);

+

+                [DllImport("ode", EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);

+

+                [DllImport("ode", EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);

+

+                [DllImport("ode", EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);

+

+                [DllImport("ode", EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);

+

+                [DllImport("ode", EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);

+

+                [DllImport("ode", EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetParameters(out Mass mass, dReal themass,

+                         dReal cgx, dReal cgy, dReal cgz,

+                         dReal i11, dReal i22, dReal i33,

+                         dReal i12, dReal i13, dReal i23);

+

+                [DllImport("ode", EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);

+

+                [DllImport("ode", EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]

+                public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);

+

+                [DllImport("ode", EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);

+

+                [DllImport("ode", EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassSetZero(out Mass mass);

+

+                [DllImport("ode", EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]

+                public static extern void MassTranslate(out Mass mass, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]

+                public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);

+

+                [DllImport("ode", EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]

+                public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);

+

+                [DllImport("ode", EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]

+                public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);

+

+                [DllImport("ode", EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]

+                public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);

+

+                [DllImport("ode", EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]

+                public static extern void QfromR(out Quaternion q, ref Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]

+                public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);

+

+                [DllImport("ode", EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]

+                public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);

+

+                [DllImport("ode", EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]

+                public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);

+

+                [DllImport("ode", EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]

+                public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);

+

+                [DllImport("ode", EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]

+                public static extern void QSetIdentity(out Quaternion q);

+

+                [DllImport("ode", EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);

+

+                [DllImport("ode", EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);

+

+                [DllImport("ode", EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal RandReal();

+

+                [DllImport("ode", EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]

+                public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);

+

+                [DllImport("ode", EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]

+                public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);

+

+                [DllImport("ode", EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]

+                public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);

+

+                [DllImport("ode", EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]

+                public static extern void RfromQ(out Matrix3 R, ref Quaternion q);

+

+                [DllImport("ode", EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]

+                public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);

+

+                [DllImport("ode", EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]

+                public static extern void RSetIdentity(out Matrix3 R);

+

+                [DllImport("ode", EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]

+                public static extern void SetValue(out dReal a, int n);

+

+                [DllImport("ode", EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]

+                public static extern void SetZero(out dReal a, int n);

+

+                [DllImport("ode", EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr SimpleSpaceCreate(IntPtr space);

+

+                [DllImport("ode", EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]

+                public static extern void SolveCholesky(ref dReal L, out dReal b, int n);

+

+                [DllImport("ode", EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]

+                public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);

+

+                [DllImport("ode", EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]

+                public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);

+

+                [DllImport("ode", EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]

+                public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);

+

+                [DllImport("ode", EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]

+                public static extern void SpaceAdd(IntPtr space, IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]

+                public static extern void SpaceClean(IntPtr space);

+

+                [DllImport("ode", EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]

+                public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);

+

+                [DllImport("ode", EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]

+                public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);

+

+                [DllImport("ode", EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]

+                public static extern void SpaceDestroy(IntPtr space);

+

+                [DllImport("ode", EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]

+                public static extern bool SpaceGetCleanup(IntPtr space);

+

+                [DllImport("ode", EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]

+                public static extern int SpaceGetNumGeoms(IntPtr space);

+

+                [DllImport("ode", EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr SpaceGetGeom(IntPtr space, int i);

+

+                [DllImport("ode", EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]

+                public static extern bool SpaceQuery(IntPtr space, IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]

+                public static extern void SpaceRemove(IntPtr space, IntPtr geom);

+

+                [DllImport("ode", EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]

+                public static extern void SpaceSetCleanup(IntPtr space, bool mode);

+

+                [DllImport("ode", EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]

+                public static extern void VectorScale(out dReal a, ref dReal d, int n);

+

+                [DllImport("ode", EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]

+                public static extern IntPtr WorldCreate();

+

+                [DllImport("ode", EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldDestroy(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]

+                public static extern bool WorldGetAutoDisableFlag(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]

+                public static extern int WorldGetAutoDisableSteps(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal WorldGetAutoDisableTime(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]

+                public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal WorldGetCFM(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal WorldGetERP(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);

+

+                [DllImport("ode", EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldGetGravity(IntPtr world, out dReal X);

+

+                [DllImport("ode", EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]

+                public static extern int WorldGetQuickStepNumIterations(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]

+                public static extern dReal WorldGetQuickStepW(IntPtr world);

+

+                [DllImport("ode", EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);

+

+                [DllImport("ode", EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);

+

+                [DllImport("ode", EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldQuickStep(IntPtr world, dReal stepsize);

+

+                [DllImport("ode", EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);

+

+                [DllImport("ode", EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);

+

+                [DllImport("ode", EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);

+

+                [DllImport("ode", EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);

+

+                [DllImport("ode", EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);

+

+                [DllImport("ode", EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);

+

+                [DllImport("ode", EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetCFM(IntPtr world, dReal cfm);

+

+                [DllImport("ode", EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);

+

+                [DllImport("ode", EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);

+

+                [DllImport("ode", EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetERP(IntPtr world, dReal erp);

+

+                [DllImport("ode", EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);

+

+                [DllImport("ode", EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);

+

+                [DllImport("ode", EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);

+

+                [DllImport("ode", EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldStep(IntPtr world, dReal stepsize);

+

+                [DllImport("ode", EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]

+                public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);

+        }

+}

diff --git a/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua
new file mode 100644
index 0000000..b53d54c
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/Ode/premake.lua
@@ -0,0 +1,31 @@
+package.name = "Ode.NET"

+package.kind = "dll"

+package.language = "c#"

+

+-- Build options

+

+  package.defines = { }

+

+  if (options["with-doubles"]) then

+    table.insert(package.defines, "dDOUBLE")

+  else

+    table.insert(package.defines, "dSINGLE")

+  end

+

+  if (options["no-unsafe"]) then

+    table.insert(package.defines, "dNO_UNSAFE_CODE")

+  else

+    package.buildflags = { "unsafe" }

+  end

+  

+

+-- Files & Libraries

+

+  package.files = {

+    "AssemblyInfo.cs",

+    "Ode.cs"

+  }

+

+  package.links = {

+    "System"

+  }

diff --git a/libraries/ode-0.9/contrib/Ode.NET/README.TXT b/libraries/ode-0.9/contrib/Ode.NET/README.TXT
new file mode 100644
index 0000000..e6f1262
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/README.TXT
@@ -0,0 +1,73 @@
+Ode.NET - .NET bindings for ODE

+Jason Perkins (starkos@gmail.com)

+

+ THIS IS A WORK IN PROGRESS! I'm not done yet!

+ 

+

+--------------------------------------------------------------------- 

+ INSTALLATION

+--------------------------------------------------------------------- 

+

+ Note that this binding uses a C# 2.0 feature (the

+ UnmanagedFunctionPointer attribute). You will need to use

+ Visual Studio 2005 (C# Express is fine) or Mono's gmcs

+ compiler.

+ 

+ Start by getting or building ODE as a shared library (DLL).

+ 

+ The simplest way to build the bindings is probably to create a 

+ new library assembly in your tool of choice and drop in the files

+ Ode/Ode.cs and Ode/AssemblyInfo.cs. Define the symbol`dDOUBLE` if 

+ you used double-precision math in your ode.dll. Build, done.

+ 

+ For testing purposes, I have also created bindings for the 

+ Drawstuff library and a C# version of the BoxStack demo. You can

+ throw all of these files into a console executable and run it to

+ see the demo.

+ 

+ If you happen to have Premake installed (http://premake.sf.net/),

+ you can generate build scripts for the library with:

+ 

+  premake --target (toolset)                 # for single precision

+  premake --with-doubles --target (toolset)  # for double precision

+

+ To build the test application too, use:

+ 

+  premake --with-tests --target (toolset)

+  

+ To build with Mono, you must add the --dotnet parameter to enable

+ support .NET 2.0:

+ 

+   premake --dotnet mono2 --target gnu

+   

+

+--------------------------------------------------------------------- 

+ USAGE

+--------------------------------------------------------------------- 

+

+ I have tried to keep things as close to the original C API as I can,

+ rather than forcing a class structure on everyone. Everything is

+ contained within the `Ode.NET` namespace inside a static class

+ named `d`. All ODE IDs are replaced with IntPtrs. A quick example:

+ 

+   using Ode.NET;

+   

+   IntPtr world = d.WorldCreate();

+   IntPtr body = d.BodyCreate(world);

+ 

+ Take a look at Tests/BoxStack.cs for a more complete example.

+ 

+ 

+--------------------------------------------------------------------- 

+ KNOWN ISSUES

+--------------------------------------------------------------------- 

+

+ I'm not done yet, so many functions are still missing.

+

+ It is not possible to implement dBodyGetPosition(), dBodyGetRotation(),

+ etc. without resorting to unsafe code, which I was trying to avoid.

+ This binding uses the .NET friendly dBodyCopyPosition(), 

+ dBodyCopyRotation(), etc. instead. 

+  

+ Collision response (contact joints) do not work when built under 

+ Mono as double-precision. I have not tried to track down why.

diff --git a/libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs b/libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs
new file mode 100644
index 0000000..1077124
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/Tests/BoxStack.cs
@@ -0,0 +1,260 @@
+using System;

+using System.Collections.Generic;

+using Drawstuff.NET;

+

+namespace Ode.NET

+{

+#if dDOUBLE

+        using dReal = System.Double;

+#else

+        using dReal = System.Single;

+#endif

+

+        public class TestBoxStack

+        {

+                #region Description of convex shape

+

+                static dReal[] planes = 

+                        {

+                                1.0f, 0.0f, 0.0f, 0.25f,

+                                0.0f, 1.0f, 0.0f, 0.25f,

+                                0.0f, 0.0f, 1.0f, 0.25f,

+                                0.0f, 0.0f, -1.0f, 0.25f,

+                                0.0f, -1.0f, 0.0f, 0.25f,

+                                -1.0f, 0.0f , 0.0f, 0.25f

+                        };

+

+                static dReal[] points =

+                        {

+                                0.25f, 0.25f, 0.25f,

+                                -0.25f, 0.25f, 0.25f,

+                                0.25f, -0.25f, 0.25f,

+                                -0.25f, -0.25f, 0.25f,

+                                0.25f, 0.25f, -0.25f,

+                                -0.25f,0.25f,-0.25f,

+                                0.25f,-0.25f,-0.25f,

+                                -0.25f,-0.25f,-0.25f,

+                        };

+

+                static int[] polygons =

+                {

+                        4, 0, 2, 6, 4,

+                        4, 1, 0, 4, 5,

+                        4, 0, 1, 3, 2,

+                        4, 3, 1, 5, 7,

+                        4, 2, 3, 7, 6,

+                        4, 5, 4, 6, 7,

+                };

+

+                #endregion

+

+                const int NUM = 100;

+                const float DENSITY = 5.0f;

+                const int MAX_CONTACTS = 8;

+                

+                static IntPtr world;

+                static IntPtr space;

+                static IntPtr contactgroup;

+

+                static Queue<IntPtr> obj = new Queue<IntPtr>();

+

+                static d.Vector3 xyz = new d.Vector3(2.1640f, -1.3079f, 1.7600f);

+                static d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);

+

+                static d.NearCallback nearCallback = near;

+                static d.ContactGeom[] contacts = new d.ContactGeom[MAX_CONTACTS];

+                static d.Contact contact;

+

+

+                // Called when window is opened - sets up viewpoint and prints usage

+                static void start(int unused)

+                {

+                        ds.SetViewpoint(ref xyz, ref hpr);

+                        Console.WriteLine("To drop another object, press:");

+                        Console.WriteLine("   b for box.");

+                        Console.WriteLine("   s for sphere.");

+                        Console.WriteLine("   c for capsule.");

+                        Console.WriteLine("   y for cylinder.");

+                        Console.WriteLine("   v for a convex object.");

+                        Console.WriteLine("   x for a composite object.");

+                        Console.WriteLine("To select an object, press space.");

+                        Console.WriteLine("To disable the selected object, press d.");

+                        Console.WriteLine("To enable the selected object, press e.");

+                        Console.WriteLine("To toggle showing the geom AABBs, press a.");

+                        Console.WriteLine("To toggle showing the contact points, press t.");

+                        Console.WriteLine("To toggle dropping from random position/orientation, press r.");

+                        Console.WriteLine("To save the current state to 'state.dif', press 1.");

+                }

+

+

+                // Near callback - creates contact joints

+                static void near(IntPtr space, IntPtr g1, IntPtr g2)

+                {

+                        IntPtr b1 = d.GeomGetBody(g1);

+                        IntPtr b2 = d.GeomGetBody(g2);

+                        if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))

+                                return;

+

+                        int count = d.Collide(g1, g2, MAX_CONTACTS, contacts, d.ContactGeom.SizeOf);

+                        for (int i = 0; i < count; ++i)

+                        {

+                                contact.geom = contacts[i];

+                                IntPtr joint = d.JointCreateContact(world, contactgroup, ref contact);

+                                d.JointAttach(joint, b1, b2);

+                        }

+                }

+

+

+                // Adds a new object to the scene - attaches a body to the geom and

+                // sets the initial position and orientation

+                static void addObject(IntPtr geom, d.Mass mass)

+                {

+                        // Create a body for this object

+                        IntPtr body = d.BodyCreate(world);

+                        d.GeomSetBody(geom, body);

+                        d.BodySetMass(body, ref mass);

+                        obj.Enqueue(geom);

+

+                        // Set the position of the new object

+                        d.Matrix3 R;

+                        d.BodySetPosition(body, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() + 2);

+                        d.RFromAxisAndAngle(out R, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() * 2 - 1, d.RandReal() * 10 - 5);

+                        d.BodySetRotation(body, ref R);

+

+                        // Cap the total number of objects

+                        if (obj.Count > NUM)

+                        {

+                                geom = obj.Dequeue();

+                                body = d.GeomGetBody(geom);

+                                d.BodyDestroy(body);

+                                d.GeomDestroy(geom);

+                        }

+                }

+

+

+                // Keyboard callback

+                static void command(int cmd)

+                {

+                        IntPtr geom;

+                        d.Mass mass;

+                        d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);

+

+                        Char ch = Char.ToLower((Char)cmd);

+                        switch ((Char)ch)

+                        {

+                        case 'b':

+                                d.MassSetBox(out mass, DENSITY, sides.X, sides.Y, sides.Z);

+                                geom = d.CreateBox(space, sides.X, sides.Y, sides.Z);

+                                addObject(geom, mass);

+                                break;

+

+                        case 'c':

+                                sides.X *= 0.5f;

+                                d.MassSetCapsule(out mass, DENSITY, 3, sides.X, sides.Y);

+                                geom = d.CreateCapsule(space, sides.X, sides.Y);

+                                addObject(geom, mass);

+                                break;

+

+                        case 'v':

+                                d.MassSetBox(out mass, DENSITY, 0.25f, 0.25f, 0.25f);

+                                geom = d.CreateConvex(space, planes, planes.Length / 4, points, points.Length / 3, polygons);

+                                addObject(geom, mass);

+                                break;

+                        }

+                }

+

+

+                // Draw an object in the scene

+                static void drawGeom(IntPtr geom)

+                {

+                        IntPtr body = d.GeomGetBody(geom);

+

+                        d.Vector3 pos;

+                        d.BodyCopyPosition(body, out pos);

+

+                        d.Matrix3 R;

+                        d.BodyCopyRotation(body, out R);

+

+                        d.GeomClassID type = d.GeomGetClass(geom);

+                        switch (type)

+                        {

+                        case d.GeomClassID.BoxClass:

+                                d.Vector3 sides;

+                                d.GeomBoxGetLengths(geom, out sides);

+                                ds.DrawBox(ref pos, ref R, ref sides);

+                                break;

+                        case d.GeomClassID.CapsuleClass:

+                                dReal radius, length;

+                                d.GeomCapsuleGetParams(geom, out radius, out length);

+                                ds.DrawCapsule(ref pos, ref R, length, radius);

+                                break;

+                        case d.GeomClassID.ConvexClass:

+                                ds.DrawConvex(ref pos, ref R, planes, planes.Length / 4, points, points.Length / 3, polygons);

+                                break;

+                        }

+                }

+

+

+                // Called once per frame; updates the scene

+                static void step(int pause)

+                {

+                        d.SpaceCollide(space, IntPtr.Zero, nearCallback);

+                        if (pause == 0)

+                                d.WorldQuickStep(world, 0.02f);

+                        d.JointGroupEmpty(contactgroup);

+

+                        ds.SetColor(1.0f, 1.0f, 0.0f);

+                        ds.SetTexture(ds.Texture.Wood);

+

+                        foreach (IntPtr geom in obj)

+                        {

+                                drawGeom(geom);

+                        }

+                }

+

+

+                static void Main(string[] args)

+                {

+                        // Setup pointers to drawstuff callback functions

+                        ds.Functions fn;

+                        fn.version = ds.VERSION;

+                        fn.start = new ds.CallbackFunction(start);

+                        fn.step = new ds.CallbackFunction(step);

+                        fn.command = new ds.CallbackFunction(command);

+                        fn.stop = null;

+                        fn.path_to_textures = "../../../../drawstuff/textures";

+                        if (args.Length > 0)

+                        {

+                                fn.path_to_textures = args[0];

+                        }

+

+                        // Set up contact response parameters

+                        contact.surface.mode = d.ContactFlags.Bounce | d.ContactFlags.SoftCFM;

+                        contact.surface.mu = d.Infinity;

+                        contact.surface.mu2 = 0.0f;

+                        contact.surface.bounce = 0.1f;

+                        contact.surface.bounce_vel = 0.1f;

+                        contact.surface.soft_cfm = 0.01f;

+

+                        // Initialize the scene

+                        world = d.WorldCreate();

+                        space = d.HashSpaceCreate(IntPtr.Zero);

+                        contactgroup = d.JointGroupCreate(0);

+                        d.WorldSetGravity(world, 0.0f, 0.0f, -0.5f);

+                        d.WorldSetCFM(world, 1e-5f);

+                        d.WorldSetAutoDisableFlag(world, true);

+                        d.WorldSetContactMaxCorrectingVel(world, 0.1f);

+                        d.WorldSetContactSurfaceLayer(world, 0.001f);

+                        d.CreatePlane(space, 0, 0, 1, 0);

+

+                        // Run the scene

+                        ds.SimulationLoop(args.Length, args, 352, 288, ref fn);

+

+                        // Clean up

+                        d.JointGroupDestroy(contactgroup);

+                        d.SpaceDestroy(space);

+                        d.WorldDestroy(world);

+                        d.CloseODE();

+                }

+        }

+}

diff --git a/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua
new file mode 100644
index 0000000..5253ae1
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/Tests/premake.lua
@@ -0,0 +1,27 @@
+-- This function creates the test packages

+function maketest(name)

+

+  package = newpackage()

+  package.name = name

+  package.kind = "exe"

+  package.language = "c#"

+

+  if (options["with-doubles"]) then

+    package.defines = { "dDOUBLE" }

+  else

+    package.defines = { "dSINGLE " }

+  end

+

+  package.links = { 

+    "System", 

+    "Ode.NET", 

+    "Drawstuff.NET" 

+  }

+

+  package.files = {

+    name .. ".cs"

+  }

+

+end

+

+maketest("BoxStack")

diff --git a/libraries/ode-0.9/contrib/Ode.NET/premake.lua b/libraries/ode-0.9/contrib/Ode.NET/premake.lua
new file mode 100644
index 0000000..c25a017
--- /dev/null
+++ b/libraries/ode-0.9/contrib/Ode.NET/premake.lua
@@ -0,0 +1,29 @@
+project.name = "Ode.NET"

+

+-- Target checking

+

+  if (target and target ~= "vs2005" and target ~= "gnu") then

+    error("Ode.NET requires a .NET 2.0 compiler")

+  end

+      

+

+-- Project options

+

+  addoption("with-doubles",  "Use double instead of float as base numeric type")

+  addoption("with-tests",    "Builds the test applications and DrawStuff library")

+  addoption("no-unsafe",     "Exclude functions using unsafe code (dBodyGetPosition, etc.)")

+

+

+-- Build settings

+

+  project.config["Debug"].bindir = "bin/Debug"

+  project.config["Release"].bindir = "bin/Release"

+  

+

+-- Packages

+

+  if (options["with-tests"]) then

+    dopackage("Tests")

+    dopackage("Drawstuff")

+  end

+  dopackage("Ode")

diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT
new file mode 100644
index 0000000..b2b1995
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE-BSD.TXT
@@ -0,0 +1,37 @@
+

+This is the BSD-style license for The ODE Model Processor

+----------------------------------------------------------

+

+The ODE Model Processor

+Copyright (c) 2007, Department Of Information Science,

+University of Otago, Dunedin, New Zealand.

+All rights reserved.

+

+Author: Richard Barrington <barri662@student.otago.ac.nz>

+

+Redistribution and use in source and binary forms, with or without

+modification, are permitted provided that the following conditions

+are met:

+

+Redistributions of source code must retain the above copyright notice,

+this list of conditions and the following disclaimer.

+

+Redistributions in binary form must reproduce the above copyright notice,

+this list of conditions and the following disclaimer in the documentation

+and/or other materials provided with the distribution.

+

+Neither the names of the copyright owner nor the names of its

+contributors may be used to endorse or promote products derived from

+this software without specific prior written permission.

+

+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT

+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED

+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR

+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF

+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING

+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT
new file mode 100644
index 0000000..cfe59bc
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/LICENSE.TXT
@@ -0,0 +1,502 @@
+                  GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+     59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+                            Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+                  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+                            NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+                     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the library's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of 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.
+
+    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 GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  <signature of Ty Coon>, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln
new file mode 100644
index 0000000..6a3f521
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor.sln
@@ -0,0 +1,20 @@
+

+Microsoft Visual Studio Solution File, Format Version 9.00

+# Visual C# Express 2005

+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "OdeModelProcessor", "OdeModelProcessor\OdeModelProcessor.csproj", "{246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}"

+EndProject

+Global

+        GlobalSection(SolutionConfigurationPlatforms) = preSolution

+                Debug|Any CPU = Debug|Any CPU

+                Release|Any CPU = Release|Any CPU

+        EndGlobalSection

+        GlobalSection(ProjectConfigurationPlatforms) = postSolution

+                {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Debug|Any CPU.ActiveCfg = Debug|Any CPU

+                {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Debug|Any CPU.Build.0 = Debug|Any CPU

+                {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Release|Any CPU.ActiveCfg = Release|Any CPU

+                {246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}.Release|Any CPU.Build.0 = Release|Any CPU

+        EndGlobalSection

+        GlobalSection(SolutionProperties) = preSolution

+                HideSolutionNode = FALSE

+        EndGlobalSection

+EndGlobal

diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs
new file mode 100644
index 0000000..9c974eb
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.cs
@@ -0,0 +1,354 @@
+/*

+ * The ODE Model Processor

+ * -----------------------

+ * 

+ * Copyright 2007, Department of Information Science,

+ * University of Otago, Dunedin, New Zealand.

+ * 

+ * Author: Richard Barrington <barri662@student.otago.ac.nz>

+ * 

+ * This is a Content Processor and Tag library written for use with

+ * Microsoft Visual C# 2005 Express Edition and Microsoft XNA Game 

+ * Studio Express 1.0.

+ * 

+ * It can be used to read .x model vertex and index data before 

+ * insertion into the content pipeline. This is used to build ODE

+ * Triangle Meshes which are then used for collision detection that

+ * is more accurate than the default XNA bounding boxes or spheres.

+ * 

+ * Usage is simple:

+ * Build the library and reference the DLL in your project.

+ * Add the DLL to the Content Pipeline

+ * Set the content processor for you .x models to OdeModelProcessor.

+ * 

+ * Create triangle meshes as follows:

+ * 1) Create a space, but only one for all of models.

+ * 2) Create a triangle data.

+ * 3) Load the model.

+ * 4) Retreive the tag from the model.

+ * 6) Build the triangle mesh by calling d.GeomTriMeshDataBuildSimple.

+ * 

+ * Eg:

+ * IntPtr space = d.SimpleSpaceCreate(IntPtr.Zero);

+ * IntPtr triangleData = d.GeomTriMeshDataCreate();

+ * Model obj = content.Load<Model>("Content\\mycube");

+ * OdeTag tag = (OdeTag)obj.Tag;

+ * IntPtr vertexArray = tag.getVertices();

+ * IntPtr indexArray = tag.getIndices();

+ * d.GeomTriMeshDataBuildSimple

+ * (

+ *     triangleData,

+ *     vertexArray, tag.getVertexStride(), tag.getVertexCount(),

+ *     indexArray, tag.getIndexCount(), tag.getIndexStride()

+ * );

+ * IntPtr triangleMesh = d.CreateTriMesh(space, triangleData, null, null, null);

+ * 

+ * You can load multiple models and test for collisions with something

+ * like this in the update method:

+ * 

+ * d.GeomSetPosition(odeTri1, obj1Position.X, obj1Position.Y, obj1Position.Z);

+ * d.GeomSetPosition(odeTri2, obj2Position.X, obj2Position.Y, obj2Position.Z);

+ * int numberOfContacts = d.Collide(odeTri1, odeTri2, ODE_CONTACTS,

+ *     contactGeom, d.ContactGeom.SizeOf);

+ * 

+ * Where odeTri1 and odeTri2 are triangle meshes you've created, obj1Position

+ * and obj2Position are the positions of your rendered models in the scene,

+ * ODE_CONTACTS is a constant defining the maximum number of contacts

+ * to test for, contactGeom is a d.ContactGeom[] of length ODE_CONTACTS.

+ * 

+ * If numberOfContacts is greater than 0, you have a collision.

+ * 

+ * Other ODE functions such as d.SpaceCollide() also work; see ODE.NET BoxTest.cs.

+ * 

+ * This library is free software; you can redistribute it and/or

+ * modify it under the same terms as the ODE and ODE.Net libraries.

+ * Specifically, the terms are one 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.

+ * 

+ */

+

+using System;

+using System.Collections.Generic;

+using System.Text;

+using Microsoft.Xna.Framework;

+using Microsoft.Xna.Framework.Content;

+using Microsoft.Xna.Framework.Graphics;

+using Microsoft.Xna.Framework.Design;

+using Microsoft.Xna.Framework.Content.Pipeline;

+using Microsoft.Xna.Framework.Content.Pipeline.Graphics;

+using Microsoft.Xna.Framework.Content.Pipeline.Processors;

+using Microsoft.Xna.Framework.Content.Pipeline.Serialization.Compiler;

+using Ode.NET;

+using System.Runtime.InteropServices;

+

+namespace OdeModelProcessor

+{

+    /*

+     * Container for vertex and index data in a format

+     * that ODE.Net can use

+     */

+    public class OdeTag

+    {

+

+        private float[] vertexData;

+        private int[] indexData;

+        private const int indexStride = (sizeof(int));

+        private const int vertexStride = (3 * sizeof(float));

+

+        /* Constructors */

+        public OdeTag()

+        {

+            vertexData = new float[0];

+            indexData = new int[0];

+        }

+        

+        public OdeTag(float[] vertexData, int[] indexData)

+        {

+            this.vertexData = vertexData;

+            this.indexData = indexData;

+        }

+        

+        /* Data setter */

+        public void setData(float[] vertexData, int[] indexData)

+        {

+            this.vertexData = vertexData;

+            this.indexData = indexData;

+        }

+

+        /* Data appenders */

+        public void appendVertexData(float[] vertexData)

+        {

+            int newVertexDataLength = vertexData.Length;

+            float[] tempVertexArray = new float[newVertexDataLength + this.vertexData.Length];

+            this.vertexData.CopyTo(tempVertexArray, 0);

+            vertexData.CopyTo(tempVertexArray, this.vertexData.Length);

+            this.vertexData = tempVertexArray;

+        }

+

+        public void appendIndexData(int[] indexData)

+        {

+            int newIndexDataLength = indexData.Length;

+            int[] tempIndexArray = new int[newIndexDataLength + this.indexData.Length];

+            this.indexData.CopyTo(tempIndexArray, 0);

+            indexData.CopyTo(tempIndexArray, this.indexData.Length);

+            this.indexData = tempIndexArray;

+        }

+

+        /* Data getters */

+        public float[] getVertexData()

+        {

+            return this.vertexData;

+        }

+

+        public int[] getIndexData()

+        {

+            return this.indexData;

+        }

+

+        /* Native data getters */

+        public IntPtr getVertices()

+        {

+            int count = getVertexData().Length;

+            int memsize = count * Marshal.SizeOf(getVertexData()[0].GetType());

+            IntPtr pointer = Marshal.AllocCoTaskMem(memsize);

+            Marshal.Copy(getVertexData(), 0, pointer, count);

+            return pointer;

+        }

+

+        public IntPtr getIndices()

+        {

+            int count = getIndexData().Length;

+            int memsize = count * Marshal.SizeOf(getIndexData()[0].GetType());

+            IntPtr pointer = Marshal.AllocCoTaskMem(memsize);

+            Marshal.Copy(getIndexData(), 0, pointer, count);

+            return pointer;

+        }

+

+        /* Count getters */

+        public int getVertexCount()

+        {

+            return vertexData.Length/3;

+        }

+

+        public int getIndexCount()

+        {

+            return indexData.Length;

+        }

+

+        /* Stride getters */

+        public int getVertexStride()

+        {

+            return vertexStride;

+        }

+

+        public int getIndexStride()

+        {

+            return indexStride;

+        }

+

+        /*

+         * Convienience method to build the mesh and return it. The triangleData

+         * is passed in to allow the calling application to delete it afterwards.

+         *

+         * Be sure to destroy the returned TriangleMesh in the client application.

+         * 

+         * Can't destroy the index and vertex arrays here though, so best to handle

+         * this manually - only use this method if nothing else makes sense.

+         */ 

+        public IntPtr getTriangleMesh(IntPtr space, IntPtr triangleData)

+        {

+            d.GeomTriMeshDataBuildSimple(

+                triangleData,

+                getVertices(), getVertexStride(), getVertexCount(),

+                getIndices(), getIndexCount(), getIndexStride()

+            );

+            return d.CreateTriMesh(space, triangleData, null, null, null);

+        }

+

+    }

+

+    /*

+     * Subclass of the XNA .x model processor, which creates and appends a tag

+     * containing vertex and index data for ODE.Net to use.

+     */ 

+    [ContentProcessor]

+    public class OdeModelProcessor : ModelProcessor

+    {

+        private OdeTag tag;

+        private int indexOffset = 0;

+

+        public override ModelContent Process(NodeContent input, ContentProcessorContext context)

+        {

+            tag = new OdeTag();

+            GenerateVerticesRecursive( input );

+            ModelContent model = base.Process(input, context);

+            model.Tag = tag;

+            indexOffset = 0;

+            return model;

+        }

+

+        public void GenerateVerticesRecursive(NodeContent input)

+        {

+            

+            MeshContent mesh = input as MeshContent;

+            

+            if (mesh != null)

+            {

+                GeometryContentCollection gc = mesh.Geometry;

+                foreach (GeometryContent g in gc)

+                {

+                    VertexContent vc = g.Vertices;

+                    IndirectPositionCollection ipc = vc.Positions;

+                    IndexCollection ic = g.Indices;

+

+                    float[] vertexData = new float[ipc.Count * 3];

+                    for (int i = 0; i < ipc.Count; i++)

+                    {

+                        

+                        Vector3 v0 = ipc[i];

+                        vertexData[(i * 3) + 0] = v0.X;

+                        vertexData[(i * 3) + 1] = v0.Y;

+                        vertexData[(i * 3) + 2] = v0.Z;                        

+                        

+                    }

+

+                    int[] indexData = new int[ic.Count];

+                    for (int j = 0; j < ic.Count; j ++)

+                    {

+

+                        indexData[j] = ic[j] + indexOffset;

+

+                    }

+

+                    tag.appendVertexData(vertexData);

+                    tag.appendIndexData(indexData);

+                    indexOffset += ipc.Count;

+                }

+                

+            }

+

+            foreach (NodeContent child in input.Children)

+            {

+                GenerateVerticesRecursive(child);

+            }

+

+        }

+

+    }

+

+    /* Writer for the OdeTag class */

+    [ContentTypeWriter]

+    public class OdeTagWriter : ContentTypeWriter<OdeTag>

+    {

+

+        protected override void Write(ContentWriter output, OdeTag value)

+        {

+            float[] vertexData = value.getVertexData();

+            int[] indexData = value.getIndexData();

+            output.Write(vertexData.Length);

+            output.Write(indexData.Length);

+            for (int j = 0; j < vertexData.Length; j++)

+            {

+                output.Write(vertexData[j]);

+            }

+            for (int i = 0; i < indexData.Length; i++)

+            {

+                output.Write(indexData[i]);

+            }

+        }

+

+        public override string GetRuntimeType(TargetPlatform targetPlatform)

+        {

+            return typeof(OdeTag).AssemblyQualifiedName;

+        }

+

+        public override string GetRuntimeReader(TargetPlatform targetPlatform)

+        {

+            return "OdeModelProcessor.OdeTagReader, OdeModelProcessor, Version=1.0.0.0, Culture=neutral";

+        }

+

+    }

+

+    /* Reader for the OdeTag class */

+    public class OdeTagReader : ContentTypeReader<OdeTag>

+    {

+        protected override OdeTag Read(ContentReader input, OdeTag existingInstance)

+        {

+            float[] vertexData = new float[input.ReadInt32()];

+            int[] indexData = new int[input.ReadInt32()];

+            for (int j = 0; j < vertexData.Length; j++)

+            {

+                vertexData[j] = input.ReadSingle();

+            }

+            for (int i = 0; i < indexData.Length; i++)

+            {

+                indexData[i] = input.ReadInt32();

+            }

+

+            OdeTag tag = null;

+            if (existingInstance == null)

+            {

+                tag = new OdeTag(vertexData, indexData);

+            }

+            else

+            {

+                tag = existingInstance;

+                tag.setData(vertexData, indexData);

+            }

+            return tag;

+        }

+    }

+}

diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj
new file mode 100644
index 0000000..3a36a12
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/OdeModelProcessor.csproj
@@ -0,0 +1,69 @@
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">

+  <PropertyGroup>

+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>

+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>

+    <ProductVersion>8.0.50727</ProductVersion>

+    <SchemaVersion>2.0</SchemaVersion>

+    <ProjectGuid>{246F3075-FEE3-45F9-8CB6-47DADBFFD1F2}</ProjectGuid>

+    <OutputType>Library</OutputType>

+    <AppDesignerFolder>Properties</AppDesignerFolder>

+    <RootNamespace>OdeModelProcessor</RootNamespace>

+    <AssemblyName>OdeModelProcessor</AssemblyName>

+  </PropertyGroup>

+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">

+    <DebugSymbols>true</DebugSymbols>

+    <DebugType>full</DebugType>

+    <Optimize>false</Optimize>

+    <OutputPath>bin\Debug\</OutputPath>

+    <DefineConstants>DEBUG</DefineConstants>

+    <ErrorReport>prompt</ErrorReport>

+    <WarningLevel>4</WarningLevel>

+    <AllowUnsafeBlocks>false</AllowUnsafeBlocks>

+    <UseVSHostingProcess>false</UseVSHostingProcess>

+  </PropertyGroup>

+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">

+    <DebugType>pdbonly</DebugType>

+    <Optimize>true</Optimize>

+    <OutputPath>bin\Release\</OutputPath>

+    <DefineConstants>

+    </DefineConstants>

+    <ErrorReport>prompt</ErrorReport>

+    <WarningLevel>4</WarningLevel>

+    <AllowUnsafeBlocks>false</AllowUnsafeBlocks>

+    <UseVSHostingProcess>false</UseVSHostingProcess>

+  </PropertyGroup>

+  <ItemGroup>

+    <Reference Include="Microsoft.Xna.Framework, Version=1.0.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=x86" />

+    <Reference Include="Microsoft.Xna.Framework.Content.Pipeline, Version=1.0.0.0, Culture=neutral, PublicKeyToken=6d5c3888ef60e27d, processorArchitecture=x86" />

+    <Reference Include="Ode.NET, Version=0.7.0.0, Culture=neutral, processorArchitecture=MSIL">

+      <SpecificVersion>False</SpecificVersion>

+      <HintPath>..\..\ODE\Ode.NET-0.8\bin\Release\Ode.NET.dll</HintPath>

+    </Reference>

+    <Reference Include="System" />

+    <Reference Include="System.Data" />

+    <Reference Include="System.Xml" />

+  </ItemGroup>

+  <ItemGroup>

+    <Compile Include="OdeModelProcessor.cs" />

+    <Compile Include="Properties\AssemblyInfo.cs" />

+    <Compile Include="Properties\Settings.Designer.cs">

+      <AutoGen>True</AutoGen>

+      <DesignTimeSharedInput>True</DesignTimeSharedInput>

+      <DependentUpon>Settings.settings</DependentUpon>

+    </Compile>

+  </ItemGroup>

+  <ItemGroup>

+    <None Include="Properties\Settings.settings">

+      <Generator>SettingsSingleFileGenerator</Generator>

+      <LastGenOutput>Settings.Designer.cs</LastGenOutput>

+    </None>

+  </ItemGroup>

+  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />

+  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 

+       Other similar extension points exist, see Microsoft.Common.targets.

+  <Target Name="BeforeBuild">

+  </Target>

+  <Target Name="AfterBuild">

+  </Target>

+  -->

+</Project>
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..1ecad3e
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/AssemblyInfo.cs
@@ -0,0 +1,35 @@
+using System.Reflection;

+using System.Runtime.CompilerServices;

+using System.Runtime.InteropServices;

+

+// General Information about an assembly is controlled through the following 

+// set of attributes. Change these attribute values to modify the information

+// associated with an assembly.

+[assembly: AssemblyTitle("OdeModelProcessor")]

+[assembly: AssemblyDescription("")]

+[assembly: AssemblyConfiguration("")]

+[assembly: AssemblyCompany("")]

+[assembly: AssemblyProduct("OdeModelProcessor")]

+[assembly: AssemblyCopyright("Copyright ©  2007")]

+[assembly: AssemblyTrademark("")]

+[assembly: AssemblyCulture("")]

+

+// Setting ComVisible to false makes the types in this assembly not visible 

+// to COM components.  If you need to access a type in this assembly from 

+// COM, set the ComVisible attribute to true on that type.

+[assembly: ComVisible(false)]

+

+// The following GUID is for the ID of the typelib if this project is exposed to COM

+[assembly: Guid("29c5609a-cd5f-480b-b4ef-5c11de022268")]

+

+// Version information for an assembly consists of the following four values:

+//

+//      Major Version

+//      Minor Version 

+//      Build Number

+//      Revision

+//

+// You can specify all the values or you can default the Revision and Build Numbers 

+// by using the '*' as shown below:

+[assembly: AssemblyVersion("1.0.0.0")]

+[assembly: AssemblyFileVersion("1.0.0.0")]

diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs
new file mode 100755
index 0000000..6ab60a8
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.Designer.cs
@@ -0,0 +1,26 @@
+//------------------------------------------------------------------------------

+// <auto-generated>

+//     This code was generated by a tool.

+//     Runtime Version:2.0.50727.832

+//

+//     Changes to this file may cause incorrect behavior and will be lost if

+//     the code is regenerated.

+// </auto-generated>

+//------------------------------------------------------------------------------

+

+namespace OdeModelProcessor.Properties {

+    

+    

+    [global::System.Runtime.CompilerServices.CompilerGeneratedAttribute()]

+    [global::System.CodeDom.Compiler.GeneratedCodeAttribute("Microsoft.VisualStudio.Editors.SettingsDesigner.SettingsSingleFileGenerator", "8.0.0.0")]

+    internal sealed partial class Settings : global::System.Configuration.ApplicationSettingsBase {

+        

+        private static Settings defaultInstance = ((Settings)(global::System.Configuration.ApplicationSettingsBase.Synchronized(new Settings())));

+        

+        public static Settings Default {

+            get {

+                return defaultInstance;

+            }

+        }

+    }

+}

diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings
new file mode 100755
index 0000000..15034e7
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/OdeModelProcessor/Properties/Settings.settings
@@ -0,0 +1,6 @@
+<?xml version='1.0' encoding='utf-8'?>

+<SettingsFile xmlns="http://schemas.microsoft.com/VisualStudio/2004/01/settings" CurrentProfile="(Default)">

+  <Profiles>

+    <Profile Name="(Default)" />

+  </Profiles>

+</SettingsFile>

diff --git a/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT b/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT
new file mode 100644
index 0000000..97683f9
--- /dev/null
+++ b/libraries/ode-0.9/contrib/OdeModelProcessor/README.TXT
@@ -0,0 +1,78 @@
+The ODE Model Processor

+-----------------------

+

+Copyright 2007, Department of Information Science,

+University of Otago, Dunedin, New Zealand.

+

+Author: Richard Barrington <barri662@student.otago.ac.nz>

+

+This is a Content Processor and Tag library written for use with

+Microsoft Visual C# 2005 Express Edition and Microsoft XNA Game 

+Studio Express 1.0.

+

+It can be used to read .x model vertex and index data before 

+insertion into the content pipeline. This is used to build ODE

+Triangle Meshes which are then used for collision detection that

+is more accurate than the default XNA bounding boxes or spheres.

+

+Usage is fairly simple:

+Build the library and reference the DLL in your project.

+Add the DLL to the Content Pipeline

+Set the content processor for you .x models to OdeModelProcessor.

+

+Create triangle meshes as follows:

+1) Create a space, but only one for all of models.

+2) Create a triangle data.

+3) Load the model.

+4) Retreive the tag from the model.

+6) Build the triangle mesh by calling d.GeomTriMeshDataBuildSimple.

+

+Eg:

+IntPtr space = d.SimpleSpaceCreate(IntPtr.Zero);

+IntPtr triangleData = d.GeomTriMeshDataCreate();

+Model obj = content.Load<Model>("Content\\mycube");

+OdeTag tag = (OdeTag)obj.Tag;

+IntPtr vertexArray = tag.getVertices();

+IntPtr indexArray = tag.getIndices();

+d.GeomTriMeshDataBuildSimple

+(

+        triangleData,

+        vertexArray, tag.getVertexStride(), tag.getVertexCount(),

+        indexArray, tag.getIndexCount(), tag.getIndexStride()

+);

+IntPtr triangleMesh = d.CreateTriMesh(space, triangleData, null, null, null);

+

+You can load multiple models and test for collisions with something

+like this in the update method:

+

+d.GeomSetPosition(odeTri1, obj1Position.X, obj1Position.Y, obj1Position.Z);

+d.GeomSetPosition(odeTri2, obj2Position.X, obj2Position.Y, obj2Position.Z);

+int numberOfContacts = d.Collide(odeTri1, odeTri2, ODE_CONTACTS,

+                           contactGeom, d.ContactGeom.SizeOf);

+

+Where odeTri1 and odeTri2 are triangle meshes you've created, obj1Position

+and obj2Position are the positions of your rendered models in the scene,

+ODE_CONTACTS is a constant defining the maximum number of contacts

+to test for, contactGeom is a d.ContactGeom[] of length ODE_CONTACTS.

+

+If numberOfContacts is greater than 0, you have a collision.

+

+Other ODE functions such as d.SpaceCollide() also work; see ODE.NET BoxTest.cs.

+

+This library is free software; you can redistribute it and/or

+modify it under the same terms as the ODE and ODE.Net libraries.

+Specifically, the terms are one 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.

diff --git a/libraries/ode-0.9/contrib/README b/libraries/ode-0.9/contrib/README
new file mode 100644
index 0000000..dbfaf7f
--- /dev/null
+++ b/libraries/ode-0.9/contrib/README
@@ -0,0 +1,19 @@
+This directory contains ODE-related things that have been generously
+contributed by ODE's users. Why is this stuff here and not integrated
+into the main ODE source tree?  There may be several reasons:
+
+  * The author(s) and ODE maintainers(s) may not have had time to do
+    the job.
+
+  * It may not be finished.
+
+  * It may contribute functionality that is useful but not considered
+    to be part of ODE's core.
+
+No guarantees are made about the code in this directory - it may not
+be documented, it may not have been tested, and it may not even
+compile for you.
+
+Each package has its own subdirectory, with a README file in that
+directory explaining what the package is.
+
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h b/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h
new file mode 100644
index 0000000..b445353
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/collision_std_internal.h
@@ -0,0 +1,100 @@
+//Benoit CHAPEROT 2003-2004 www.jstarlab.com
+#ifndef _ODE_COLLISION_STD_INTERNAL_H_
+#define _ODE_COLLISION_STD_INTERNAL_H_
+
+#include <ode/common.h>
+#include "collision_kernel.h"
+
+struct dxSphere : public dxGeom {
+  dReal radius;         // sphere radius
+  dxSphere (dSpaceID space, dReal _radius);
+  void computeAABB();
+};
+
+
+struct dxBox : public dxGeom {
+  dVector3 side;        // side lengths (x,y,z)
+  dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz);
+  void computeAABB();
+};
+
+
+struct dxCCylinder : public dxGeom {
+  dReal radius,lz;      // radius, length along z axis
+  dxCCylinder (dSpaceID space, dReal _radius, dReal _length);
+  void computeAABB();
+};
+
+
+struct dxPlane : public dxGeom {
+  dReal p[4];
+  dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d);
+  void computeAABB();
+};
+
+struct dxCylinder : public dxGeom {
+  dReal radius,lz;      // radius, length along z axis
+  dxCylinder (dSpaceID space, dReal _radius, dReal _length);
+  void computeAABB();
+};
+
+struct dxCone : public dxGeom {
+  dReal radius,lz;
+  dxCone(dSpaceID space, dReal _radius,dReal _length);
+  ~dxCone();
+  void computeAABB();
+};
+
+struct dxRay : public dxGeom {
+  dReal length;
+  dxRay (dSpaceID space, dReal _length);
+  void computeAABB();
+};
+
+struct dxTerrainY : public dxGeom {
+  dReal m_vLength;
+  dReal *m_pHeights;
+  dReal m_vMinHeight;
+  dReal m_vMaxHeight;
+  dReal m_vNodeLength;
+  int   m_nNumNodesPerSide;
+  int   m_nNumNodesPerSideShift;
+  int   m_nNumNodesPerSideMask;
+  int   m_bFinite;
+  dxTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
+  ~dxTerrainY();
+  void computeAABB();
+  dReal GetHeight(dReal x,dReal z);
+  dReal GetHeight(int x,int z);
+  int dCollideTerrainUnit(int x,int z,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip);
+  bool IsOnTerrain(int nx,int nz,int w,dReal *pos);
+};
+
+struct dxTerrainZ : public dxGeom {
+  dReal m_vLength;
+  dReal *m_pHeights;
+  dReal m_vMinHeight;
+  dReal m_vMaxHeight;
+  dReal m_vNodeLength;
+  int   m_nNumNodesPerSide;
+  int   m_nNumNodesPerSideShift;
+  int   m_nNumNodesPerSideMask;
+  int   m_bFinite;
+  dxTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
+  ~dxTerrainZ();
+  void computeAABB();
+  dReal GetHeight(dReal x,dReal y);
+  dReal GetHeight(int x,int y);
+  int dCollideTerrainUnit(int x,int y,dxGeom *o2,int numMaxContacts,int flags,dContactGeom *contact, int skip);
+  bool IsOnTerrain(int nx,int ny,int w,dReal *pos);
+};
+
+#ifndef MIN
+#define MIN(a,b)        ((a<b)?a:b)
+#endif
+
+#ifndef MAX
+#define MAX(a,b)        ((a>b)?a:b)
+#endif
+
+#endif //_ODE_COLLISION_STD_INTERNAL_H_
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp
new file mode 100644
index 0000000..8c5c3be
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/dCone.cpp
@@ -0,0 +1,504 @@
+//Benoit CHAPEROT 2003-2004 www.jstarlab.com
+//some code inspired by Magic Software
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_kernel.h"
+#include "collision_std.h"
+#include "collision_std_internal.h"
+#include "collision_util.h"
+#include <drawstuff/drawstuff.h>
+#include "windows.h"
+#include "ode\ode.h"
+
+#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
+const dReal fEPSILON = 1e-9f;
+
+dxCone::dxCone (dSpaceID space, dReal _radius,dReal _length) :
+dxGeom (space,1)
+{
+        dAASSERT(_radius > 0.f);
+        dAASSERT(_length > 0.f);
+        type = dConeClass;
+        radius = _radius;
+        lz = _length;
+}
+
+dxCone::~dxCone()
+{
+}
+
+void dxCone::computeAABB()
+{
+  const dMatrix3& R = final_posr->R;
+  const dVector3& pos = final_posr->pos;
+
+        dReal xrange = dFabs(R[2]  * lz) + radius;
+        dReal yrange = dFabs(R[6]  * lz) + radius;
+        dReal zrange = dFabs(R[10] * lz) + radius;
+        aabb[0] = pos[0] - xrange;
+        aabb[1] = pos[0] + xrange;
+        aabb[2] = pos[1] - yrange;
+        aabb[3] = pos[1] + yrange;
+        aabb[4] = pos[2] - zrange;
+        aabb[5] = pos[2] + zrange;
+}
+
+dGeomID dCreateCone(dSpaceID space, dReal _radius,dReal _length)
+{
+        return new dxCone(space,_radius,_length);
+}
+
+void dGeomConeSetParams (dGeomID g, dReal _radius, dReal _length)
+{
+        dUASSERT (g && g->type == dConeClass,"argument not a cone");
+        dAASSERT (_radius > 0.f);
+        dAASSERT (_length > 0.f);
+  g->recomputePosr();
+        dxCone *c = (dxCone*) g;
+        c->radius = _radius;
+        c->lz = _length;
+        dGeomMoved (g);
+}
+
+
+void dGeomConeGetParams (dGeomID g, dReal *_radius, dReal *_length)
+{
+        dUASSERT (g && g->type == dConeClass,"argument not a cone");
+  g->recomputePosr();
+        dxCone *c = (dxCone*) g;
+        *_radius = c->radius;
+        *_length = c->lz;
+}
+
+//positive inside
+dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z)
+{
+        dUASSERT (g && g->type == dConeClass,"argument not a cone");
+
+   g->recomputePosr();
+        dxCone *cone = (dxCone*) g;
+
+        dVector3 tmp,q;
+        tmp[0] = x - cone->final_posr->pos[0];
+        tmp[1] = y - cone->final_posr->pos[1];
+        tmp[2] = z - cone->final_posr->pos[2];
+        dMULTIPLY1_331 (q,cone->final_posr->R,tmp);
+
+        dReal r = cone->radius;
+        dReal h = cone->lz;
+
+        dReal d0 = (r - r*q[2]/h) - dSqrt(q[0]*q[0]+q[1]*q[1]);
+        dReal d1 = q[2];
+        dReal d2 = h-q[2];
+        
+        if (d0 < d1) {
+    if (d0 < d2) return d0; else return d2;
+        }
+        else {
+        if (d1 < d2) return d1; else return d2;
+        }
+}
+
+//plane plane
+bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4],
+        dVector3 LinePos,dVector3 LineDir)
+{
+    // If Cross(N0,N1) is zero, then either planes are parallel and separated
+    // or the same plane.  In both cases, 'false' is returned.  Otherwise,
+    // the intersection line is
+    //
+    //   L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
+    //
+    // for some coefficients c0 and c1 and for t any real number (the line
+    // parameter).  Taking dot products with the normals,
+    //
+    //   d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
+    //   d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
+    //
+    // which are two equations in two unknowns.  The solution is
+    //
+    //   c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
+    //   c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
+    //
+    // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
+/*
+    Real fN00 = rkPlane0.Normal().SquaredLength();
+    Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
+    Real fN11 = rkPlane1.Normal().SquaredLength();
+    Real fDet = fN00*fN11 - fN01*fN01;
+
+    if ( Math::FAbs(fDet) < gs_fEpsilon )
+        return false;
+
+    Real fInvDet = 1.0f/fDet;
+    Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
+    Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;
+
+    rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
+    rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
+    return true;
+*/
+        dReal fN00 = dLENGTHSQUARED(Plane0);
+    dReal fN01 = dDOT(Plane0,Plane1);
+    dReal fN11 = dLENGTHSQUARED(Plane1);
+    dReal fDet = fN00*fN11 - fN01*fN01;
+
+    if ( fabs(fDet) < fEPSILON)
+        return false;
+
+    dReal fInvDet = 1.0f/fDet;
+    dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet;
+    dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet;
+
+    dCROSS(LineDir,=,Plane0,Plane1);
+        dNormalize3(LineDir);
+
+        dVector3 Temp0,Temp1;
+        dOPC(Temp0,*,Plane0,fC0);
+        dOPC(Temp1,*,Plane1,fC1);
+        dOP(LinePos,+,Temp0,Temp1);
+
+    return true;
+}
+
+//plane ray
+bool FindIntersectionPlaneRay(const dReal Plane[4],
+                                          const dVector3 &LinePos,const dVector3 &LineDir,
+                                          dReal &u,dVector3 &Pos)
+{
+/*
+        u = (A*X1 + B*Y1 + C*Z1 + D) / (A*(X1-X2) + B*(Y1-Y2)+C*(Z1-Z2))        
+*/      
+        dReal fDet = -dDot(Plane,LineDir,3);
+
+        if ( fabs(fDet) < fEPSILON)
+        return false;
+
+        u = (dDot(Plane,LinePos,3) - Plane[3]) / fDet;
+        dOPC(Pos,*,LineDir,u);
+        dOPE(Pos,+=,LinePos);
+
+        return true;
+}
+
+int SolveQuadraticPolynomial(dReal a,dReal b,dReal c,dReal &x0,dReal &x1)
+{
+        dReal d = b*b - 4*a*c;
+        int NumRoots = 0;
+        dReal dr;
+
+        if (d < 0.f)
+                return NumRoots;
+
+        if (d == 0.f)
+        {
+                NumRoots = 1;
+                dr = 0.f;
+        }
+        else
+        {
+                NumRoots = 2;
+                dr = sqrtf(d);
+        }
+
+        x0 = (-b -dr) / (2.f * a);
+        x1 = (-b +dr) / (2.f * a);
+
+        return NumRoots;
+}
+/*
+const int VALID_INTERSECTION    = 1<<0;
+const int POS_TEST_FAILEDT0             = 1<<0;
+const int POS_TEST_FAILEDT1             = 1<<1;
+*/
+int ProcessConeRayIntersectionPoint(    dReal r,dReal h,
+                                                                                const dVector3 &q,const dVector3 &v,dReal t,
+                                                                                dVector3 &p,
+                                                                                dVector3 &n,
+                                                                                int &f)
+{
+        dOPC(p,*,v,t);
+        dOPE(p,+=,q);
+        n[0] = 2*p[0];
+        n[1] = 2*p[1];
+        n[2] = -2*p[2]*r*r/(h*h);
+
+        f = 0;
+        if (p[2] > h)   return 0;
+        if (p[2] < 0)   return 0;
+        if (t > 1)              return 0;
+        if (t < 0)              return 0;
+
+        return 1;
+}
+
+//cone ray
+//line in cone space (position,direction)
+//distance from line position (direction normalized)(if any)
+//return the number of intersection
+int FindIntersectionConeRay(dReal r,dReal h,    
+                                         const dVector3 &q,const dVector3 &v,dContactGeom *pContact)
+{
+        dVector3 qp,vp;
+        dOPE(qp,=,q);
+        dOPE(vp,=,v);
+        qp[2] = h-q[2];
+        vp[2] = -v[2];
+        dReal ts = (r/h);
+        ts *= ts;
+        dReal a = vp[0]*vp[0] + vp[1]*vp[1] - ts*vp[2]*vp[2];
+        dReal b = 2.f*qp[0]*vp[0] + 2.f*qp[1]*vp[1] - 2.f*ts*qp[2]*vp[2];
+        dReal c = qp[0]*qp[0] + qp[1]*qp[1] - ts*qp[2]*qp[2];
+
+/*
+        dReal a = v[0]*v[0] + v[1]*v[1] - (v[2]*v[2]*r*r) / (h*h);
+        dReal b = 2.f*q[0]*v[0] + 2.f*q[1]*v[1] + 2.f*r*r*v[2]/h - 2*r*r*q[0]*v[0]/(h*h);
+        dReal c = q[0]*q[0] + q[1]*q[1] + 2*r*r*q[2]/h - r*r*q[2]/(h*h) - r*r;
+*/
+        int nNumRoots=SolveQuadraticPolynomial(a,b,c,pContact[0].depth,pContact[1].depth);
+        int flag = 0;
+
+        dContactGeom ValidContact[2];
+
+        int nNumValidContacts = 0;
+        for (int i=0;i<nNumRoots;i++)
+        {
+                if (ProcessConeRayIntersectionPoint(r,h,q,v,pContact[i].depth,pContact[i].pos,
+                        pContact[i].normal,flag))
+                {
+                        ValidContact[nNumValidContacts] = pContact[i];
+                        nNumValidContacts++;
+                }
+        }
+
+        dOP(qp,+,q,v);
+
+        if ((nNumValidContacts < 2) && (v[2] != 0.f))
+        {
+                dReal d = (0.f-q[2]) / (v[2]); 
+                if ((d>=0) && (d<=1))
+                {
+                        dOPC(vp,*,v,d);
+                        dOP(qp,+,q,vp);
+
+                        if (qp[0]*qp[0]+qp[1]*qp[1] < r*r)
+                        {
+                                dOPE(ValidContact[nNumValidContacts].pos,=,qp);
+                                ValidContact[nNumValidContacts].normal[0] = 0.f;
+                                ValidContact[nNumValidContacts].normal[1] = 0.f;
+                                ValidContact[nNumValidContacts].normal[2] = -1.f;
+                                ValidContact[nNumValidContacts].depth = d;
+                                nNumValidContacts++;
+                        }
+                }
+        }
+
+        if (nNumValidContacts == 2)
+        {
+                if (ValidContact[0].depth > ValidContact[1].depth)
+                {
+                        pContact[0] = ValidContact[1];
+                        pContact[1] = ValidContact[0];
+                }
+                else
+                {
+                        pContact[0] = ValidContact[0];
+                        pContact[1] = ValidContact[1];
+                }
+        }
+        else if (nNumValidContacts == 1)
+        {
+                pContact[0] = ValidContact[0];
+        }
+
+        return nNumValidContacts;
+}
+
+int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags,
+                                                 dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));
+        dIASSERT (o1->type == dConeClass);
+        dIASSERT (o2->type == dPlaneClass);
+        dxCone *cone = (dxCone*) o1;
+        dxPlane *plane = (dxPlane*) o2;
+
+        contact->g1 = o1;
+        contact->g2 = o2;
+
+        dVector3 p0,p1,pp0,pp1;
+        dOPE(p0,=,cone->final_posr->pos);
+        p1[0] = cone->final_posr->R[0*4+2] * cone->lz + p0[0];
+        p1[1] = cone->final_posr->R[1*4+2] * cone->lz + p0[1];
+        p1[2] = cone->final_posr->R[2*4+2] * cone->lz + p0[2];
+
+        dReal u;
+        FindIntersectionPlaneRay(plane->p,p0,plane->p,u,pp0);
+        FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1);
+
+        if (dDISTANCE(pp0,pp1) < fEPSILON)
+        {
+                p1[0] = cone->final_posr->R[0*4+0] * cone->lz + p0[0];
+                p1[1] = cone->final_posr->R[1*4+0] * cone->lz + p0[1];
+                p1[2] = cone->final_posr->R[2*4+0] * cone->lz + p0[2];
+                FindIntersectionPlaneRay(plane->p,p1,plane->p,u,pp1);
+                dIASSERT(dDISTANCE(pp0,pp1) >= fEPSILON);
+        }
+        dVector3 h,r0,r1;
+        h[0] = cone->final_posr->R[0*4+2];
+        h[1] = cone->final_posr->R[1*4+2];
+        h[2] = cone->final_posr->R[2*4+2];
+        
+        dOP(r0,-,pp0,pp1);
+        dCROSS(r1,=,h,r0);
+        dCROSS(r0,=,r1,h);
+        dNormalize3(r0);
+        dOPEC(h,*=,cone->lz);
+        dOPEC(r0,*=,cone->radius);
+
+        dVector3 p[3];
+        dOP(p[0],+,cone->final_posr->pos,h);
+        dOP(p[1],+,cone->final_posr->pos,r0);
+        dOP(p[2],-,cone->final_posr->pos,r0);
+        
+        int numMaxContacts = flags & 0xffff;
+        if (numMaxContacts == 0) 
+                numMaxContacts = 1;
+
+        int n=0;
+        for (int i=0;i<3;i++)
+        {
+                dReal d = dGeomPlanePointDepth(o2, p[i][0], p[i][1], p[i][2]);
+
+                if (d>0.f)
+                {
+                        CONTACT(contact,n*skip)->g1 = o1;
+                        CONTACT(contact,n*skip)->g2 = o2;
+                        dOPE(CONTACT(contact,n*skip)->normal,=,plane->p); 
+                        dOPE(CONTACT(contact,n*skip)->pos,=,p[i]); 
+                        CONTACT(contact,n*skip)->depth = d;
+                        n++;
+
+                        if (n == numMaxContacts)
+                                return n;
+                }
+        }
+        
+        return n;
+}
+
+int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags,
+                                                 dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));
+        dIASSERT (o1->type == dRayClass);
+        dIASSERT (o2->type == dConeClass);
+        dxRay *ray = (dxRay*) o1;
+        dxCone *cone = (dxCone*) o2;
+
+        contact->g1 = o1;
+        contact->g2 = o2;
+
+        dVector3 tmp,q,v;
+        tmp[0] = ray->final_posr->pos[0] - cone->final_posr->pos[0];
+        tmp[1] = ray->final_posr->pos[1] - cone->final_posr->pos[1];
+        tmp[2] = ray->final_posr->pos[2] - cone->final_posr->pos[2];
+        dMULTIPLY1_331 (q,cone->final_posr->R,tmp);
+        tmp[0] = ray->final_posr->R[0*4+2] * ray->length;
+        tmp[1] = ray->final_posr->R[1*4+2] * ray->length;
+        tmp[2] = ray->final_posr->R[2*4+2] * ray->length;
+        dMULTIPLY1_331 (v,cone->final_posr->R,tmp);
+
+        dReal r = cone->radius;
+        dReal h = cone->lz;
+
+        dContactGeom Contact[2];
+
+        if (FindIntersectionConeRay(r,h,q,v,Contact))
+        {
+                dMULTIPLY0_331(contact->normal,cone->final_posr->R,Contact[0].normal);
+                dMULTIPLY0_331(contact->pos,cone->final_posr->R,Contact[0].pos);
+                dOPE(contact->pos,+=,cone->final_posr->pos);
+                contact->depth = Contact[0].depth * dLENGTH(v);
+/*
+                dMatrix3 RI;
+                dRSetIdentity (RI);
+                dVector3 ss;
+                ss[0] = 0.01f;
+                ss[1] = 0.01f;
+                ss[2] = 0.01f;
+
+                dsSetColorAlpha (1,0,0,0.8f);
+                dsDrawBox(contact->pos,RI,ss);
+*/              
+                return 1;
+        }
+
+        return 0;
+}
+
+int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));
+        dIASSERT (o1->type == dConeClass);
+        dIASSERT (o2->type == dSphereClass);
+        dxCone          *cone = (dxCone*) o1;
+        
+        dxSphere ASphere(0,cone->radius);
+        dGeomSetRotation(&ASphere,cone->final_posr->R);
+        dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
+
+        return dCollideSphereSphere(&ASphere, o2, flags, contact, skip);
+}
+
+int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));
+        dIASSERT (o1->type == dConeClass);
+        dIASSERT (o2->type == dBoxClass);
+        dxCone          *cone = (dxCone*) o1;
+        
+        dxSphere ASphere(0,cone->radius);
+        dGeomSetRotation(&ASphere,cone->final_posr->R);
+        dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
+
+        return dCollideSphereBox(&ASphere, o2, flags, contact, skip);
+}
+
+int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));
+        dIASSERT (o1->type == dCCylinderClass);
+        dIASSERT (o2->type == dConeClass);
+        dxCone          *cone = (dxCone*) o2;
+        
+        dxSphere ASphere(0,cone->radius);
+        dGeomSetRotation(&ASphere,cone->final_posr->R);
+        dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
+
+        return dCollideCCylinderSphere(o1, &ASphere, flags, contact, skip);
+}
+
+extern int dCollideSTL(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));
+        dIASSERT (o1->type == dTriMeshClass);
+        dIASSERT (o2->type == dConeClass);
+        dxCone          *cone = (dxCone*) o2;
+
+        dxSphere ASphere(0,cone->radius);
+        dGeomSetRotation(&ASphere,cone->final_posr->R);
+        dGeomSetPosition(&ASphere,cone->final_posr->pos[0],cone->final_posr->pos[1],cone->final_posr->pos[2]);
+
+        return dCollideSTL(o1, &ASphere, flags, contact, skip);
+}
+
+
+        
+
+
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
new file mode 100644
index 0000000..ff779ac
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainY.cpp
@@ -0,0 +1,662 @@
+//Benoit CHAPEROT 2003-2004 www.jstarlab.com
+//some code inspired by Magic Software
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_kernel.h"
+#include "collision_std.h"
+#include "collision_std_internal.h"
+#include "collision_util.h"
+//#include <drawstuff/drawstuff.h>
+#include "windows.h"
+#include "ode\ode.h"
+
+#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
+#define MAXCONTACT 10
+#define TERRAINTOL 0.0f
+
+static bool IsAPowerOfTwo(int f)
+{
+        dAASSERT(f!=0);
+        while ((f&1) != 1)      
+                f >>= 1;
+
+        return (f == 1);
+}
+
+static int GetPowerOfTwo(int f)
+{
+        dAASSERT(f!=0);
+        int n = 0;
+        while ((f&1) != 1)
+        {
+                n++;
+                f >>= 1;
+        }
+        
+        return n;
+}
+
+dxTerrainY::dxTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
+dxGeom (space,bPlaceable)
+{
+        dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
+        dIASSERT(pHeights);
+        dIASSERT(vLength > 0.f);
+        dIASSERT(nNumNodesPerSide > 0);
+        type = dTerrainYClass;
+        m_vLength = vLength;
+        m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide];
+        dIASSERT(m_pHeights);
+        m_nNumNodesPerSide = nNumNodesPerSide;
+        m_vNodeLength = m_vLength / m_nNumNodesPerSide;
+        m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide);
+        m_nNumNodesPerSideMask  = m_nNumNodesPerSide - 1;
+        m_vMinHeight = dInfinity;
+        m_vMaxHeight = -dInfinity;
+        m_bFinite = bFinite;
+
+        for (int i=0;i<nNumNodesPerSide * nNumNodesPerSide;i++)
+        {
+                m_pHeights[i] = pHeights[i];
+                if (m_pHeights[i] < m_vMinHeight)       m_vMinHeight = m_pHeights[i];
+                if (m_pHeights[i] > m_vMaxHeight)       m_vMaxHeight = m_pHeights[i];
+        }
+}
+
+dxTerrainY::~dxTerrainY()
+{
+        dIASSERT(m_pHeights);
+        delete [] m_pHeights;
+}
+
+void dxTerrainY::computeAABB()
+{
+        if (m_bFinite)
+        {
+                if (gflags & GEOM_PLACEABLE)
+                {
+                        dReal dx[6],dy[6],dz[6];
+                        dx[0] = 0;
+                        dx[1] = final_posr->R[0] * m_vLength;
+                        dx[2] = final_posr->R[1] * m_vMinHeight;
+                        dx[3] = final_posr->R[1] * m_vMaxHeight;
+                        dx[4] = 0;
+                        dx[5] = final_posr->R[2] * m_vLength;
+
+                        dy[0] = 0;
+                        dy[1] = final_posr->R[4] * m_vLength;
+                        dy[2] = final_posr->R[5] * m_vMinHeight;
+                        dy[3] = final_posr->R[5] * m_vMaxHeight;
+                        dy[4] = 0;
+                        dy[5] = final_posr->R[6] * m_vLength;
+
+                        dz[0]  = 0;
+                        dz[1]  = final_posr->R[8] * m_vLength;
+                        dz[2]  = final_posr->R[9] * m_vMinHeight;
+                        dz[3]  = final_posr->R[9] * m_vMaxHeight;
+                        dz[4]  = 0;
+                        dz[5]  = final_posr->R[10] * m_vLength;
+
+                        aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]);
+                        aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]);
+                        aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]);
+                        aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]);
+                        aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]);
+                        aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]);
+                }
+                else
+                {
+                        aabb[0] = 0;
+                        aabb[1] = m_vLength;
+                        aabb[2] = m_vMinHeight;
+                        aabb[3] = m_vMaxHeight;
+                        aabb[4] = 0;
+                        aabb[5] = m_vLength;
+                }
+        }
+        else
+        {
+                if (gflags & GEOM_PLACEABLE)
+                {
+                        aabb[0] = -dInfinity;
+                        aabb[1] = dInfinity;
+                        aabb[2] = -dInfinity;
+                        aabb[3] = dInfinity;
+                        aabb[4] = -dInfinity;
+                        aabb[5] = dInfinity;
+                }
+                else
+                {
+                        aabb[0] = -dInfinity;
+                        aabb[1] = dInfinity;
+                        aabb[2] = m_vMinHeight;
+                        aabb[3] = m_vMaxHeight;
+                        aabb[4] = -dInfinity;
+                        aabb[5] = dInfinity;
+                }
+        }
+}
+
+dReal dxTerrainY::GetHeight(int x,int z)
+{
+        return m_pHeights[      (((unsigned int)(z) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
+                                        +        ((unsigned int)(x) & m_nNumNodesPerSideMask)];
+}
+
+dReal dxTerrainY::GetHeight(dReal x,dReal z)
+{
+        int nX          = int(floor(x / m_vNodeLength));
+        int nZ          = int(floor(z / m_vNodeLength));
+        dReal dx        = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
+        dReal dz        = (z - (dReal(nZ) * m_vNodeLength)) / m_vNodeLength;
+        dIASSERT((dx >= 0.f) && (dx <= 1.f));
+        dIASSERT((dz >= 0.f) && (dz <= 1.f));
+
+        dReal y,y0;
+        
+        if (dx + dz < 1.f)
+        {
+                y0      = GetHeight(nX,nZ);
+                y       = y0    
+                        + (GetHeight(nX+1,nZ) - y0) * dx
+                        + (GetHeight(nX,nZ+1) - y0) * dz;
+        }
+        else
+        {
+                y0      = GetHeight(nX+1,nZ+1);
+                y       = y0    
+                        + (GetHeight(nX+1,nZ) - y0) * (1.f - dz)
+                        + (GetHeight(nX,nZ+1) - y0) * (1.f - dx);
+        }
+
+        return y;       
+}
+
+bool dxTerrainY::IsOnTerrain(int nx,int nz,int w,dReal *pos)
+{
+        dVector3 Min,Max;
+        Min[0] = nx * m_vNodeLength;
+        Min[2] = nz * m_vNodeLength;
+        Max[0] = (nx+1) * m_vNodeLength;
+        Max[2] = (nz+1) * m_vNodeLength;
+        dReal Tol = m_vNodeLength * TERRAINTOL;
+        
+        if ((pos[0]<Min[0]-Tol) || (pos[0]>Max[0]+Tol))
+                return false;
+
+        if ((pos[2]<Min[2]-Tol) || (pos[2]>Max[2]+Tol))
+                return false;
+
+        dReal dx        = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
+        dReal dz        = (pos[2] - (dReal(nz) * m_vNodeLength)) / m_vNodeLength;
+
+        if ((w == 0) && (dx + dz > 1.f+TERRAINTOL))
+                return false;
+
+        if ((w == 1) && (dx + dz < 1.f-TERRAINTOL))
+                return false;
+
+        return true;
+}
+
+dGeomID dCreateTerrainY(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
+{
+        return new dxTerrainY(space, pHeights,vLength,nNumNodesPerSide,bFinite,bPlaceable);
+}
+
+dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z)
+{
+        dUASSERT (g && g->type == dTerrainYClass,"argument not a terrain");
+  g->recomputePosr();
+        dxTerrainY *t = (dxTerrainY*) g;
+        return t->GetHeight(x,z) - y;
+}
+
+typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z);
+#define RECOMPUTE_RAYNORMAL
+//#define DO_RAYDEPTH
+
+#define DMESS(A)        \
+                        dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).",     \
+                                        x,z,A,  \
+                                        pContact->depth,        \
+                                        dGeomSphereGetRadius(o2),               \
+                                        pContact->pos[0],       \
+                                        pContact->pos[1],       \
+                                        pContact->pos[2],       \
+                                        pContact->normal[0],    \
+                                        pContact->normal[1],    \
+                                        pContact->normal[2]);
+/*
+(y is up)
+
+A-B-E.x
+|/|
+C-D
+|
+F
+.
+z
+*/
+int dxTerrainY::dCollideTerrainUnit(
+        int x,int z,dxGeom *o2,int numMaxContacts,
+        int flags,dContactGeom *contact, int skip)
+{
+        dColliderFn *CollideRayN;
+        dColliderFn *CollideNPlane;
+        dGetDepthFn *GetDepth;
+        int numContacts = 0;
+        int numPlaneContacts = 0;
+        int i;
+        
+        if (numContacts == numMaxContacts)
+                return numContacts;
+
+        dContactGeom PlaneContact[MAXCONTACT];
+        flags = (flags & 0xffff0000) | MAXCONTACT;
+        
+        switch (o2->type)
+        {
+        case dSphereClass:
+                CollideRayN             = dCollideRaySphere;
+                CollideNPlane   = dCollideSpherePlane;
+                GetDepth                = dGeomSpherePointDepth;
+                break;
+        case dBoxClass:
+                CollideRayN             = dCollideRayBox;
+                CollideNPlane   = dCollideBoxPlane;
+                GetDepth                = dGeomBoxPointDepth;
+                break;
+        case dCCylinderClass:
+                CollideRayN             = dCollideRayCCylinder;
+                CollideNPlane   = dCollideCCylinderPlane;
+                GetDepth                = dGeomCCylinderPointDepth;
+                break;
+        case dRayClass:
+                CollideRayN             = NULL;
+                CollideNPlane   = dCollideRayPlane;
+                GetDepth                = NULL;
+                break;
+        case dConeClass:
+                CollideRayN             = dCollideRayCone;
+                CollideNPlane   = dCollideConePlane;
+                GetDepth                = dGeomConePointDepth;
+                break;
+        default:
+                dIASSERT(0);
+        }
+
+        dReal Plane[4],lBD,lCD,lBC;
+        dVector3 A,B,C,D,BD,CD,BC,AB,AC;
+        A[0] = x * m_vNodeLength;
+        A[2] = z* m_vNodeLength;
+        A[1] = GetHeight(x,z);
+        B[0] = (x+1) * m_vNodeLength;
+        B[2] = z * m_vNodeLength;
+        B[1] = GetHeight(x+1,z);
+        C[0] = x * m_vNodeLength;
+        C[2] = (z+1) * m_vNodeLength;
+        C[1] = GetHeight(x,z+1);
+        D[0] = (x+1) * m_vNodeLength;
+        D[2] = (z+1) * m_vNodeLength;
+        D[1] = GetHeight(x+1,z+1);
+
+        dOP(BC,-,C,B);
+        lBC = dLENGTH(BC);
+        dOPEC(BC,/=,lBC);
+
+        dOP(BD,-,D,B);
+        lBD = dLENGTH(BD);
+        dOPEC(BD,/=,lBD);
+
+        dOP(CD,-,D,C);
+        lCD = dLENGTH(CD);
+        dOPEC(CD,/=,lCD);
+
+        dOP(AB,-,B,A);
+        dNormalize3(AB);
+
+        dOP(AC,-,C,A);
+        dNormalize3(AC);
+
+        if (CollideRayN)
+        {
+#ifdef RECOMPUTE_RAYNORMAL
+                dVector3 E,F;
+                dVector3 CE,FB,AD;
+                dVector3 Normal[3];
+                E[0] = (x+2) * m_vNodeLength;
+                E[2] = z * m_vNodeLength;
+                E[1] = GetHeight(x+2,z);
+                F[0] = x * m_vNodeLength;
+                F[2] = (z+2) * m_vNodeLength;
+                F[1] = GetHeight(x,z+2);
+                dOP(AD,-,D,A);
+                dNormalize3(AD);
+                dOP(CE,-,E,C);
+                dNormalize3(CE);
+                dOP(FB,-,B,F);
+                dNormalize3(FB);
+
+                //BC
+                dCROSS(Normal[0],=,BC,AD);
+                dNormalize3(Normal[0]);
+
+                //BD
+                dCROSS(Normal[1],=,BD,CE);
+                dNormalize3(Normal[1]);
+
+                //CD
+                dCROSS(Normal[2],=,CD,FB);
+                dNormalize3(Normal[2]);
+#endif          
+                int nA[3],nB[3];
+                dContactGeom ContactA[3],ContactB[3];
+                dxRay rayBC(0,lBC);     
+                dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]);
+                nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom));
+                dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]);
+                nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom));
+                
+                dxRay rayBD(0,lBD);     
+                dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]);
+                nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom));
+                dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]);
+                nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom));
+        
+                dxRay rayCD(0,lCD);     
+                dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]);
+                nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom));
+                dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]);
+                nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom));
+        
+                for (i=0;i<3;i++)
+                {
+                        if (nA[i] & nB[i])
+                        {
+                                dContactGeom *pContact = CONTACT(contact,numContacts*skip);
+                                pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2;
+                                pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2;
+                                pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2;
+#ifdef RECOMPUTE_RAYNORMAL
+                                pContact->normal[0] = -Normal[i][0];
+                                pContact->normal[1] = -Normal[i][1];
+                                pContact->normal[2] = -Normal[i][2];
+#else
+                                pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2;        //0.f;
+                                pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2;        //0.f;
+                                pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2;        //-1.f;
+                                dNormalize3(pContact->normal);
+#endif
+#ifdef DO_RAYDEPTH
+                                dxRay rayV(0,1000.f);
+                                dGeomRaySet(&rayV,      pContact->pos[0],
+                                                                        pContact->pos[1],
+                                                                        pContact->pos[2],
+                                                                        -pContact->normal[0],
+                                                                        -pContact->normal[1],
+                                                                        -pContact->normal[2]);
+                
+                                dContactGeom ContactV;
+                                if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
+                                {
+                                        pContact->depth = ContactV.depth;
+                                        numContacts++;  
+                                }
+#else
+
+            if (GetDepth == NULL)
+               {
+                                   dxRay rayV(0,1000.f);
+                                   dGeomRaySet(&rayV,   pContact->pos[0],
+                                                                           pContact->pos[1],
+                                                                           pContact->pos[2],
+                                                                           -pContact->normal[0],
+                                                                           -pContact->normal[1],
+                                                                           -pContact->normal[2]);
+                
+                                   dContactGeom ContactV;
+
+                                   if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
+                                      {
+                                           pContact->depth = ContactV.depth;
+                                           numContacts++;       
+                                      }
+               }
+            else
+               {
+                                   pContact->depth =  GetDepth(o2,
+                                      pContact->pos[0],
+                                      pContact->pos[1],
+                                      pContact->pos[2]);
+                                   numContacts++;
+               }
+
+#endif
+                                if (numContacts == numMaxContacts)
+                                        return numContacts;
+
+                        }
+                }
+        }
+
+        dCROSS(Plane,=,AC,AB);
+        dNormalize3(Plane);
+        Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
+        dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
+        numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom));
+
+        for (i=0;i<numPlaneContacts;i++)
+        {
+                if (IsOnTerrain(x,z,0,PlaneContact[i].pos))
+                {
+                        dContactGeom *pContact = CONTACT(contact,numContacts*skip);
+                        pContact->pos[0] = PlaneContact[i].pos[0];
+                        pContact->pos[1] = PlaneContact[i].pos[1];
+                        pContact->pos[2] = PlaneContact[i].pos[2];
+                        pContact->normal[0] = -PlaneContact[i].normal[0];
+                        pContact->normal[1] = -PlaneContact[i].normal[1];
+                        pContact->normal[2] = -PlaneContact[i].normal[2];
+                        pContact->depth = PlaneContact[i].depth;
+
+                        //DMESS(0);
+                        numContacts++;
+
+                        if (numContacts == numMaxContacts)
+                                        return numContacts;
+                }
+        }
+
+        dCROSS(Plane,=,BD,CD);
+        dNormalize3(Plane);
+        Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
+        dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
+        numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom));
+
+        for (i=0;i<numPlaneContacts;i++)
+        {
+                if (IsOnTerrain(x,z,1,PlaneContact[i].pos))
+                {
+                        dContactGeom *pContact = CONTACT(contact,numContacts*skip);
+                        pContact->pos[0] = PlaneContact[i].pos[0];
+                        pContact->pos[1] = PlaneContact[i].pos[1];
+                        pContact->pos[2] = PlaneContact[i].pos[2];
+                        pContact->normal[0] = -PlaneContact[i].normal[0];
+                        pContact->normal[1] = -PlaneContact[i].normal[1];
+                        pContact->normal[2] = -PlaneContact[i].normal[2];
+                        pContact->depth = PlaneContact[i].depth;
+                        //DMESS(1);
+                        numContacts++;
+
+                        if (numContacts == numMaxContacts)
+                                        return numContacts;
+                }
+        }
+
+        return numContacts;
+}
+
+int dCollideTerrainY(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));
+        dIASSERT (o1->type == dTerrainYClass);
+        int i,j;
+
+        if ((flags & 0xffff) == 0)
+                flags = (flags & 0xffff0000) | 1;
+
+        int numMaxTerrainContacts = (flags & 0xffff);
+        dxTerrainY *terrain = (dxTerrainY*) o1;
+
+        dReal aabbbak[6];
+        int gflagsbak;
+
+        dVector3 pos0;
+        int numTerrainContacts = 0;
+
+        dxPosR *bak;
+   dxPosR X1;
+        
+        if (terrain->gflags & GEOM_PLACEABLE)
+        {
+                dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos);
+                dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0);
+                dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R);
+                bak = o2->final_posr;
+                o2->final_posr = &X1;
+                memcpy(aabbbak,o2->aabb,sizeof(dReal)*6);
+                gflagsbak = o2->gflags;
+                o2->computeAABB();
+        }
+
+        int nMinX       = int(floor(o2->aabb[0] / terrain->m_vNodeLength));
+        int nMaxX       = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1;
+        int nMinZ       = int(floor(o2->aabb[4] / terrain->m_vNodeLength));
+        int nMaxZ       = int(floor(o2->aabb[5] / terrain->m_vNodeLength)) + 1;
+
+        if (terrain->m_bFinite)
+        {
+                nMinX = MAX(nMinX,0);
+                nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide);
+                nMinZ = MAX(nMinZ,0);
+                nMaxZ = MIN(nMaxZ,terrain->m_nNumNodesPerSide);
+
+                if ((nMinX >= nMaxX) || (nMinZ >= nMaxZ))
+                        goto dCollideTerrainYExit;
+        }
+        
+        dVector3 AabbTop;
+        AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2;
+        AabbTop[2] = (o2->aabb[4]+o2->aabb[5]) / 2;
+        AabbTop[1] = o2->aabb[3];
+        if (o2->type != dRayClass)
+        {
+                dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[2]) - AabbTop[1];
+                if (AabbTopDepth > 0.f)
+                {
+                        contact->depth = AabbTopDepth;
+                        dReal MaxDepth = (o2->aabb[3]-o2->aabb[2]) / 2;
+                        if (contact->depth > MaxDepth)
+                                contact->depth = MaxDepth;
+                        contact->g1 = o1;
+                        contact->g2 = o2;
+                        dOPE(contact->pos,=,AabbTop);
+                        contact->normal[0] = 0.f;
+                        contact->normal[1] = -1.f;
+                        contact->normal[2] = 0.f;
+
+                        numTerrainContacts = 1;
+                        goto dCollideTerrainYExit;
+                }
+        }
+                
+        for (i=nMinX;i<nMaxX;i++)
+        {
+                for (j=nMinZ;j<nMaxZ;j++)
+                {
+                        numTerrainContacts += terrain->dCollideTerrainUnit(
+                                i,j,o2,numMaxTerrainContacts - numTerrainContacts,
+                                flags,CONTACT(contact,numTerrainContacts*skip),skip     );
+                }
+        }
+
+        dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
+
+        for (i=0; i<numTerrainContacts; i++) 
+        {
+                CONTACT(contact,i*skip)->g1 = o1;
+                CONTACT(contact,i*skip)->g2 = o2;
+        }
+
+dCollideTerrainYExit:
+
+        if (terrain->gflags & GEOM_PLACEABLE)
+        {
+      o2->final_posr = bak;
+                memcpy(o2->aabb,aabbbak,sizeof(dReal)*6);
+                o2->gflags = gflagsbak;
+
+                for (i=0; i<numTerrainContacts; i++) 
+                {
+                        dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
+                        dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0);
+                        dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos);
+
+                        dOPE(pos0,=,CONTACT(contact,i*skip)->normal);
+                        dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0);
+                }
+        }
+
+        return numTerrainContacts;
+}
+/*
+void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
+{
+        float A[3],B[3],C[3],D[3];
+        float R[12];
+        float pos[3];
+        if (pR)
+                memcpy(R,pR,sizeof(R));
+        else
+        {
+                memset(R,0,sizeof(R));
+                R[0] = 1.f;
+                R[5] = 1.f;
+                R[10] = 1.f;
+        }
+        
+        if (ppos)
+                memcpy(pos,ppos,sizeof(pos));
+        else
+                memset(pos,0,sizeof(pos));
+        
+        float vx,vz;
+        vx = vLength * x;
+        vz = vLength * z;
+        
+        int i;
+        for (i=0;i<nNumNodesPerSide;i++)
+        {
+                for (int j=0;j<nNumNodesPerSide;j++)
+                {
+                        A[0] = i * vNodeLength + vx;
+                        A[2] = j * vNodeLength + vz;
+                        A[1] = GetHeight(i,j,nNumNodesPerSide,pHeights);
+                        B[0] = (i+1) * vNodeLength + vx;
+                        B[2] = j * vNodeLength + vz;
+                        B[1] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
+                        C[0] = i * vNodeLength + vx;
+                        C[2] = (j+1) * vNodeLength + vz;
+                        C[1] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
+                        D[0] = (i+1) * vNodeLength + vx;
+                        D[2] = (j+1) * vNodeLength + vz;
+                        D[1] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
+                        dsDrawTriangle(pos,R,C,B,A,1);
+                        dsDrawTriangle(pos,R,D,B,C,1);
+                }
+        }
+}
+*/
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
new file mode 100644
index 0000000..d47c402
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/dTerrainZ.cpp
@@ -0,0 +1,659 @@
+//Benoit CHAPEROT 2003-2004 www.jstarlab.com
+//some code inspired by Magic Software
+#include <ode/common.h>
+#include <ode/collision.h>
+#include <ode/matrix.h>
+#include <ode/rotation.h>
+#include <ode/odemath.h>
+#include "collision_kernel.h"
+#include "collision_std.h"
+#include "collision_std_internal.h"
+#include "collision_util.h"
+//#include <drawstuff/drawstuff.h>
+#include "windows.h"
+#include "ode\ode.h"
+
+#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
+#define MAXCONTACT 10
+#define TERRAINTOL 0.0f
+
+static bool IsAPowerOfTwo(int f)
+{
+        dAASSERT(f!=0);
+        while ((f&1) != 1)      
+                f >>= 1;
+
+        return (f == 1);
+}
+
+static int GetPowerOfTwo(int f)
+{
+        dAASSERT(f!=0);
+        int n = 0;
+        while ((f&1) != 1)
+        {
+                n++;
+                f >>= 1;
+        }
+        
+        return n;
+}
+
+dxTerrainZ::dxTerrainZ (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable) :
+dxGeom (space,bPlaceable)
+{
+        dIASSERT(IsAPowerOfTwo(nNumNodesPerSide));
+        dIASSERT(pHeights);
+        dIASSERT(vLength > 0.f);
+        dIASSERT(nNumNodesPerSide > 0);
+        type = dTerrainZClass;
+        m_vLength = vLength;
+        m_pHeights = new dReal[nNumNodesPerSide * nNumNodesPerSide];
+        dIASSERT(m_pHeights);
+        m_nNumNodesPerSide = nNumNodesPerSide;
+        m_vNodeLength = m_vLength / m_nNumNodesPerSide;
+        m_nNumNodesPerSideShift = GetPowerOfTwo(m_nNumNodesPerSide);
+        m_nNumNodesPerSideMask  = m_nNumNodesPerSide - 1;
+        m_vMinHeight = dInfinity;
+        m_vMaxHeight = -dInfinity;
+        m_bFinite = bFinite;
+
+        for (int i=0;i<nNumNodesPerSide * nNumNodesPerSide;i++)
+        {
+                m_pHeights[i] = pHeights[i];
+                if (m_pHeights[i] < m_vMinHeight)       m_vMinHeight = m_pHeights[i];
+                if (m_pHeights[i] > m_vMaxHeight)       m_vMaxHeight = m_pHeights[i];
+        }
+}
+
+dxTerrainZ::~dxTerrainZ()
+{
+        dIASSERT(m_pHeights);
+        delete [] m_pHeights;
+}
+
+void dxTerrainZ::computeAABB()
+{
+        if (m_bFinite)
+        {
+                if (gflags & GEOM_PLACEABLE)
+                {
+                        dReal dx[6],dy[6],dz[6];
+                        dx[0] = 0;
+                        dx[1] = final_posr->R[0] * m_vLength;
+                        dx[2] = 0;
+                        dx[3] = final_posr->R[1] * m_vLength;
+                        dx[4] = final_posr->R[2] * m_vMinHeight;
+                        dx[5] = final_posr->R[2] * m_vMaxHeight;
+                        
+                        dy[0] = 0;
+                        dy[1] = final_posr->R[4] * m_vLength;
+                        dy[2] = 0;
+                        dy[3] = final_posr->R[5] * m_vLength;
+                        dy[4] = final_posr->R[6] * m_vMinHeight;
+                        dy[5] = final_posr->R[6] * m_vMaxHeight;
+                        
+                        dz[0]  = 0;
+                        dz[1]  = final_posr->R[8] * m_vLength;
+                        dz[2]  = 0;
+                        dz[3]  = final_posr->R[9] * m_vLength;
+                        dz[4]  = final_posr->R[10] * m_vMinHeight;
+                        dz[5]  = final_posr->R[10] * m_vMaxHeight;
+
+                        aabb[0] = final_posr->pos[0] + MIN(dx[0],dx[1]) + MIN(dx[2],dx[3]) + MIN(dx[4],dx[5]);
+                        aabb[1] = final_posr->pos[0] + MAX(dx[0],dx[1]) + MAX(dx[2],dx[3]) + MAX(dx[4],dx[5]);
+                        aabb[2] = final_posr->pos[1] + MIN(dy[0],dy[1]) + MIN(dy[2],dy[3]) + MIN(dy[4],dy[5]);
+                        aabb[3] = final_posr->pos[1] + MAX(dy[0],dy[1]) + MAX(dy[2],dy[3]) + MAX(dy[4],dy[5]);
+                        aabb[4] = final_posr->pos[2] + MIN(dz[0],dz[1]) + MIN(dz[2],dz[3]) + MIN(dz[4],dz[5]);
+                        aabb[5] = final_posr->pos[2] + MAX(dz[0],dz[1]) + MAX(dz[2],dz[3]) + MAX(dz[4],dz[5]);
+                }
+                else
+                {
+                        aabb[0] = 0;
+                        aabb[1] = m_vLength;
+                        aabb[2] = 0;
+                        aabb[3] = m_vLength;
+                        aabb[4] = m_vMinHeight;
+                        aabb[5] = m_vMaxHeight;
+                }
+        }
+        else
+        {
+                if (gflags & GEOM_PLACEABLE)
+                {
+                        aabb[0] = -dInfinity;
+                        aabb[1] = dInfinity;
+                        aabb[2] = -dInfinity;
+                        aabb[3] = dInfinity;
+                        aabb[4] = -dInfinity;
+                        aabb[5] = dInfinity;
+                }
+                else
+                {
+                        aabb[0] = -dInfinity;
+                        aabb[1] = dInfinity;
+                        aabb[2] = -dInfinity;
+                        aabb[3] = dInfinity;
+                        aabb[4] = m_vMinHeight;
+                        aabb[5] = m_vMaxHeight;
+                }
+        }
+}
+
+dReal dxTerrainZ::GetHeight(int x,int y)
+{
+        return m_pHeights[      (((unsigned int)(y) & m_nNumNodesPerSideMask) << m_nNumNodesPerSideShift)
+                                        +        ((unsigned int)(x) & m_nNumNodesPerSideMask)];
+}
+
+dReal dxTerrainZ::GetHeight(dReal x,dReal y)
+{
+        int nX          = int(floor(x / m_vNodeLength));
+        int nY          = int(floor(y / m_vNodeLength));
+        dReal dx        = (x - (dReal(nX) * m_vNodeLength)) / m_vNodeLength;
+        dReal dy        = (y - (dReal(nY) * m_vNodeLength)) / m_vNodeLength;
+        dIASSERT((dx >= 0.f) && (dx <= 1.f));
+        dIASSERT((dy >= 0.f) && (dy <= 1.f));
+
+        dReal z,z0;
+        
+        if (dx + dy < 1.f)
+        {
+                z0      = GetHeight(nX,nY);
+                z       = z0    
+                        + (GetHeight(nX+1,nY) - z0) * dx
+                        + (GetHeight(nX,nY+1) - z0) * dy;
+        }
+        else
+        {
+                z0      = GetHeight(nX+1,nY+1);
+                z       = z0    
+                        + (GetHeight(nX+1,nY) - z0) * (1.f - dy)
+                        + (GetHeight(nX,nY+1) - z0) * (1.f - dx);
+        }
+
+        return z;       
+}
+
+bool dxTerrainZ::IsOnTerrain(int nx,int ny,int w,dReal *pos)
+{
+        dVector3 Min,Max;
+        Min[0] = nx * m_vNodeLength;
+        Min[1] = ny * m_vNodeLength;
+        Max[0] = (nx+1) * m_vNodeLength;
+        Max[1] = (ny+1) * m_vNodeLength;
+        dReal Tol = m_vNodeLength * TERRAINTOL;
+        
+        if ((pos[0]<Min[0]-Tol) || (pos[0]>Max[0]+Tol))
+                return false;
+
+        if ((pos[1]<Min[1]-Tol) || (pos[1]>Max[1]+Tol))
+                return false;
+
+        dReal dx        = (pos[0] - (dReal(nx) * m_vNodeLength)) / m_vNodeLength;
+        dReal dy        = (pos[1] - (dReal(ny) * m_vNodeLength)) / m_vNodeLength;
+
+        if ((w == 0) && (dx + dy > 1.f+TERRAINTOL))
+                return false;
+
+        if ((w == 1) && (dx + dy < 1.f-TERRAINTOL))
+                return false;
+
+        return true;
+}
+
+dGeomID dCreateTerrainZ(dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable)
+{
+        return new dxTerrainZ(space, pHeights,vLength,nNumNodesPerSide, bFinite, bPlaceable);
+}
+
+dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z)
+{
+        dUASSERT (g && g->type == dTerrainZClass,"argument not a terrain");
+  g->recomputePosr();
+        dxTerrainZ *t = (dxTerrainZ*) g;
+        return t->GetHeight(x,y) - z;
+}
+
+typedef dReal dGetDepthFn(dGeomID g, dReal x, dReal y, dReal z);
+#define RECOMPUTE_RAYNORMAL
+//#define DO_RAYDEPTH
+
+#define DMESS(A)        \
+                        dMessage(0,"Contact Plane (%d %d %d) %.5e %.5e (%.5e %.5e %.5e)(%.5e %.5e %.5e)).",     \
+                                        x,y,A,  \
+                                        pContact->depth,        \
+                                        dGeomSphereGetRadius(o2),               \
+                                        pContact->pos[0],       \
+                                        pContact->pos[1],       \
+                                        pContact->pos[2],       \
+                                        pContact->normal[0],    \
+                                        pContact->normal[1],    \
+                                        pContact->normal[2]);
+/*
+(z is up)
+
+y
+.
+F
+|
+C-D
+|\|
+A-B-E.x
+*/
+int dxTerrainZ::dCollideTerrainUnit(
+        int x,int y,dxGeom *o2,int numMaxContacts,
+        int flags,dContactGeom *contact, int skip)
+{
+        dColliderFn *CollideRayN;
+        dColliderFn *CollideNPlane;
+        dGetDepthFn *GetDepth;
+        int numContacts = 0;
+        int numPlaneContacts = 0;
+        int i;
+        
+        if (numContacts == numMaxContacts)
+                return numContacts;
+
+        dContactGeom PlaneContact[MAXCONTACT];
+        flags = (flags & 0xffff0000) | MAXCONTACT;
+        
+        switch (o2->type)
+        {
+        case dSphereClass:
+                CollideRayN             = dCollideRaySphere;
+                CollideNPlane   = dCollideSpherePlane;
+                GetDepth                = dGeomSpherePointDepth;
+                break;
+        case dBoxClass:
+                CollideRayN             = dCollideRayBox;
+                CollideNPlane   = dCollideBoxPlane;
+                GetDepth                = dGeomBoxPointDepth;
+                break;
+        case dCCylinderClass:
+                CollideRayN             = dCollideRayCCylinder;
+                CollideNPlane   = dCollideCCylinderPlane;
+                GetDepth                = dGeomCCylinderPointDepth;
+                break;
+        case dRayClass:
+                CollideRayN             = NULL;
+                CollideNPlane   = dCollideRayPlane;
+                GetDepth                = NULL;
+                break;
+        case dConeClass:
+                CollideRayN             = dCollideRayCone;
+                CollideNPlane   = dCollideConePlane;
+                GetDepth                = dGeomConePointDepth;
+                break;
+        default:
+                dIASSERT(0);
+        }
+
+        dReal Plane[4],lBD,lCD,lBC;
+        dVector3 A,B,C,D,BD,CD,BC,AB,AC;
+        A[0] = x * m_vNodeLength;
+        A[1] = y * m_vNodeLength;
+        A[2] = GetHeight(x,y);
+        B[0] = (x+1) * m_vNodeLength;
+        B[1] = y * m_vNodeLength;
+        B[2] = GetHeight(x+1,y);
+        C[0] = x * m_vNodeLength;
+        C[1] = (y+1) * m_vNodeLength;
+        C[2] = GetHeight(x,y+1);
+        D[0] = (x+1) * m_vNodeLength;
+        D[1] = (y+1) * m_vNodeLength;
+        D[2] = GetHeight(x+1,y+1);
+
+        dOP(BC,-,C,B);
+        lBC = dLENGTH(BC);
+        dOPEC(BC,/=,lBC);
+
+        dOP(BD,-,D,B);
+        lBD = dLENGTH(BD);
+        dOPEC(BD,/=,lBD);
+
+        dOP(CD,-,D,C);
+        lCD = dLENGTH(CD);
+        dOPEC(CD,/=,lCD);
+
+        dOP(AB,-,B,A);
+        dNormalize3(AB);
+
+        dOP(AC,-,C,A);
+        dNormalize3(AC);
+
+        if (CollideRayN)
+        {
+#ifdef RECOMPUTE_RAYNORMAL
+                dVector3 E,F;
+                dVector3 CE,FB,AD;
+                dVector3 Normal[3];
+                E[0] = (x+2) * m_vNodeLength;
+                E[1] = y * m_vNodeLength;
+                E[2] = GetHeight(x+2,y);
+                F[0] = x * m_vNodeLength;
+                F[1] = (y+2) * m_vNodeLength;
+                F[2] = GetHeight(x,y+2);
+                dOP(AD,-,D,A);
+                dNormalize3(AD);
+                dOP(CE,-,E,C);
+                dNormalize3(CE);
+                dOP(FB,-,B,F);
+                dNormalize3(FB);
+
+                //BC
+                dCROSS(Normal[0],=,AD,BC);
+                dNormalize3(Normal[0]);
+
+                //BD
+                dCROSS(Normal[1],=,CE,BD);
+                dNormalize3(Normal[1]);
+
+                //CD
+                dCROSS(Normal[2],=,FB,CD);
+                dNormalize3(Normal[2]);
+#endif          
+                int nA[3],nB[3];
+                dContactGeom ContactA[3],ContactB[3];
+                dxRay rayBC(0,lBC);     
+                dGeomRaySet(&rayBC, B[0], B[1], B[2], BC[0], BC[1], BC[2]);
+                nA[0] = CollideRayN(&rayBC,o2,flags,&ContactA[0],sizeof(dContactGeom));
+                dGeomRaySet(&rayBC, C[0], C[1], C[2], -BC[0], -BC[1], -BC[2]);
+                nB[0] = CollideRayN(&rayBC,o2,flags,&ContactB[0],sizeof(dContactGeom));
+                
+                dxRay rayBD(0,lBD);     
+                dGeomRaySet(&rayBD, B[0], B[1], B[2], BD[0], BD[1], BD[2]);
+                nA[1] = CollideRayN(&rayBD,o2,flags,&ContactA[1],sizeof(dContactGeom));
+                dGeomRaySet(&rayBD, D[0], D[1], D[2], -BD[0], -BD[1], -BD[2]);
+                nB[1] = CollideRayN(&rayBD,o2,flags,&ContactB[1],sizeof(dContactGeom));
+        
+                dxRay rayCD(0,lCD);     
+                dGeomRaySet(&rayCD, C[0], C[1], C[2], CD[0], CD[1], CD[2]);
+                nA[2] = CollideRayN(&rayCD,o2,flags,&ContactA[2],sizeof(dContactGeom));
+                dGeomRaySet(&rayCD, D[0], D[1], D[2], -CD[0], -CD[1], -CD[2]);
+                nB[2] = CollideRayN(&rayCD,o2,flags,&ContactB[2],sizeof(dContactGeom));
+        
+                for (i=0;i<3;i++)
+                {
+                        if (nA[i] & nB[i])
+                        {
+                                dContactGeom *pContact = CONTACT(contact,numContacts*skip);
+                                pContact->pos[0] = (ContactA[i].pos[0] + ContactB[i].pos[0])/2;
+                                pContact->pos[1] = (ContactA[i].pos[1] + ContactB[i].pos[1])/2;
+                                pContact->pos[2] = (ContactA[i].pos[2] + ContactB[i].pos[2])/2;
+#ifdef RECOMPUTE_RAYNORMAL
+                                pContact->normal[0] = -Normal[i][0];
+                                pContact->normal[1] = -Normal[i][1];
+                                pContact->normal[2] = -Normal[i][2];
+#else
+                                pContact->normal[0] = (ContactA[i].normal[0] + ContactB[i].normal[0])/2;        //0.f;
+                                pContact->normal[1] = (ContactA[i].normal[1] + ContactB[i].normal[1])/2;        //0.f;
+                                pContact->normal[2] = (ContactA[i].normal[2] + ContactB[i].normal[2])/2;        //-1.f;
+                                dNormalize3(pContact->normal);
+#endif
+#ifdef DO_RAYDEPTH
+                                dxRay rayV(0,1000.f);
+                                dGeomRaySet(&rayV,      pContact->pos[0],
+                                                                        pContact->pos[1],
+                                                                        pContact->pos[2],
+                                                                        -pContact->normal[0],
+                                                                        -pContact->normal[1],
+                                                                        -pContact->normal[2]);
+                
+                                dContactGeom ContactV;
+                                if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
+                                {
+                                        pContact->depth = ContactV.depth;
+                                        numContacts++;  
+                                }
+#else
+            if (GetDepth == NULL)
+               {
+                                   dxRay rayV(0,1000.f);
+                                   dGeomRaySet(&rayV,   pContact->pos[0],
+                                                                           pContact->pos[1],
+                                                                           pContact->pos[2],
+                                                                           -pContact->normal[0],
+                                                                           -pContact->normal[1],
+                                                                           -pContact->normal[2]);
+                
+                                   dContactGeom ContactV;
+                                   if (CollideRayN(&rayV,o2,flags,&ContactV,sizeof(dContactGeom)))
+                                   {
+                                           pContact->depth = ContactV.depth;
+                                           numContacts++;       
+                                   }
+               }
+            else
+               {
+                                   pContact->depth =  GetDepth(o2,
+                                   pContact->pos[0],
+                                   pContact->pos[1],
+                                   pContact->pos[2]);
+                                   numContacts++;
+               }
+#endif
+                                if (numContacts == numMaxContacts)
+                                        return numContacts;
+
+                        }
+                }
+        }
+
+        dCROSS(Plane,=,AB,AC);
+        dNormalize3(Plane);
+        Plane[3] = Plane[0] * A[0] + Plane[1] * A[1] + Plane[2] * A[2];
+        dxPlane planeABC(0,Plane[0],Plane[1],Plane[2],Plane[3]);
+        numPlaneContacts = CollideNPlane(o2,&planeABC,flags,PlaneContact,sizeof(dContactGeom));
+
+        for (i=0;i<numPlaneContacts;i++)
+        {
+                if (IsOnTerrain(x,y,0,PlaneContact[i].pos))
+                {
+                        dContactGeom *pContact = CONTACT(contact,numContacts*skip);
+                        pContact->pos[0] = PlaneContact[i].pos[0];
+                        pContact->pos[1] = PlaneContact[i].pos[1];
+                        pContact->pos[2] = PlaneContact[i].pos[2];
+                        pContact->normal[0] = -PlaneContact[i].normal[0];
+                        pContact->normal[1] = -PlaneContact[i].normal[1];
+                        pContact->normal[2] = -PlaneContact[i].normal[2];
+                        pContact->depth = PlaneContact[i].depth;
+
+                        //DMESS(0);
+                        numContacts++;
+
+                        if (numContacts == numMaxContacts)
+                                        return numContacts;
+                }
+        }
+
+        dCROSS(Plane,=,CD,BD);
+        dNormalize3(Plane);
+        Plane[3] = Plane[0] * D[0] + Plane[1] * D[1] + Plane[2] * D[2];
+        dxPlane planeDCB(0,Plane[0],Plane[1],Plane[2],Plane[3]);
+        numPlaneContacts = CollideNPlane(o2,&planeDCB,flags,PlaneContact,sizeof(dContactGeom));
+
+        for (i=0;i<numPlaneContacts;i++)
+        {
+                if (IsOnTerrain(x,y,1,PlaneContact[i].pos))
+                {
+                        dContactGeom *pContact = CONTACT(contact,numContacts*skip);
+                        pContact->pos[0] = PlaneContact[i].pos[0];
+                        pContact->pos[1] = PlaneContact[i].pos[1];
+                        pContact->pos[2] = PlaneContact[i].pos[2];
+                        pContact->normal[0] = -PlaneContact[i].normal[0];
+                        pContact->normal[1] = -PlaneContact[i].normal[1];
+                        pContact->normal[2] = -PlaneContact[i].normal[2];
+                        pContact->depth = PlaneContact[i].depth;
+                        //DMESS(1);
+                        numContacts++;
+
+                        if (numContacts == numMaxContacts)
+                                        return numContacts;
+                }
+        }
+
+        return numContacts;
+}
+
+int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip)
+{
+        dIASSERT (skip >= (int)sizeof(dContactGeom));
+        dIASSERT (o1->type == dTerrainZClass);
+        int i,j;
+
+        if ((flags & 0xffff) == 0)
+                flags = (flags & 0xffff0000) | 1;
+
+        int numMaxTerrainContacts = (flags & 0xffff);
+        dxTerrainZ *terrain = (dxTerrainZ*) o1;
+
+        dReal aabbbak[6];
+        int gflagsbak;
+
+        dVector3 pos0;
+        int numTerrainContacts = 0;
+
+        dxPosR *bak;
+   dxPosR X1;
+
+        if (terrain->gflags & GEOM_PLACEABLE)
+        {
+                dOP(pos0,-,o2->final_posr->pos,terrain->final_posr->pos);
+                dMULTIPLY1_331(X1.pos,terrain->final_posr->R,pos0);
+                dMULTIPLY1_333(X1.R,terrain->final_posr->R,o2->final_posr->R);
+                bak = o2->final_posr;
+                o2->final_posr = &X1;
+                memcpy(aabbbak,o2->aabb,sizeof(dReal)*6);
+                gflagsbak = o2->gflags;
+                o2->computeAABB();
+        }
+
+        int nMinX       = int(floor(o2->aabb[0] / terrain->m_vNodeLength));
+        int nMaxX       = int(floor(o2->aabb[1] / terrain->m_vNodeLength)) + 1;
+        int nMinY       = int(floor(o2->aabb[2] / terrain->m_vNodeLength));
+        int nMaxY       = int(floor(o2->aabb[3] / terrain->m_vNodeLength)) + 1;
+
+        if (terrain->m_bFinite)
+        {
+                nMinX = MAX(nMinX,0);
+                nMaxX = MIN(nMaxX,terrain->m_nNumNodesPerSide);
+                nMinY = MAX(nMinY,0);
+                nMaxY = MIN(nMaxY,terrain->m_nNumNodesPerSide);
+
+                if ((nMinX >= nMaxX) || (nMinY >= nMaxY))
+                        goto dCollideTerrainZExit;
+        }
+
+        dVector3 AabbTop;
+        AabbTop[0] = (o2->aabb[0]+o2->aabb[1]) / 2;
+        AabbTop[1] = (o2->aabb[2]+o2->aabb[3]) / 2;
+        AabbTop[2] = o2->aabb[5];
+        if (o2->type != dRayClass)
+        {
+                dReal AabbTopDepth = terrain->GetHeight(AabbTop[0],AabbTop[1]) - AabbTop[2];
+                if (AabbTopDepth > 0.f)
+                {
+                        contact->depth = AabbTopDepth;
+                        dReal MaxDepth = (o2->aabb[5]-o2->aabb[4]) / 2;
+                        if (contact->depth > MaxDepth)
+                                contact->depth = MaxDepth;
+                        contact->g1 = o1;
+                        contact->g2 = o2;
+                        dOPE(contact->pos,=,AabbTop);
+                        contact->normal[0] = 0.f;
+                        contact->normal[1] = 0.f;
+                        contact->normal[2] = -1.f;
+
+                        numTerrainContacts = 1;
+                        goto dCollideTerrainZExit;
+                }
+        }
+        
+        for (i=nMinX;i<nMaxX;i++)
+        {
+                for (j=nMinY;j<nMaxY;j++)
+                {
+                        numTerrainContacts += terrain->dCollideTerrainUnit(
+                                i,j,o2,numMaxTerrainContacts - numTerrainContacts,
+                                flags,CONTACT(contact,numTerrainContacts*skip),skip     );
+                }
+        }
+
+        dIASSERT(numTerrainContacts <= numMaxTerrainContacts);
+
+        for (i=0; i<numTerrainContacts; i++) 
+        {
+                CONTACT(contact,i*skip)->g1 = o1;
+                CONTACT(contact,i*skip)->g2 = o2;
+        }
+
+dCollideTerrainZExit:
+
+        if (terrain->gflags & GEOM_PLACEABLE)
+        {
+      o2->final_posr = bak;
+                memcpy(o2->aabb,aabbbak,sizeof(dReal)*6);
+                o2->gflags = gflagsbak;
+
+                for (i=0; i<numTerrainContacts; i++) 
+                {
+                        dOPE(pos0,=,CONTACT(contact,i*skip)->pos);
+                        dMULTIPLY0_331(CONTACT(contact,i*skip)->pos,terrain->final_posr->R,pos0);
+                        dOP(CONTACT(contact,i*skip)->pos,+,CONTACT(contact,i*skip)->pos,terrain->final_posr->pos);
+
+                        dOPE(pos0,=,CONTACT(contact,i*skip)->normal);
+                        dMULTIPLY0_331(CONTACT(contact,i*skip)->normal,terrain->final_posr->R,pos0);
+                }
+        }
+
+        return numTerrainContacts;
+}
+/*
+void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
+{
+        float A[3],B[3],C[3],D[3];
+        float R[12];
+        float pos[3];
+        if (pR)
+                memcpy(R,pR,sizeof(R));
+        else
+        {
+                memset(R,0,sizeof(R));
+                R[0] = 1.f;
+                R[5] = 1.f;
+                R[10] = 1.f;
+        }
+        
+        if (ppos)
+                memcpy(pos,ppos,sizeof(pos));
+        else
+                memset(pos,0,sizeof(pos));
+        
+        float vx,vz;
+        vx = vLength * x;
+        vz = vLength * z;
+        
+        int i;
+        for (i=0;i<nNumNodesPerSide;i++)
+        {
+                for (int j=0;j<nNumNodesPerSide;j++)
+                {
+                        A[0] = i * vNodeLength + vx;
+                        A[1] = j * vNodeLength + vz;
+                        A[2] = GetHeight(i,j,nNumNodesPerSide,pHeights);
+                        B[0] = (i+1) * vNodeLength + vx;
+                        B[1] = j * vNodeLength + vz;
+                        B[2] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
+                        C[0] = i * vNodeLength + vx;
+                        C[1] = (j+1) * vNodeLength + vz;
+                        C[2] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
+                        D[0] = (i+1) * vNodeLength + vx;
+                        D[1] = (j+1) * vNodeLength + vz;
+                        D[2] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
+                        dsDrawTriangle(pos,R,C,A,B,1);
+                        dsDrawTriangle(pos,R,D,C,B,1);
+                }
+        }
+}
+*/
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt b/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt
new file mode 100644
index 0000000..0428846
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/readme.txt
@@ -0,0 +1,322 @@
+Benoit CHAPEROT 2003-2004 www.jstarlab.com
+Support for terrain and cones, collision and drawing.
+
+Terrains can be with z up (dTerrainZ) or y up (dTerrainY).
+Terrains are defined by a height field.
+Terrains are now placeable.
+Terrains can now be finite or infinite (repeat themselve in the x and (y or z) directions).
+
+Terrains can potentially collide with everything that collides with planes and rays; 
+see the switch statement.
+
+Cones currently collides only with terrain and planes and rays.
+Cones, with high radius to height ratios are perfect to simulate vehicle wheels on terrains.
+
+
+
+There was an error in the depths returned by dCollideTerrain.
+Plus now contacts are not sorted according to their depths.
+Contact sorting is now supposed to be done externally. 
+Not all dCollide functions seem to sort contacts according to depth.
+Requesting a high number of contacts, sorting them and then considering only the most significant contacts
+is a good way I think to improve stability.
+* Cones Collisions with spheres, boxes, ccylinder and trimesh are now roughly approximated using sphere collisions.
+
+You will need to complete the following operations (with ODE 0.039):
+
+*** add to folder ode\src:
+
+dCone.cpp        
+dTerrainY.cpp 
+dTerrainZ.cpp 
+collision_std_internal.h
+
+On linux => edit each .cpp file and comment out     #include "windows.h"    &    #include "ode\ode.h"
+
+
+*** add to drawstuff\src\drawstuff.cpp:
+
+static void drawCone(float l, float r)
+{
+  int i;
+  float tmp,ny,nz,a,ca,sa;
+  const int n = 24;     // number of sides to the cone (divisible by 4)
+
+  a = float(M_PI*2.0)/float(n);
+  sa = (float) sin(a);
+  ca = (float) cos(a);
+
+  // draw top
+  glShadeModel (GL_FLAT);
+  ny=1; nz=0;             // normal vector = (0,ny,nz)
+  glBegin (GL_TRIANGLE_FAN);
+  glNormal3d (0,0,1);
+  glVertex3d (0,0,l);
+  for (i=0; i<=n; i++) {
+    if (i==1 || i==n/2+1)
+      setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]);
+    glNormal3d (ny*r,nz*r,0);
+    glVertex3d (ny*r,nz*r,0);
+    if (i==1 || i==n/2+1)
+      setColor (color[0],color[1],color[2],color[3]);
+
+    // rotate ny,nz
+    tmp = ca*ny - sa*nz;
+    nz = sa*ny + ca*nz;
+    ny = tmp;
+  }
+  glEnd();
+
+  // draw bottom
+  ny=1; nz=0;             // normal vector = (0,ny,nz)
+  glBegin (GL_TRIANGLE_FAN);
+  glNormal3d (0,0,-1);
+  glVertex3d (0,0,0);
+  for (i=0; i<=n; i++) {
+    if (i==1 || i==n/2+1)
+      setColor (color[0]*0.75f,color[1]*0.75f,color[2]*0.75f,color[3]);
+    glNormal3d (0,0,-1);
+    glVertex3d (ny*r,nz*r,0);
+    if (i==1 || i==n/2+1)
+      setColor (color[0],color[1],color[2],color[3]);
+
+    // rotate ny,nz
+    tmp = ca*ny + sa*nz;
+    nz = -sa*ny + ca*nz;
+    ny = tmp;
+  }
+  glEnd();
+}
+
+void dsDrawCone (const float pos[3], const float R[12], float length, float radius)
+{
+        if (current_state != 2) dsError ("drawing function called outside simulation loop");
+        setupDrawingMode();
+        glShadeModel (GL_SMOOTH);
+        setTransform (pos,R);
+        drawCone (length,radius);
+        glPopMatrix();
+        
+        if (use_shadows) {
+                setShadowDrawingMode();
+                setShadowTransform();
+                setTransform (pos,R);
+                drawCone (length,radius);
+                glPopMatrix();
+                glPopMatrix();
+                glDepthRange (0,1);
+        }
+}
+
+void dsDrawConeD (const double pos[3], const double R[12], float length, float radius)
+{
+  int i;
+  float pos2[3],R2[12];
+  for (i=0; i<3; i++) pos2[i]=(float)pos[i];
+  for (i=0; i<12; i++) R2[i]=(float)R[i];
+  dsDrawCone(pos2,R2,length,radius);
+}
+
+static float GetHeight(int x,int y,int nNumNodesPerSide,float *pHeights)
+{
+        int nNumNodesPerSideMask = nNumNodesPerSide - 1;
+        return pHeights[        (((unsigned int)(y) & nNumNodesPerSideMask) * nNumNodesPerSide)
+                                        +        ((unsigned int)(x) & nNumNodesPerSideMask)];
+}
+
+void dsDrawTerrainY(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
+{
+        float A[3],B[3],C[3],D[3];
+        float R[12];
+        float pos[3];
+        if (pR)
+                memcpy(R,pR,sizeof(R));
+        else
+        {
+                memset(R,0,sizeof(R));
+                R[0] = 1.f;
+                R[5] = 1.f;
+                R[10] = 1.f;
+        }
+        
+        if (ppos)
+                memcpy(pos,ppos,sizeof(pos));
+        else
+                memset(pos,0,sizeof(pos));
+        
+        float vx,vz;
+        vx = vLength * x;
+        vz = vLength * z;
+        
+        int i;
+        for (i=0;i<nNumNodesPerSide;i++)
+        {
+                for (int j=0;j<nNumNodesPerSide;j++)
+                {
+                        A[0] = i * vNodeLength + vx;
+                        A[2] = j * vNodeLength + vz;
+                        A[1] = GetHeight(i,j,nNumNodesPerSide,pHeights);
+                        B[0] = (i+1) * vNodeLength + vx;
+                        B[2] = j * vNodeLength + vz;
+                        B[1] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
+                        C[0] = i * vNodeLength + vx;
+                        C[2] = (j+1) * vNodeLength + vz;
+                        C[1] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
+                        D[0] = (i+1) * vNodeLength + vx;
+                        D[2] = (j+1) * vNodeLength + vz;
+                        D[1] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
+                        dsDrawTriangle(pos,R,C,B,A,1);
+                        dsDrawTriangle(pos,R,D,B,C,1);
+                }
+        }
+}
+
+void dsDrawTerrainZ(int x,int z,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos)
+{
+        float A[3],B[3],C[3],D[3];
+        float R[12];
+        float pos[3];
+        if (pR)
+                memcpy(R,pR,sizeof(R));
+        else
+        {
+                memset(R,0,sizeof(R));
+                R[0] = 1.f;
+                R[5] = 1.f;
+                R[10] = 1.f;
+        }
+        
+        if (ppos)
+                memcpy(pos,ppos,sizeof(pos));
+        else
+                memset(pos,0,sizeof(pos));
+        
+        float vx,vz;
+        vx = vLength * x;
+        vz = vLength * z;
+        
+        int i;
+        for (i=0;i<nNumNodesPerSide;i++)
+        {
+                for (int j=0;j<nNumNodesPerSide;j++)
+                {
+                        A[0] = i * vNodeLength + vx;
+                        A[1] = j * vNodeLength + vz;
+                        A[2] = GetHeight(i,j,nNumNodesPerSide,pHeights);
+                        B[0] = (i+1) * vNodeLength + vx;
+                        B[1] = j * vNodeLength + vz;
+                        B[2] = GetHeight(i+1,j,nNumNodesPerSide,pHeights);
+                        C[0] = i * vNodeLength + vx;
+                        C[1] = (j+1) * vNodeLength + vz;
+                        C[2] = GetHeight(i,j+1,nNumNodesPerSide,pHeights);
+                        D[0] = (i+1) * vNodeLength + vx;
+                        D[1] = (j+1) * vNodeLength + vz;
+                        D[2] = GetHeight(i+1,j+1,nNumNodesPerSide,pHeights);
+                        dsDrawTriangle(pos,R,C,A,B,1);
+                        dsDrawTriangle(pos,R,D,C,B,1);
+                }
+        }
+}
+
+*** add to include\drawstuff\drawstuff.h:
+void dsDrawCone (const float pos[3], const float R[12], float length, float radius);
+void dsDrawConeD (const double pos[3], const double R[12], float length, float radius);
+void dsDrawTerrainY(int x,int y,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos);
+void dsDrawTerrainZ(int x,int y,float vLength,float vNodeLength,int nNumNodesPerSide,float *pHeights,const float *pR,const float *ppos);
+
+*** add in include\ode\collision.h line 77:
+/* class numbers - each geometry object needs a unique number */
+enum {
+  dSphereClass = 0,
+  dBoxClass,
+  dCCylinderClass,
+  dCylinderClass,
+  dPlaneClass,
+  dRayClass,
+  dGeomTransformClass,
+  dTriMeshClass,
+
+  dTerrainYClass,       //here
+  dTerrainZClass,       //here
+  dConeClass,           //here
+
+  dFirstSpaceClass,
+  dSimpleSpaceClass = dFirstSpaceClass,
+  dHashSpaceClass,
+  dQuadTreeSpaceClass,
+
+  dLastSpaceClass = dQuadTreeSpaceClass,
+
+  dFirstUserClass,
+  dLastUserClass = dFirstUserClass + dMaxUserClasses - 1,
+  dGeomNumClasses
+};
+
+dGeomID dCreateTerrainY (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
+dReal dGeomTerrainYPointDepth (dGeomID g, dReal x, dReal y, dReal z);
+dGeomID dCreateTerrainZ (dSpaceID space, dReal *pHeights,dReal vLength,int nNumNodesPerSide, int bFinite, int bPlaceable);
+dReal dGeomTerrainZPointDepth (dGeomID g, dReal x, dReal y, dReal z);
+
+dGeomID dCreateCone(dSpaceID space, dReal radius, dReal length);
+void dGeomConeSetParams (dGeomID cone, dReal radius, dReal length);
+void dGeomConeGetParams (dGeomID cone, dReal *radius, dReal *length);
+dReal dGeomConePointDepth(dGeomID g, dReal x, dReal y, dReal z);
+
+*** add in include\ode\odemath.h:
+#define dOP(a,op,b,c) \
+  (a)[0] = ((b)[0]) op ((c)[0]); \
+  (a)[1] = ((b)[1]) op ((c)[1]); \
+  (a)[2] = ((b)[2]) op ((c)[2]);
+#define dOPC(a,op,b,c) \
+  (a)[0] = ((b)[0]) op (c); \
+  (a)[1] = ((b)[1]) op (c); \
+  (a)[2] = ((b)[2]) op (c);
+#define dOPE(a,op,b) \
+  (a)[0] op ((b)[0]); \
+  (a)[1] op ((b)[1]); \
+  (a)[2] op ((b)[2]);
+#define dOPEC(a,op,c) \
+  (a)[0] op (c); \
+  (a)[1] op (c); \
+  (a)[2] op (c);
+#define dLENGTH(a) \
+        (dSqrt( ((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]) ));
+#define dLENGTHSQUARED(a) \
+        (((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]));
+
+*** add in ode\src\collision_kernel.cpp function  'static void initColliders()'  next to other 'setCollider' calls:
+  setCollider (dTerrainYClass,dSphereClass,&dCollideTerrainY);
+  setCollider (dTerrainYClass,dBoxClass,&dCollideTerrainY);
+  setCollider (dTerrainYClass,dCCylinderClass,&dCollideTerrainY);
+  setCollider (dTerrainYClass,dRayClass,&dCollideTerrainY);
+  setCollider (dTerrainYClass,dConeClass,&dCollideTerrainY);
+
+  setCollider (dTerrainZClass,dSphereClass,&dCollideTerrainZ);
+  setCollider (dTerrainZClass,dBoxClass,&dCollideTerrainZ);
+  setCollider (dTerrainZClass,dCCylinderClass,&dCollideTerrainZ);
+  setCollider (dTerrainZClass,dRayClass,&dCollideTerrainZ);
+  setCollider (dTerrainZClass,dConeClass,&dCollideTerrainZ);
+
+  setCollider (dRayClass,dConeClass,&dCollideRayCone);
+  setCollider (dConeClass,dPlaneClass,&dCollideConePlane);
+  setCollider (dConeClass,dSphereClass,&dCollideConeSphere);
+  setCollider (dConeClass,dBoxClass,&dCollideConeBox);
+  setCollider (dCCylinderClass,dConeClass,&dCollideCCylinderCone);
+  setCollider (dTriMeshClass,dConeClass,&dCollideTriMeshCone);
+
+*** add in ode\src\collision_std.h:
+int dCollideTerrainY(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
+int dCollideTerrainZ(dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
+
+int dCollideConePlane (dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
+int dCollideRayCone (dxGeom *o1, dxGeom *o2, int flags,dContactGeom *contact, int skip);
+int dCollideConeSphere(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideConeBox(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideCCylinderCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+int dCollideTriMeshCone(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip);
+
+*** add dCone.cpp, dTerrainY.cpp and dTerrainZ.cpp to the the ODE_SRC variable in the makefile
+On Linux => add dCone.cpp, dTerrainY.cpp and dTerrainZ.cpp to the the libode_a_SOURCES variable in the ode/src/Makefile.am file.
+
+*** now you can now test using file test_boxstackb.cpp (to add in folder ode\test).
+
diff --git a/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp b/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp
new file mode 100644
index 0000000..f1fa592
--- /dev/null
+++ b/libraries/ode-0.9/contrib/TerrainAndCone/test_boxstackb.cpp
@@ -0,0 +1,1375 @@
+/*************************************************************************
+
+
+*                                                                                                                                                *
+
+
+* Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.                *
+
+
+* All rights reserved.  Email: russ@q12.org   Web: www.q12.org           *
+
+
+*                                                                                                                                                *
+
+
+* This library is free software; you can redistribute it and/or          *
+
+
+* modify it under the terms of EITHER:                                                           *
+
+
+*        (1) The GNU Lesser General Public License as published by the Free  *
+
+
+*                Software Foundation; either version 2.1 of the License, or (at  *
+
+
+*                your option) any later version. The text of the GNU Lesser      *
+
+
+*                General Public License is included with this library in the     *
+
+
+*                file LICENSE.TXT.                                                                                               *
+
+
+*        (2) The BSD-style license that is included with this library in         *
+
+
+*                the file LICENSE-BSD.TXT.                                                                               *
+
+
+*                                                                                                                                                *
+
+
+* This library is distributed in the hope that it will be useful,                *
+
+
+* but WITHOUT ANY WARRANTY; without even the implied warranty of                 *
+
+
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files     *
+
+
+* LICENSE.TXT and LICENSE-BSD.TXT for more details.                                      *
+
+
+*                                                                                                                                                *
+
+
+*************************************************************************/
+
+
+
+
+
+#include <ode/ode.h>
+
+
+#include <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
+
+
+
+
+
+const dReal vTerrainLength = 4.f;
+
+
+const dReal vTerrainHeight = 0.5f;
+
+
+const int TERRAINNODES = 4;
+
+
+dReal pTerrainHeights[TERRAINNODES*TERRAINNODES];
+
+
+
+
+
+dGeomID terrainZ = NULL;
+
+
+dGeomID terrainY = NULL;
+
+
+
+
+
+#define NUM 20                  // max number of objects
+
+
+#define DENSITY (5.0)           // density of all objects
+
+
+#define GPB 3                   // maximum number of geometries per body
+
+
+#define MAX_CONTACTS 4          // maximum number of contact points per body
+
+
+
+
+
+
+
+
+// dynamics and collision objects
+
+
+
+
+
+struct MyObject {
+
+
+        dBodyID body;           // the body
+
+
+        dGeomID geom[GPB];              // geometries representing this body
+
+
+};
+
+
+
+
+
+static int num=0;               // number of objects in simulation
+
+
+static int nextobj=0;           // next object to recycle if num==NUM
+
+
+static dWorldID world;
+
+
+static dSpaceID space;
+
+
+static MyObject obj[NUM];
+
+
+static dJointGroupID contactgroup;
+
+
+static int selected = -1;       // selected object
+
+
+static int show_aabb = 0;       // show geom AABBs?
+
+
+static int show_contacts = 0;   // show contact points?
+
+
+static int random_pos = 1;      // drop objects from random position?
+
+
+
+
+
+
+
+
+// this is called by dSpaceCollide when two objects in space are
+
+
+// potentially colliding.
+
+
+
+
+
+static void nearCallback (void *data, dGeomID o1, dGeomID o2)
+
+
+{
+
+
+        int i;
+
+
+        // if (o1->body && o2->body) return;
+
+
+        
+
+
+        // exit without doing anything if the two bodies are connected by a joint
+
+
+        dBodyID b1 = dGeomGetBody(o1);
+
+
+        dBodyID b2 = dGeomGetBody(o2);
+
+
+        if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
+
+
+        
+
+
+        dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
+
+
+        for (i=0; i<MAX_CONTACTS; i++) {
+
+
+                contact[i].surface.mode = dContactBounce | dContactApprox1;     //dContactSoftCFM;
+
+
+                contact[i].surface.mu = dInfinity;
+
+
+                contact[i].surface.mu2 = 0;
+
+
+                contact[i].surface.bounce = 0.1;
+
+
+                contact[i].surface.bounce_vel = 0.1;
+
+
+                contact[i].surface.soft_cfm = 0.01;
+
+
+        }
+
+
+        if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
+
+
+                sizeof(dContact))) {
+
+
+                dMatrix3 RI;
+
+
+                dRSetIdentity (RI);
+
+
+                const dReal ss[3] = {0.02,0.02,0.02};
+
+
+                for (i=0; i<numc; i++) {
+
+
+                        dJointID c = dJointCreateContact (world,contactgroup,contact+i);
+
+
+                        dJointAttach (c,b1,b2);
+
+
+                        if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
+
+
+                }
+
+
+        }
+
+
+}
+
+
+
+
+
+
+
+
+// start simulation - set viewpoint
+
+
+
+
+
+static void start()
+
+
+{
+
+
+        static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
+
+
+        static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
+
+
+        dsSetViewpoint (xyz,hpr);
+
+
+        printf ("To drop another object, press:\n");
+
+
+        printf ("   b for box.\n");
+
+
+        printf ("   s for sphere.\n");
+
+
+        printf ("   c for cylinder.\n");
+
+
+        printf ("   x for a composite object.\n");
+
+
+        printf ("To select an object, press space.\n");
+
+
+        printf ("To disable the selected object, press d.\n");
+
+
+        printf ("To enable the selected object, press e.\n");
+
+
+        printf ("To toggle showing the geom AABBs, press a.\n");
+
+
+        printf ("To toggle showing the contact points, press t.\n");
+
+
+        printf ("To toggle dropping from random position/orientation, press r.\n");
+
+
+}
+
+
+
+
+
+
+
+
+char locase (char c)
+
+
+{
+
+
+        if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
+
+
+        else return c;
+
+
+}
+
+
+
+
+
+
+
+
+// called when a key pressed
+
+
+
+
+
+static void command (int cmd)
+
+
+{
+
+
+        int i,j,k;
+
+
+        dReal sides[3];
+
+
+        dMass m;
+
+
+        
+
+
+        cmd = locase (cmd);
+
+
+        if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
+
+
+                /* || cmd == 'l' */) {
+
+
+                if (num < NUM) {
+
+
+                        i = num;
+
+
+                        num++;
+
+
+                }
+
+
+                else {
+
+
+                        i = nextobj;
+
+
+                        nextobj++;
+
+
+                        if (nextobj >= num) nextobj = 0;
+
+
+                        
+
+
+                        // destroy the body and geoms for slot i
+
+
+                        dBodyDestroy (obj[i].body);
+
+
+                        for (k=0; k < GPB; k++) {
+
+
+                                if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
+
+
+                        }
+
+
+                        memset (&obj[i],0,sizeof(obj[i]));
+
+
+                }
+
+
+                
+
+
+                obj[i].body = dBodyCreate (world);
+
+
+                for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
+
+
+                
+
+
+                dMatrix3 R;
+
+
+                if (random_pos) {
+
+
+                        dBodySetPosition (obj[i].body,
+
+
+                                dRandReal()*2-1,dRandReal()*2+1,dRandReal()+3);
+
+
+                        dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+
+
+                                dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+
+
+                }
+
+
+                else {
+
+
+                        dReal maxheight = 0;
+
+
+                        for (k=0; k<num; k++) {
+
+
+                                const dReal *pos = dBodyGetPosition (obj[k].body);
+
+
+                                if (pos[2] > maxheight) maxheight = pos[2];
+
+
+                        }
+
+
+                        dBodySetPosition (obj[i].body, 0,maxheight+1,maxheight+3);
+
+
+                        dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
+
+
+                }
+
+
+                dBodySetRotation (obj[i].body,R);
+
+
+                dBodySetData (obj[i].body,(void*) i);
+
+
+                
+
+
+                if (cmd == 'b') {
+
+
+                        dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
+
+
+                        obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
+
+
+                }
+
+
+                else if (cmd == 'c') {
+
+
+                        sides[0] *= 0.5;
+
+
+                        dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
+
+
+                        obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
+
+
+                }
+
+
+                /*
+
+
+                // cylinder option not yet implemented
+
+
+                else if (cmd == 'l') {
+
+
+                sides[1] *= 0.5;
+
+
+                dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
+
+
+                obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
+
+
+                }
+
+
+                */
+
+
+                else if (cmd == 's') {
+
+
+                        sides[0] *= 0.5;
+
+
+                        dMassSetSphere (&m,DENSITY,sides[0]);
+
+
+                        obj[i].geom[0] = dCreateSphere (space,sides[0]);
+
+
+                }
+
+
+                else if (cmd == 'x') {
+
+
+                        dGeomID g2[GPB];                // encapsulated geometries
+
+
+                        dReal dpos[GPB][3];     // delta-positions for encapsulated geometries
+
+
+                        
+
+
+                        // start accumulating masses for the encapsulated geometries
+
+
+                        dMass m2;
+
+
+                        dMassSetZero (&m);
+
+
+                        
+
+
+                        // set random delta positions
+
+
+                        for (j=0; j<GPB; j++) {
+
+
+                                for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
+
+
+                        }
+
+
+                        
+
+
+                        for (k=0; k<GPB; k++) {
+
+
+                                obj[i].geom[k] = dCreateGeomTransform (space);
+
+
+                                dGeomTransformSetCleanup (obj[i].geom[k],1);
+
+
+                                if (k==0) {
+
+
+                                        dReal radius = dRandReal()*0.25+0.05;
+
+
+                                        g2[k] = dCreateSphere (0,radius);
+
+
+                                        dMassSetSphere (&m2,DENSITY,radius);
+
+
+                                }
+
+
+                                else if (k==1) {
+
+
+                                        g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
+
+
+                                        dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
+
+
+                                }
+
+
+                                else {
+
+
+                                        dReal radius = dRandReal()*0.1+0.05;
+
+
+                                        dReal length = dRandReal()*1.0+0.1;
+
+
+                                        g2[k] = dCreateCCylinder (0,radius,length);
+
+
+                                        dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
+
+
+                                }
+
+
+                                dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
+
+
+                                
+
+
+                                // set the transformation (adjust the mass too)
+
+
+                                dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
+
+
+                                dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
+
+
+                                dMatrix3 Rtx;
+
+
+                                dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+
+
+                                        dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+
+
+                                dGeomSetRotation (g2[k],Rtx);
+
+
+                                dMassRotate (&m2,Rtx);
+
+
+                                
+
+
+                                // add to the total mass
+
+
+                                dMassAdd (&m,&m2);
+
+
+                        }
+
+
+                        
+
+
+                        // move all encapsulated objects so that the center of mass is (0,0,0)
+
+
+                        for (k=0; k<2; k++) {
+
+
+                                dGeomSetPosition (g2[k],
+
+
+                                        dpos[k][0]-m.c[0],
+
+
+                                        dpos[k][1]-m.c[1],
+
+
+                                        dpos[k][2]-m.c[2]);
+
+
+                        }
+
+
+                        dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
+
+
+                }
+
+
+                
+
+
+                for (k=0; k < GPB; k++) {
+
+
+                        if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
+
+
+                }
+
+
+                
+
+
+                dBodySetMass (obj[i].body,&m);
+
+
+  }
+
+
+  
+
+
+  if (cmd == ' ') {
+
+
+          selected++;
+
+
+          if (selected >= num) selected = 0;
+
+
+          if (selected < 0) selected = 0;
+
+
+  }
+
+
+  else if (cmd == 'd' && selected >= 0 && selected < num) {
+
+
+          dBodyDisable (obj[selected].body);
+
+
+  }
+
+
+  else if (cmd == 'e' && selected >= 0 && selected < num) {
+
+
+          dBodyEnable (obj[selected].body);
+
+
+  }
+
+
+  else if (cmd == 'a') {
+
+
+          show_aabb ^= 1;
+
+
+  }
+
+
+  else if (cmd == 't') {
+
+
+          show_contacts ^= 1;
+
+
+  }
+
+
+  else if (cmd == 'r') {
+
+
+          random_pos ^= 1;
+
+
+  }
+
+
+}
+
+
+
+
+
+
+
+
+// draw a geom
+
+
+
+
+
+void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
+
+
+{
+
+
+        int i;
+
+
+        
+
+
+        if (!g) return;
+
+
+        if (!pos) pos = dGeomGetPosition (g);
+
+
+        if (!R) R = dGeomGetRotation (g);
+
+
+        
+
+
+        int type = dGeomGetClass (g);
+
+
+        if (type == dBoxClass) {
+
+
+                dVector3 sides;
+
+
+                dGeomBoxGetLengths (g,sides);
+
+
+                dsDrawBox (pos,R,sides);
+
+
+        }
+
+
+        else if (type == dSphereClass) {
+
+
+                dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
+
+
+        }
+
+
+        else if (type == dCCylinderClass) {
+
+
+                dReal radius,length;
+
+
+                dGeomCCylinderGetParams (g,&radius,&length);
+
+
+                dsDrawCappedCylinder (pos,R,length,radius);
+
+
+        }
+
+
+        /*
+
+
+        // cylinder option not yet implemented
+
+
+        else if (type == dCylinderClass) {
+
+
+        dReal radius,length;
+
+
+        dGeomCylinderGetParams (g,&radius,&length);
+
+
+        dsDrawCylinder (pos,R,length,radius);
+
+
+        }
+
+
+        */
+
+
+        else if (type == dGeomTransformClass) {
+
+
+                dGeomID g2 = dGeomTransformGetGeom (g);
+
+
+                const dReal *pos2 = dGeomGetPosition (g2);
+
+
+                const dReal *R2 = dGeomGetRotation (g2);
+
+
+                dVector3 actual_pos;
+
+
+                dMatrix3 actual_R;
+
+
+                dMULTIPLY0_331 (actual_pos,R,pos2);
+
+
+                actual_pos[0] += pos[0];
+
+
+                actual_pos[1] += pos[1];
+
+
+                actual_pos[2] += pos[2];
+
+
+                dMULTIPLY0_333 (actual_R,R,R2);
+
+
+                drawGeom (g2,actual_pos,actual_R,0);
+
+
+        }
+
+
+        
+
+
+        if (show_aabb) {
+
+
+                // draw the bounding box for this geom
+
+
+                dReal aabb[6];
+
+
+                dGeomGetAABB (g,aabb);
+
+
+                dVector3 bbpos;
+
+
+                for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
+
+
+                dVector3 bbsides;
+
+
+                for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
+
+
+                dMatrix3 RI;
+
+
+                dRSetIdentity (RI);
+
+
+                dsSetColorAlpha (1,0,0,0.5);
+
+
+                dsDrawBox (bbpos,RI,bbsides);
+
+
+        }
+
+
+}
+
+
+
+
+
+
+
+
+// simulation loop
+
+
+
+
+
+static void simLoop (int pause)
+
+
+{
+
+
+        dsSetColor (0,0,2);
+
+
+        dSpaceCollide (space,0,&nearCallback);
+
+
+        if (!pause) dWorldStep (world,0.05);
+
+
+
+
+
+        dAASSERT(terrainY);
+
+
+        dAASSERT(terrainZ);
+
+
+        dsSetColor (0,1,0);
+
+
+        dsDrawTerrainY(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainY),dGeomGetPosition(terrainY));
+
+
+        dsDrawTerrainZ(0,0,vTerrainLength,vTerrainLength/TERRAINNODES,TERRAINNODES,pTerrainHeights,dGeomGetRotation(terrainZ),dGeomGetPosition(terrainZ));
+
+
+
+
+
+        if (show_aabb) 
+
+
+        {
+
+
+                dReal aabb[6];
+
+
+                dGeomGetAABB (terrainY,aabb);
+
+
+                dVector3 bbpos;
+
+
+                int i;
+
+
+                for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
+
+
+                dVector3 bbsides;
+
+
+                for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
+
+
+                dMatrix3 RI;
+
+
+                dRSetIdentity (RI);
+
+
+                dsSetColorAlpha (1,0,0,0.5);
+
+
+                dsDrawBox (bbpos,RI,bbsides);
+
+
+
+
+
+                dGeomGetAABB (terrainZ,aabb);
+
+
+                for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
+
+
+                for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
+
+
+                dsDrawBox (bbpos,RI,bbsides);
+
+
+        }
+
+
+
+
+
+        dsSetColor (1,1,0);
+
+
+        
+
+
+        // remove all contact joints
+
+
+        dJointGroupEmpty (contactgroup);
+
+
+        
+
+
+        dsSetColor (1,1,0);
+
+
+        dsSetTexture (DS_WOOD);
+
+
+        for (int i=0; i<num; i++) {
+
+
+                for (int j=0; j < GPB; j++) {
+
+
+                        if (i==selected) {
+
+
+                                dsSetColor (0,0.7,1);
+
+
+                        }
+
+
+                        else if (! dBodyIsEnabled (obj[i].body)) {
+
+
+                                dsSetColor (1,0,0);
+
+
+                        }
+
+
+                        else {
+
+
+                                dsSetColor (1,1,0);
+
+
+                        }
+
+
+                        drawGeom (obj[i].geom[j],0,0,show_aabb);
+
+
+                }
+
+
+        }
+
+
+}
+
+
+
+
+
+
+
+
+int main (int argc, char **argv)
+
+
+{
+
+
+        // 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); //-0.5
+
+
+        dWorldSetCFM (world,1e-5);
+
+
+        dCreatePlane (space,0,0,1,0);
+
+
+        memset (obj,0,sizeof(obj));
+
+
+
+
+
+        for (int i=0;i<TERRAINNODES*TERRAINNODES;i++)   pTerrainHeights[i] = vTerrainHeight * dRandReal();
+
+
+        terrainY = dCreateTerrainY(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);
+
+
+        terrainZ = dCreateTerrainZ(space,pTerrainHeights,vTerrainLength,TERRAINNODES,1,1);
+
+
+
+
+
+        dMatrix3 R;
+
+
+        dRFromZAxis(R, 0.2f, 0.2f, 0.2f);
+
+
+        dGeomSetPosition(terrainY,0.f,0.f,0.5f);
+
+
+        dGeomSetRotation(terrainY,R);
+
+
+        dGeomSetPosition(terrainZ,0.f,0.f,0.5f);
+
+
+        dGeomSetRotation(terrainZ,R);
+
+
+
+
+
+        // run simulation
+
+
+        dsSimulationLoop (argc,argv,352,288,&fn);
+
+
+        
+
+
+        dJointGroupDestroy (contactgroup);
+
+
+        dSpaceDestroy (space);
+
+
+        dWorldDestroy (world);
+
+
+        
+
+
+        return 0;
+
+
+}
+
+
diff --git a/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp b/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp
new file mode 100644
index 0000000..215da25
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp
@@ -0,0 +1,1445 @@
+#include <ode/ode.h>
+#include "dCylinder.h"
+// given a pointer `p' to a dContactGeom, return the dContactGeom at
+// p + skip bytes.
+
+struct dxCylinder {     // cylinder
+  dReal radius,lz;      // radius, length along y axis //
+};
+
+int dCylinderClassUser = -1;
+
+#define NUMC_MASK (0xffff)
+
+#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
+
+/////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////circleIntersection//////////////////////////////////////////////////
+//this does following:
+//takes two circles as normals to planes n1,n2, center points cp1,cp2,and radiuses r1,r2
+//finds line on which circles' planes intersect
+//finds four points O1,O2 - intersection between the line and sphere with center cp1 radius r1
+//                                      O3,O4 - intersection between the line and sphere with center cp2 radius r2
+//returns false if there is no intersection
+//computes distances O1-O3, O1-O4, O2-O3, O2-O4
+//in "point" returns mean point between intersection points with smallest distance
+/////////////////////////////////////////////////////////////////////////////////////////////////
+inline bool circleIntersection(const dReal* n1,const dReal* cp1,dReal r1,const dReal* n2,const dReal* cp2,dReal r2,dVector3 point){
+dReal c1=dDOT14(cp1,n1);
+dReal c2=dDOT14(cp2,n2);
+dReal cos=dDOT44(n1,n2);
+dReal cos_2=cos*cos;
+dReal sin_2=1-cos_2;
+dReal p1=(c1-c2*cos)/sin_2;
+dReal p2=(c2-c1*cos)/sin_2;
+dVector3 lp={p1*n1[0]+p2*n2[0],p1*n1[4]+p2*n2[4],p1*n1[8]+p2*n2[8]};
+dVector3 n;
+dCROSS144(n,=,n1,n2);
+dVector3 LC1={lp[0]-cp1[0],lp[1]-cp1[1],lp[2]-cp1[2]};
+dVector3 LC2={lp[0]-cp2[0],lp[1]-cp2[1],lp[2]-cp2[2]};
+dReal A,B,C,B_A,B_A_2,D;
+dReal t1,t2,t3,t4;
+A=dDOT(n,n);
+B=dDOT(LC1,n);
+C=dDOT(LC1,LC1)-r1*r1;
+B_A=B/A;
+B_A_2=B_A*B_A;
+D=B_A_2-C;
+if(D<0.f){      //somewhat strange solution 
+                        //- it is needed to set some 
+                        //axis to sepparate cylinders
+                        //when their edges approach
+        t1=-B_A+sqrtf(-D);
+        t2=-B_A-sqrtf(-D);
+//      return false;
+        }
+else{
+t1=-B_A-sqrtf(D);
+t2=-B_A+sqrtf(D);
+}
+B=dDOT(LC2,n);
+C=dDOT(LC2,LC2)-r2*r2;
+B_A=B/A;
+B_A_2=B_A*B_A;
+D=B_A_2-C;
+
+if(D<0.f) {
+        t3=-B_A+sqrtf(-D);
+        t4=-B_A-sqrtf(-D);
+//      return false;
+        }
+else{
+t3=-B_A-sqrtf(D);
+t4=-B_A+sqrtf(D);
+}
+dVector3 O1={lp[0]+n[0]*t1,lp[1]+n[1]*t1,lp[2]+n[2]*t1};
+dVector3 O2={lp[0]+n[0]*t2,lp[1]+n[1]*t2,lp[2]+n[2]*t2};
+
+dVector3 O3={lp[0]+n[0]*t3,lp[1]+n[1]*t3,lp[2]+n[2]*t3};
+dVector3 O4={lp[0]+n[0]*t4,lp[1]+n[1]*t4,lp[2]+n[2]*t4};
+
+dVector3 L1_3={O3[0]-O1[0],O3[1]-O1[1],O3[2]-O1[2]};
+dVector3 L1_4={O4[0]-O1[0],O4[1]-O1[1],O4[2]-O1[2]};
+
+dVector3 L2_3={O3[0]-O2[0],O3[1]-O2[1],O3[2]-O2[2]};
+dVector3 L2_4={O4[0]-O2[0],O4[1]-O2[1],O4[2]-O2[2]};
+
+
+dReal l1_3=dDOT(L1_3,L1_3);
+dReal l1_4=dDOT(L1_4,L1_4);
+
+dReal l2_3=dDOT(L2_3,L2_3);
+dReal l2_4=dDOT(L2_4,L2_4);
+
+
+if (l1_3<l1_4)
+        if(l2_3<l2_4)
+                if(l1_3<l2_3)
+                        {
+                        //l1_3;
+                        point[0]=0.5f*(O1[0]+O3[0]);
+                        point[1]=0.5f*(O1[1]+O3[1]);
+                        point[2]=0.5f*(O1[2]+O3[2]);
+                        }
+                else{
+                        //l2_3;
+                        point[0]=0.5f*(O2[0]+O3[0]);
+                        point[1]=0.5f*(O2[1]+O3[1]);
+                        point[2]=0.5f*(O2[2]+O3[2]);
+                        }
+        else
+                if(l1_3<l2_4)
+                        {
+                        //l1_3;
+                        point[0]=0.5f*(O1[0]+O3[0]);
+                        point[1]=0.5f*(O1[1]+O3[1]);
+                        point[2]=0.5f*(O1[2]+O3[2]);
+                        }
+                else{
+                        //l2_4;
+                        point[0]=0.5f*(O2[0]+O4[0]);
+                        point[1]=0.5f*(O2[1]+O4[1]);
+                        point[2]=0.5f*(O2[2]+O4[2]);
+                        }
+
+else
+        if(l2_3<l2_4)
+                if(l1_4<l2_3)
+                        {
+                        //l1_4;
+                        point[0]=0.5f*(O1[0]+O4[0]);
+                        point[1]=0.5f*(O1[1]+O4[1]);
+                        point[2]=0.5f*(O1[2]+O4[2]);
+                        }
+                else{
+                        //l2_3;
+                        point[0]=0.5f*(O2[0]+O3[0]);
+                        point[1]=0.5f*(O2[1]+O3[1]);
+                        point[2]=0.5f*(O2[2]+O3[2]);
+                        }
+        else
+                if(l1_4<l2_4)
+                        {
+                        //l1_4;
+                        point[0]=0.5f*(O1[0]+O4[0]);
+                        point[1]=0.5f*(O1[1]+O4[1]);
+                        point[2]=0.5f*(O1[2]+O4[2]);
+                        }
+                else{
+                        //l2_4;
+                        point[0]=0.5f*(O2[0]+O4[0]);
+                        point[1]=0.5f*(O2[1]+O4[1]);
+                        point[2]=0.5f*(O2[2]+O4[2]);
+                        }
+
+return true;
+}
+
+
+
+
+void lineClosestApproach (const dVector3 pa, const dVector3 ua,
+                                 const dVector3 pb, const dVector3 ub,
+                                 dReal *alpha, dReal *beta)
+{
+  dVector3 p;
+  p[0] = pb[0] - pa[0];
+  p[1] = pb[1] - pa[1];
+  p[2] = pb[2] - pa[2];
+  dReal uaub = dDOT(ua,ub);
+  dReal q1 =  dDOT(ua,p);
+  dReal q2 = -dDOT(ub,p);
+  dReal d = 1-uaub*uaub;
+  if (d <= 0) {
+    // @@@ this needs to be made more robust
+    *alpha = 0;
+    *beta  = 0;
+  }
+  else {
+    d = dRecip(d);
+    *alpha = (q1 + uaub*q2)*d;
+    *beta  = (uaub*q1 + q2)*d;
+  }
+}
+
+
+// @@@ some stuff to optimize here, reuse code in contact point calculations.
+
+extern "C" int dCylBox (const dVector3 p1, const dMatrix3 R1,
+                        const dReal radius,const dReal lz, const dVector3 p2,
+                        const dMatrix3 R2, const dVector3 side2,
+                        dVector3 normal, dReal *depth, int *code,
+                        int maxc, dContactGeom *contact, int skip)
+{
+  dVector3 p,pp,normalC;
+  const dReal *normalR = 0;
+  dReal B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
+    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,sQ21,sQ22,sQ23;
+  int i,invert_normal;
+
+  // get vector from centers of box 1 to box 2, relative to box 1
+  p[0] = p2[0] - p1[0];
+  p[1] = p2[1] - p1[1];
+  p[2] = p2[2] - p1[2];
+  dMULTIPLY1_331 (pp,R1,p);             // get pp = p relative to body 1
+
+  // get side lengths / 2
+  //A1 =radius; A2 = lz*REAL(0.5); A3 = radius;
+  dReal hlz=lz/2.f;
+  B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
+
+  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
+  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
+  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
+
+  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
+  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
+  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
+
+  
+  //   * see if the axis separates the box with cylinder. if so, return 0.
+  //   * find the depth of the penetration along the separating axis (s2)
+  //   * if this is the largest depth so far, record it.
+  // the normal vector will be set to the separating axis with the smallest
+  // depth. note: normalR is set to point to a column of R1 or R2 if that is
+  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
+  // set to a vector relative to body 1. invert_normal is 1 if the sign of
+  // the normal should be flipped.
+
+#define TEST(expr1,expr2,norm,cc) \
+  s2 = dFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  if (s2 > s) { \
+    s = s2; \
+    normalR = norm; \
+    invert_normal = ((expr1) < 0); \
+    *code = (cc); \
+  }
+
+  s = -dInfinity;
+  invert_normal = 0;
+  *code = 0;
+
+  // separating axis = cylinder ax u2
+ //used when a box vertex touches a flat face of the cylinder
+  TEST (pp[1],(hlz + B1*Q21 + B2*Q22 + B3*Q23),R1+1,0);
+
+
+  // separating axis = box axis v1,v2,v3
+  //used when cylinder edge touches box face
+  //there is two ways to compute sQ: sQ21=sqrtf(1.f-Q21*Q21); or sQ21=sqrtf(Q23*Q23+Q22*Q22); 
+  //if we did not need Q23 and Q22 the first way might be used to quiken the routine but then it need to 
+  //check if Q21<=1.f, becouse it may slightly exeed 1.f.
+
+ 
+  sQ21=sqrtf(Q23*Q23+Q22*Q22);
+  TEST (dDOT41(R2+0,p),(radius*sQ21 + hlz*Q21 + B1),R2+0,1);
+
+  sQ22=sqrtf(Q23*Q23+Q21*Q21);
+  TEST (dDOT41(R2+1,p),(radius*sQ22 + hlz*Q22 + B2),R2+1,2);
+
+  sQ23=sqrtf(Q22*Q22+Q21*Q21);
+  TEST (dDOT41(R2+2,p),(radius*sQ23 + hlz*Q23 + B3),R2+2,3);
+
+ 
+#undef TEST
+#define TEST(expr1,expr2,n1,n2,n3,cc) \
+  s2 = dFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  if (s2 > s) { \
+      s = s2; \
+          normalR = 0; \
+      normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
+      invert_normal = ((expr1) < 0); \
+      *code = (cc); \
+    } 
+ 
+
+
+// separating axis is a normal to the cylinder axis passing across the nearest box vertex
+//used when a box vertex touches the lateral surface of the cylinder
+
+dReal proj,boxProj,cos,sin,cos1,cos3;
+dVector3 tAx,Ax,pb;
+{
+//making Ax which is perpendicular to cyl ax to box position//
+proj=dDOT14(p2,R1+1)-dDOT14(p1,R1+1);
+
+Ax[0]=p2[0]-p1[0]-R1[1]*proj;
+Ax[1]=p2[1]-p1[1]-R1[5]*proj;
+Ax[2]=p2[2]-p1[2]-R1[9]*proj;
+dNormalize3(Ax);
+//using Ax find box vertex which is nearest to the cylinder axis
+        dReal sign;
+    
+    for (i=0; i<3; i++) pb[i] = p2[i];
+    sign = (dDOT14(Ax,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
+    sign = (dDOT14(Ax,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1];
+    sign = (dDOT14(Ax,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];
+
+//building axis which is normal to cylinder ax to the nearest box vertex
+proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1);
+
+Ax[0]=pb[0]-p1[0]-R1[1]*proj;
+Ax[1]=pb[1]-p1[1]-R1[5]*proj;
+Ax[2]=pb[2]-p1[2]-R1[9]*proj;
+dNormalize3(Ax);
+}
+
+boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+
+                dFabs(dDOT14(Ax,R2+1)*B2)+
+                dFabs(dDOT14(Ax,R2+2)*B3);
+
+TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(radius+boxProj),Ax[0],Ax[1],Ax[2],4);
+
+
+//next three test used to handle collisions between cylinder circles and box ages
+proj=dDOT14(p1,R2+0)-dDOT14(p2,R2+0);
+
+tAx[0]=-p1[0]+p2[0]+R2[0]*proj;
+tAx[1]=-p1[1]+p2[1]+R2[4]*proj;
+tAx[2]=-p1[2]+p2[2]+R2[8]*proj;
+dNormalize3(tAx);
+
+//now tAx is normal to first ax of the box to cylinder center
+//making perpendicular to tAx lying in the plane which is normal to the cylinder axis
+//it is tangent in the point where projection of tAx on cylinder's ring intersect edge circle
+
+cos=dDOT14(tAx,R1+0);
+sin=dDOT14(tAx,R1+2);
+tAx[0]=R1[2]*cos-R1[0]*sin;
+tAx[1]=R1[6]*cos-R1[4]*sin;
+tAx[2]=R1[10]*cos-R1[8]*sin;
+
+
+//use cross between tAx and first ax of the box as separating axix 
+
+dCROSS114(Ax,=,tAx,R2+0);
+dNormalize3(Ax);
+
+boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+
+                dFabs(dDOT14(Ax,R2+0)*B1)+
+                dFabs(dDOT14(Ax,R2+2)*B3);
+
+  cos=dFabs(dDOT14(Ax,R1+1));
+  cos1=dDOT14(Ax,R1+0);
+  cos3=dDOT14(Ax,R1+2);
+  sin=sqrtf(cos1*cos1+cos3*cos3);
+
+TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],5);
+
+
+//same thing with the second axis of the box
+proj=dDOT14(p1,R2+1)-dDOT14(p2,R2+1);
+
+tAx[0]=-p1[0]+p2[0]+R2[1]*proj;
+tAx[1]=-p1[1]+p2[1]+R2[5]*proj;
+tAx[2]=-p1[2]+p2[2]+R2[9]*proj;
+dNormalize3(tAx);
+
+
+cos=dDOT14(tAx,R1+0);
+sin=dDOT14(tAx,R1+2);
+tAx[0]=R1[2]*cos-R1[0]*sin;
+tAx[1]=R1[6]*cos-R1[4]*sin;
+tAx[2]=R1[10]*cos-R1[8]*sin;
+
+dCROSS114(Ax,=,tAx,R2+1);
+dNormalize3(Ax);
+
+boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+
+                dFabs(dDOT14(Ax,R2+1)*B2)+
+                dFabs(dDOT14(Ax,R2+2)*B3);
+
+  cos=dFabs(dDOT14(Ax,R1+1));
+  cos1=dDOT14(Ax,R1+0);
+  cos3=dDOT14(Ax,R1+2);
+  sin=sqrtf(cos1*cos1+cos3*cos3);
+TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],6);
+
+//same thing with the third axis of the box
+proj=dDOT14(p1,R2+2)-dDOT14(p2,R2+2);
+
+Ax[0]=-p1[0]+p2[0]+R2[2]*proj;
+Ax[1]=-p1[1]+p2[1]+R2[6]*proj;
+Ax[2]=-p1[2]+p2[2]+R2[10]*proj;
+dNormalize3(tAx);
+
+cos=dDOT14(tAx,R1+0);
+sin=dDOT14(tAx,R1+2);
+tAx[0]=R1[2]*cos-R1[0]*sin;
+tAx[1]=R1[6]*cos-R1[4]*sin;
+tAx[2]=R1[10]*cos-R1[8]*sin;
+
+dCROSS114(Ax,=,tAx,R2+2);
+dNormalize3(Ax);
+boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+
+                dFabs(dDOT14(Ax,R2+2)*B3)+
+                dFabs(dDOT14(Ax,R2+0)*B1);
+
+  cos=dFabs(dDOT14(Ax,R1+1));
+  cos1=dDOT14(Ax,R1+0);
+  cos3=dDOT14(Ax,R1+2);
+  sin=sqrtf(cos1*cos1+cos3*cos3);
+TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],7);
+
+
+#undef TEST
+
+// note: cross product axes need to be scaled when s is computed.
+// normal (n1,n2,n3) is relative to box 1.
+
+#define TEST(expr1,expr2,n1,n2,n3,cc) \
+  s2 = dFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
+  if (l > 0) { \
+    s2 /= l; \
+    if (s2 > s) { \
+      s = s2; \
+      normalR = 0; \
+      normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
+      invert_normal = ((expr1) < 0); \
+      *code = (cc); \
+    } \
+  }
+
+//crosses between cylinder axis and box axes
+  // separating axis = u2 x (v1,v2,v3)
+  TEST(pp[0]*R31-pp[2]*R11,(radius+B2*Q23+B3*Q22),R31,0,-R11,8);
+  TEST(pp[0]*R32-pp[2]*R12,(radius+B1*Q23+B3*Q21),R32,0,-R12,9);
+  TEST(pp[0]*R33-pp[2]*R13,(radius+B1*Q22+B2*Q21),R33,0,-R13,10);
+
+
+#undef TEST
+
+  // if we get to this point, the boxes interpenetrate. compute the normal
+  // in global coordinates.
+  if (normalR) {
+    normal[0] = normalR[0];
+    normal[1] = normalR[4];
+    normal[2] = normalR[8];
+  }
+  else {
+          if(*code>7) dMULTIPLY0_331 (normal,R1,normalC);
+          else {normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2];}
+  }
+  if (invert_normal) {
+    normal[0] = -normal[0];
+    normal[1] = -normal[1];
+    normal[2] = -normal[2];
+  }
+  *depth = -s;
+
+  // compute contact point(s)
+
+  if (*code > 7) {
+ //find point on the cylinder pa deepest along normal
+    dVector3 pa;
+    dReal sign, cos1,cos3,factor;
+
+
+    for (i=0; i<3; i++) pa[i] = p1[i];
+
+        cos1 = dDOT14(normal,R1+0);
+        cos3 = dDOT14(normal,R1+2) ;
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+
+        cos1/=factor;
+        cos3/=factor;
+        
+    for (i=0; i<3; i++) pa[i] += cos1 * radius * R1[i*4];
+
+    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) pa[i] += sign * hlz * R1[i*4+1];
+
+  
+    for (i=0; i<3; i++) pa[i] += cos3 * radius * R1[i*4+2];
+
+    // find vertex of the box  deepest along normal 
+    dVector3 pb;
+    for (i=0; i<3; i++) pb[i] = p2[i];
+    sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
+    sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1];
+    sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];
+
+
+    dReal alpha,beta;
+    dVector3 ua,ub;
+    for (i=0; i<3; i++) ua[i] = R1[1 + i*4];
+    for (i=0; i<3; i++) ub[i] = R2[*code-8 + i*4];
+
+    lineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
+    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
+    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
+
+    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
+    contact[0].depth = *depth;
+    return 1;
+  }
+
+
+        if(*code==4){
+                for (i=0; i<3; i++) contact[0].pos[i] = pb[i];
+                contact[0].depth = *depth;
+                return 1;
+                                }
+  
+
+  dVector3 vertex;
+  if (*code == 0) {
+   
+    dReal sign;
+    for (i=0; i<3; i++) vertex[i] = p2[i];
+    sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) vertex[i] += sign * B1 * R2[i*4];
+    sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) vertex[i] += sign * B2 * R2[i*4+1];
+    sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
+    for (i=0; i<3; i++) vertex[i] += sign * B3 * R2[i*4+2];
+  }
+  else {
+   
+    dReal sign,cos1,cos3,factor;
+    for (i=0; i<3; i++) vertex[i] = p1[i];
+    cos1 = dDOT14(normal,R1+0) ;
+        cos3 = dDOT14(normal,R1+2);
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+        factor= factor ? factor : 1.f;
+        cos1/=factor;
+        cos3/=factor;
+    for (i=0; i<3; i++) vertex[i] += cos1 * radius * R1[i*4];
+
+    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) vertex[i] += sign * hlz * R1[i*4+1];
+   
+    for (i=0; i<3; i++) vertex[i] += cos3 * radius * R1[i*4+2];
+  }
+  for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
+  contact[0].depth = *depth;
+  return 1;
+}
+
+//****************************************************************************
+
+extern "C" int dCylCyl (const dVector3 p1, const dMatrix3 R1,
+                        const dReal radius1,const dReal lz1, const dVector3 p2,
+                        const dMatrix3 R2, const dReal radius2,const dReal lz2,
+                        dVector3 normal, dReal *depth, int *code,
+                        int maxc, dContactGeom *contact, int skip)
+{
+  dVector3 p,pp1,pp2,normalC;
+  const dReal *normalR = 0;
+  dReal hlz1,hlz2,s,s2;
+  int i,invert_normal;
+
+  // get vector from centers of box 1 to box 2, relative to box 1
+  p[0] = p2[0] - p1[0];
+  p[1] = p2[1] - p1[1];
+  p[2] = p2[2] - p1[2];
+  dMULTIPLY1_331 (pp1,R1,p);            // get pp1 = p relative to body 1
+  dMULTIPLY1_331 (pp2,R2,p);
+  // get side lengths / 2
+  hlz1 = lz1*REAL(0.5);
+  hlz2 = lz2*REAL(0.5); 
+
+ dReal proj,cos,sin,cos1,cos3;
+
+
+
+#define TEST(expr1,expr2,norm,cc) \
+  s2 = dFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  if (s2 > s) { \
+    s = s2; \
+    normalR = norm; \
+    invert_normal = ((expr1) < 0); \
+    *code = (cc); \
+  }
+
+  s = -dInfinity;
+  invert_normal = 0;
+  *code = 0;
+
+  cos=dFabs(dDOT44(R1+1,R2+1));
+  sin=sqrtf(1.f-(cos>1.f ? 1.f : cos));
+
+  TEST (pp1[1],(hlz1 + radius2*sin + hlz2*cos ),R1+1,0);//pp
+
+  TEST (pp2[1],(radius1*sin + hlz1*cos + hlz2),R2+1,1);
+
+
+
+  // note: cross product axes need to be scaled when s is computed.
+ 
+#undef TEST
+#define TEST(expr1,expr2,n1,n2,n3,cc) \
+  s2 = dFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  if (s2 > s) { \
+      s = s2; \
+          normalR = 0; \
+      normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
+      invert_normal = ((expr1) < 0); \
+      *code = (cc); \
+    } 
+ 
+
+dVector3 tAx,Ax,pa,pb;
+
+//cross between cylinders' axes
+dCROSS144(Ax,=,R1+1,R2+1);
+dNormalize3(Ax);
+TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+radius2,Ax[0],Ax[1],Ax[2],6);
+
+
+{
+ 
+    dReal sign, factor;
+
+        //making ax which is perpendicular to cyl1 ax passing across cyl2 position//
+                //(project p on cyl1 flat surface )
+    for (i=0; i<3; i++) pb[i] = p2[i];
+        //cos1 = dDOT14(p,R1+0);
+        //cos3 = dDOT14(p,R1+2) ;
+        tAx[0]=pp1[0]*R1[0]+pp1[2]*R1[2];
+        tAx[1]=pp1[0]*R1[4]+pp1[2]*R1[6];
+        tAx[2]=pp1[0]*R1[8]+pp1[2]*R1[10];
+        dNormalize3(tAx);
+
+//find deepest point pb of cyl2 on opposite direction of tAx
+        cos1 = dDOT14(tAx,R2+0);
+        cos3 = dDOT14(tAx,R2+2) ;
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+        cos1/=factor;
+        cos3/=factor;
+    for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4];
+
+    sign = (dDOT14(tAx,R2+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1];
+
+    for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2];
+
+//making perpendicular to cyl1 ax passing across pb
+        proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1);
+
+        Ax[0]=pb[0]-p1[0]-R1[1]*proj;
+        Ax[1]=pb[1]-p1[1]-R1[5]*proj;
+        Ax[2]=pb[2]-p1[2]-R1[9]*proj;
+
+}
+
+dNormalize3(Ax);
+
+
+  cos=dFabs(dDOT14(Ax,R2+1));
+  cos1=dDOT14(Ax,R2+0);
+  cos3=dDOT14(Ax,R2+2);
+  sin=sqrtf(cos1*cos1+cos3*cos3);
+
+TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+cos*hlz2+sin*radius2,Ax[0],Ax[1],Ax[2],3);
+
+
+
+{
+   
+   dReal sign, factor;
+        
+    for (i=0; i<3; i++) pa[i] = p1[i];
+
+        //making ax which is perpendicular to cyl2 ax passing across cyl1 position//
+        //(project p on cyl2 flat surface )
+        //cos1 = dDOT14(p,R2+0);
+        //cos3 = dDOT14(p,R2+2) ;
+        tAx[0]=pp2[0]*R2[0]+pp2[2]*R2[2];
+        tAx[1]=pp2[0]*R2[4]+pp2[2]*R2[6];
+        tAx[2]=pp2[0]*R2[8]+pp2[2]*R2[10];
+        dNormalize3(tAx);
+
+        cos1 = dDOT14(tAx,R1+0);
+        cos3 = dDOT14(tAx,R1+2) ;
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+        cos1/=factor;
+        cos3/=factor;
+
+//find deepest point pa of cyl2 on direction of tAx
+    for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4];
+
+    sign = (dDOT14(tAx,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1];
+
+  
+    for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2];
+
+        proj=dDOT14(pa,R2+1)-dDOT14(p2,R2+1);
+
+        Ax[0]=pa[0]-p2[0]-R2[1]*proj;
+        Ax[1]=pa[1]-p2[1]-R2[5]*proj;
+        Ax[2]=pa[2]-p2[2]-R2[9]*proj;
+
+}
+dNormalize3(Ax);
+
+
+
+  cos=dFabs(dDOT14(Ax,R1+1));
+  cos1=dDOT14(Ax,R1+0);
+  cos3=dDOT14(Ax,R1+2);
+  sin=sqrtf(cos1*cos1+cos3*cos3);
+
+TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius2+cos*hlz1+sin*radius1,Ax[0],Ax[1],Ax[2],4);
+
+
+////test circl
+
+//@ this needed to set right normal when cylinders edges intersect
+//@ the most precise axis for this test may be found as a line between nearest points of two
+//@ circles. But it needs comparatively a lot of computation.
+//@ I use a trick which lets not to solve quadric equation. 
+//@ In the case when cylinder eidges touches the test below rather accurate.
+//@ I still not sure about problems with sepparation but they have not been revealed during testing.
+dVector3 point;
+{
+ dVector3 ca,cb; 
+ dReal sign;
+ for (i=0; i<3; i++) ca[i] = p1[i];
+ for (i=0; i<3; i++) cb[i] = p2[i];
+//find two nearest flat rings
+ sign = (pp1[1] > 0) ? REAL(1.0) : REAL(-1.0);
+ for (i=0; i<3; i++) ca[i] += sign * hlz1 * R1[i*4+1];
+
+ sign = (pp2[1] > 0) ? REAL(1.0) : REAL(-1.0);
+ for (i=0; i<3; i++) cb[i] -= sign * hlz2 * R2[i*4+1];
+
+ dVector3 tAx,tAx1;
+        circleIntersection(R1+1,ca,radius1,R2+1,cb,radius2,point);
+
+        Ax[0]=point[0]-ca[0];
+        Ax[1]=point[1]-ca[1];
+        Ax[2]=point[2]-ca[2];
+
+        cos1 = dDOT14(Ax,R1+0);
+        cos3 = dDOT14(Ax,R1+2) ;
+
+        tAx[0]=cos3*R1[0]-cos1*R1[2];
+        tAx[1]=cos3*R1[4]-cos1*R1[6];
+        tAx[2]=cos3*R1[8]-cos1*R1[10];
+
+        Ax[0]=point[0]-cb[0];
+        Ax[1]=point[1]-cb[1];
+        Ax[2]=point[2]-cb[2];
+
+
+        cos1 = dDOT14(Ax,R2+0);
+        cos3 = dDOT14(Ax,R2+2) ;
+
+        tAx1[0]=cos3*R2[0]-cos1*R2[2];
+        tAx1[1]=cos3*R2[4]-cos1*R2[6];
+        tAx1[2]=cos3*R2[8]-cos1*R2[10];
+        dCROSS(Ax,=,tAx,tAx1);
+        
+
+ 
+
+dNormalize3(Ax);
+dReal cyl1Pr,cyl2Pr;
+
+ cos=dFabs(dDOT14(Ax,R1+1));
+ cos1=dDOT14(Ax,R1+0);
+ cos3=dDOT14(Ax,R1+2);
+ sin=sqrtf(cos1*cos1+cos3*cos3);
+ cyl1Pr=cos*hlz1+sin*radius1;
+
+ cos=dFabs(dDOT14(Ax,R2+1));
+ cos1=dDOT14(Ax,R2+0);
+ cos3=dDOT14(Ax,R2+2);
+ sin=sqrtf(cos1*cos1+cos3*cos3);
+ cyl2Pr=cos*hlz2+sin*radius2;
+TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],cyl1Pr+cyl2Pr,Ax[0],Ax[1],Ax[2],5);
+
+
+}
+
+
+#undef TEST
+
+
+
+  // if we get to this point, the cylinders interpenetrate. compute the normal
+  // in global coordinates.
+  if (normalR) {
+    normal[0] = normalR[0];
+    normal[1] = normalR[4];
+    normal[2] = normalR[8];
+  }
+  else {
+                normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2];
+                }
+  if (invert_normal) {
+    normal[0] = -normal[0];
+    normal[1] = -normal[1];
+    normal[2] = -normal[2];
+  }
+
+  *depth = -s;
+
+  // compute contact point(s)
+
+        if(*code==3){
+                for (i=0; i<3; i++) contact[0].pos[i] = pb[i];
+                contact[0].depth = *depth;
+                return 1;
+                                }
+
+        if(*code==4){
+                for (i=0; i<3; i++) contact[0].pos[i] = pa[i];
+                contact[0].depth = *depth;
+                return 1;
+                                }
+
+        if(*code==5){
+                for (i=0; i<3; i++) contact[0].pos[i] = point[i];
+                contact[0].depth = *depth;
+                return 1;
+                                }
+
+if (*code == 6) {
+            dVector3 pa;
+    dReal sign, cos1,cos3,factor;
+
+
+    for (i=0; i<3; i++) pa[i] = p1[i];
+
+        cos1 = dDOT14(normal,R1+0);
+        cos3 = dDOT14(normal,R1+2) ;
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+
+        cos1/=factor;
+        cos3/=factor;
+        
+    for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4];
+
+    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1];
+
+  
+    for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2];
+
+    // find a point pb on the intersecting edge of cylinder 2
+    dVector3 pb;
+    for (i=0; i<3; i++) pb[i] = p2[i];
+        cos1 = dDOT14(normal,R2+0);
+        cos3 = dDOT14(normal,R2+2) ;
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+
+        cos1/=factor;
+        cos3/=factor;
+        
+    for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4];
+
+    sign = (dDOT14(normal,R2+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1];
+
+  
+    for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2];
+
+        
+        dReal alpha,beta;
+        dVector3 ua,ub;
+        for (i=0; i<3; i++) ua[i] = R1[1 + i*4];
+        for (i=0; i<3; i++) ub[i] = R2[1 + i*4];
+        lineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
+        for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
+        for (i=0; i<3; i++) pb[i] += ub[i]*beta;
+
+    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
+    contact[0].depth = *depth;
+    return 1;
+  }
+
+  // okay, we have a face-something intersection (because the separating
+  // axis is perpendicular to a face).
+
+  // @@@ temporary: make deepest point on the "other" cylinder the contact point.
+  // @@@ this kind of works, but we need multiple contact points for stability,
+  // @@@ especially for face-face contact.
+
+  dVector3 vertex;
+  if (*code == 0) {
+    // flat face from cylinder 1 touches a edge/face from cylinder 2.
+    dReal sign,cos1,cos3,factor;
+    for (i=0; i<3; i++) vertex[i] = p2[i];
+    cos1 = dDOT14(normal,R2+0) ;
+        cos3 = dDOT14(normal,R2+2);
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+
+        cos1/=factor;
+        cos3/=factor;
+    for (i=0; i<3; i++) vertex[i] -= cos1 * radius2 * R2[i*4];
+
+    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) vertex[i] -= sign * hlz2 * R2[i*4+1];
+   
+    for (i=0; i<3; i++) vertex[i] -= cos3 * radius2 * R2[i*4+2];
+  }
+  else {
+     // flat face from cylinder 2 touches a edge/face from cylinder 1.
+    dReal sign,cos1,cos3,factor;
+    for (i=0; i<3; i++) vertex[i] = p1[i];
+    cos1 = dDOT14(normal,R1+0) ;
+        cos3 = dDOT14(normal,R1+2);
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+
+        cos1/=factor;
+        cos3/=factor;
+    for (i=0; i<3; i++) vertex[i] += cos1 * radius1 * R1[i*4];
+
+    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) vertex[i] += sign * hlz1 * R1[i*4+1];
+   
+    for (i=0; i<3; i++) vertex[i] += cos3 * radius1 * R1[i*4+2];
+  }
+  for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
+  contact[0].depth = *depth;
+  return 1;
+}
+
+//****************************************************************************
+
+
+int dCollideCylS (dxGeom *o1, dxGeom *o2, int flags,
+                dContactGeom *contact, int skip)
+{
+ 
+
+  dIASSERT (skip >= (int)sizeof(dContactGeom));
+  dIASSERT (dGeomGetClass(o2) == dSphereClass);
+  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
+  const dReal* p1=dGeomGetPosition(o1);
+  const dReal* p2=dGeomGetPosition(o2);
+  const dReal* R=dGeomGetRotation(o1);
+  dVector3 p,normalC,normal;
+  const dReal *normalR = 0;
+  dReal cylRadius;
+  dReal hl;
+  dGeomCylinderGetParams(o1,&cylRadius,&hl);
+  dReal sphereRadius;
+  sphereRadius=dGeomSphereGetRadius(o2);
+  
+  int i,invert_normal;
+
+  // get vector from centers of cyl to shere
+  p[0] = p2[0] - p1[0];
+  p[1] = p2[1] - p1[1];
+  p[2] = p2[2] - p1[2];
+ 
+dReal s,s2;
+unsigned char code;
+#define TEST(expr1,expr2,norm,cc) \
+  s2 = dFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  if (s2 > s) { \
+    s = s2; \
+    normalR = norm; \
+    invert_normal = ((expr1) < 0); \
+    code = (cc); \
+  }
+
+  s = -dInfinity;
+  invert_normal = 0;
+  code = 0;
+
+  // separating axis cyl ax 
+
+  TEST (dDOT14(p,R+1),sphereRadius+hl,R+1,2);
+  // note: cross product axes need to be scaled when s is computed.
+  // normal (n1,n2,n3) is relative to 
+#undef TEST
+#define TEST(expr1,expr2,n1,n2,n3,cc) \
+  s2 = dFabs(expr1) - (expr2); \
+  if (s2 > 0) return 0; \
+  if (s2 > s) { \
+      s = s2; \
+          normalR = 0; \
+      normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
+      invert_normal = ((expr1) < 0); \
+      code = (cc); \
+    } 
+ 
+//making ax which is perpendicular to cyl1 ax to sphere center//
+ 
+dReal proj,cos,sin,cos1,cos3;
+dVector3 Ax;
+        proj=dDOT14(p2,R+1)-dDOT14(p1,R+1);
+
+        Ax[0]=p2[0]-p1[0]-R[1]*proj;
+        Ax[1]=p2[1]-p1[1]-R[5]*proj;
+        Ax[2]=p2[2]-p1[2]-R[9]*proj;
+dNormalize3(Ax);
+TEST(dDOT(p,Ax),sphereRadius+cylRadius,Ax[0],Ax[1],Ax[2],9);
+
+
+Ax[0]=p[0];
+Ax[1]=p[1];
+Ax[2]=p[2];
+dNormalize3(Ax);
+
+        dVector3 pa;
+    dReal sign, factor;
+    for (i=0; i<3; i++) pa[i] = p1[i];
+
+        cos1 = dDOT14(Ax,R+0);
+        cos3 = dDOT14(Ax,R+2) ;
+        factor=sqrtf(cos1*cos1+cos3*cos3);
+        cos1/=factor;
+        cos3/=factor;
+    for (i=0; i<3; i++) pa[i] += cos1 * cylRadius * R[i*4];
+    sign = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0);
+    for (i=0; i<3; i++) pa[i] += sign * hl * R[i*4+1];
+    for (i=0; i<3; i++) pa[i] += cos3 * cylRadius  * R[i*4+2];
+
+Ax[0]=p2[0]-pa[0];
+Ax[1]=p2[1]-pa[1];
+Ax[2]=p2[2]-pa[2];
+dNormalize3(Ax);
+
+ cos=dFabs(dDOT14(Ax,R+1));
+ cos1=dDOT14(Ax,R+0);
+ cos3=dDOT14(Ax,R+2);
+ sin=sqrtf(cos1*cos1+cos3*cos3);
+TEST(dDOT(p,Ax),sphereRadius+cylRadius*sin+hl*cos,Ax[0],Ax[1],Ax[2],14);
+
+
+#undef TEST
+
+  if (normalR) {
+    normal[0] = normalR[0];
+    normal[1] = normalR[4];
+    normal[2] = normalR[8];
+  }
+  else {
+
+        normal[0] = normalC[0];
+        normal[1] = normalC[1];
+        normal[2] = normalC[2];
+                }
+  if (invert_normal) {
+    normal[0] = -normal[0];
+    normal[1] = -normal[1];
+    normal[2] = -normal[2];
+  }
+   // compute contact point(s)
+contact->depth=-s;
+contact->normal[0]=-normal[0];
+contact->normal[1]=-normal[1];
+contact->normal[2]=-normal[2];
+contact->g1=const_cast<dxGeom*> (o1);
+contact->g2=const_cast<dxGeom*> (o2);
+contact->pos[0]=p2[0]-normal[0]*sphereRadius;
+contact->pos[1]=p2[1]-normal[1]*sphereRadius;
+contact->pos[2]=p2[2]-normal[2]*sphereRadius;
+return 1;
+}
+
+
+
+int dCollideCylB (dxGeom *o1, dxGeom *o2, int flags,
+                dContactGeom *contact, int skip)
+{
+  dVector3 normal;
+  dReal depth;
+  int code;
+  dReal cylRadius,cylLength;
+  dVector3 boxSides;
+  dGeomCylinderGetParams(o1,&cylRadius,&cylLength);
+  dGeomBoxGetLengths(o2,boxSides);
+  int num = dCylBox(dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius,cylLength, 
+                                        dGeomGetPosition(o2),dGeomGetRotation(o2),boxSides,
+                                        normal,&depth,&code,flags & NUMC_MASK,contact,skip);
+  for (int i=0; i<num; i++) {
+    CONTACT(contact,i*skip)->normal[0] = -normal[0];
+    CONTACT(contact,i*skip)->normal[1] = -normal[1];
+    CONTACT(contact,i*skip)->normal[2] = -normal[2];
+    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
+    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
+  }
+  return num;
+}
+
+int dCollideCylCyl (dxGeom *o1, dxGeom *o2, int flags,
+                dContactGeom *contact, int skip)
+{
+  dVector3 normal;
+  dReal depth;
+  int code;
+dReal cylRadius1,cylRadius2;
+dReal cylLength1,cylLength2;
+dGeomCylinderGetParams(o1,&cylRadius1,&cylLength1);
+dGeomCylinderGetParams(o2,&cylRadius2,&cylLength2);
+int num = dCylCyl (dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius1,cylLength1,
+                                   dGeomGetPosition(o2),dGeomGetRotation(o2),cylRadius2,cylLength2,
+                                     normal,&depth,&code,flags & NUMC_MASK,contact,skip);
+
+  for (int i=0; i<num; i++) {
+    CONTACT(contact,i*skip)->normal[0] = -normal[0];
+    CONTACT(contact,i*skip)->normal[1] = -normal[1];
+    CONTACT(contact,i*skip)->normal[2] = -normal[2];
+    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
+    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
+  }
+  return num;
+}
+
+struct dxPlane {
+  dReal p[4];
+};
+
+
+int dCollideCylPlane 
+        (
+        dxGeom *o1, dxGeom *o2, int flags,
+                          dContactGeom *contact, int skip){
+  dIASSERT (skip >= (int)sizeof(dContactGeom));
+  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
+  dIASSERT (dGeomGetClass(o2) == dPlaneClass);
+  contact->g1 = const_cast<dxGeom*> (o1);
+  contact->g2 = const_cast<dxGeom*> (o2);
+  
+ unsigned int ret = 0;
+
+ dReal radius;
+ dReal hlz;
+ dGeomCylinderGetParams(o1,&radius,&hlz);
+ hlz /= 2;
+ 
+ const dReal *R =       dGeomGetRotation(o1);// rotation of cylinder
+ const dReal* p =       dGeomGetPosition(o1);
+ dVector4 n;            // normal vector
+ dReal pp;
+ dGeomPlaneGetParams (o2, n);
+ pp=n[3];
+ dReal cos1,sin1;
+  cos1=dFabs(dDOT14(n,R+1));
+
+cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
+sin1=sqrtf(REAL(1.)-cos1*cos1);
+//////////////////////////////
+
+dReal sidePr=cos1*hlz+sin1*radius;
+
+dReal dist=-pp+dDOT(n,p);
+dReal outDepth=sidePr-dist;
+
+if(outDepth<0.f) return 0;
+
+dVector3 pos;
+
+
+/////////////////////////////////////////// from geom.cpp dCollideBP
+  dReal Q1 = dDOT14(n,R+0);
+  dReal Q2 = dDOT14(n,R+1);
+  dReal Q3 = dDOT14(n,R+2);
+  dReal factor =sqrtf(Q1*Q1+Q3*Q3);
+  factor= factor ? factor :1.f;
+  dReal A1 = radius *           Q1/factor;
+  dReal A2 = hlz*Q2;
+  dReal A3 = radius *           Q3/factor;
+
+  pos[0]=p[0];
+  pos[1]=p[1];
+  pos[2]=p[2];
+
+  pos[0]-= A1*R[0];
+  pos[1]-= A1*R[4];
+  pos[2]-= A1*R[8];
+
+  pos[0]-= A3*R[2];
+  pos[1]-= A3*R[6];
+  pos[2]-= A3*R[10];
+
+  pos[0]-= A2>0 ? hlz*R[1]:-hlz*R[1];
+  pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5];
+  pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9];
+  
+ 
+
+  contact->pos[0] = pos[0];
+  contact->pos[1] = pos[1];
+  contact->pos[2] = pos[2];
+   contact->depth = outDepth;
+  ret=1;
+ 
+if(dFabs(Q2)>M_SQRT1_2){
+
+  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0];
+  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4];
+  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8];
+  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1);
+
+  if(CONTACT(contact,ret*skip)->depth>0.f)
+  ret++;
+  
+  
+  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2];
+  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6];
+  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10];
+  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3);
+
+  if(CONTACT(contact,ret*skip)->depth>0.f) ret++;
+} else {
+
+  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]);
+  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]);
+  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]);
+  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2);
+
+  if(CONTACT(contact,ret*skip)->depth>0.f) ret++;
+}
+
+
+
+ for (unsigned int i=0; i<ret; i++) {
+    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
+    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
+        CONTACT(contact,i*skip)->normal[0] =n[0];
+        CONTACT(contact,i*skip)->normal[1] =n[1];
+        CONTACT(contact,i*skip)->normal[2] =n[2];
+  }
+  return ret;  
+}
+
+int dCollideCylRay(dxGeom *o1, dxGeom *o2, int flags,
+                   dContactGeom *contact, int skip) {
+  dIASSERT (skip >= (int)sizeof(dContactGeom));
+  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
+  dIASSERT (dGeomGetClass(o2) == dRayClass);
+  contact->g1 = const_cast<dxGeom*> (o1);
+  contact->g2 = const_cast<dxGeom*> (o2);
+  dReal radius;
+  dReal lz;
+  dGeomCylinderGetParams(o1,&radius,&lz);
+  dReal lz2=lz*REAL(0.5);
+  const dReal *R = dGeomGetRotation(o1); // rotation of the cylinder
+  const dReal *p = dGeomGetPosition(o1); // position of the cylinder
+  dVector3 start,dir;
+  dGeomRayGet(o2,start,dir); // position and orientation of the ray
+  dReal length = dGeomRayGetLength(o2);
+
+  // compute some useful info
+  dVector3 cs,q,r;
+  dReal C,k;
+  cs[0] = start[0] - p[0];
+  cs[1] = start[1] - p[1];
+  cs[2] = start[2] - p[2];
+  k = dDOT41(R+1,cs);   // position of ray start along cyl axis (Y)
+  q[0] = k*R[0*4+1] - cs[0];
+  q[1] = k*R[1*4+1] - cs[1];
+  q[2] = k*R[2*4+1] - cs[2];
+  C = dDOT(q,q) - radius*radius;
+  // if C < 0 then ray start position within infinite extension of cylinder
+  // if ray start position is inside the cylinder
+  int inside_cyl=0;
+  if (C<0 && !(k<-lz2 || k>lz2)) inside_cyl=1;
+  // compute ray collision with infinite cylinder, except for the case where
+  // the ray is outside the cylinder but within the infinite cylinder
+  // (it that case the ray can only hit endcaps)
+  if (!inside_cyl && C < 0) {
+    // set k to cap position to check
+    if (k < 0) k = -lz2; else k = lz2;
+  }
+  else {
+    dReal uv = dDOT41(R+1,dir);
+    r[0] = uv*R[0*4+1] - dir[0];
+    r[1] = uv*R[1*4+1] - dir[1];
+    r[2] = uv*R[2*4+1] - dir[2];
+    dReal A = dDOT(r,r);
+    dReal B = 2*dDOT(q,r);
+    k = B*B-4*A*C;
+    if (k < 0) {
+      // the ray does not intersect the infinite cylinder, but if the ray is
+      // inside and parallel to the cylinder axis it may intersect the end
+      // caps. set k to cap position to check.
+      if (!inside_cyl) return 0;
+      if (uv < 0) k = -lz2; else k = lz2;
+    }
+    else {
+      k = dSqrt(k);
+      A = dRecip (2*A);
+      dReal alpha = (-B-k)*A;
+      if (alpha < 0) {
+        alpha = (-B+k)*A;
+        if (alpha<0) return 0;
+      }
+      if (alpha>length) return 0;
+      // the ray intersects the infinite cylinder. check to see if the
+      // intersection point is between the caps
+      contact->pos[0] = start[0] + alpha*dir[0];
+      contact->pos[1] = start[1] + alpha*dir[1];
+      contact->pos[2] = start[2] + alpha*dir[2];
+      q[0] = contact->pos[0] - p[0];
+      q[1] = contact->pos[1] - p[1];
+      q[2] = contact->pos[2] - p[2];
+      k = dDOT14(q,R+1);
+      dReal nsign = inside_cyl ? -1 : 1;
+      if (k >= -lz2 && k <= lz2) {
+        contact->normal[0] = nsign * (contact->pos[0] -
+                                      (p[0] + k*R[0*4+1]));
+        contact->normal[1] = nsign * (contact->pos[1] -
+                                      (p[1] + k*R[1*4+1]));
+        contact->normal[2] = nsign * (contact->pos[2] -
+                                      (p[2] + k*R[2*4+1]));
+        dNormalize3 (contact->normal);
+        contact->depth = alpha;
+        return 1;
+      }
+      // the infinite cylinder intersection point is not between the caps.
+      // set k to cap position to check.
+      if (k < 0) k = -lz2; else k = lz2;
+    }
+  }
+  // check for ray intersection with the caps. k must indicate the cap
+  // position to check
+  // perform a ray plan interesection
+  // R+1 is the plan normal
+  q[0] = start[0] - (p[0] + k*R[0*4+1]);
+  q[1] = start[1] - (p[1] + k*R[1*4+1]);
+  q[2] = start[2] - (p[2] + k*R[2*4+1]);
+  dReal alpha = -dDOT14(q,R+1);
+  dReal k2 = dDOT14(dir,R+1);
+  if (k2==0) return 0; // ray parallel to the plane
+  alpha/=k2;
+  if (alpha<0 || alpha>length) return 0; // too short
+  contact->pos[0]=start[0]+alpha*dir[0];
+  contact->pos[1]=start[1]+alpha*dir[1];
+  contact->pos[2]=start[2]+alpha*dir[2];
+  dReal nsign = (k<0)?-1:1;
+  contact->normal[0]=nsign*R[0*4+1];
+  contact->normal[1]=nsign*R[1*4+1];
+  contact->normal[2]=nsign*R[2*4+1];
+  contact->depth=alpha;
+  return 1;
+}
+
+static  dColliderFn * dCylinderColliderFn (int num)
+{
+  if (num == dBoxClass) return (dColliderFn *) &dCollideCylB;
+  else if (num == dSphereClass) return (dColliderFn *) &dCollideCylS;
+  else if (num == dCylinderClassUser) return (dColliderFn *) &dCollideCylCyl;
+  else if (num == dPlaneClass) return (dColliderFn *) &dCollideCylPlane;
+  else if (num == dRayClass) return (dColliderFn *) &dCollideCylRay;
+  return 0;
+}
+
+
+static  void dCylinderAABB (dxGeom *geom, dReal aabb[6])
+{
+  dReal radius,lz;
+  dGeomCylinderGetParams(geom,&radius,&lz);
+const dReal* R= dGeomGetRotation(geom);
+const dReal* pos= dGeomGetPosition(geom);
+  dReal xrange =  dFabs (R[0] *radius) +
+    REAL(0.5) *dFabs (R[1] * lz) + dFabs (R[2] * radius);
+
+  dReal yrange = dFabs (R[4] *radius) +
+    REAL(0.5) * dFabs (R[5] * lz) + dFabs (R[6] * radius);
+
+  dReal zrange =  dFabs (R[8] * radius) +
+    REAL(0.5) *dFabs (R[9] * lz) + dFabs (R[10] * radius);
+
+  aabb[0] = pos[0] - xrange;
+  aabb[1] = pos[0] + xrange;
+  aabb[2] = pos[1] - yrange;
+  aabb[3] = pos[1] + yrange;
+  aabb[4] = pos[2] - zrange;
+  aabb[5] = pos[2] + zrange;
+}
+
+dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz)
+{
+ dAASSERT (r > 0 && lz > 0);
+ if (dCylinderClassUser == -1)
+  {
+    dGeomClass c;
+    c.bytes = sizeof (dxCylinder);
+    c.collider = &dCylinderColliderFn;
+    c.aabb = &dCylinderAABB;
+    c.aabb_test = 0;
+    c.dtor = 0;
+    dCylinderClassUser=dCreateGeomClass (&c);
+
+  }
+
+  dGeomID g = dCreateGeom (dCylinderClassUser);
+  if (space) dSpaceAdd (space,g);
+  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
+
+  c->radius = r;
+  c->lz = lz;
+  return g;
+}
+
+
+
+void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length)
+{
+  dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser,"argument not a cylinder");
+  dAASSERT (radius > 0 && length > 0);
+  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
+  c->radius = radius;
+  c->lz = length;
+}
+
+
+
+void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length)
+{
+  dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser ,"argument not a cylinder");
+  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
+  *radius = c->radius;
+  *length = c->lz;
+}
+
+/*
+void dMassSetCylinder (dMass *m, dReal density,
+                  dReal radius, dReal length)
+{
+  dAASSERT (m);
+  dMassSetZero (m);
+  dReal M = length*M_PI*radius*radius*density;
+  m->mass = M;
+  m->_I(0,0) = M/REAL(4.0) * (ly*ly + lz*lz);
+  m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz);
+  m->_I(2,2) = M/REAL(4.0) * (lx*lx + ly*ly);
+
+# ifndef dNODEBUG
+  checkMass (m);
+# endif
+}
+*/
diff --git a/libraries/ode-0.9/contrib/dCylinder/dCylinder.h b/libraries/ode-0.9/contrib/dCylinder/dCylinder.h
new file mode 100644
index 0000000..06e3a0b
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dCylinder/dCylinder.h
@@ -0,0 +1,14 @@
+
+#ifndef dCylinder_h
+#define dCylinder_h
+
+struct dxCylinder;
+extern int dCylinderClassUser;
+
+
+dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz);
+void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length);
+
+void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length);
+#endif //dCylinder_h
+
diff --git a/libraries/ode-0.9/contrib/dCylinder/readme.txt b/libraries/ode-0.9/contrib/dCylinder/readme.txt
new file mode 100644
index 0000000..facd13e
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dCylinder/readme.txt
@@ -0,0 +1,62 @@
+readme.txt
+
+WARNING: THIS IS NOT VERY RELIABLE CODE.  IT HAS BUGS.  YOUR
+         SUCCESS MAY VARY.  CONTRIBUTIONS OF FIXES/REWRITES ARE
+         WELCOME.
+
+///////////////////////////////////////////////////////////////////////
+
+Cylinder geometry class.
+
+New in this version:
+
+Cylinder class implemented as User Geometry Class so it now can be
+used with old and new ODE collision detection.
+
+Cylinder - Ray has been contributed by Olivier Michel.
+
+THE IDENTIFIER dCylinderClass HAS BEEN REPLACED BY dCylinderClassUser
+
+to avoid conflict with dCylinderClass in the enum definite in collision.h
+
+///////////////////////////////////////////////////////////////////////
+The dCylinder class includes the following collisions:
+
+Cylinder - Box
+Cylinder - Cylinder
+Cylinder - Sphere
+Cylinder - Plane
+Cylinder - Ray (contributed by Olivier Michel)
+
+Cylinder aligned along axis - Y when created. (Not like Capped
+Cylinder which aligned along axis - Z).
+
+Interface is just the same as  Capped Cylinder has.
+
+Use functions which have one "C" instead of double "C".
+
+to create:
+dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length);
+
+to set params:
+void dGeomCylinderSetParams (dGeomID cylinder,
+                              dReal radius, dReal length);
+
+
+to get params:
+void dGeomCylinderGetParams (dGeomID cylinder,
+                              dReal *radius, dReal *length);
+
+Return in radius and length the parameters of the given cylinder.
+
+Identification number of the class:
+ dCylinderClassUser
+
+ I do not include a function that sets inertia tensor for cylinder.
+ One may use existing ODE functions dMassSetCappedCylinder or dMassSetBox.
+ To set exact tensor for cylinder use dMassSetParameters.
+ Remember cylinder aligned along axis - Y.
+ 
+ ///////////////////////////////////////////////////////////////////////////
+ Konstantin Slipchenko
+ February 5, 2002
diff --git a/libraries/ode-0.9/contrib/dRay/Include/dRay.h b/libraries/ode-0.9/contrib/dRay/Include/dRay.h
new file mode 100644
index 0000000..f6caea8
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/Include/dRay.h
@@ -0,0 +1,15 @@
+#include "ode\ode.h"
+
+/* Class ID */
+extern int dRayClass;
+
+/* Creates a ray */
+dxGeom* dGeomCreateRay(dSpaceID space, dReal Length);
+
+/* Set/Get length */
+void dGeomRaySetLength(dxGeom* g, dReal Length);
+dReal dGeomRayGetLength(dxGeom* g);
+
+/* Utility function to override the ray's pos + rot */
+void dGeomRaySet(dxGeom* g, dVector3 Origin, dVector3 Direction);
+void dGeomRayGet(dxGeom* g, dVector3 Origin, dVector3 Direction);
diff --git a/libraries/ode-0.9/contrib/dRay/README.txt b/libraries/ode-0.9/contrib/dRay/README.txt
new file mode 100644
index 0000000..8997208
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/README.txt
@@ -0,0 +1,16 @@
+From: "Erwin de Vries" <erwin@vo.com>
+To: <ode@q12.org>
+Subject: [ODE] dRay class
+Date: Thu, 25 Jul 2002 13:05:28 +0200
+
+Yesterday and today i've written a dRay class. It interacts with dPlane,
+dSphere, dBox and dCCylinder. It does not generate full contact information.
+It only generates the pos member. I dont think its useful to anyone to go
+through hoops and find a reasonable normal and penetration depth, as i dont
+think anyone will want to use it for dynamics. Just for CD.
+
+It should compile in single and double precision mode, and should be
+platform independant. I hope.
+
+The next Tri-Collider release using Opcode 1.1 will also implement a ray
+collision function along with some other not too interesting improvements.
diff --git a/libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp b/libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp
new file mode 100644
index 0000000..faa8b14
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/Test/test_ray.cpp
@@ -0,0 +1,1372 @@
+/*************************************************************************
+
+
+ *                                                                       *
+
+
+ * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
+
+
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+
+
+ *                                                                       *
+
+
+ * This library is free software; you can redistribute it and/or         *
+
+
+ * modify it under the terms of EITHER:                                  *
+
+
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+
+
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+
+
+ *       your option) any later version. The text of the GNU Lesser      *
+
+
+ *       General Public License is included with this library in the     *
+
+
+ *       file LICENSE.TXT.                                               *
+
+
+ *   (2) The BSD-style license that is included with this library in     *
+
+
+ *       the file LICENSE-BSD.TXT.                                       *
+
+
+ *                                                                       *
+
+
+ * This library is distributed in the hope that it will be useful,       *
+
+
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+
+
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+
+
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+
+
+ *                                                                       *
+
+
+ *************************************************************************/
+
+
+
+
+
+#include <ode/ode.h>
+
+
+#include <dRay.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 NUM 20                  // max number of objects
+
+
+#define DENSITY (5.0)           // density of all objects
+
+
+#define GPB 3                   // maximum number of geometries per body
+
+
+
+
+
+
+
+
+// dynamics and collision objects
+
+
+
+
+
+struct MyObject {
+
+
+  dBodyID body;                 // the body
+
+
+  dGeomID geom[GPB];            // geometries representing this body
+
+
+};
+
+
+
+
+
+static int num=0;               // number of objects in simulation
+
+
+static int nextobj=0;           // next object to recycle if num==NUM
+
+
+static dWorldID world;
+
+
+static dSpaceID space;
+
+
+static MyObject obj[NUM];
+
+
+static dJointGroupID contactgroup;
+
+
+static int selected = -1;       // selected object
+
+
+
+
+
+static dGeomID* Rays;
+
+
+static int RayCount;
+
+
+
+
+
+// this is called by dSpaceCollide when two objects in space are
+
+
+// potentially colliding.
+
+
+
+
+
+static void nearCallback (void *data, dGeomID o1, dGeomID o2)
+
+
+{
+
+
+  int i;
+
+
+  // if (o1->body && o2->body) return;
+
+
+
+
+
+  // exit without doing anything if the two bodies are connected by a joint
+
+
+  dBodyID b1 = dGeomGetBody(o1);
+
+
+  dBodyID b2 = dGeomGetBody(o2);
+
+
+  if (b1 && b2 && dAreConnected (b1,b2)) return;
+
+
+
+
+
+  dContact contact[32];                 // up to 3 contacts per box
+
+
+  for (i=0; i<32; i++) {
+
+
+    contact[i].surface.mode = dContactBounce; //dContactMu2;
+
+
+    contact[i].surface.mu = dInfinity;
+
+
+    contact[i].surface.mu2 = 0;
+
+
+    contact[i].surface.bounce = 0.5;
+
+
+    contact[i].surface.bounce_vel = 0.1;
+
+
+  }
+
+
+  if (int numc = dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) {
+
+
+     dMatrix3 RI;
+
+
+     dRSetIdentity (RI);
+
+
+     const dReal ss[3] = {0.02,0.02,0.02};
+
+
+    for (i=0; i<numc; i++) {
+
+
+                if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){
+
+
+                        dMatrix3 Rotation;
+
+
+                        dRSetIdentity(Rotation);
+
+
+                        dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01));
+
+
+                        continue;
+
+
+                }
+
+
+
+
+
+      dJointID c = dJointCreateContact (world,contactgroup,contact+i);
+
+
+      dJointAttach (c,b1,b2);
+
+
+       //dsDrawBox (contact[i].geom.pos,RI,ss);
+
+
+
+
+
+          
+
+
+    }
+
+
+  }
+
+
+}
+
+
+
+
+
+// start simulation - set viewpoint
+
+
+
+
+
+static void start()
+
+
+{
+
+
+  static float xyz[3] = {2.1640f,-1.3079f,1.7600f};
+
+
+  static float hpr[3] = {125.5000f,-17.0000f,0.0000f};
+
+
+  dsSetViewpoint (xyz,hpr);
+
+
+  printf ("To drop another object, press:\n");
+
+
+  printf ("   b for box.\n");
+
+
+  printf ("   s for sphere.\n");
+
+
+  printf ("   c for cylinder.\n");
+
+
+  printf ("   x for a composite object.\n");
+
+
+  printf ("To select an object, press space.\n");
+
+
+  printf ("To disable the selected object, press d.\n");
+
+
+  printf ("To enable the selected object, press e.\n");
+
+
+}
+
+
+
+
+
+
+
+
+char locase (char c)
+
+
+{
+
+
+  if (c >= 'A' && c <= 'Z') return c - ('a'-'A');
+
+
+  else return c;
+
+
+}
+
+
+
+
+
+
+
+
+// called when a key pressed
+
+
+
+
+
+static void command (int cmd)
+
+
+{
+
+
+  int i,j,k;
+
+
+  dReal sides[3];
+
+
+  dMass m;
+
+
+
+
+
+  cmd = locase (cmd);
+
+
+  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x') {
+
+
+    if (num < NUM) {
+
+
+      i = num;
+
+
+      num++;
+
+
+    }
+
+
+    else {
+
+
+      i = nextobj;
+
+
+      nextobj++;
+
+
+      if (nextobj >= num) nextobj = 0;
+
+
+
+
+
+      // destroy the body and geoms for slot i
+
+
+      dBodyDestroy (obj[i].body);
+
+
+      for (k=0; k < GPB; k++) {
+
+
+        if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
+
+
+      }
+
+
+      memset (&obj[i],0,sizeof(obj[i]));
+
+
+    }
+
+
+
+
+
+    obj[i].body = dBodyCreate (world);
+
+
+    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;
+
+
+
+
+
+    dBodySetPosition (obj[i].body,
+
+
+                      dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1);
+
+
+    dMatrix3 R;
+
+
+    dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+
+
+                        dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+
+
+    dBodySetRotation (obj[i].body,R);
+
+
+    dBodySetData (obj[i].body,(void*) i);
+
+
+
+
+
+    if (cmd == 'b') {
+
+
+      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
+
+
+      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
+
+
+    }
+
+
+    else if (cmd == 'c') {
+
+
+      sides[0] *= 0.5;
+
+
+      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
+
+
+      obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
+
+
+    }
+
+
+    else if (cmd == 's') {
+
+
+      sides[0] *= 0.5;
+
+
+      dMassSetSphere (&m,DENSITY,sides[0]);
+
+
+      obj[i].geom[0] = dCreateSphere (space,sides[0]);
+
+
+    }
+
+
+    else if (cmd == 'x') {
+
+
+      dGeomID g2[GPB];          // encapsulated geometries
+
+
+      dReal dpos[GPB][3];       // delta-positions for encapsulated geometries
+
+
+
+
+
+      // start accumulating masses for the encapsulated geometries
+
+
+      dMass m2;
+
+
+      dMassSetZero (&m);
+
+
+
+
+
+      // set random delta positions
+
+
+      for (j=0; j<GPB; j++) {
+
+
+        for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
+
+
+      }
+
+
+
+
+
+      for (k=0; k<3; k++) {
+
+
+        obj[i].geom[k] = dCreateGeomTransform (space);
+
+
+        dGeomTransformSetCleanup (obj[i].geom[k],1);
+
+
+        if (k==0) {
+
+
+          dReal radius = dRandReal()*0.25+0.05;
+
+
+          g2[k] = dCreateSphere (0,radius);
+
+
+          dMassSetSphere (&m2,DENSITY,radius);
+
+
+        }
+
+
+        else if (k==1) {
+
+
+          g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
+
+
+          dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
+
+
+        }
+
+
+        else {
+
+
+          dReal radius = dRandReal()*0.1+0.05;
+
+
+          dReal length = dRandReal()*1.0+0.1;
+
+
+          g2[k] = dCreateCCylinder (0,radius,length);
+
+
+          dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
+
+
+        }
+
+
+        dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
+
+
+
+
+
+        // set the transformation (adjust the mass too)
+
+
+        dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
+
+
+        dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
+
+
+        dMatrix3 Rtx;
+
+
+        dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
+
+
+                            dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
+
+
+        dGeomSetRotation (g2[k],Rtx);
+
+
+        dMassRotate (&m2,Rtx);
+
+
+
+
+
+        // add to the total mass
+
+
+        dMassAdd (&m,&m2);
+
+
+      }
+
+
+
+
+
+      // move all encapsulated objects so that the center of mass is (0,0,0)
+
+
+      for (k=0; k<2; k++) {
+
+
+        dGeomSetPosition (g2[k],
+
+
+                          dpos[k][0]-m.c[0],
+
+
+                          dpos[k][1]-m.c[1],
+
+
+                          dpos[k][2]-m.c[2]);
+
+
+      }
+
+
+      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
+
+
+    }
+
+
+
+
+
+    for (k=0; k < GPB; k++) {
+
+
+      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
+
+
+    }
+
+
+
+
+
+    dBodySetMass (obj[i].body,&m);
+
+
+  }
+
+
+
+
+
+  if (cmd == ' ') {
+
+
+    selected++;
+
+
+    if (selected >= num) selected = 0;
+
+
+    if (selected < 0) selected = 0;
+
+
+  }
+
+
+  else if (cmd == 'd' && selected >= 0 && selected < num) {
+
+
+    dBodyDisable (obj[selected].body);
+
+
+  }
+
+
+  else if (cmd == 'e' && selected >= 0 && selected < num) {
+
+
+    dBodyEnable (obj[selected].body);
+
+
+  }
+
+
+}
+
+
+
+
+
+
+
+
+// draw a geom
+
+
+
+
+
+void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
+
+
+{
+
+
+  if (!g) return;
+
+
+  if (!pos) pos = dGeomGetPosition (g);
+
+
+  if (!R) R = dGeomGetRotation (g);
+
+
+
+
+
+  int type = dGeomGetClass (g);
+
+
+  if (type == dBoxClass) {
+
+
+    dVector3 sides;
+
+
+    dGeomBoxGetLengths (g,sides);
+
+
+    dsDrawBox (pos,R,sides);
+
+
+  }
+
+
+  else if (type == dSphereClass) {
+
+
+    dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
+
+
+  }
+
+
+  else if (type == dCCylinderClass) {
+
+
+    dReal radius,length;
+
+
+    dGeomCCylinderGetParams (g,&radius,&length);
+
+
+    dsDrawCappedCylinder (pos,R,length,radius);
+
+
+  }
+
+
+  else if (type == dGeomTransformClass) {
+
+
+    dGeomID g2 = dGeomTransformGetGeom (g);
+
+
+    const dReal *pos2 = dGeomGetPosition (g2);
+
+
+    const dReal *R2 = dGeomGetRotation (g2);
+
+
+    dVector3 actual_pos;
+
+
+    dMatrix3 actual_R;
+
+
+    dMULTIPLY0_331 (actual_pos,R,pos2);
+
+
+    actual_pos[0] += pos[0];
+
+
+    actual_pos[1] += pos[1];
+
+
+    actual_pos[2] += pos[2];
+
+
+    dMULTIPLY0_333 (actual_R,R,R2);
+
+
+    drawGeom (g2,actual_pos,actual_R);
+
+
+  }
+
+
+}
+
+
+
+
+
+
+
+
+// simulation loop
+
+
+
+
+
+static void simLoop (int pause)
+
+
+{
+
+
+  dsSetColor (0,0,2);
+
+
+  dSpaceCollide (space,0,&nearCallback);
+
+
+  if (!pause) dWorldStep (world,0.05);
+
+
+
+
+
+  // remove all contact joints
+
+
+  dJointGroupEmpty (contactgroup);
+
+
+
+
+
+  dsSetColor (1,1,0);
+
+
+  dsSetTexture (DS_WOOD);
+
+
+  for (int i=0; i<num; i++) {
+
+
+    int color_changed = 0;
+
+
+    if (i==selected) {
+
+
+      dsSetColor (0,0.7,1);
+
+
+      color_changed = 1;
+
+
+    }
+
+
+    else if (! dBodyIsEnabled (obj[i].body)) {
+
+
+      dsSetColor (1,0,0);
+
+
+      color_changed = 1;
+
+
+    }
+
+
+    for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
+
+
+    if (color_changed) dsSetColor (1,1,0);
+
+
+  }
+
+
+
+
+
+  {for (int i = 0; i < RayCount; i++){
+
+
+          dVector3 Origin, Direction;
+
+
+          dGeomRayGet(Rays[i], Origin, Direction);
+
+
+  
+
+
+          dReal Length = dGeomRayGetLength(Rays[i]);
+
+
+
+
+
+          dVector3 End;
+
+
+          End[0] = Origin[0] + (Direction[0] * Length);
+
+
+          End[1] = Origin[1] + (Direction[1] * Length);
+
+
+          End[2] = Origin[2] + (Direction[2] * Length);
+
+
+          End[3] = Origin[3] + (Direction[3] * Length);
+
+
+  
+
+
+          dsDrawLine(Origin, End);
+
+
+  }}
+
+
+}
+
+
+
+
+
+
+
+
+int main (int argc, char **argv)
+
+
+{
+
+
+  // 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();
+
+
+  contactgroup = dJointGroupCreate (0);
+
+
+  dWorldSetGravity (world,0,0,-0.5);
+
+
+  dWorldSetCFM (world,1e-5);
+
+
+  dCreatePlane (space,0,0,1,0);
+
+
+  memset (obj,0,sizeof(obj));
+
+
+
+
+
+  dVector3 Origin, Direction;
+
+
+  
+
+
+  RayCount = 5;
+
+
+  Rays = new dGeomID[RayCount];
+
+
+
+
+
+  /* Ray 0 */
+
+
+  Origin[0] = 1;
+
+
+  Origin[1] = 1;
+
+
+  Origin[2] = 1.5;
+
+
+  Origin[3] = 0;
+
+
+
+
+
+  Direction[0] = 0.0f;
+
+
+  Direction[1] = 0.0f;
+
+
+  Direction[2] = -1;
+
+
+  Direction[3] = 0;
+
+
+
+
+
+  dNormalize3(Direction);
+
+
+
+
+
+  Rays[0] = dGeomCreateRay(space, 5.0f);
+
+
+  dGeomRaySet(Rays[0], Origin, Direction);
+
+
+
+
+
+  /* Ray 1 */
+
+
+  Origin[0] = 0;
+
+
+  Origin[1] = 10;
+
+
+  Origin[2] = 0.25;
+
+
+  Origin[3] = 0;
+
+
+
+
+
+  Direction[0] = 0.0f;
+
+
+  Direction[1] = -1.0f;
+
+
+  Direction[2] = 0.0f;
+
+
+  Direction[3] = 0;
+
+
+
+
+
+  dNormalize3(Direction);
+
+
+
+
+
+  Rays[1] = dGeomCreateRay(space, 20.0f);
+
+
+  dGeomRaySet(Rays[1], Origin, Direction);
+
+
+
+
+
+  /* Ray 2 */
+
+
+  Origin[0] = -10;
+
+
+  Origin[1] = 0;
+
+
+  Origin[2] = 0.20;
+
+
+  Origin[3] = 0;
+
+
+
+
+
+  Direction[0] = 1.0f;
+
+
+  Direction[1] = 0.0f;
+
+
+  Direction[2] = 0.0f;
+
+
+  Direction[3] = 0;
+
+
+
+
+
+  dNormalize3(Direction);
+
+
+
+
+
+  Rays[2] = dGeomCreateRay(space, 20.0f);
+
+
+  dGeomRaySet(Rays[2], Origin, Direction);
+
+
+
+
+
+  /* Ray 3 */
+
+
+  Origin[0] = -9;
+
+
+  Origin[1] = 11;
+
+
+  Origin[2] = 0.15;
+
+
+  Origin[3] = 0;
+
+
+
+
+
+  Direction[0] = 1.0f;
+
+
+  Direction[1] = -1.0f;
+
+
+  Direction[2] = 0.0f;
+
+
+  Direction[3] = 0;
+
+
+
+
+
+  dNormalize3(Direction);
+
+
+
+
+
+  Rays[3] = dGeomCreateRay(space, 20.0f);
+
+
+  dGeomRaySet(Rays[3], Origin, Direction);
+
+
+
+
+
+  /* Ray 4 */
+
+
+  Origin[0] = -0.1;
+
+
+  Origin[1] = 0.3;
+
+
+  Origin[2] = 0.30;
+
+
+  Origin[3] = 0;
+
+
+
+
+
+  Direction[0] = 0.3f;
+
+
+  Direction[1] = 0.5f;
+
+
+  Direction[2] = 1.0f;
+
+
+  Direction[3] = 0;
+
+
+
+
+
+  Rays[4] = dGeomCreateRay(space, 5.0f);
+
+
+  dGeomRaySet(Rays[4], Origin, Direction);
+
+
+
+
+
+  // run simulation
+
+
+  dsSimulationLoop (argc,argv,352,288,&fn);
+
+
+
+
+
+  dJointGroupDestroy (contactgroup);
+
+
+  dSpaceDestroy (space);
+
+
+  dWorldDestroy (world);
+
+
+
+
+
+  return 0;
+
+
+}
+
+
diff --git a/libraries/ode-0.9/contrib/dRay/dRay.cpp b/libraries/ode-0.9/contrib/dRay/dRay.cpp
new file mode 100644
index 0000000..e3a426e
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/dRay.cpp
@@ -0,0 +1,119 @@
+#include "Include\dRay.h"
+#include "dxRay.h"
+
+int dRayClass = -1;
+
+void dAABBRay(dxGeom* Ray, dReal AABB[6]){
+        dVector3 Start, End;
+        dGeomRayGet(Ray, Start, End);
+        dReal Length = dGeomRayGetLength(Ray);
+
+        End[0] = Start[0] + End[0] * Length;
+        End[1] = Start[1] + End[1] * Length;
+        End[2] = Start[2] + End[2] * Length;
+        End[3] = Start[3] + End[3] * Length;
+
+        if (Start[0] < End[0]){
+                AABB[0] = Start[0];
+                AABB[1] = End[0];
+        }
+        else{
+                AABB[0] = End[0];
+                AABB[1] = Start[0];
+        }
+
+        if (Start[1] < End[1]){
+                AABB[2] = Start[1];
+                AABB[3] = End[1];
+        }
+        else{
+                AABB[2] = End[1];
+                AABB[3] = Start[1];
+        }
+
+        if (Start[2] < End[2]){
+                AABB[4] = Start[2];
+                AABB[5] = End[2];
+        }
+        else{
+                AABB[4] = End[2];
+                AABB[5] = Start[2];
+        }
+        // Should we tweak the box to have a minimum size for axis aligned lines? How small should it be?
+}
+
+dColliderFn* dRayColliderFn(int num){
+        if (num == dPlaneClass) return (dColliderFn*)&dCollidePR;
+        if (num == dSphereClass) return (dColliderFn*)&dCollideSR;
+        if (num == dBoxClass) return (dColliderFn*)&dCollideBR;
+        if (num == dCCylinderClass) return (dColliderFn*)&dCollideCCR;
+        return 0;
+}
+
+dxGeom* dGeomCreateRay(dSpaceID space, dReal Length){
+        if (dRayClass == -1){
+                dGeomClass c;
+                c.bytes = sizeof(dxRay);
+                c.collider = &dRayColliderFn;
+                c.aabb = &dAABBRay;
+                c.aabb_test = 0;
+                c.dtor = 0;
+
+                dRayClass = dCreateGeomClass(&c);
+        }
+
+        dxGeom* g = dCreateGeom(dRayClass);
+        if (space) dSpaceAdd(space, g);
+
+        dGeomRaySetLength(g, Length);
+        return g;
+}
+
+void dGeomRaySetLength(dxGeom* g, dReal Length){
+        ((dxRay*)dGeomGetClassData(g))->Length = Length;
+}
+
+dReal dGeomRayGetLength(dxGeom* g){
+        return ((dxRay*)dGeomGetClassData(g))->Length;
+}
+
+void dGeomRaySet(dxGeom* g, dVector3 Origin, dVector3 Direction){
+        dGeomSetPosition(g, Origin[0], Origin[1], Origin[2]);
+
+        dVector3 Up, Right;
+        dPlaneSpace(Direction, Up, Right);
+
+        Origin[3] = Up[3] = Right[3] = REAL(0.0);
+
+        dMatrix3 Rotation;
+        Rotation[0 * 4 + 0] = Right[0];
+        Rotation[1 * 4 + 0] = Right[1];
+        Rotation[2 * 4 + 0] = Right[2];
+        Rotation[3 * 4 + 0] = Right[3];
+
+        Rotation[0 * 4 + 1] = Up[0];
+        Rotation[1 * 4 + 1] = Up[1];
+        Rotation[2 * 4 + 1] = Up[2];
+        Rotation[3 * 4 + 1] = Up[3];
+
+        Rotation[0 * 4 + 2] = Direction[0];
+        Rotation[1 * 4 + 2] = Direction[1];
+        Rotation[2 * 4 + 2] = Direction[2];
+        Rotation[3 * 4 + 2] = Direction[3];
+
+        dGeomSetRotation(g, Rotation);
+}
+
+void dGeomRayGet(dxGeom* g, dVector3 Origin, dVector3 Direction){
+        const dReal* Position = dGeomGetPosition(g);
+        Origin[0] = Position[0];
+        Origin[1] = Position[1];
+        Origin[2] = Position[2];
+        Origin[3] = Position[3];
+
+        const dReal* Rotation = dGeomGetRotation(g);
+        Direction[0] = Rotation[0 * 4 + 2];
+        Direction[1] = Rotation[1 * 4 + 2];
+        Direction[2] = Rotation[2 * 4 + 2];
+        Direction[3] = Rotation[3 * 4 + 2];
+}
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_Box.cpp b/libraries/ode-0.9/contrib/dRay/dRay_Box.cpp
new file mode 100644
index 0000000..d2a0d9c
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/dRay_Box.cpp
@@ -0,0 +1,134 @@
+// Ripped from Magic Software
+
+#include "Include\dRay.h"
+#include "dxRay.h"
+
+bool Clip(dReal Denom, dReal Numer, dReal& T0, dReal& T1){
+    // Return value is 'true' if line segment intersects the current test
+    // plane.  Otherwise 'false' is returned in which case the line segment
+    // is entirely clipped.
+        
+    if (Denom > REAL(0.0)){
+                if (Numer > Denom * T1){
+            return false;
+                }
+
+        if (Numer > Denom * T0){
+            T0 = Numer / Denom;
+                }
+        return true;
+    }
+    else if (Denom < REAL(0.0)){
+        if (Numer > Denom * T0){
+            return false;
+                }
+
+        if (Numer > Denom * T1){
+            T1 = Numer / Denom;
+                }
+        return true;
+    }
+    else return Numer <= REAL(0.0);
+}
+
+bool FindIntersection(const dVector3 Origin, const dVector3 Direction, const dVector3 Extents, dReal& T0, dReal& T1){
+    dReal SaveT0 = T0;
+        dReal SaveT1 = T1;
+
+    bool NotEntirelyClipped =
+        Clip(+Direction[0], -Origin[0] - Extents[0], T0, T1) &&
+        Clip(-Direction[0], +Origin[0] - Extents[0], T0, T1) &&
+        Clip(+Direction[1], -Origin[1] - Extents[1], T0, T1) &&
+        Clip(-Direction[1], +Origin[1] - Extents[1], T0, T1) &&
+        Clip(+Direction[2], -Origin[2] - Extents[2], T0, T1) &&
+        Clip(-Direction[2], +Origin[2] - Extents[2], T0, T1);
+
+    return NotEntirelyClipped && (T0 != SaveT0 || T1 != SaveT1);
+}
+
+int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
+        const dVector3& Position = *(const dVector3*)dGeomGetPosition(BoxGeom);
+        const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(BoxGeom);
+        dVector3 Extents;
+        dGeomBoxGetLengths(BoxGeom, Extents);
+        Extents[0] /= 2;
+        Extents[1] /= 2;
+        Extents[2] /= 2;
+        Extents[3] /= 2;
+
+        dVector3 Origin, Direction;
+        dGeomRayGet(RayGeom, Origin, Direction);
+        dReal Length = dGeomRayGetLength(RayGeom);
+
+        dVector3 Diff;
+        Diff[0] = Origin[0] - Position[0];
+        Diff[1] = Origin[1] - Position[1];
+        Diff[2] = Origin[2] - Position[2];
+        Diff[3] = Origin[3] - Position[3];
+
+        Direction[0] *= Length;
+        Direction[1] *= Length;
+        Direction[2] *= Length;
+        Direction[3] *= Length;
+
+        dVector3 Rot[3];
+        Decompose(Rotation, Rot);
+
+        dVector3 TransOrigin;
+        TransOrigin[0] = dDOT(Diff, Rot[0]);
+        TransOrigin[1] = dDOT(Diff, Rot[1]);
+        TransOrigin[2] = dDOT(Diff, Rot[2]);
+        TransOrigin[3] = REAL(0.0);
+
+        dVector3 TransDirection;
+        TransDirection[0] = dDOT(Direction, Rot[0]);
+        TransDirection[1] = dDOT(Direction, Rot[1]);
+        TransDirection[2] = dDOT(Direction, Rot[2]);
+        TransDirection[3] = REAL(0.0);
+
+        dReal T[2];
+        T[0] = 0.0f;
+        T[1] = dInfinity;
+
+        bool Intersect = FindIntersection(TransOrigin, TransDirection, Extents, T[0], T[1]);
+
+        if (Intersect){
+                if (T[0] > REAL(0.0)){
+                        dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
+                        Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
+                        Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
+                        Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
+                        Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
+                        //Contact0->normal = 0;
+                        Contact0->depth = 0.0f;
+                        Contact0->g1 = RayGeom;
+                        Contact0->g2 = BoxGeom;
+
+                        dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
+                        Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
+                        Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
+                        Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
+                        Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
+                        //Contact1->normal = 0;
+                        Contact1->depth = 0.0f;
+                        Contact1->g1 = RayGeom;
+                        Contact1->g2 = BoxGeom;
+
+                        return 2;
+                }
+                else{
+                        dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
+                        Contact->pos[0] = Origin[0] + T[1] * Direction[0];
+                        Contact->pos[1] = Origin[1] + T[1] * Direction[1];
+                        Contact->pos[2] = Origin[2] + T[1] * Direction[2];
+                        Contact->pos[3] = Origin[3] + T[1] * Direction[3];
+                        //Contact->normal = 0;
+                        Contact->depth = 0.0f;
+                        Contact->g1 = RayGeom;
+                        Contact->g2 = BoxGeom;
+
+                        return 1;
+                }
+        }
+        else return 0;
+}
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp b/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
new file mode 100644
index 0000000..b9ea0c0
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/dRay_CCylinder.cpp
@@ -0,0 +1,199 @@
+// Ripped from Magic Software
+
+#include "Include\dRay.h"
+#include "dxRay.h"
+
+int Find(const dVector3 Origin, dVector3 Direction, dReal Length, const dVector3 CCPos, const dMatrix3 CCRot, dReal CCRadius, dReal CCLength, dReal T[2]){
+        dVector3 U, V, W;
+        Decompose(CCRot, U, V, W);
+
+        dVector3 CCOrigin;
+        CCOrigin[0] = CCPos[0] - (W[0] * CCLength / 2);
+        CCOrigin[1] = CCPos[1] - (W[1] * CCLength / 2);
+        CCOrigin[2] = CCPos[2] - (W[2] * CCLength / 2);
+        CCOrigin[3] = CCPos[3] - (W[3] * CCLength / 2);
+        
+        dVector3 D;
+        D[0] = dDOT(U, Direction);
+        D[1] = dDOT(V, Direction);
+        D[2] = dDOT(W, Direction);
+
+        dReal DMag = Length;
+        dReal InvDMag = REAL(1.0) / DMag;
+
+        dVector3 Diff;
+        Diff[0] = Origin[0] - CCOrigin[0];
+        Diff[1] = Origin[1] - CCOrigin[1];
+        Diff[2] = Origin[2] - CCOrigin[2];
+        Diff[3] = Origin[3] - CCOrigin[3];
+
+        dVector3 P;
+        P[0] = dDOT(U, Diff);
+        P[1] = dDOT(V, Diff);
+        P[2] = dDOT(W, Diff);
+
+        dReal CCRadiusSq = CCRadius * CCRadius;
+
+        dReal Epsilon = 1e-12f;
+
+        if (dFabs(D[2]) >= REAL(1.0) - Epsilon){        // line is parallel to capsule axis
+                dReal Discr =  CCRadiusSq - P[0] * P[0] - P[1] * P[1];
+
+                if (Discr >= REAL(0.0)){
+            dReal Root = dSqrt(Discr);
+            T[0] = (-P[2] + Root) * InvDMag;
+            T[1] = (CCLength - P[2] + Root) * InvDMag;
+            return 2;
+        }
+        else return 0;
+        }
+
+        // test intersection with infinite cylinder
+    dReal A = D[0] * D[0] + D[1] * D[1];
+    dReal B = P[0] * D[0] + P[1] * D[1];
+    dReal C = P[0] * P[0] + P[1] * P[1] - CCRadiusSq;
+    dReal Discr = B * B - A * C;
+    if (Discr < REAL(0.0)){     // line does not intersect infinite cylinder
+        return 0;
+    }
+
+        int Count = 0;
+
+    if (Discr > REAL(0.0)){     // line intersects infinite cylinder in two places
+        dReal Root = dSqrt(Discr);
+        dReal Inv = REAL(1.0) / A;
+
+                dReal TTemp = (-B - Root) * Inv;
+
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (REAL(0.0) <= Tmp && Tmp <= CCLength){
+                        T[Count++] = TTemp * InvDMag;
+                }
+                
+                
+        TTemp = (-B + Root) * Inv;
+        Tmp = P[2] + TTemp * D[2];
+        if (REAL(0.0) <= Tmp && Tmp <= CCLength){
+                        T[Count++] = TTemp * InvDMag;
+                }
+
+        if (Count == 2){        // line intersects capsule wall in two places
+            return 2;
+        }
+    }
+        else{   // line is tangent to infinite cylinder
+        dReal TTemp = -B / A;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (REAL(0.0) <= Tmp && Tmp <= CCLength){
+            T[0] = TTemp * InvDMag;
+            return 1;
+        }
+    }
+
+        // test intersection with bottom hemisphere
+    // fA = 1
+    B += P[2] * D[2];
+    C += P[2] * P[2];
+    Discr = B * B - C;
+    if (Discr > REAL(0.0)){
+        dReal Root = dSqrt(Discr);
+        dReal TTemp = -B - Root;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (Tmp <= REAL(0.0)){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+
+        TTemp = -B + Root;
+        Tmp = P[2] + TTemp * D[2];
+        if (Tmp <= REAL(0.0)){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+    }
+    else if (Discr == REAL(0.0)){
+        dReal TTemp = -B;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (Tmp <= REAL(0.0)){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+    }
+
+        // test intersection with top hemisphere
+    // fA = 1
+    B -= D[2] * CCLength;
+    C += CCLength * (CCLength - REAL(2.0) * P[2]);
+
+    Discr = B * B - C;
+    if (Discr > REAL(0.0)){
+        dReal Root = dSqrt(Discr);
+        dReal TTemp = -B - Root;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (Tmp >= CCLength){
+
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+
+        TTemp = -B + Root;
+        Tmp = P[2] + TTemp * D[2];
+        if (Tmp >= CCLength){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+    }
+    else if (Discr == REAL(0.0)){
+        dReal TTemp = -B;
+        dReal Tmp = P[2] + TTemp * D[2];
+        if (Tmp >= CCLength){
+            T[Count++] = TTemp * InvDMag;
+            if (Count == 2){
+                return 2;
+                        }
+        }
+    }
+        return Count;
+}
+
+int dCollideCCR(dxGeom* RayGeom, dxGeom* CCGeom, int Flags, dContactGeom* Contacts, int Stride){
+        const dVector3& CCPos = *(const dVector3*)dGeomGetPosition(CCGeom);
+        const dMatrix3& CCRot = *(const dMatrix3*)dGeomGetRotation(CCGeom);
+
+        dReal CCRadius, CCLength;
+        dGeomCCylinderGetParams(CCGeom, &CCRadius, &CCLength);
+
+        dVector3 Origin, Direction;
+        dGeomRayGet(RayGeom, Origin, Direction);
+        dReal Length = dGeomRayGetLength(RayGeom);
+
+        dReal T[2];
+    int Count = Find(Origin, Direction, Length, CCPos, CCRot, CCRadius, CCLength, T);
+        int ContactCount = 0;
+        for (int i = 0; i < Count; i++){
+                if (T[i] >= 0.0){
+                        dContactGeom* Contact = CONTACT(Flags, Contacts, ContactCount, Stride);
+                        Contact->pos[0] = Origin[0] + T[i] * Direction[0] * Length;
+                        Contact->pos[1] = Origin[1] + T[i] * Direction[1] * Length;
+                        Contact->pos[2] = Origin[2] + T[i] * Direction[2] * Length;
+                        Contact->pos[3] = Origin[3] + T[i] * Direction[3] * Length;
+                        //Contact->normal = 0;
+                        Contact->depth = 0.0f;
+                        Contact->g1 = RayGeom;
+                        Contact->g2 = CCGeom;
+
+                        ContactCount++;
+                }
+        }
+    return ContactCount;
+}
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp b/libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp
new file mode 100644
index 0000000..cf03c5b
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/dRay_Plane.cpp
@@ -0,0 +1,35 @@
+// Ripped from Paul Bourke
+
+#include "Include\dRay.h"
+#include "dxRay.h"
+
+int dCollidePR(dxGeom* RayGeom, dxGeom* PlaneGeom, int Flags, dContactGeom* Contact, int Stride){
+        dVector3 Plane;
+        dGeomPlaneGetParams(PlaneGeom, Plane);
+
+        dVector3 Origin, Direction;
+        dGeomRayGet(RayGeom, Origin, Direction);
+
+        dReal Length = dGeomRayGetLength(RayGeom);
+
+        dReal Denom = Plane[0] * Direction[0] + Plane[1] * Direction[1] + Plane[2] * Direction[2];
+        if (dFabs(Denom) < 0.00001f){
+                return 0;       // Ray never hits
+        }
+        
+        float T = -(Plane[3] + Plane[0] * Origin[0] + Plane[1] * Origin[1] + Plane[2] * Origin[2]) / Denom;
+        
+        if (T < 0 || T > Length){
+                return 0;       // Ray hits but not within boundaries
+        }
+
+        Contact->pos[0] = Origin[0] + T * Direction[0];
+        Contact->pos[1] = Origin[1] + T * Direction[1];
+        Contact->pos[2] = Origin[2] + T * Direction[2];
+        Contact->pos[3] = REAL(0.0);
+        //Contact->normal = 0;
+        Contact->depth = 0.0f;
+        Contact->g1 = RayGeom;
+        Contact->g2 = PlaneGeom;
+        return 1;
+}
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp b/libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp
new file mode 100644
index 0000000..8e1ac39
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/dRay_Sphere.cpp
@@ -0,0 +1,95 @@
+// Ripped from Magic Software
+
+#include "Include\dRay.h"
+#include "dxRay.h"
+
+int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
+        const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
+        dReal Radius = dGeomSphereGetRadius(SphereGeom);
+
+        dVector3 Origin, Direction;
+        dGeomRayGet(RayGeom, Origin, Direction);
+        dReal Length = dGeomRayGetLength(RayGeom);
+
+        dVector3 Diff;
+        Diff[0] = Origin[0] - Position[0];
+        Diff[1] = Origin[1] - Position[1];
+        Diff[2] = Origin[2] - Position[2];
+        Diff[3] = Origin[3] - Position[3];
+
+        Direction[0] *= Length;
+        Direction[1] *= Length;
+        Direction[2] *= Length;
+        Direction[3] *= Length;
+
+        dReal A = Length * Length;
+        dReal B = dDOT(Diff, Direction);
+        dReal C = dDOT(Diff, Diff) - (Radius * Radius);
+
+        dReal Discr = B * B - A * C;
+        if (Discr < REAL(0.0)){
+                return 0;
+        }
+        else if (Discr > REAL(0.0)){
+                dReal T[2];
+                dReal Root = dSqrt(Discr);
+                dReal InvA = REAL(1.0) / A;
+                T[0] = (-B - Root) * InvA;
+                T[1] = (-B + Root) * InvA;
+
+                if (T[0] >= REAL(0.0)){
+                        dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
+                        Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
+                        Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
+                        Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
+                        Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
+                        //Contact0->normal = 0;
+                        Contact0->depth = 0.0f;
+                        Contact0->g1 = RayGeom;
+                        Contact0->g2 = SphereGeom;
+
+                        dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
+                        Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
+                        Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
+                        Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
+                        Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
+                        //Contact1->normal = 0;
+                        Contact1->depth = 0.0f;
+                        Contact1->g1 = RayGeom;
+                        Contact1->g2 = SphereGeom;
+
+                        return 2;
+                }
+                else if (T[1] >= REAL(0.0)){
+                        dContactGeom* Contact = CONTACT(Flags, Contacts, 1, Stride);
+                        Contact->pos[0] = Origin[0] + T[1] * Direction[0];
+                        Contact->pos[1] = Origin[1] + T[1] * Direction[1];
+                        Contact->pos[2] = Origin[2] + T[1] * Direction[2];
+                        Contact->pos[3] = Origin[3] + T[1] * Direction[3];
+                        //Contact->normal = 0;
+                        Contact->depth = 0.0f;
+                        Contact->g1 = RayGeom;
+                        Contact->g2 = SphereGeom;
+
+            return 1;
+                }
+                else return 0;
+        }
+        else{
+                dReal T;
+                T = -B / A;
+                if (T >= REAL(0.0)){
+                        dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
+                        Contact->pos[0] = Origin[0] + T * Direction[0];
+                        Contact->pos[1] = Origin[1] + T * Direction[1];
+                        Contact->pos[2] = Origin[2] + T * Direction[2];
+                        Contact->pos[3] = Origin[3] + T * Direction[3];
+                        //Contact->normal = 0;
+                        Contact->depth = 0.0f;
+                        Contact->g1 = RayGeom;
+                        Contact->g2 = SphereGeom;
+            return 1;
+                }
+                else return 0;
+        }
+}
\ No newline at end of file
diff --git a/libraries/ode-0.9/contrib/dRay/dxRay.h b/libraries/ode-0.9/contrib/dRay/dxRay.h
new file mode 100644
index 0000000..0fd1d2d
--- /dev/null
+++ b/libraries/ode-0.9/contrib/dRay/dxRay.h
@@ -0,0 +1,32 @@
+struct dxRay{
+        dReal Length;
+};
+
+inline void Decompose(const dMatrix3 Matrix, dVector3 Right, dVector3 Up, dVector3 Direction){
+        Right[0] = Matrix[0 * 4 + 0];
+        Right[1] = Matrix[1 * 4 + 0];
+        Right[2] = Matrix[2 * 4 + 0];
+        Right[3] = Matrix[3 * 4 + 0];
+        Up[0] = Matrix[0 * 4 + 1];
+        Up[1] = Matrix[1 * 4 + 1];
+        Up[2] = Matrix[2 * 4 + 1];
+        Up[3] = Matrix[3 * 4 + 1];
+        Direction[0] = Matrix[0 * 4 + 2];
+        Direction[1] = Matrix[1 * 4 + 2];
+        Direction[2] = Matrix[2 * 4 + 2];
+        Direction[3] = Matrix[3 * 4 + 2];
+}
+
+inline void Decompose(const dMatrix3 Matrix, dVector3 Vectors[3]){
+        Decompose(Matrix, Vectors[0], Vectors[1], Vectors[2]);
+}
+
+inline dContactGeom* CONTACT(int Flags, dContactGeom* Contacts, int Index, int Stride){
+        dIASSERT(Index >= 0 && Index < (Flags & 0x0ffff));
+        return ((dContactGeom*)(((char*)Contacts) + (Index * Stride)));
+}
+
+int dCollidePR(dxGeom* RayGeom, dxGeom* PlaneGeom, int Flags, dContactGeom* Contacts, int Stride);
+int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride);
+int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride);
+int dCollideCCR(dxGeom* RayGeom, dxGeom* CCylinderGeom, int Flags, dContactGeom* Contacts, int Stride);
\ No newline at end of file