diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt
index a24248c44..a5d95e7f8 100644
--- a/src/BulletDynamics/CMakeLists.txt
+++ b/src/BulletDynamics/CMakeLists.txt
@@ -20,6 +20,18 @@ ADD_LIBRARY(LibBulletDynamics
 	ConstraintSolver/btSolve2LinearConstraint.h
 	ConstraintSolver/btTypedConstraint.cpp
 	ConstraintSolver/btTypedConstraint.h
+	ConstraintSolver/btOdeContactJoint.cpp
+	ConstraintSolver/btOdeContactJoint.h
+	ConstraintSolver/btOdeJoint.cpp
+	ConstraintSolver/btOdeJoint.h
+	ConstraintSolver/btOdeQuickstepConstraintSolver.cpp
+	ConstraintSolver/btOdeQuickstepConstraintSolver.h
+	ConstraintSolver/btOdeTypedJoint.cpp
+	ConstraintSolver/btOdeTypedJoint.h
+	ConstraintSolver/btSorLcp.cpp
+	ConstraintSolver/btSorLcp.h
+	ConstraintSolver/btOdeQuickstepConstraintSolver.h
+	ConstraintSolver/btOdeMacros.h	
 	Dynamics/Bullet-C-API.cpp
 	Dynamics/btDiscreteDynamicsWorld.cpp
 	Dynamics/btDiscreteDynamicsWorld.h
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp b/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp
new file mode 100644
index 000000000..83f3d4c88
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp
@@ -0,0 +1,277 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btOdeContactJoint.h"
+#include "btOdeSolverBody.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+
+//this constant needs to be set up so different solvers give 'similar' results
+#define FRICTION_CONSTANT 120.f
+
+
+btOdeContactJoint::btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1)
+:m_manifold(manifold),
+m_index(index),
+m_swapBodies(swap),
+m_body0(body0),
+m_body1(body1)
+{
+}
+
+int m_numRows = 3;
+
+
+void btOdeContactJoint::GetInfo1(Info1 *info)
+{
+	info->m = m_numRows;
+	//friction adds another 2...
+	
+	info->nub = 0;
+}
+
+#define dCROSS(a,op,b,c) \
+  (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
+  (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
+  (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
+
+#define M_SQRT12 btScalar(0.7071067811865475244008443621048490)
+
+#define dRecipSqrt(x) ((float)(1.0f/btSqrt(float(x))))		/* reciprocal square root */
+
+
+
+void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q)
+{
+  if (btFabs(n[2]) > M_SQRT12) {
+    // choose p in y-z plane
+    btScalar a = n[1]*n[1] + n[2]*n[2];
+    btScalar k = dRecipSqrt (a);
+    p[0] = 0;
+    p[1] = -n[2]*k;
+    p[2] = n[1]*k;
+    // set q = n x p
+    q[0] = a*k;
+    q[1] = -n[0]*p[2];
+    q[2] = n[0]*p[1];
+  }
+  else {
+    // choose p in x-y plane
+    btScalar a = n[0]*n[0] + n[1]*n[1];
+    btScalar k = dRecipSqrt (a);
+    p[0] = -n[1]*k;
+    p[1] = n[0]*k;
+    p[2] = 0;
+    // set q = n x p
+    q[0] = -n[2]*p[1];
+    q[1] = n[2]*p[0];
+    q[2] = a*k;
+  }
+}
+
+
+
+void btOdeContactJoint::GetInfo2(Info2 *info)
+{
+	
+	int s = info->rowskip;
+	int s2 = 2*s;
+	
+	float swapFactor = m_swapBodies ? -1.f : 1.f;
+	
+	// get normal, with sign adjusted for body1/body2 polarity
+	dVector3 normal;
+	
+	
+	btManifoldPoint& point = m_manifold->getContactPoint(m_index);
+	
+	normal[0] = swapFactor*point.m_normalWorldOnB.x();
+	normal[1] = swapFactor*point.m_normalWorldOnB.y();
+	normal[2] = swapFactor*point.m_normalWorldOnB.z();
+	normal[3] = 0;	// @@@ hmmm
+	
+	assert(m_body0);
+	//	if (GetBody0())
+	btVector3 relativePositionA;
+	{
+		relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition;
+		dVector3 c1;
+		c1[0] = relativePositionA.x();
+		c1[1] = relativePositionA.y();
+		c1[2] = relativePositionA.z();
+		
+		// set jacobian for normal
+		info->J1l[0] = normal[0];
+		info->J1l[1] = normal[1];
+		info->J1l[2] = normal[2];
+		dCROSS (info->J1a,=,c1,normal);
+		
+	}
+
+	btVector3 relativePositionB(0,0,0);
+	if (m_body1)
+	{
+		//		if (GetBody1())
+		
+		{
+			dVector3 c2;
+			btVector3	posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0);
+			relativePositionB = point.getPositionWorldOnB() - posBody1;
+			
+			//			for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
+			//					  j->node[1].body->pos[i];
+			c2[0] = relativePositionB.x();
+			c2[1] = relativePositionB.y();
+			c2[2] = relativePositionB.z();
+			
+			info->J2l[0] = -normal[0];
+			info->J2l[1] = -normal[1];
+			info->J2l[2] = -normal[2];
+			dCROSS (info->J2a,= -,c2,normal);
+		}
+	}
+	
+	btScalar k = info->fps * info->erp;
+	
+	float depth = -point.getDistance();
+//	if (depth < 0.f)
+//		depth = 0.f;
+	
+	info->c[0] = k * depth;
+	//float maxvel = .2f;
+
+//	if (info->c[0] > maxvel)
+//		info->c[0] = maxvel;
+
+
+	//can override it, not necessary
+//	info->cfm[0] = 0.f;
+//	info->cfm[1] = 0.f;
+//	info->cfm[2] = 0.f;
+	
+	
+	
+	// set LCP limits for normal
+	info->lo[0] = 0;
+	info->hi[0] = 1e30f;//dInfinity;
+	info->lo[1] = 0;
+	info->hi[1] = 0.f;
+	info->lo[2] = 0.f;
+	info->hi[2] = 0.f;
+
+#define DO_THE_FRICTION_2
+#ifdef DO_THE_FRICTION_2
+	// now do jacobian for tangential forces
+	dVector3 t1,t2;	// two vectors tangential to normal
+	
+	dVector3 c1;
+	c1[0] = relativePositionA.x();
+	c1[1] = relativePositionA.y();
+	c1[2] = relativePositionA.z();
+	
+	dVector3 c2;
+	c2[0] = relativePositionB.x();
+	c2[1] = relativePositionB.y();
+	c2[2] = relativePositionB.z();
+	
+	//combined friction is available in the contact point
+	float friction = 0.25;//FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction;
+	
+	// first friction direction
+	if (m_numRows >= 2) 
+	{
+		
+		
+		
+		dPlaneSpace1 (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 (1) { //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 (0) {//j->contact.surface.mode & dContactMotion1) {
+			//info->c[1] = j->contact.surface.motion1;
+		}
+		// set LCP bounds and friction index. this depends on the approximation
+		// mode
+		//1e30f
+		
+		
+		info->lo[1] = -friction;//-j->contact.surface.mu;
+		info->hi[1] = friction;//j->contact.surface.mu;
+		if (1)//j->contact.surface.mode & dContactApprox1_1) 
+			info->findex[1] = 0;
+		
+		// set slip (constraint force mixing)
+		if (0)//j->contact.surface.mode & dContactSlip1)
+		{
+			//	info->cfm[1] = j->contact.surface.slip1;
+		} else
+		{
+			//info->cfm[1] = 0.f;
+		}
+	}
+	
+	// second friction direction
+	if (m_numRows >= 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 (1) { //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 (0){//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 (0){//j->contact.surface.mode & dContactMu2) {
+			//info->lo[2] = -j->contact.surface.mu2;
+			//info->hi[2] = j->contact.surface.mu2;
+		}
+		else {
+			info->lo[2] = -friction;
+			info->hi[2] = friction;
+		}
+		if (0)//j->contact.surface.mode & dContactApprox1_2) 
+			
+		{
+			info->findex[2] = 0;
+		}
+		// set slip (constraint force mixing)
+		if (0) //j->contact.surface.mode & dContactSlip2)
+			
+		{
+			//info->cfm[2] = j->contact.surface.slip2;
+			
+		}
+	}
+	
+#endif //DO_THE_FRICTION_2
+	
+}
+
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h b/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h
new file mode 100644
index 000000000..10a6f7684
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h
@@ -0,0 +1,50 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef CONTACT_JOINT_H
+#define CONTACT_JOINT_H
+
+#include "btOdeJoint.h"
+struct btOdeSolverBody;
+class btPersistentManifold;
+
+class btOdeContactJoint : public btOdeJoint
+{
+	btPersistentManifold* m_manifold;
+	int		m_index;
+	bool	m_swapBodies;
+	btOdeSolverBody* m_body0;
+	btOdeSolverBody* m_body1;
+
+
+public:
+
+	btOdeContactJoint() {};
+
+	btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1);
+
+	//btOdeJoint interface for solver
+
+	virtual void GetInfo1(Info1 *info);
+
+	virtual void GetInfo2(Info2 *info);
+
+
+	
+
+};
+
+#endif //CONTACT_JOINT_H
+
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp b/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp
new file mode 100644
index 000000000..b8bcc0f40
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp
@@ -0,0 +1,25 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btOdeJoint.h"
+
+btOdeJoint::btOdeJoint()
+{
+
+}
+btOdeJoint::~btOdeJoint()
+{
+
+}
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeJoint.h b/src/BulletDynamics/ConstraintSolver/btOdeJoint.h
new file mode 100644
index 000000000..7fae83067
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeJoint.h
@@ -0,0 +1,94 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef btOdeJoint_H
+#define btOdeJoint_H
+
+struct btOdeSolverBody;
+class btOdeJoint;
+
+#include "LinearMath/btScalar.h"
+
+struct BU_ContactJointNode {
+  btOdeJoint *joint;		// pointer to enclosing btOdeJoint object
+  btOdeSolverBody* body;			// *other* body this joint is connected to
+};
+typedef btScalar dVector3[4];
+
+
+class btOdeJoint  {
+  
+public:
+	// 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).
+
+  btOdeJoint();
+  virtual ~btOdeJoint();
+  
+
+  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).
+    btScalar 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.
+    btScalar *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.
+    btScalar *c,*cfm;
+
+    // lo and hi limits for variables (set to -/+ infinity on entry).
+    btScalar *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.
+
+  virtual void GetInfo1 (Info1 *info)=0;
+  virtual void GetInfo2 (Info2 *info)=0;
+
+  int flags;			// dJOINT_xxx flags
+  BU_ContactJointNode node[2];		// connections to bodies. node[1].body can be 0
+   btScalar lambda[6];		// lambda generated by last step
+};
+
+
+#endif //btOdeJoint_H
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeMacros.h b/src/BulletDynamics/ConstraintSolver/btOdeMacros.h
new file mode 100644
index 000000000..614198bd3
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeMacros.h
@@ -0,0 +1,212 @@
+/*
+ * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
+ Bullet Continuous Collision Detection and Physics Library
+ Bullet is Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#define ODE_MACROS
+#ifdef ODE_MACROS
+
+#include "LinearMath/btScalar.h"
+
+typedef btScalar dVector4[4];
+typedef btScalar dMatrix3[4*3];
+#define dInfinity FLT_MAX
+
+
+
+#define dRecip(x) ((float)(1.0f/(x)))				/* reciprocal */
+
+
+
+#define dMULTIPLY0_331NEW(A,op,B,C) \
+{\
+	btScalar tmp[3];\
+	tmp[0] = C.getX();\
+	tmp[1] = C.getY();\
+	tmp[2] = C.getZ();\
+	dMULTIPLYOP0_331(A,op,B,tmp);\
+}
+
+#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
+#define dMULTIPLYOP0_331(A,op,B,C) \
+  (A)[0] op dDOT1((B),(C)); \
+  (A)[1] op dDOT1((B+4),(C)); \
+  (A)[2] op dDOT1((B+8),(C));
+
+#define dAASSERT btAssert
+#define dIASSERT btAssert
+
+#define REAL float
+#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
+inline btScalar dDOT1 (const btScalar *a, const btScalar *b)
+{ return dDOTpq(a,b,1,1); }
+#define dDOT14(a,b) dDOTpq(a,b,1,4)
+
+#define dCROSS(a,op,b,c) \
+  (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
+  (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
+  (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
+
+/*
+ * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
+ * A is stored by rows, and has `skip' elements per row. the matrix is
+ * assumed to be already zero, so this does not write zero elements!
+ * if (plus,minus) is (+,-) then a positive version will be written.
+ * if (plus,minus) is (-,+) then a negative version will be written.
+ */
+
+#define dCROSSMAT(A,a,skip,plus,minus) \
+do { \
+  (A)[1] = minus (a)[2]; \
+  (A)[2] = plus (a)[1]; \
+  (A)[(skip)+0] = plus (a)[2]; \
+  (A)[(skip)+2] = minus (a)[0]; \
+  (A)[2*(skip)+0] = minus (a)[1]; \
+  (A)[2*(skip)+1] = plus (a)[0]; \
+} while(0)
+
+
+#define dMULTIPLYOP2_333(A,op,B,C) \
+  (A)[0] op dDOT1((B),(C)); \
+  (A)[1] op dDOT1((B),(C+4)); \
+  (A)[2] op dDOT1((B),(C+8)); \
+  (A)[4] op dDOT1((B+4),(C)); \
+  (A)[5] op dDOT1((B+4),(C+4)); \
+  (A)[6] op dDOT1((B+4),(C+8)); \
+  (A)[8] op dDOT1((B+8),(C)); \
+  (A)[9] op dDOT1((B+8),(C+4)); \
+  (A)[10] op dDOT1((B+8),(C+8));
+
+#define dMULTIPLYOP0_333(A,op,B,C) \
+  (A)[0] op dDOT14((B),(C)); \
+  (A)[1] op dDOT14((B),(C+1)); \
+  (A)[2] op dDOT14((B),(C+2)); \
+  (A)[4] op dDOT14((B+4),(C)); \
+  (A)[5] op dDOT14((B+4),(C+1)); \
+  (A)[6] op dDOT14((B+4),(C+2)); \
+  (A)[8] op dDOT14((B+8),(C)); \
+  (A)[9] op dDOT14((B+8),(C+1)); \
+  (A)[10] op dDOT14((B+8),(C+2));
+
+#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C)
+#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C)
+#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C)
+
+
+////////////////////////////////////////////////////////////////////
+#define EFFICIENT_ALIGNMENT 16
+#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1)
+/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
+ * up to 15 bytes per allocation, depending on what alloca() returns.
+ */
+
+#define dALLOCA16(n) \
+  ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))
+
+//#define ALLOCA dALLOCA16
+
+typedef const btScalar *dRealPtr;
+typedef btScalar *dRealMutablePtr;
+//#define dRealArray(name,n) btScalar name[n];
+//#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar));
+
+///////////////////////////////////////////////////////////////////////////////
+
+ //Remotion: 10.10.2007
+#define ALLOCA(size) stackAlloc->allocate( dEFFICIENT_SIZE(size) );
+
+//#define dRealAllocaArray(name,size) btScalar *name = (btScalar*) stackAlloc->allocate(dEFFICIENT_SIZE(size)*sizeof(btScalar));
+#define dRealAllocaArray(name,size) btScalar *name = NULL; \
+	int memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \
+	if (memNeeded_##name < stackAlloc->getAvailableMemory()) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \
+	else{ btAssert(memNeeded_##name < stackAlloc->getAvailableMemory()); name = (btScalar*) alloca(memNeeded_##name); } 
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+#if 0
+inline void dSetZero1 (btScalar *a, int n)
+{
+  dAASSERT (a && n >= 0);
+  while (n > 0) {
+    *(a++) = 0;
+    n--;
+  }
+}
+
+inline void dSetValue1 (btScalar *a, int n, btScalar value)
+{
+  dAASSERT (a && n >= 0);
+  while (n > 0) {
+    *(a++) = value;
+    n--;
+  }
+}
+#else
+
+/// This macros are for MSVC and XCode compilers. Remotion.
+
+
+#include <string.h> //for memset
+
+//Remotion: 10.10.2007
+//------------------------------------------------------------------------------
+#define IS_ALIGNED_16(x)	((size_t(x)&15)==0)
+//------------------------------------------------------------------------------
+inline void dSetZero1 (btScalar *dest, int size)
+{
+	dAASSERT (dest && size >= 0);
+	memset(dest, 0, size * sizeof(btScalar));
+}
+//------------------------------------------------------------------------------
+inline void dSetValue1 (btScalar *dest, int size, btScalar val)
+{
+	dAASSERT (dest && size >= 0);
+	int n_mod4 = size & 3;		
+	int n4 = size - n_mod4;
+/*#ifdef __USE_SSE__
+//it is not supported on double precision, todo...
+	if(IS_ALIGNED_16(dest)){
+		__m128 xmm0 = _mm_set_ps1(val);
+		for (int i=0; i<n4; i+=4)
+		{
+			_mm_store_ps(&dest[i],xmm0);
+		}
+	}else
+#endif
+	*/
+
+	{
+		for (int i=0; i<n4; i+=4) // Unrolled Loop
+		{
+			dest[i  ] = val;
+			dest[i+1] = val;
+			dest[i+2] = val;
+			dest[i+3] = val;
+		}
+	}
+	for (int  i=n4; i<size; i++){
+		dest[i] = val;
+	}
+}
+#endif
+/////////////////////////////////////////////////////////////////////
+
+
+#endif //USE_SOR_SOLVER
+
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp
new file mode 100644
index 000000000..1e59e77a9
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp
@@ -0,0 +1,405 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "btOdeQuickstepConstraintSolver.h"
+
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "btOdeJoint.h"
+#include "btOdeContactJoint.h"
+#include "btOdeTypedJoint.h"
+#include "btOdeSolverBody.h"
+#include <new.h>
+#include "LinearMath/btQuickprof.h"
+
+#include "LinearMath/btIDebugDraw.h"
+
+#define USE_SOR_SOLVER
+
+#include "btSorLcp.h"
+
+#include <math.h>
+#include <float.h>//FLT_MAX
+#ifdef WIN32
+#include <memory.h>
+#endif
+#include <string.h>
+#include <stdio.h>
+
+#if defined (WIN32)
+#include <malloc.h>
+#else
+#if defined (__FreeBSD__)
+#include <stdlib.h>
+#else
+#include <alloca.h>
+#endif
+#endif
+
+class btOdeJoint;
+
+//see below
+//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island
+
+
+btOdeQuickstepConstraintSolver::btOdeQuickstepConstraintSolver():
+        m_cfm(0.f),//1e-5f),
+        m_erp(0.4f)
+{
+}
+
+
+//iterative lcp and penalty method
+btScalar btOdeQuickstepConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
+{
+
+    m_CurBody = 0;
+    m_CurJoint = 0;
+    m_CurTypedJoint = 0;
+	int j;
+
+	int max_contacts = 0; /// should be 4 //Remotion
+	for ( j=0;j<numManifolds;j++){
+		btPersistentManifold* manifold = manifoldPtr[j];
+		if (manifold->getNumContacts() > max_contacts)  max_contacts = manifold->getNumContacts();
+	}
+	//if(max_contacts > 4)
+	//	printf(" max_contacts > 4");
+
+	int numBodies = 0;
+	m_odeBodies.clear();
+	m_odeBodies.reserve(numBulletBodies + 1); //???
+	// btOdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES];
+
+    int numJoints = 0;
+	m_joints.clear();
+	m_joints.reserve(numManifolds * max_contacts + 4 + numConstraints + 1); //???
+   // btOdeJoint* joints [ODE_MAX_SOLVER_JOINTS*2];
+
+	m_SolverBodyArray.resize(numBulletBodies + 1);
+	m_JointArray.resize(numManifolds * max_contacts + 4);
+	m_TypedJointArray.resize(numConstraints + 1);
+
+
+	//capture contacts
+	int body0=-1,body1=-1;
+    for (j=0;j<numManifolds;j++)
+    {
+        btPersistentManifold* manifold = manifoldPtr[j];
+        if (manifold->getNumContacts() > 0)
+        {
+            body0 = ConvertBody((btRigidBody*)manifold->getBody0(),m_odeBodies,numBodies);
+            body1 = ConvertBody((btRigidBody*)manifold->getBody1(),m_odeBodies,numBodies);
+            ConvertConstraint(manifold,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer);
+        }
+    }
+
+    //capture constraints
+    for (j=0;j<numConstraints;j++)
+    {
+    	btTypedConstraint * typedconstraint = constraints[j];
+		body0 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyA(),m_odeBodies,numBodies);
+		body1 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyB(),m_odeBodies,numBodies);
+		ConvertTypedConstraint(typedconstraint,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer);
+    }
+	//if(numBodies > numBulletBodies) 
+	//	printf(" numBodies > numBulletBodies");
+	//if(numJoints > numManifolds * 4 + numConstraints) 
+	//	printf(" numJoints > numManifolds * 4 + numConstraints");
+
+
+     m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do
+
+    //write back resulting velocities
+    for (int i=0;i<numBodies;i++)
+    {
+        if (m_odeBodies[i]->m_invMass)
+        {
+            m_odeBodies[i]->m_originalBody->setLinearVelocity(m_odeBodies[i]->m_linearVelocity);
+            m_odeBodies[i]->m_originalBody->setAngularVelocity(m_odeBodies[i]->m_angularVelocity);
+        }
+    }
+ 
+
+	/// Remotion, just free all this here
+	m_odeBodies.clear();
+	m_joints.clear();
+
+	m_SolverBodyArray.clear();
+	m_JointArray.clear();
+	m_TypedJointArray.clear();
+
+    return 0.f;
+
+}
+
+/////////////////////////////////////////////////////////////////////////////////
+
+
+typedef btScalar dQuaternion[4];
+#define _R(i,j) R[(i)*4+(j)]
+
+void dRfromQ1 (dMatrix3 R, const dQuaternion q)
+{
+    // q = (s,vx,vy,vz)
+    btScalar qq1 = 2.f*q[1]*q[1];
+    btScalar qq2 = 2.f*q[2]*q[2];
+    btScalar qq3 = 2.f*q[3]*q[3];
+    _R(0,0) = 1.f - qq2 - qq3;
+    _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
+    _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
+    _R(0,3) = 0.f;
+
+    _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
+    _R(1,1) = 1.f - qq1 - qq3;
+    _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
+    _R(1,3) = 0.f;
+
+    _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
+    _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
+    _R(2,2) = 1.f - qq1 - qq2;
+    _R(2,3) = 0.f;
+
+}
+
+
+
+//int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btOdeSolverBody** bodies,int& numBodies)
+int btOdeQuickstepConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies)
+{
+    assert(orgBody);
+    if (!orgBody || (orgBody->getInvMass() == 0.f) )
+    {
+        return -1;
+    }
+
+    if (orgBody->getCompanionId()>=0)
+    {
+        return orgBody->getCompanionId();
+    }
+    //first try to find
+    int i,j;
+
+    //if not found, create a new body
+   // btOdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies];
+	btOdeSolverBody* body = &m_SolverBodyArray[numBodies];
+	bodies.push_back(body); // Remotion 10.10.07: 
+
+    orgBody->setCompanionId(numBodies);
+
+    numBodies++;
+
+    body->m_originalBody = orgBody;
+
+    body->m_facc.setValue(0,0,0,0);
+    body->m_tacc.setValue(0,0,0,0);
+
+    body->m_linearVelocity = orgBody->getLinearVelocity();
+    body->m_angularVelocity = orgBody->getAngularVelocity();
+    body->m_invMass = orgBody->getInvMass();
+    body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition();
+    body->m_friction = orgBody->getFriction();
+
+    //are the indices the same ?
+    for (i=0;i<4;i++)
+    {
+        for ( j=0;j<3;j++)
+        {
+            body->m_invI[i+4*j] = 0.f;
+            body->m_I[i+4*j] = 0.f;
+        }
+    }
+    body->m_invI[0+4*0] = 	orgBody->getInvInertiaDiagLocal().x();
+    body->m_invI[1+4*1] = 	orgBody->getInvInertiaDiagLocal().y();
+    body->m_invI[2+4*2] = 	orgBody->getInvInertiaDiagLocal().z();
+
+    body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x();
+    body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y();
+    body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z();
+
+
+
+
+    dQuaternion q;
+
+    q[1] = orgBody->getOrientation().x();
+    q[2] = orgBody->getOrientation().y();
+    q[3] = orgBody->getOrientation().z();
+    q[0] = orgBody->getOrientation().w();
+
+    dRfromQ1(body->m_R,q);
+
+    return numBodies-1;
+}
+
+
+
+
+
+
+
+
+
+void btOdeQuickstepConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,
+											btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
+											const btAlignedObjectArray< btOdeSolverBody*> &bodies,
+											int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
+{
+
+
+    manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(),
+                                   ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform());
+
+    int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
+
+    int i,numContacts = manifold->getNumContacts();
+
+    bool swapBodies = (bodyId0 < 0);
+
+
+    btOdeSolverBody* body0,*body1;
+
+    if (swapBodies)
+    {
+        bodyId0 = _bodyId1;
+        bodyId1 = _bodyId0;
+
+        body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1();
+        body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0();
+
+    }
+    else
+    {
+        body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0();
+        body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1();
+    }
+
+    assert(bodyId0 >= 0);
+
+    btVector3 color(0,1,0);
+    for (i=0;i<numContacts;i++)
+    {
+
+        if (debugDrawer)
+        {
+            const btManifoldPoint& cp = manifold->getContactPoint(i);
+
+            debugDrawer->drawContactPoint(
+                cp.m_positionWorldOnB,
+                cp.m_normalWorldOnB,
+                cp.getDistance(),
+                cp.getLifeTime(),
+                color);
+
+        }
+        //assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS);
+
+//		if (manifold->getContactPoint(i).getDistance() < 0.0f)
+        {
+
+            btOdeContactJoint* cont = new (&m_JointArray[m_CurJoint++]) btOdeContactJoint( manifold ,i, swapBodies,body0,body1);
+            //btOdeContactJoint* cont = new (&gJointArray[m_CurJoint++]) btOdeContactJoint( manifold ,i, swapBodies,body0,body1);
+
+            cont->node[0].joint = cont;
+            cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0;
+
+            cont->node[1].joint = cont;
+            cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0;
+
+           // joints[numJoints++] = cont;
+			joints.push_back(cont); // Remotion 10.10.07: 
+			numJoints++;
+
+            for (int i=0;i<6;i++)
+                cont->lambda[i] = 0.f;
+
+            cont->flags = 0;
+        }
+    }
+
+    //create a new contact constraint
+}
+
+void btOdeQuickstepConstraintSolver::ConvertTypedConstraint(
+					btTypedConstraint * constraint,
+					btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
+					const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer)
+{
+
+    int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
+    bool swapBodies = (bodyId0 < 0);
+
+
+    btOdeSolverBody* body0,*body1;
+
+    if (swapBodies)
+    {
+        bodyId0 = _bodyId1;
+        bodyId1 = _bodyId0;
+
+        body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1();
+        body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0();
+
+    }
+    else
+    {
+        body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0();
+        body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1();
+    }
+
+    assert(bodyId0 >= 0);
+
+
+	//assert (m_CurTypedJoint < ODE_MAX_SOLVER_JOINTS);
+
+
+	btOdeTypedJoint * cont = NULL;
+
+	// Determine constraint type
+	int joint_type = constraint->getConstraintType();
+	switch(joint_type)
+	{
+	case POINT2POINT_CONSTRAINT_TYPE:
+	case D6_CONSTRAINT_TYPE:
+		cont = new (&m_TypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1);
+		//cont = new (&gTypedJointArray[m_CurTypedJoint ++]) btOdeTypedJoint(constraint,0, swapBodies,body0,body1);
+		break;
+
+	};
+
+	if(cont)
+	{
+		cont->node[0].joint = cont;
+		cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0;
+
+		cont->node[1].joint = cont;
+		cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0;
+
+		// joints[numJoints++] = cont;
+		joints.push_back(cont); // Remotion 10.10.07: 
+		numJoints++;
+
+		for (int i=0;i<6;i++)
+			cont->lambda[i] = 0.f;
+
+		cont->flags = 0;
+	}
+
+}
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h
new file mode 100644
index 000000000..3a47ba4ab
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h
@@ -0,0 +1,109 @@
+/*
+ * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
+ Bullet Continuous Collision Detection and Physics Library
+ Bullet is Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef ODE_CONSTRAINT_SOLVER_H
+#define ODE_CONSTRAINT_SOLVER_H
+
+#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
+
+#include "LinearMath/btAlignedObjectArray.h"
+#include "btOdeContactJoint.h"
+#include "btOdeTypedJoint.h"
+#include "btOdeSolverBody.h"
+#include "btSorLcp.h"
+
+class btRigidBody;
+struct	btOdeSolverBody;
+class btOdeJoint;
+
+/// btOdeQuickstepConstraintSolver is one of the available solvers for Bullet dynamics framework
+/// It uses an adapted version quickstep solver from the Open Dynamics Engine project
+class btOdeQuickstepConstraintSolver : public btConstraintSolver
+{
+private:
+	int		m_CurBody;
+	int		m_CurJoint;
+	int		m_CurTypedJoint;
+
+	float	m_cfm;
+	float	m_erp;
+
+	btSorLcpSolver	m_SorLcpSolver;
+
+	btAlignedObjectArray<btOdeSolverBody*> m_odeBodies;
+	btAlignedObjectArray<btOdeJoint*>		 m_joints;
+
+	btAlignedObjectArray<btOdeSolverBody>  m_SolverBodyArray;
+	btAlignedObjectArray<btOdeContactJoint>   m_JointArray;
+	btAlignedObjectArray<btOdeTypedJoint>  m_TypedJointArray;
+
+
+private:
+	int  ConvertBody(btRigidBody* body,btAlignedObjectArray< btOdeSolverBody*> &bodies,int& numBodies);
+	void ConvertConstraint(btPersistentManifold* manifold,
+							btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
+							const btAlignedObjectArray< btOdeSolverBody*> &bodies,
+							int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
+
+	void ConvertTypedConstraint(
+							btTypedConstraint * constraint,
+							btAlignedObjectArray<btOdeJoint*> &joints,int& numJoints,
+							const btAlignedObjectArray< btOdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer);
+
+
+public:
+
+	btOdeQuickstepConstraintSolver();
+
+	virtual ~btOdeQuickstepConstraintSolver() {}
+
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher);
+
+	///setConstraintForceMixing, the cfm adds some positive value to the main diagonal
+	///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy'
+	void	setConstraintForceMixing(float cfm) {
+		m_cfm  = cfm;
+	}
+
+	///setErrorReductionParamter sets the maximum amount of error reduction
+	///which limits energy addition during penetration depth recovery
+	void	setErrorReductionParamter(float erp)
+	{
+		m_erp = erp;
+	}
+
+	///clear internal cached data and reset random seed
+	void reset()
+	{
+		m_SorLcpSolver.dRand2_seed = 0;
+	}
+
+	void	setRandSeed(unsigned long seed)
+	{
+		m_SorLcpSolver.dRand2_seed = seed;
+	}
+	unsigned long	getRandSeed() const
+	{
+		return m_SorLcpSolver.dRand2_seed;
+	}
+};
+
+
+
+
+#endif //ODE_CONSTRAINT_SOLVER_H
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.cpp b/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.cpp
new file mode 100644
index 000000000..e69de29bb
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h b/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h
new file mode 100644
index 000000000..fc47a0932
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h
@@ -0,0 +1,48 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef ODE_SOLVER_BODY_H
+#define ODE_SOLVER_BODY_H
+
+class	btRigidBody;
+#include "LinearMath/btVector3.h"
+typedef btScalar dMatrix3[4*3];
+
+///ODE's quickstep needs just a subset of the rigidbody data in its own layout, so make a temp copy
+struct	btOdeSolverBody 
+{
+	btRigidBody*	m_originalBody;
+
+	btVector3		m_centerOfMassPosition;
+	/// for ode solver-binding
+	dMatrix3		m_R;//temp
+	dMatrix3		m_I;
+	dMatrix3		m_invI;
+
+	int				m_odeTag;
+	float			m_invMass;
+	float			m_friction;
+
+	btVector3		m_tacc;//temp
+	btVector3		m_facc;
+
+	btVector3		m_linearVelocity;
+	btVector3		m_angularVelocity;
+		
+};
+
+
+#endif //#ifndef ODE_SOLVER_BODY_H
+
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp b/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp
new file mode 100644
index 000000000..e61cb9bb3
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp
@@ -0,0 +1,451 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+#include "btOdeTypedJoint.h"
+#include "btOdeSolverBody.h"
+#include "btOdeMacros.h"
+#include <stdio.h>
+
+void btOdeTypedJoint::GetInfo1(Info1 *info)
+{
+    int joint_type = m_constraint->getConstraintType();
+    switch (joint_type)
+    {
+    case POINT2POINT_CONSTRAINT_TYPE:
+    {
+        OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
+        p2pjoint.GetInfo1(info);
+    }
+    break;
+    case D6_CONSTRAINT_TYPE:
+    {
+        OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
+        d6joint.GetInfo1(info);
+    }
+    break;
+    };
+}
+
+void btOdeTypedJoint::GetInfo2(Info2 *info)
+{
+    int joint_type = m_constraint->getConstraintType();
+    switch (joint_type)
+    {
+    case POINT2POINT_CONSTRAINT_TYPE:
+    {
+        OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
+        p2pjoint.GetInfo2(info);
+    }
+    break;
+    case D6_CONSTRAINT_TYPE:
+    {
+        OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
+        d6joint.GetInfo2(info);
+    }
+    break;
+    };
+}
+
+
+OdeP2PJoint::OdeP2PJoint(
+    btTypedConstraint * constraint,
+    int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1):
+        btOdeTypedJoint(constraint,index,swap,body0,body1)
+{
+}
+
+
+void OdeP2PJoint::GetInfo1(Info1 *info)
+{
+    info->m = 3;
+    info->nub = 3;
+}
+
+
+void OdeP2PJoint::GetInfo2(Info2 *info)
+{
+
+    btPoint2PointConstraint * p2pconstraint = this->getP2PConstraint();
+
+    //retrieve matrices
+    btTransform body0_trans;
+    if (m_body0)
+    {
+        body0_trans = m_body0->m_originalBody->getCenterOfMassTransform();
+    }
+//    btScalar body0_mat[12];
+//    body0_mat[0] = body0_trans.getBasis()[0][0];
+//    body0_mat[1] = body0_trans.getBasis()[0][1];
+//    body0_mat[2] = body0_trans.getBasis()[0][2];
+//    body0_mat[4] = body0_trans.getBasis()[1][0];
+//    body0_mat[5] = body0_trans.getBasis()[1][1];
+//    body0_mat[6] = body0_trans.getBasis()[1][2];
+//    body0_mat[8] = body0_trans.getBasis()[2][0];
+//    body0_mat[9] = body0_trans.getBasis()[2][1];
+//    body0_mat[10] = body0_trans.getBasis()[2][2];
+
+    btTransform body1_trans;
+
+    if (m_body1)
+    {
+        body1_trans = m_body1->m_originalBody->getCenterOfMassTransform();
+    }
+//    btScalar body1_mat[12];
+//    body1_mat[0] = body1_trans.getBasis()[0][0];
+//    body1_mat[1] = body1_trans.getBasis()[0][1];
+//    body1_mat[2] = body1_trans.getBasis()[0][2];
+//    body1_mat[4] = body1_trans.getBasis()[1][0];
+//    body1_mat[5] = body1_trans.getBasis()[1][1];
+//    body1_mat[6] = body1_trans.getBasis()[1][2];
+//    body1_mat[8] = body1_trans.getBasis()[2][0];
+//    body1_mat[9] = body1_trans.getBasis()[2][1];
+//    body1_mat[10] = body1_trans.getBasis()[2][2];
+
+
+
+
+    // anchor points in global coordinates with respect to body PORs.
+
+
+    int s = info->rowskip;
+
+    // set jacobian
+    info->J1l[0] = 1;
+    info->J1l[s+1] = 1;
+    info->J1l[2*s+2] = 1;
+
+
+    btVector3 a1,a2;
+
+    a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA();
+    //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
+    dCROSSMAT (info->J1a,a1,s,-,+);
+    if (m_body1)
+    {
+        info->J2l[0] = -1;
+        info->J2l[s+1] = -1;
+        info->J2l[2*s+2] = -1;
+        a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB();
+        //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
+        dCROSSMAT (info->J2a,a2,s,+,-);
+    }
+
+
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    if (m_body1)
+    {
+        for (int j=0; j<3; j++)
+        {
+            info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
+                              a1[j] - body0_trans.getOrigin()[j]);
+        }
+    }
+    else
+    {
+        for (int j=0; j<3; j++)
+        {
+            info->c[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] -
+                              body0_trans.getOrigin()[j]);
+        }
+    }
+}
+
+
+///////////////////limit motor support
+
+/*! \pre testLimitValue must be called on limot*/
+int bt_get_limit_motor_info2(
+	btRotationalLimitMotor * limot,
+	btRigidBody * body0, btRigidBody * body1,
+	btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational)
+{
+
+
+    int srow = row * info->rowskip;
+
+    // if the joint is powered, or has joint limits, add in the extra row
+    int powered = limot->m_enableMotor;
+    int limit = limot->m_currentLimit;
+
+    if (powered || limit)
+    {
+        btScalar *J1 = rotational ? info->J1a : info->J1l;
+        btScalar *J2 = rotational ? info->J2a : info->J2l;
+
+        J1[srow+0] = ax1[0];
+        J1[srow+1] = ax1[1];
+        J1[srow+2] = ax1[2];
+        if (body1)
+        {
+            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.
+
+        btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+        if (!rotational && body1)
+        {
+            btVector3 c;
+            c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0]
+            				-body0->getCenterOfMassPosition()[0]);
+            c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1]
+            				-body0->getCenterOfMassPosition()[1]);
+            c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2]
+            				-body0->getCenterOfMassPosition()[2]);
+
+			ltd = c.cross(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 && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
+
+        if (powered)
+        {
+            info->cfm[row] = 0.0f;//limot->m_normalCFM;
+            if (! limit)
+            {
+                info->c[row] = limot->m_targetVelocity;
+                info->lo[row] = -limot->m_maxMotorForce;
+                info->hi[row] = limot->m_maxMotorForce;
+            }
+        }
+
+        if (limit)
+        {
+            btScalar k = info->fps * limot->m_ERP;
+            info->c[row] = -k * limot->m_currentLimitError;
+            info->cfm[row] = 0.0f;//limot->m_stopCFM;
+
+            if (limot->m_loLimit == limot->m_hiLimit)
+            {
+                // 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] = SIMD_INFINITY;
+                }
+                else
+                {
+                    // high limit
+                    info->lo[row] = -SIMD_INFINITY;
+                    info->hi[row] = 0;
+                }
+
+                // deal with bounce
+                if (limot->m_bounce > 0)
+                {
+                    // calculate joint velocity
+                    btScalar vel;
+                    if (rotational)
+                    {
+                        vel = body0->getAngularVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getAngularVelocity().dot(ax1);
+                    }
+                    else
+                    {
+                        vel = body0->getLinearVelocity().dot(ax1);
+                        if (body1)
+                            vel -= body1->getLinearVelocity().dot(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)
+                        {
+                            btScalar newc = -limot->m_bounce* vel;
+                            if (newc > info->c[row]) info->c[row] = newc;
+                        }
+                    }
+                    else
+                    {
+                        // high limit - all those computations are reversed
+                        if (vel > 0)
+                        {
+                            btScalar newc = -limot->m_bounce * vel;
+                            if (newc < info->c[row]) info->c[row] = newc;
+                        }
+                    }
+                }
+            }
+        }
+        return 1;
+    }
+    else return 0;
+}
+
+
+///////////////////OdeD6Joint
+
+
+
+
+
+OdeD6Joint::OdeD6Joint(
+    btTypedConstraint * constraint,
+    int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1):
+        btOdeTypedJoint(constraint,index,swap,body0,body1)
+{
+}
+
+
+void OdeD6Joint::GetInfo1(Info1 *info)
+{
+	btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
+	//prepare constraint
+	d6constraint->calculateTransforms();
+    info->m = 3;
+    info->nub = 3;
+
+    //test angular limits
+    for (int i=0;i<3 ;i++ )
+    {
+    	//if(i==2) continue;
+		if(d6constraint->testAngularLimitMotor(i))
+		{
+			info->m++;
+		}
+    }
+
+
+}
+
+
+int OdeD6Joint::setLinearLimits(Info2 *info)
+{
+
+    btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
+
+    //retrieve matrices
+    btTransform body0_trans;
+    if (m_body0)
+    {
+        body0_trans = m_body0->m_originalBody->getCenterOfMassTransform();
+    }
+
+    btTransform body1_trans;
+
+    if (m_body1)
+    {
+        body1_trans = m_body1->m_originalBody->getCenterOfMassTransform();
+    }
+
+    // anchor points in global coordinates with respect to body PORs.
+
+    int s = info->rowskip;
+
+    // set jacobian
+    info->J1l[0] = 1;
+    info->J1l[s+1] = 1;
+    info->J1l[2*s+2] = 1;
+
+
+    btVector3 a1,a2;
+
+    a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
+    //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
+    dCROSSMAT (info->J1a,a1,s,-,+);
+    if (m_body1)
+    {
+        info->J2l[0] = -1;
+        info->J2l[s+1] = -1;
+        info->J2l[2*s+2] = -1;
+        a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
+
+        //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
+        dCROSSMAT (info->J2a,a2,s,+,-);
+    }
+
+
+    // set right hand side
+    btScalar k = info->fps * info->erp;
+    if (m_body1)
+    {
+        for (int j=0; j<3; j++)
+        {
+            info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
+                              a1[j] - body0_trans.getOrigin()[j]);
+        }
+    }
+    else
+    {
+        for (int j=0; j<3; j++)
+        {
+            info->c[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] -
+                              body0_trans.getOrigin()[j]);
+        }
+    }
+
+    return 3;
+
+}
+
+int OdeD6Joint::setAngularLimits(Info2 *info, int row_offset)
+{
+	btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
+	int row = row_offset;
+	//solve angular limits
+    for (int i=0;i<3 ;i++ )
+    {
+    	//if(i==2) continue;
+		if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
+		{
+			btVector3 axis = d6constraint->getAxis(i);
+			row += bt_get_limit_motor_info2(
+				d6constraint->getRotationalLimitMotor(i),
+				m_body0->m_originalBody,m_body1->m_originalBody,info,row,axis,1);
+		}
+    }
+
+    return row;
+}
+
+void OdeD6Joint::GetInfo2(Info2 *info)
+{
+    int row = setLinearLimits(info);
+    setAngularLimits(info, row);
+}
+
diff --git a/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h b/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h
new file mode 100644
index 000000000..429bd7813
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h
@@ -0,0 +1,111 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+/*
+2007-09-09
+Added support for typed joints by Francisco León
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
+#ifndef TYPED_JOINT_H
+#define TYPED_JOINT_H
+
+#include "btOdeJoint.h"
+#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+
+struct btOdeSolverBody;
+
+class btOdeTypedJoint : public btOdeJoint
+{
+public:
+	btTypedConstraint * m_constraint;
+	int		m_index;
+	bool	m_swapBodies;
+	btOdeSolverBody* m_body0;
+	btOdeSolverBody* m_body1;
+
+	btOdeTypedJoint(){}
+	btOdeTypedJoint(
+		btTypedConstraint * constraint,
+		int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1):
+			m_constraint(constraint),
+			m_index(index),
+			m_swapBodies(swap),
+			m_body0(body0),
+			m_body1(body1)
+	{
+	}
+
+	virtual void GetInfo1(Info1 *info);
+	virtual void GetInfo2(Info2 *info);
+};
+
+
+
+class OdeP2PJoint : public btOdeTypedJoint
+{
+protected:
+	inline btPoint2PointConstraint * getP2PConstraint()
+	{
+		return static_cast<btPoint2PointConstraint * >(m_constraint);
+	}
+public:
+
+	OdeP2PJoint() {};
+
+	OdeP2PJoint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1);
+
+	//btOdeJoint interface for solver
+
+	virtual void GetInfo1(Info1 *info);
+
+	virtual void GetInfo2(Info2 *info);
+};
+
+
+class OdeD6Joint : public btOdeTypedJoint
+{
+protected:
+	inline btGeneric6DofConstraint * getD6Constraint()
+	{
+		return static_cast<btGeneric6DofConstraint * >(m_constraint);
+	}
+
+	int setLinearLimits(Info2 *info);
+	int setAngularLimits(Info2 *info, int row_offset);
+
+public:
+
+	OdeD6Joint() {};
+
+	OdeD6Joint(btTypedConstraint* constraint,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1);
+
+	//btOdeJoint interface for solver
+
+	virtual void GetInfo1(Info1 *info);
+
+	virtual void GetInfo2(Info2 *info);
+};
+
+//! retrieves the constraint info from a btRotationalLimitMotor object
+/*! \pre testLimitValue must be called on limot*/
+int bt_get_limit_motor_info2(
+	btRotationalLimitMotor * limot,
+	btRigidBody * body0, btRigidBody * body1,
+	btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational);
+
+#endif //CONTACT_JOINT_H
+
diff --git a/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp b/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp
new file mode 100644
index 000000000..23698fa0e
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp
@@ -0,0 +1,683 @@
+/*
+ * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
+ Bullet Continuous Collision Detection and Physics Library
+ Bullet is Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btSorLcp.h"
+#include "btOdeSolverBody.h"
+
+#ifdef USE_SOR_SOLVER
+
+// SOR LCP taken from ode quickstep, for comparisons to Bullet sequential impulse solver.
+#include "LinearMath/btScalar.h"
+
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include <math.h>
+#include <float.h>//FLT_MAX
+#ifdef WIN32
+#include <memory.h>
+#endif
+#include <string.h>
+#include <stdio.h>
+
+#if defined (WIN32)
+#include <malloc.h>
+#else
+#if defined (__FreeBSD__)
+#include <stdlib.h>
+#else
+#include <alloca.h>
+#endif
+#endif
+
+#include "btOdeJoint.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+////////////////////////////////////////////////////////////////////
+//math stuff
+#include "btOdeMacros.h"
+
+//***************************************************************************
+// configuration
+
+// for the SOR and CG methods:
+// uncomment the following line to use warm starting. this definitely
+// help for motor-driven joints. unfortunately it appears to hurt
+// with high-friction contacts using the SOR method. use with care
+
+//#define WARM_STARTING 1
+
+// for the SOR method:
+// uncomment the following line to randomly reorder constraint rows
+// during the solution. depending on the situation, this can help a lot
+// or hardly at all, but it doesn't seem to hurt.
+
+#define RANDOMLY_REORDER_CONSTRAINTS 1
+
+//***************************************************************************
+// various common computations involving the matrix J
+// compute iMJ = inv(M)*J'
+inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
+	//OdeSolverBody* const *body,
+	 const btAlignedObjectArray<btOdeSolverBody*> &body,
+	dRealPtr invI)
+{
+	int i,j;
+	dRealMutablePtr iMJ_ptr = iMJ;
+	dRealMutablePtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];
+		int b2 = jb[i*2+1];
+		btScalar k = body[b1]->m_invMass;
+		for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
+		dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
+		if (b2 >= 0) {
+			k = body[b2]->m_invMass;
+			for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
+			dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
+		}
+		J_ptr += 12;
+		iMJ_ptr += 12;
+	}
+}
+
+#if 0
+static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb,
+	dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2)
+{
+	int i,j;
+
+
+
+	dRealMutablePtr out_ptr1 = out + onlyBody1*6;
+
+	for (j=0; j<6; j++)
+		out_ptr1[j] = 0;
+
+	if (onlyBody2 >= 0)
+	{
+		out_ptr1 = out + onlyBody2*6;
+
+		for (j=0; j<6; j++)
+			out_ptr1[j] = 0;
+	}
+
+	dRealPtr iMJ_ptr = iMJ;
+	for (i=0; i<m; i++) {
+
+		int b1 = jb[i*2];
+
+		dRealMutablePtr out_ptr = out + b1*6;
+		if ((b1 == onlyBody1) || (b1 == onlyBody2))
+		{
+			for (j=0; j<6; j++)
+				out_ptr[j] += iMJ_ptr[j] * in[i] ;
+		}
+
+		iMJ_ptr += 6;
+
+		int b2 = jb[i*2+1];
+		if ((b2 == onlyBody1) || (b2 == onlyBody2))
+		{
+			if (b2 >= 0)
+			{
+					out_ptr = out + b2*6;
+					for (j=0; j<6; j++)
+						out_ptr[j] += iMJ_ptr[j] * in[i];
+			}
+		}
+
+		iMJ_ptr += 6;
+
+	}
+}
+#endif
+
+
+// compute out = inv(M)*J'*in.
+
+#if 0
+static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
+	dRealMutablePtr in, dRealMutablePtr out)
+{
+	int i,j;
+	dSetZero1 (out,6*nb);
+	dRealPtr iMJ_ptr = iMJ;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];
+		int b2 = jb[i*2+1];
+		dRealMutablePtr out_ptr = out + b1*6;
+		for (j=0; j<6; j++)
+			out_ptr[j] += iMJ_ptr[j] * in[i];
+		iMJ_ptr += 6;
+		if (b2 >= 0) {
+			out_ptr = out + b2*6;
+			for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i];
+		}
+		iMJ_ptr += 6;
+	}
+}
+#endif
+
+
+// compute out = J*in.
+inline void multiply_J (int m, dRealMutablePtr J, int *jb,
+	dRealMutablePtr in, dRealMutablePtr out)
+{
+	int i,j;
+	dRealPtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		int b1 = jb[i*2];
+		int b2 = jb[i*2+1];
+		btScalar sum = 0;
+		dRealMutablePtr in_ptr = in + b1*6;
+		for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
+		J_ptr += 6;
+		if (b2 >= 0) {
+			in_ptr = in + b2*6;
+			for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
+		}
+		J_ptr += 6;
+		out[i] = sum;
+	}
+}
+
+//***************************************************************************
+// SOR-LCP method
+
+// nb is the number of bodies in the body array.
+// J is an m*12 matrix of constraint rows
+// jb is an array of first and second body numbers for each constraint row
+// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
+//
+// this returns lambda and fc (the constraint force).
+// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
+//
+// b, lo and hi are modified on exit
+
+//------------------------------------------------------------------------------
+ATTRIBUTE_ALIGNED16(struct) IndexError {
+	btScalar error;		// error to sort on
+	int findex;
+	int index;		// row index
+};
+
+//------------------------------------------------------------------------------
+void btSorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, 
+	const btAlignedObjectArray<btOdeSolverBody*> &body,
+	dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
+	dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
+	int numiter,float overRelax,
+	btStackAlloc* stackAlloc
+	)
+{
+	//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
+	AutoBlockSa asaBlock(stackAlloc);
+
+	const int num_iterations = numiter;
+	const float sor_w = overRelax;		// SOR over-relaxation parameter
+
+	int i,j;
+
+#ifdef WARM_STARTING
+	// for warm starting, this seems to be necessary to prevent
+	// jerkiness in motor-driven joints. i have no idea why this works.
+	for (i=0; i<m; i++) lambda[i] *= 0.9;
+#else
+	dSetZero1 (lambda,m);
+#endif
+
+	// the lambda computed at the previous iteration.
+	// this is used to measure error for when we are reordering the indexes.
+	dRealAllocaArray (last_lambda,m);
+
+	// a copy of the 'hi' vector in case findex[] is being used
+	dRealAllocaArray (hicopy,m);
+	memcpy (hicopy,hi,m*sizeof(float));
+
+	// precompute iMJ = inv(M)*J'
+	dRealAllocaArray (iMJ,m*12);
+	compute_invM_JT (m,J,iMJ,jb,body,invI);
+
+	// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
+	// as we change lambda.
+#ifdef WARM_STARTING
+	multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
+#else
+	dSetZero1 (invMforce,nb*6);
+#endif
+
+	// precompute 1 / diagonals of A
+	dRealAllocaArray (Ad,m);
+	dRealPtr iMJ_ptr = iMJ;
+	dRealMutablePtr J_ptr = J;
+	for (i=0; i<m; i++) {
+		float sum = 0;
+		for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
+		if (jb[i*2+1] >= 0) {
+			for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
+		}
+		iMJ_ptr += 12;
+		J_ptr += 12;
+		Ad[i] = sor_w / sum;//(sum + cfm[i]);
+	}
+
+	// scale J and b by Ad
+	J_ptr = J;
+	for (i=0; i<m; i++) {
+		for (j=0; j<12; j++) {
+			J_ptr[0] *= Ad[i];
+			J_ptr++;
+		}
+		rhs[i] *= Ad[i];
+	}
+
+	// scale Ad by CFM
+	for (i=0; i<m; i++)
+		Ad[i] *= cfm[i];
+
+	// order to solve constraint rows in
+	//IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
+	IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError));
+	
+
+#ifndef REORDER_CONSTRAINTS
+	// make sure constraints with findex < 0 come first.
+	j=0;
+	for (i=0; i<m; i++)
+		if (findex[i] < 0)
+			order[j++].index = i;
+	for (i=0; i<m; i++)
+		if (findex[i] >= 0)
+			order[j++].index = i;
+	dIASSERT (j==m);
+#endif
+
+	for (int iteration=0; iteration < num_iterations; iteration++) {
+
+#ifdef REORDER_CONSTRAINTS
+		// constraints with findex < 0 always come first.
+		if (iteration < 2) {
+			// for the first two iterations, solve the constraints in
+			// the given order
+			for (i=0; i<m; i++) {
+				order[i].error = i;
+				order[i].findex = findex[i];
+				order[i].index = i;
+			}
+		}
+		else {
+			// sort the constraints so that the ones converging slowest
+			// get solved last. use the absolute (not relative) error.
+			for (i=0; i<m; i++) {
+				float v1 = dFabs (lambda[i]);
+				float v2 = dFabs (last_lambda[i]);
+				float max = (v1 > v2) ? v1 : v2;
+				if (max > 0) {
+					//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
+					order[i].error = dFabs(lambda[i]-last_lambda[i]);
+				}
+				else {
+					order[i].error = dInfinity;
+				}
+				order[i].findex = findex[i];
+				order[i].index = i;
+			}
+		}
+		qsort (order,m,sizeof(IndexError),&compare_index_error);
+#endif
+#ifdef RANDOMLY_REORDER_CONSTRAINTS
+                if ((iteration & 7) == 0) {
+			for (i=1; i<m; ++i) {
+				IndexError tmp = order[i];
+				int swapi = dRandInt2(i+1);
+				order[i] = order[swapi];
+				order[swapi] = tmp;
+			}
+                }
+#endif
+
+		//@@@ potential optimization: swap lambda and last_lambda pointers rather
+		//    than copying the data. we must make sure lambda is properly
+		//    returned to the caller
+		memcpy (last_lambda,lambda,m*sizeof(float));
+
+		for (int i=0; i<m; i++) {
+			// @@@ potential optimization: we could pre-sort J and iMJ, thereby
+			//     linearizing access to those arrays. hmmm, this does not seem
+			//     like a win, but we should think carefully about our memory
+			//     access pattern.
+
+			int index = order[i].index;
+			J_ptr = J + index*12;
+			iMJ_ptr = iMJ + index*12;
+
+			// set the limits for this constraint. note that 'hicopy' is used.
+			// this is the place where the QuickStep method differs from the
+			// direct LCP solving method, since that method only performs this
+			// limit adjustment once per time step, whereas this method performs
+			// once per iteration per constraint row.
+			// the constraints are ordered so that all lambda[] values needed have
+			// already been computed.
+			if (findex[index] >= 0) {
+				hi[index] = btFabs (hicopy[index] * lambda[findex[index]]);
+				lo[index] = -hi[index];
+			}
+
+			int b1 = jb[index*2];
+			int b2 = jb[index*2+1];
+			float delta = rhs[index] - lambda[index]*Ad[index];
+			dRealMutablePtr fc_ptr = invMforce + 6*b1;
+
+			// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
+			delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
+				fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
+				fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
+			// @@@ potential optimization: handle 1-body constraints in a separate
+			//     loop to avoid the cost of test & jump?
+			if (b2 >= 0) {
+				fc_ptr = invMforce + 6*b2;
+				delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
+					fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
+					fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
+			}
+
+			// compute lambda and clamp it to [lo,hi].
+			// @@@ potential optimization: does SSE have clamping instructions
+			//     to save test+jump penalties here?
+			float new_lambda = lambda[index] + delta;
+			if (new_lambda < lo[index]) {
+				delta = lo[index]-lambda[index];
+				lambda[index] = lo[index];
+			}
+			else if (new_lambda > hi[index]) {
+				delta = hi[index]-lambda[index];
+				lambda[index] = hi[index];
+			}
+			else {
+				lambda[index] = new_lambda;
+			}
+
+			//@@@ a trick that may or may not help
+			//float ramp = (1-((float)(iteration+1)/(float)num_iterations));
+			//delta *= ramp;
+
+			// update invMforce.
+			// @@@ potential optimization: SIMD for this and the b2 >= 0 case
+			fc_ptr = invMforce + 6*b1;
+			fc_ptr[0] += delta * iMJ_ptr[0];
+			fc_ptr[1] += delta * iMJ_ptr[1];
+			fc_ptr[2] += delta * iMJ_ptr[2];
+			fc_ptr[3] += delta * iMJ_ptr[3];
+			fc_ptr[4] += delta * iMJ_ptr[4];
+			fc_ptr[5] += delta * iMJ_ptr[5];
+			// @@@ potential optimization: handle 1-body constraints in a separate
+			//     loop to avoid the cost of test & jump?
+			if (b2 >= 0) {
+				fc_ptr = invMforce + 6*b2;
+				fc_ptr[0] += delta * iMJ_ptr[6];
+				fc_ptr[1] += delta * iMJ_ptr[7];
+				fc_ptr[2] += delta * iMJ_ptr[8];
+				fc_ptr[3] += delta * iMJ_ptr[9];
+				fc_ptr[4] += delta * iMJ_ptr[10];
+				fc_ptr[5] += delta * iMJ_ptr[11];
+			}
+		}
+	}
+	//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
+}
+
+//------------------------------------------------------------------------------
+void btSorLcpSolver::SolveInternal1 (
+			float global_cfm,
+			float global_erp,
+			const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
+			btAlignedObjectArray<btOdeJoint*> &joint, 
+			int nj, const btContactSolverInfo& solverInfo,
+			btStackAlloc* stackAlloc)
+{
+	//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
+	AutoBlockSa asaBlock(stackAlloc);
+
+	int numIter = solverInfo.m_numIterations;
+	float sOr = solverInfo.m_sor;
+
+	int i,j;
+
+	btScalar stepsize1 = dRecip(solverInfo.m_timeStep);
+
+	// number all bodies in the body list - set their tag values
+	for (i=0; i<nb; i++)
+		body[i]->m_odeTag = i;
+
+	// make a local copy of the joint array, because we might want to modify it.
+	// (the "btOdeJoint *const*" declaration says we're allowed to modify the joints
+	// but not the joint array, because the caller might need it unchanged).
+	//@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
+	//btOdeJoint **joint = (btOdeJoint**) alloca (nj * sizeof(btOdeJoint*));
+	//memcpy (joint,_joint,nj * sizeof(btOdeJoint*));
+
+	// for all bodies, compute the inertia tensor and its inverse in the global
+	// frame, and compute the rotational force and add it to the torque
+	// accumulator. I and invI are a vertical stack of 3x4 matrices, one per body.
+	dRealAllocaArray (I,3*4*nb);
+	dRealAllocaArray (invI,3*4*nb);
+/*	for (i=0; i<nb; i++) {
+		dMatrix3 tmp;
+		// compute inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
+		// compute inverse inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
+		dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
+		// compute rotational force
+		dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp);
+	}
+*/
+	for (i=0; i<nb; i++) {
+		dMatrix3 tmp;
+		// compute inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->m_I,body[i]->m_R);
+		dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp);
+
+		// compute inverse inertia tensor in global frame
+		dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R);
+		dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp);
+		// compute rotational force
+//		dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity);
+//		dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp);
+	}
+
+
+
+
+	// get joint information (m = total constraint dimension, nub = number of unbounded variables).
+	// joints with m=0 are inactive and are removed from the joints array
+	// entirely, so that the code that follows does not consider them.
+	//@@@ do we really need to save all the info1's
+	btOdeJoint::Info1 *info = (btOdeJoint::Info1*) ALLOCA (nj*sizeof(btOdeJoint::Info1));
+	
+	for (i=0, j=0; j<nj; j++) {	// i=dest, j=src
+		joint[j]->GetInfo1 (info+i);
+		dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
+		if (info[i].m > 0) {
+			joint[i] = joint[j];
+			i++;
+		}
+	}
+	nj = i;
+
+	// create the row offset array
+	int m = 0;
+	int *ofs = (int*) ALLOCA (nj*sizeof(int));
+	for (i=0; i<nj; i++) {
+		ofs[i] = m;
+		m += info[i].m;
+	}
+
+	// if there are constraints, compute the constraint force
+	dRealAllocaArray (J,m*12);
+	int *jb = (int*) ALLOCA (m*2*sizeof(int));
+	if (m > 0) {
+		// create a constraint equation right hand side vector `c', a constraint
+		// force mixing vector `cfm', and LCP low and high bound vectors, and an
+		// 'findex' vector.
+		dRealAllocaArray (c,m);
+		dRealAllocaArray (cfm,m);
+		dRealAllocaArray (lo,m);
+		dRealAllocaArray (hi,m);
+
+		int *findex = (int*) ALLOCA (m*sizeof(int));
+
+		dSetZero1 (c,m);
+		dSetValue1 (cfm,m,global_cfm);
+		dSetValue1 (lo,m,-dInfinity);
+		dSetValue1 (hi,m, dInfinity);
+		for (i=0; i<m; i++) findex[i] = -1;
+
+		// get jacobian data from constraints. an m*12 matrix will be created
+		// to store the two jacobian blocks from each constraint. it has this
+		// format:
+		//
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \    .
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2  }-- jacobian for joint 0, body 1 and body 2 (3 rows)
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
+		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
+		//   etc...
+		//
+		//   (lll) = linear jacobian data
+		//   (aaa) = angular jacobian data
+		//
+		dSetZero1 (J,m*12);
+		btOdeJoint::Info2 Jinfo;
+		Jinfo.rowskip = 12;
+		Jinfo.fps = stepsize1;
+		Jinfo.erp = global_erp;
+		for (i=0; i<nj; i++) {
+			Jinfo.J1l = J + ofs[i]*12;
+			Jinfo.J1a = Jinfo.J1l + 3;
+			Jinfo.J2l = Jinfo.J1l + 6;
+			Jinfo.J2a = Jinfo.J1l + 9;
+			Jinfo.c = c + ofs[i];
+			Jinfo.cfm = cfm + ofs[i];
+			Jinfo.lo = lo + ofs[i];
+			Jinfo.hi = hi + ofs[i];
+			Jinfo.findex = findex + ofs[i];
+			joint[i]->GetInfo2 (&Jinfo);
+
+			if (Jinfo.c[0] > solverInfo.m_maxErrorReduction)
+				Jinfo.c[0] = solverInfo.m_maxErrorReduction;
+
+			// adjust returned findex values for global index numbering
+			for (j=0; j<info[i].m; j++) {
+				if (findex[ofs[i] + j] >= 0)
+					findex[ofs[i] + j] += ofs[i];
+			}
+		}
+
+		// create an array of body numbers for each joint row
+		int *jb_ptr = jb;
+		for (i=0; i<nj; i++) {
+			int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1;
+			int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1;
+			for (j=0; j<info[i].m; j++) {
+				jb_ptr[0] = b1;
+				jb_ptr[1] = b2;
+				jb_ptr += 2;
+			}
+		}
+		dIASSERT (jb_ptr == jb+2*m);
+
+		// compute the right hand side `rhs'
+		dRealAllocaArray (tmp1,nb*6);
+		// put v/h + invM*fe into tmp1
+		for (i=0; i<nb; i++) {
+			btScalar body_invMass = body[i]->m_invMass;
+			for (j=0; j<3; j++)
+				tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1;
+			dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc);
+			for (j=0; j<3; j++)
+				tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1;
+		}
+
+		// put J*tmp1 into rhs
+		dRealAllocaArray (rhs,m);
+		multiply_J (m,J,jb,tmp1,rhs);
+
+		// complete rhs
+		for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];
+
+		// scale CFM
+		for (i=0; i<m; i++)
+			cfm[i] *= stepsize1;
+
+		// load lambda from the value saved on the previous iteration
+		dRealAllocaArray (lambda,m);
+#ifdef WARM_STARTING
+		dSetZero1 (lambda,m);	//@@@ shouldn't be necessary
+		for (i=0; i<nj; i++) {
+			memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(btScalar));
+		}
+#endif
+
+		// solve the LCP problem and get lambda and invM*constraint_force
+		dRealAllocaArray (cforce,nb*6);
+
+		/// SOR_LCP
+		SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc);
+
+#ifdef WARM_STARTING
+		// save lambda for the next iteration
+		//@@@ note that this doesn't work for contact joints yet, as they are
+		// recreated every iteration
+		for (i=0; i<nj; i++) {
+			memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(btScalar));
+		}
+#endif
+
+		// note that the SOR method overwrites rhs and J at this point, so
+		// they should not be used again.
+		// add stepsize * cforce to the body velocity
+		for (i=0; i<nb; i++) {
+			for (j=0; j<3; j++)
+				body[i]->m_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j];
+			for (j=0; j<3; j++)
+				body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j];
+
+		}
+	}
+
+	// compute the velocity update:
+	// add stepsize * invM * fe to the body velocity
+	for (i=0; i<nb; i++) {
+		btScalar body_invMass = body[i]->m_invMass;
+		btVector3 linvel = body[i]->m_linearVelocity;
+		btVector3 angvel = body[i]->m_angularVelocity;
+
+		for (j=0; j<3; j++)
+		{
+			linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j];
+		}
+		for (j=0; j<3; j++)
+		{
+			body[i]->m_tacc[j] *= solverInfo.m_timeStep;
+		}
+		dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc);
+		body[i]->m_angularVelocity = angvel;
+	}
+	//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
+}
+
+
+#endif //USE_SOR_SOLVER
diff --git a/src/BulletDynamics/ConstraintSolver/btSorLcp.h b/src/BulletDynamics/ConstraintSolver/btSorLcp.h
new file mode 100644
index 000000000..41778de73
--- /dev/null
+++ b/src/BulletDynamics/ConstraintSolver/btSorLcp.h
@@ -0,0 +1,112 @@
+/*
+ * Quickstep constraint solver re-distributed under the ZLib license with permission from Russell L. Smith
+ * Original version is from Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org
+ Bullet Continuous Collision Detection and Physics Library
+ Bullet is Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#define USE_SOR_SOLVER
+#ifdef USE_SOR_SOLVER
+
+#ifndef SOR_LCP_H
+#define SOR_LCP_H
+struct	btOdeSolverBody;
+class btOdeJoint;
+#include "LinearMath/btScalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btStackAlloc.h"
+
+struct btContactSolverInfo;
+
+
+//=============================================================================
+class btSorLcpSolver //Remotion: 11.10.2007
+{
+public:
+	btSorLcpSolver()
+	{
+		dRand2_seed = 0;
+	}
+
+	void SolveInternal1 (float global_cfm,
+		float global_erp,
+		const btAlignedObjectArray<btOdeSolverBody*> &body, int nb,
+		btAlignedObjectArray<btOdeJoint*> &joint, 
+		int nj, const btContactSolverInfo& solverInfo,
+		btStackAlloc* stackAlloc
+		);
+
+public: //data
+	unsigned long dRand2_seed;
+
+protected: //typedef
+	typedef const btScalar *dRealPtr;
+	typedef btScalar *dRealMutablePtr;
+
+protected: //members
+	//------------------------------------------------------------------------------
+	SIMD_FORCE_INLINE unsigned long dRand2()
+	{
+		dRand2_seed = (1664525L*dRand2_seed + 1013904223L) & 0xffffffff;
+		return dRand2_seed;
+	}
+	//------------------------------------------------------------------------------
+	SIMD_FORCE_INLINE int dRandInt2 (int n)
+	{
+		float a = float(n) / 4294967296.0f;
+		return (int) (float(dRand2()) * a);
+	}
+	//------------------------------------------------------------------------------
+	void SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, 
+		const btAlignedObjectArray<btOdeSolverBody*> &body,
+		dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
+		dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
+		int numiter,float overRelax,
+		btStackAlloc* stackAlloc
+		);
+};
+
+
+//=============================================================================
+class AutoBlockSa //Remotion: 10.10.2007
+{
+	btStackAlloc* stackAlloc;
+	btBlock*	  saBlock;
+public:
+	AutoBlockSa(btStackAlloc* stackAlloc_)
+	{
+		stackAlloc = stackAlloc_;
+		saBlock = stackAlloc->beginBlock();
+	}
+	~AutoBlockSa()
+	{
+		stackAlloc->endBlock(saBlock);
+	}
+	//operator btBlock* () { return saBlock; }
+};
+// //Usage
+//void function(btStackAlloc* stackAlloc)
+//{
+//	AutoBlockSa(stackAlloc);
+//   ...
+//	if(...) return;
+//	return;
+//}
+//------------------------------------------------------------------------------
+
+
+#endif //SOR_LCP_H
+
+#endif //USE_SOR_SOLVER
+
diff --git a/src/btBulletDynamicsCommon.h b/src/btBulletDynamicsCommon.h
index 300d05bec..92003d2ca 100644
--- a/src/btBulletDynamicsCommon.h
+++ b/src/btBulletDynamicsCommon.h
@@ -30,8 +30,11 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
 
-
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+///Optional ODE quickstep constraint solver, redistributed under ZLib license
+#include "BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btOdeTypedJoint.h"
+
 ///Vehicle simulation, with wheel contact simulated by raycasts
 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"