svn merge -r 16334:16347 https://svn.blender.org/svnroot/bf-blender/trunk/blender + also added missing files from bullet-2.71alpha0 archiv + fixed compile errors

48 files changed, 6844 insertions, 1284 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
index 2289621e8e3..e11b49d9420 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
@@ -19,24 +19,20 @@ Written by: Marcus Hennix
 #include "btConeTwistConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
-#include "LinearMath/btSimdMinMax.h"
+#include "LinearMath/btMinMax.h"
 #include <new>
 
 btConeTwistConstraint::btConeTwistConstraint()
+:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
 {
 }
 
 
 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
 											 const btTransform& rbAFrame,const btTransform& rbBFrame)
-											 :btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+											 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 											 m_angularOnly(false)
 {
-	// flip axis for correct angles
-	m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-
 	m_swingSpan1 = btScalar(1e30);
 	m_swingSpan2 = btScalar(1e30);
 	m_twistSpan  = btScalar(1e30);
@@ -49,20 +45,11 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
 }
 
 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
-											:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),
+											:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
 											 m_angularOnly(false)
 {
 	m_rbBFrame = m_rbAFrame;
 	
-	// flip axis for correct angles
-	m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
-
-	m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
-	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-	
 	m_swingSpan1 = btScalar(1e30);
 	m_swingSpan2 = btScalar(1e30);
 	m_twistSpan  = btScalar(1e30);
@@ -142,7 +129,7 @@ void	btConeTwistConstraint::buildJacobian()
 
 	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
 	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
-	btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq;
+	btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
 
 	if (EllipseAngle > 1.0f)
 	{
@@ -246,7 +233,7 @@ void	btConeTwistConstraint::solveConstraint(btScalar	timeStep)
 
 			// Clamp the accumulated impulse
 			btScalar temp = m_accSwingLimitImpulse;
-			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, 0.0f );
+			m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
 			impulseMag = m_accSwingLimitImpulse - temp;
 
 			btVector3 impulse = m_swingAxis * impulseMag;
@@ -264,7 +251,7 @@ void	btConeTwistConstraint::solveConstraint(btScalar	timeStep)
 
 			// Clamp the accumulated impulse
 			btScalar temp = m_accTwistLimitImpulse;
-			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, 0.0f );
+			m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
 			impulseMag = m_accTwistLimitImpulse - temp;
 
 			btVector3 impulse = m_twistAxis * impulseMag;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
index 874669c80b3..f121919c8f9 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h
@@ -30,6 +30,9 @@ class btRigidBody;
 ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
 class btConeTwistConstraint : public btTypedConstraint
 {
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
 
 	btTransform m_rbAFrame; 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
index 7e8458c2c7b..7a8e9c1953d 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #ifndef CONSTRAINT_SOLVER_H
 #define CONSTRAINT_SOLVER_H
 
-#include "../../LinearMath/btScalar.h"
+#include "LinearMath/btScalar.h"
 
 class btPersistentManifold;
 class btRigidBody;
@@ -26,7 +26,7 @@ struct btContactSolverInfo;
 struct btBroadphaseProxy;
 class btIDebugDraw;
 class btStackAlloc;
-
+class	btDispatcher;
 /// btConstraintSolver provides solver interface
 class btConstraintSolver
 {
@@ -35,8 +35,15 @@ public:
 
 	virtual ~btConstraintSolver() {}
 	
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0;
+	virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
+
+	///solve a group of constraints
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;
+
+	virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;}
 
+	///clear internal cached data and reset random seed
+	virtual	void	reset() = 0;
 };
 
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
index bb3fe832592..4d7cd05feb7 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
@@ -192,8 +192,8 @@ btScalar resolveSingleFriction(
 			j1 = -vrel * cpd->m_jacDiagABInvTangent0;
 			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
 			cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
-			GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit);
-			GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit);
+			btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
+			btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
 			j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
 
 		}
@@ -206,8 +206,8 @@ btScalar resolveSingleFriction(
 			j2 = -vrel * cpd->m_jacDiagABInvTangent1;
 			btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
 			cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
-			GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit);
-			GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit);
+			btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
+			btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
 			j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
 		}
 
@@ -237,6 +237,12 @@ btScalar resolveSingleFrictionOriginal(
 	btRigidBody& body1,
 	btRigidBody& body2,
 	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo);
+
+btScalar resolveSingleFrictionOriginal(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
 	const btContactSolverInfo& solverInfo)
 {
 
@@ -270,8 +276,8 @@ btScalar resolveSingleFrictionOriginal(
 			// calculate j that moves us to zero relative velocity
 			btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
 			btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
-			GEN_set_min(total, limit);
-			GEN_set_max(total, -limit);
+			btSetMin(total, limit);
+			btSetMax(total, -limit);
 			j = total - cpd->m_accumulatedTangentImpulse0;
 			cpd->m_accumulatedTangentImpulse0 = total;
 			body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
@@ -290,8 +296,8 @@ btScalar resolveSingleFrictionOriginal(
 			// calculate j that moves us to zero relative velocity
 			btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
 			btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
-			GEN_set_min(total, limit);
-			GEN_set_max(total, -limit);
+			btSetMin(total, limit);
+			btSetMax(total, -limit);
 			j = total - cpd->m_accumulatedTangentImpulse1;
 			cpd->m_accumulatedTangentImpulse1 = total;
 			body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
@@ -388,8 +394,8 @@ btScalar resolveSingleCollisionCombined(
 				(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
 			btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
 
-			GEN_set_min(friction_impulse, normal_impulse);
-			GEN_set_max(friction_impulse, -normal_impulse);
+			btSetMin(friction_impulse, normal_impulse);
+			btSetMax(friction_impulse, -normal_impulse);
 			body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
 			body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
 		}
@@ -404,6 +410,12 @@ btScalar resolveSingleFrictionEmpty(
 	btRigidBody& body1,
 	btRigidBody& body2,
 	btManifoldPoint& contactPoint,
+	const btContactSolverInfo& solverInfo);
+
+btScalar resolveSingleFrictionEmpty(
+	btRigidBody& body1,
+	btRigidBody& body2,
+	btManifoldPoint& contactPoint,
 	const btContactSolverInfo& solverInfo)
 {
 	(void)contactPoint;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
index 0834deddeac..826e79f78bd 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h
@@ -19,8 +19,8 @@ subject to the following restrictions:
 //todo: make into a proper class working with the iterative constraint solver
 
 class btRigidBody;
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btScalar.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btScalar.h"
 struct btContactSolverInfo;
 class btManifoldPoint;
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
index c3c73e300f4..916d4581f79 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
@@ -16,10 +16,43 @@ subject to the following restrictions:
 #ifndef CONTACT_SOLVER_INFO
 #define CONTACT_SOLVER_INFO
 
+enum	btSolverMode
+{
+	SOLVER_RANDMIZE_ORDER = 1,
+	SOLVER_FRICTION_SEPARATE = 2,
+	SOLVER_USE_WARMSTARTING = 4,
+	SOLVER_CACHE_FRIENDLY = 8
+};
+
+struct btContactSolverInfoData
+{
+	
+
+	btScalar	m_tau;
+	btScalar	m_damping;
+	btScalar	m_friction;
+	btScalar	m_timeStep;
+	btScalar	m_restitution;
+	int		m_numIterations;
+	btScalar	m_maxErrorReduction;
+	btScalar	m_sor;
+	btScalar	m_erp;//used as Baumgarte factor
+	btScalar	m_erp2;//used in Split Impulse
+	int			m_splitImpulse;
+	btScalar	m_splitImpulsePenetrationThreshold;
+	btScalar	m_linearSlop;
+	btScalar	m_warmstartingFactor;
+
+	int			m_solverMode;
 
-struct btContactSolverInfo
+
+};
+
+struct btContactSolverInfo : public btContactSolverInfoData
 {
 
+	
+
 	inline btContactSolverInfo()
 	{
 		m_tau = btScalar(0.6);
@@ -28,20 +61,15 @@ struct btContactSolverInfo
 		m_restitution = btScalar(0.);
 		m_maxErrorReduction = btScalar(20.);
 		m_numIterations = 10;
-		m_erp = btScalar(0.4);
+		m_erp = btScalar(0.2);
+		m_erp2 = btScalar(0.1);
 		m_sor = btScalar(1.3);
+		m_splitImpulse = false;
+		m_splitImpulsePenetrationThreshold = -0.02f;
+		m_linearSlop = btScalar(0.0);
+		m_warmstartingFactor=btScalar(0.85);
+		m_solverMode = SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING;
 	}
-
-	btScalar	m_tau;
-	btScalar	m_damping;
-	btScalar	m_friction;
-	btScalar	m_timeStep;
-	btScalar	m_restitution;
-	int		m_numIterations;
-	btScalar	m_maxErrorReduction;
-	btScalar	m_sor;
-	btScalar	m_erp;
-
 };
 
 #endif //CONTACT_SOLVER_INFO
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index 747d10d1f8b..077b326d13a 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -4,14 +4,20 @@ 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, 
+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
+Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
 
 
 #include "btGeneric6DofConstraint.h"
@@ -19,371 +25,504 @@ subject to the following restrictions:
 #include "LinearMath/btTransformUtil.h"
 #include <new>
 
+
 static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) };
 static const int kAxisA[] = { 1, 0, 0 };
 static const int kAxisB[] = { 2, 2, 1 };
 #define GENERIC_D6_DISABLE_WARMSTARTING 1
 
-btGeneric6DofConstraint::btGeneric6DofConstraint()
+btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
+btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
 {
+	int i = index%3;
+	int j = index/3;
+	return mat[i][j];
 }
 
-btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
-: btTypedConstraint(rbA, rbB)
-, m_frameInA(frameInA)
-, m_frameInB(frameInB)
+///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
+bool	matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
 {
-	//free means upper < lower, 
-	//locked means upper == lower
-	//limited means upper > lower
-	//so start all locked
-	for (int i=0; i<6;++i)
+//	// rot =  cy*cz          -cy*sz           sy
+//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+//
+
+		if (btGetMatrixElem(mat,2) < btScalar(1.0))
+		{
+			if (btGetMatrixElem(mat,2) > btScalar(-1.0))
+			{
+				xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
+				xyz[1] = btAsin(btGetMatrixElem(mat,2));
+				xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
+				return true;
+			}
+			else
+			{
+				// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+				xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+				xyz[1] = -SIMD_HALF_PI;
+				xyz[2] = btScalar(0.0);
+				return false;
+			}
+		}
+		else
+		{
+			// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+			xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
+			xyz[1] = SIMD_HALF_PI;
+			xyz[2] = 0.0;
+
+		}
+
+
+	return false;
+}
+
+
+
+//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
+
+
+int btRotationalLimitMotor::testLimitValue(btScalar test_value)
+{
+	if(m_loLimit>m_hiLimit)
+	{
+		m_currentLimit = 0;//Free from violation
+		return 0;
+	}
+
+	if (test_value < m_loLimit)
 	{
-		m_lowerLimit[i] = btScalar(0.0);
-		m_upperLimit[i] = btScalar(0.0);
-		m_accumulatedImpulse[i] = btScalar(0.0);
+		m_currentLimit = 1;//low limit violation
+		m_currentLimitError =  test_value - m_loLimit;
+		return 1;
 	}
+	else if (test_value> m_hiLimit)
+	{
+		m_currentLimit = 2;//High limit violation
+		m_currentLimitError = test_value - m_hiLimit;
+		return 2;
+	};
 
+	m_currentLimit = 0;//Free from violation
+	return 0;
+	
 }
 
 
-void btGeneric6DofConstraint::buildJacobian()
+btScalar btRotationalLimitMotor::solveAngularLimits(
+		btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
+	 	btRigidBody * body0, btRigidBody * body1)
 {
-	btVector3	localNormalInA(0,0,0);
+    if (needApplyTorques()==false) return 0.0f;
 
-	const btVector3& pivotInA = m_frameInA.getOrigin();
-	const btVector3& pivotInB = m_frameInB.getOrigin();
+    btScalar target_velocity = m_targetVelocity;
+    btScalar maxMotorForce = m_maxMotorForce;
 
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
+	//current error correction
+    if (m_currentLimit!=0)
+    {
+        target_velocity = -m_ERP*m_currentLimitError/(timeStep);
+        maxMotorForce = m_maxLimitForce;
+    }
 
-	btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+    maxMotorForce *= timeStep;
 
-	int i;
-	//linear part
-	for (i=0;i<3;i++)
-	{
-		if (isLimited(i))
-		{
-			localNormalInA[i] = 1;
-			btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
-
-			
-			// Create linear atom
-			new (&m_jacLinear[i]) btJacobianEntry(
-				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
-				m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
-				normalWorld,
-				m_rbA.getInvInertiaDiagLocal(),
-				m_rbA.getInvMass(),
-				m_rbB.getInvInertiaDiagLocal(),
-				m_rbB.getInvMass());
-
-			//optionally disable warmstarting
-#ifdef GENERIC_D6_DISABLE_WARMSTARTING
-			m_accumulatedImpulse[i] = btScalar(0.);
-#endif //GENERIC_D6_DISABLE_WARMSTARTING
-
-			// Apply accumulated impulse
-			btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld;
-
-			m_rbA.applyImpulse( impulse_vector, rel_pos1);
-			m_rbB.applyImpulse(-impulse_vector, rel_pos2);
-
-			localNormalInA[i] = 0;
-		}
-	}
+    // current velocity difference
+    btVector3 vel_diff = body0->getAngularVelocity();
+    if (body1)
+    {
+        vel_diff -= body1->getAngularVelocity();
+    }
 
-	// angular part
-	for (i=0;i<3;i++)
-	{
-		if (isLimited(i+3))
-		{
-			btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
-			btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
 
-			// Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe
-			btVector3 axis = kSign[i] * axisA.cross(axisB);
 
-			// Create angular atom
-			new (&m_jacAng[i])	btJacobianEntry(axis,
-				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbA.getInvInertiaDiagLocal(),
-				m_rbB.getInvInertiaDiagLocal());
+    btScalar rel_vel = axis.dot(vel_diff);
 
-#ifdef GENERIC_D6_DISABLE_WARMSTARTING
-			m_accumulatedImpulse[i + 3] = btScalar(0.);
-#endif //GENERIC_D6_DISABLE_WARMSTARTING
+	// correction velocity
+    btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
 
-			// Apply accumulated impulse
-			btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
 
-			m_rbA.applyTorqueImpulse( impulse_vector);
-			m_rbB.applyTorqueImpulse(-impulse_vector);
-		}
-	}
-}
+    if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
+    {
+        return 0.0f;//no need for applying force
+    }
+
+
+	// correction impulse
+    btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
+
+	// clip correction impulse
+    btScalar clippedMotorImpulse;
+
+    //todo: should clip against accumulated impulse
+    if (unclippedMotorImpulse>0.0f)
+    {
+        clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
+    }
+    else
+    {
+        clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
+    }
+
+
+	// sort with accumulated impulses
+    btScalar	lo = btScalar(-1e30);
+    btScalar	hi = btScalar(1e30);
+
+    btScalar oldaccumImpulse = m_accumulatedImpulse;
+    btScalar sum = oldaccumImpulse + clippedMotorImpulse;
+    m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+
+    clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
+
+
+
+    btVector3 motorImp = clippedMotorImpulse * axis;
+
+
+    body0->applyTorqueImpulse(motorImp);
+    if (body1) body1->applyTorqueImpulse(-motorImp);
+
+    return clippedMotorImpulse;
+
 
-btScalar getMatrixElem(const btMatrix3x3& mat,int index)
-{
-	int row = index%3;
-	int col = index / 3;
-	return mat[row][col];
 }
 
-///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
-bool	MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
+//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
+
+//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+btScalar btTranslationalLimitMotor::solveLinearAxis(
+		btScalar timeStep,
+        btScalar jacDiagABInv,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
+        int limit_index,
+        const btVector3 & axis_normal_on_a,
+		const btVector3 & anchorPos)
 {
-    // rot =  cy*cz          -cy*sz           sy
-    //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
-    //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
 
-///	0..8
+///find relative velocity
+//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
+//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
+    btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
+    btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
+
+    btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+    btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+    btVector3 vel = vel1 - vel2;
+
+    btScalar rel_vel = axis_normal_on_a.dot(vel);
+
+
+
+/// apply displacement correction
 
-	 if (getMatrixElem(mat,2) < btScalar(1.0))
+//positional error (zeroth order error)
+    btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
+    btScalar	lo = btScalar(-1e30);
+    btScalar	hi = btScalar(1e30);
+
+    btScalar minLimit = m_lowerLimit[limit_index];
+    btScalar maxLimit = m_upperLimit[limit_index];
+
+    //handle the limits
+    if (minLimit < maxLimit)
     {
-        if (getMatrixElem(mat,2) > btScalar(-1.0))
         {
-            xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8));
-            xyz[1] = btAsin(getMatrixElem(mat,2));
-            xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0));
-            return true;
+            if (depth > maxLimit)
+            {
+                depth -= maxLimit;
+                lo = btScalar(0.);
+
+            }
+            else
+            {
+                if (depth < minLimit)
+                {
+                    depth -= minLimit;
+                    hi = btScalar(0.);
+                }
+                else
+                {
+                    return 0.0f;
+                }
+            }
         }
-        else
-        {
-            // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
-            xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
-            xyz[1] = -SIMD_HALF_PI;
-            xyz[2] = btScalar(0.0);
-            return false;
-        }
-    }
-    else
-    {
-        // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
-        xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
-        xyz[1] = SIMD_HALF_PI;
-        xyz[2] = 0.0;
-     
     }
-	 
-	return false;
+
+    btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
+
+
+
+
+    btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
+    btScalar sum = oldNormalImpulse + normalImpulse;
+    m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
+    normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
+
+    btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
+    body1.applyImpulse( impulse_vector, rel_pos1);
+    body2.applyImpulse(-impulse_vector, rel_pos2);
+    return normalImpulse;
+}
+
+//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
+
+
+btGeneric6DofConstraint::btGeneric6DofConstraint()
+        :btTypedConstraint(D6_CONSTRAINT_TYPE),
+		m_useLinearReferenceFrameA(true)
+{
+}
+
+btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
+        , m_frameInA(frameInA)
+        , m_frameInB(frameInB),
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
+
 }
 
 
-void	btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
+
+
+
+void btGeneric6DofConstraint::calculateAngleInfo()
 {
-	btScalar tau = btScalar(0.1);
-	btScalar damping = btScalar(1.0);
+	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
+
+	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
+
+
+
+	// 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.
+
+	btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
+	btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
+
+	m_calculatedAxis[1] = axis2.cross(axis0);
+	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
+	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
+
+
+//    if(m_debugDrawer)
+//    {
+//
+//    	char buff[300];
+//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
+//		m_calculatedAxisAngleDiff[0],
+//		m_calculatedAxisAngleDiff[1],
+//		m_calculatedAxisAngleDiff[2]);
+//    	m_debugDrawer->reportErrorWarning(buff);
+//    }
 
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
+}
 
-	btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-	btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-	
-	btVector3 localNormalInA(0,0,0);
-	int i;
+void btGeneric6DofConstraint::calculateTransforms()
+{
+    m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+    m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
 
-	// linear
-	for (i=0;i<3;i++)
-	{		
-		if (isLimited(i))
-		{
-			btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-			btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-		
-			localNormalInA.setValue(0,0,0);
-			localNormalInA[i] = 1;
-			btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
-
-			btScalar jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
-
-			//velocity error (first order error)
-			btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-																	m_rbB.getLinearVelocity(),angvelB);
-		
-			//positional error (zeroth order error)
-			btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); 
-			btScalar	lo = btScalar(-1e30);
-			btScalar	hi = btScalar(1e30);
-		
-			//handle the limits
-			if (m_lowerLimit[i] < m_upperLimit[i])
-			{
-				{
-					if (depth > m_upperLimit[i])
-					{
-						depth -= m_upperLimit[i];
-						lo = btScalar(0.);
-					
-					} else
-					{
-						if (depth < m_lowerLimit[i])
-						{
-							depth -= m_lowerLimit[i];
-							hi = btScalar(0.);
-						} else
-						{
-							continue;
-						}
-					}
-				}
-			}
+    calculateAngleInfo();
+}
 
-			btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
-			btScalar oldNormalImpulse = m_accumulatedImpulse[i];
-			btScalar sum = oldNormalImpulse + normalImpulse;
-			m_accumulatedImpulse[i] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
-			normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse;
-
-			btVector3 impulse_vector = normalWorld * normalImpulse;
-			m_rbA.applyImpulse( impulse_vector, rel_pos1);
-			m_rbB.applyImpulse(-impulse_vector, rel_pos2);
-			
-			localNormalInA[i] = 0;
-		}
-	}
 
-	btVector3	axis;
-	btScalar	angle;
-	btTransform	frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA;
-	btTransform	frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB;
+void btGeneric6DofConstraint::buildLinearJacobian(
+    btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+    const btVector3 & pivotAInW,const btVector3 & pivotBInW)
+{
+    new (&jacLinear) btJacobianEntry(
+        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+        pivotAInW - m_rbA.getCenterOfMassPosition(),
+        pivotBInW - m_rbB.getCenterOfMassPosition(),
+        normalWorld,
+        m_rbA.getInvInertiaDiagLocal(),
+        m_rbA.getInvMass(),
+        m_rbB.getInvInertiaDiagLocal(),
+        m_rbB.getInvMass());
 
-	btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle);
-	btQuaternion diff(axis,angle);
-	btMatrix3x3 diffMat (diff);
-	btVector3 xyz;
-	///this is not perfect, we can first check which axis are limited, and choose a more appropriate order
-	MatrixToEulerXYZ(diffMat,xyz);
+}
+
+void btGeneric6DofConstraint::buildAngularJacobian(
+    btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
+{
+    new (&jacAngular)	btJacobianEntry(jointAxisW,
+                                      m_rbA.getCenterOfMassTransform().getBasis().transpose(),
+                                      m_rbB.getCenterOfMassTransform().getBasis().transpose(),
+                                      m_rbA.getInvInertiaDiagLocal(),
+                                      m_rbB.getInvInertiaDiagLocal());
 
-	// angular
-	for (i=0;i<3;i++)
-	{
-		if (isLimited(i+3))
-		{
-			btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
-			btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
-		
-			btScalar jacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
-			
-			//velocity error (first order error)
-			btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
-																			m_rbB.getLinearVelocity(),angvelB);
-
-			//positional error (zeroth order error)
-			btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
-			btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
-
-			btScalar rel_pos = kSign[i] * axisA.dot(axisB);
-
-			btScalar	lo = btScalar(-1e30);
-			btScalar	hi = btScalar(1e30);
-		
-			//handle the twist limit
-			if (m_lowerLimit[i+3] < m_upperLimit[i+3])
-			{
-				//clamp the values
-				btScalar loLimit =  m_lowerLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : btScalar(-1e30);
-				btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : btScalar(1e30);
-
-				btScalar projAngle  = btScalar(-1.)*xyz[i];
-				
-				if (projAngle < loLimit)
-				{
-					hi = btScalar(0.);
-					rel_pos = (loLimit - projAngle);
-				} else
-				{
-					if (projAngle > hiLimit)
-					{
-						lo = btScalar(0.);
-						rel_pos = (hiLimit - projAngle);
-					} else
-					{
-						continue;
-					}
-				}
-			}
-		
-			//impulse
-			
-			btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
-			btScalar oldNormalImpulse = m_accumulatedImpulse[i+3];
-			btScalar sum = oldNormalImpulse + normalImpulse;
-			m_accumulatedImpulse[i+3] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
-			normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse;
-			
-			// Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
-			btVector3 axis = kSign[i] * axisA.cross(axisB);
-			btVector3 impulse_vector = axis * normalImpulse;
-
-			m_rbA.applyTorqueImpulse( impulse_vector);
-			m_rbB.applyTorqueImpulse(-impulse_vector);
-		}
-	}
 }
 
-void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
+bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
-	(void)timeStep;
+    btScalar angle = m_calculatedAxisAngleDiff[axis_index];
 
+    //test limits
+    m_angularLimits[axis_index].testLimitValue(angle);
+    return m_angularLimits[axis_index].needApplyTorques();
 }
 
-btScalar btGeneric6DofConstraint::computeAngle(int axis) const
-	{
-	btScalar angle = btScalar(0.f);
+void btGeneric6DofConstraint::buildJacobian()
+{
 
-	switch (axis)
-		{
-		case 0:
-			{
-			btVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1);
-			btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
-			btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
+	// Clear accumulated impulses for the next simulation step
+    m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
+    int i;
+    for(i = 0; i < 3; i++)
+    {
+        m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
+    }
+    //calculates transform
+    calculateTransforms();
+
+//  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
+//  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
+	calcAnchorPos();
+	btVector3 pivotAInW = m_AnchorPos;
+	btVector3 pivotBInW = m_AnchorPos;
+
+// not used here
+//    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
+//    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+    btVector3 normalWorld;
+    //linear part
+    for (i=0;i<3;i++)
+    {
+        if (m_linearLimits.isLimited(i))
+        {
+			if (m_useLinearReferenceFrameA)
+	            normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+			else
+	            normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
 
-			btScalar s = v1.dot(w2);
-			btScalar c = v1.dot(v2);
+            buildLinearJacobian(
+                m_jacLinear[i],normalWorld ,
+                pivotAInW,pivotBInW);
 
-			angle = btAtan2( s, c );
-			}
-			break;
+        }
+    }
 
-		case 1:
-			{
-			btVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2);
-			btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
-			btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
+    // angular part
+    for (i=0;i<3;i++)
+    {
+        //calculates error angle
+        if (testAngularLimitMotor(i))
+        {
+            normalWorld = this->getAxis(i);
+            // Create angular atom
+            buildAngularJacobian(m_jacAng[i],normalWorld);
+        }
+    }
 
-			btScalar s = w1.dot(u2);
-			btScalar c = w1.dot(w2);
 
-			angle = btAtan2( s, c );
-			}
-			break;
+}
 
-		case 2:
-			{
-			btVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0);
-			btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
-			btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
 
-			btScalar s = u1.dot(v2);
-			btScalar c = u1.dot(u2);
+void btGeneric6DofConstraint::solveConstraint(btScalar	timeStep)
+{
+    m_timeStep = timeStep;
 
-			angle = btAtan2( s, c );
-			}
-			break;
-                  default: 
-					  btAssert ( 0 ) ; 
-					  
-					  break ;
-		}
+    //calculateTransforms();
+
+    int i;
+
+    // linear
+
+    btVector3 pointInA = m_calculatedTransformA.getOrigin();
+	btVector3 pointInB = m_calculatedTransformB.getOrigin();
+
+	btScalar jacDiagABInv;
+	btVector3 linear_axis;
+    for (i=0;i<3;i++)
+    {
+        if (m_linearLimits.isLimited(i))
+        {
+            jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
+
+			if (m_useLinearReferenceFrameA)
+	            linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
+			else
+	            linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
+
+            m_linearLimits.solveLinearAxis(
+            	m_timeStep,
+            	jacDiagABInv,
+            	m_rbA,pointInA,
+                m_rbB,pointInB,
+                i,linear_axis, m_AnchorPos);
+
+        }
+    }
+
+    // angular
+    btVector3 angular_axis;
+    btScalar angularJacDiagABInv;
+    for (i=0;i<3;i++)
+    {
+        if (m_angularLimits[i].needApplyTorques())
+        {
+
+			// get axis
+			angular_axis = getAxis(i);
+
+			angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
+
+			m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
+        }
+    }
+}
+
+void	btGeneric6DofConstraint::updateRHS(btScalar	timeStep)
+{
+    (void)timeStep;
+
+}
+
+btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
+{
+    return m_calculatedAxis[axis_index];
+}
 
-		return angle;
+btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
+{
+    return m_calculatedAxisAngleDiff[axis_index];
+}
+
+void btGeneric6DofConstraint::calcAnchorPos(void)
+{
+	btScalar imA = m_rbA.getInvMass();
+	btScalar imB = m_rbB.getInvMass();
+	btScalar weight;
+	if(imB == btScalar(0.0))
+	{
+		weight = btScalar(1.0);
+	}
+	else
+	{
+		weight = imA / (imA + imB);
 	}
+	const btVector3& pA = m_calculatedTransformA.getOrigin();
+	const btVector3& pB = m_calculatedTransformB.getOrigin();
+	m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
+	return;
+} // btGeneric6DofConstraint::calcAnchorPos()
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
index b114e54fa69..f0718d2d4a0 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -4,116 +4,433 @@ 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, 
+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
+btGeneric6DofConstraint Refactored by Francisco Le?n
+email: projectileman@yahoo.com
+http://gimpact.sf.net
+*/
+
 
 #ifndef GENERIC_6DOF_CONSTRAINT_H
 #define GENERIC_6DOF_CONSTRAINT_H
 
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
 
 class btRigidBody;
 
 
+//! Rotation Limit structure for generic joints
+class btRotationalLimitMotor
+{
+public:
+    //! limit_parameters
+    //!@{
+    btScalar m_loLimit;//!< joint limit
+    btScalar m_hiLimit;//!< joint limit
+    btScalar m_targetVelocity;//!< target motor velocity
+    btScalar m_maxMotorForce;//!< max force on motor
+    btScalar m_maxLimitForce;//!< max force on limit
+    btScalar m_damping;//!< Damping.
+    btScalar m_limitSoftness;//! Relaxation factor
+    btScalar m_ERP;//!< Error tolerance factor when joint is at limit
+    btScalar m_bounce;//!< restitution factor
+    bool m_enableMotor;
+
+    //!@}
+
+    //! temp_variables
+    //!@{
+    btScalar m_currentLimitError;//!  How much is violated this limit
+    int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
+    btScalar m_accumulatedImpulse;
+    //!@}
+
+    btRotationalLimitMotor()
+    {
+    	m_accumulatedImpulse = 0.f;
+        m_targetVelocity = 0;
+        m_maxMotorForce = 0.1f;
+        m_maxLimitForce = 300.0f;
+        m_loLimit = -SIMD_INFINITY;
+        m_hiLimit = SIMD_INFINITY;
+        m_ERP = 0.5f;
+        m_bounce = 0.0f;
+        m_damping = 1.0f;
+        m_limitSoftness = 0.5f;
+        m_currentLimit = 0;
+        m_currentLimitError = 0;
+        m_enableMotor = false;
+    }
+
+    btRotationalLimitMotor(const btRotationalLimitMotor & limot)
+    {
+        m_targetVelocity = limot.m_targetVelocity;
+        m_maxMotorForce = limot.m_maxMotorForce;
+        m_limitSoftness = limot.m_limitSoftness;
+        m_loLimit = limot.m_loLimit;
+        m_hiLimit = limot.m_hiLimit;
+        m_ERP = limot.m_ERP;
+        m_bounce = limot.m_bounce;
+        m_currentLimit = limot.m_currentLimit;
+        m_currentLimitError = limot.m_currentLimitError;
+        m_enableMotor = limot.m_enableMotor;
+    }
+
+
+
+	//! Is limited
+    bool isLimited()
+    {
+    	if(m_loLimit>=m_hiLimit) return false;
+    	return true;
+    }
+
+	//! Need apply correction
+    bool needApplyTorques()
+    {
+    	if(m_currentLimit == 0 && m_enableMotor == false) return false;
+    	return true;
+    }
+
+	//! calculates  error
+	/*!
+	calculates m_currentLimit and m_currentLimitError.
+	*/
+	int testLimitValue(btScalar test_value);
+
+	//! apply the correction impulses for two bodies
+    btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
+
+
+};
+
+
+
+class btTranslationalLimitMotor
+{
+public:
+	btVector3 m_lowerLimit;//!< the constraint lower limits
+    btVector3 m_upperLimit;//!< the constraint upper limits
+    btVector3 m_accumulatedImpulse;
+    //! Linear_Limit_parameters
+    //!@{
+    btScalar	m_limitSoftness;//!< Softness for linear limit
+    btScalar	m_damping;//!< Damping for linear limit
+    btScalar	m_restitution;//! Bounce parameter for linear limit
+    //!@}
+
+    btTranslationalLimitMotor()
+    {
+    	m_lowerLimit.setValue(0.f,0.f,0.f);
+    	m_upperLimit.setValue(0.f,0.f,0.f);
+    	m_accumulatedImpulse.setValue(0.f,0.f,0.f);
+
+    	m_limitSoftness = 0.7f;
+    	m_damping = btScalar(1.0f);
+    	m_restitution = btScalar(0.5f);
+    }
+
+    btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
+    {
+    	m_lowerLimit = other.m_lowerLimit;
+    	m_upperLimit = other.m_upperLimit;
+    	m_accumulatedImpulse = other.m_accumulatedImpulse;
+
+    	m_limitSoftness = other.m_limitSoftness ;
+    	m_damping = other.m_damping;
+    	m_restitution = other.m_restitution;
+    }
+
+    //! Test limit
+	/*!
+    - free means upper < lower,
+    - locked means upper == lower
+    - limited means upper > lower
+    - limitIndex: first 3 are linear, next 3 are angular
+    */
+    inline bool	isLimited(int limitIndex)
+    {
+       return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
+    }
+
+
+    btScalar solveLinearAxis(
+    	btScalar timeStep,
+        btScalar jacDiagABInv,
+        btRigidBody& body1,const btVector3 &pointInA,
+        btRigidBody& body2,const btVector3 &pointInB,
+        int limit_index,
+        const btVector3 & axis_normal_on_a,
+		const btVector3 & anchorPos);
+
+
+};
 
 /// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
-/// btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
-/// Work in progress (is still a Hinge actually)
+/*!
+btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
+currently this limit supports rotational motors<br>
+<ul>
+<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method.
+At this moment translational motors are not supported. May be in the future. </li>
+
+<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit.
+This is accessible through btGeneric6DofConstraint.getLimitMotor method,
+This brings support for limit parameters and motors. </li>
+
+<li> Angulars limits have these possible ranges:
+<table border=1 >
+<tr
+
+	<td><b>AXIS</b></td>
+	<td><b>MIN ANGLE</b></td>
+	<td><b>MAX ANGLE</b></td>
+	<td>X</td>
+		<td>-PI</td>
+		<td>PI</td>
+	<td>Y</td>
+		<td>-PI/2</td>
+		<td>PI/2</td>
+	<td>Z</td>
+		<td>-PI/2</td>
+		<td>PI/2</td>
+</tr>
+</table>
+</li>
+</ul>
+
+*/
 class btGeneric6DofConstraint : public btTypedConstraint
 {
-	btJacobianEntry	m_jacLinear[3];			// 3 orthogonal linear constraints
-	btJacobianEntry	m_jacAng[3];		// 3 orthogonal angular constraints
+protected:
+
+	//! relative_frames
+    //!@{
+	btTransform	m_frameInA;//!< the constraint space w.r.t body A
+    btTransform	m_frameInB;//!< the constraint space w.r.t body B
+    //!@}
+
+    //! Jacobians
+    //!@{
+    btJacobianEntry	m_jacLinear[3];//!< 3 orthogonal linear constraints
+    btJacobianEntry	m_jacAng[3];//!< 3 orthogonal angular constraints
+    //!@}
 
-	btTransform	m_frameInA;			// the constraint space w.r.t body A
-	btTransform	m_frameInB;			// the constraint space w.r.t body B
+	//! Linear_Limit_parameters
+    //!@{
+    btTranslationalLimitMotor m_linearLimits;
+    //!@}
+
+
+    //! hinge_parameters
+    //!@{
+    btRotationalLimitMotor m_angularLimits[3];
+	//!@}
+
+
+protected:
+    //! temporal variables
+    //!@{
+    btScalar m_timeStep;
+    btTransform m_calculatedTransformA;
+    btTransform m_calculatedTransformB;
+    btVector3 m_calculatedAxisAngleDiff;
+    btVector3 m_calculatedAxis[3];
+    
+	btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
+
+    bool	m_useLinearReferenceFrameA;
+    
+    //!@}
+
+    btGeneric6DofConstraint&	operator=(btGeneric6DofConstraint&	other)
+    {
+        btAssert(0);
+        (void) other;
+        return *this;
+    }
+
+
+
+    void buildLinearJacobian(
+        btJacobianEntry & jacLinear,const btVector3 & normalWorld,
+        const btVector3 & pivotAInW,const btVector3 & pivotBInW);
+
+    void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
+
+
+	//! calcs the euler angles between the two bodies.
+    void calculateAngleInfo();
 
-	btScalar      m_lowerLimit[6];	// the constraint lower limits
-	btScalar      m_upperLimit[6];	// the constraint upper limits
 
-	btScalar		m_accumulatedImpulse[6];
 
-	btGeneric6DofConstraint&	operator=(btGeneric6DofConstraint&	other)
-	{
-		btAssert(0);
-		(void) other;
-		return *this;
-	}
-		
 public:
-	btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB );
-
-	btGeneric6DofConstraint();
-
-	
-	virtual void	buildJacobian();
-
-	virtual	void	solveConstraint(btScalar	timeStep);
-
-	void	updateRHS(btScalar	timeStep);
-
-	btScalar computeAngle(int axis) const;
-
-	void	setLinearLowerLimit(const btVector3& linearLower)
-	{
-		m_lowerLimit[0] = linearLower.getX();
-		m_lowerLimit[1] = linearLower.getY();
-		m_lowerLimit[2] = linearLower.getZ();
-	}
-
-	void	setLinearUpperLimit(const btVector3& linearUpper)
-	{
-		m_upperLimit[0] = linearUpper.getX();
-		m_upperLimit[1] = linearUpper.getY();
-		m_upperLimit[2] = linearUpper.getZ();
-	}
-
-	void	setAngularLowerLimit(const btVector3& angularLower)
-	{
-		m_lowerLimit[3] = angularLower.getX();
-		m_lowerLimit[4] = angularLower.getY();
-		m_lowerLimit[5] = angularLower.getZ();
-	}
-
-	void	setAngularUpperLimit(const btVector3& angularUpper)
-	{
-		m_upperLimit[3] = angularUpper.getX();
-		m_upperLimit[4] = angularUpper.getY();
-		m_upperLimit[5] = angularUpper.getZ();
-	}
-
-	//first 3 are linear, next 3 are angular
-	void SetLimit(int axis, btScalar lo, btScalar hi)
-	{
-		m_lowerLimit[axis] = lo; 
-		m_upperLimit[axis] = hi; 
-	}
-
-	//free means upper < lower, 
-	//locked means upper == lower
-	//limited means upper > lower
-	//limitIndex: first 3 are linear, next 3 are angular
-	bool	isLimited(int limitIndex)
-	{
-		return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
-	}
-
-	const btRigidBody& getRigidBodyA() const
-	{
-		return m_rbA;
-	}
-	const btRigidBody& getRigidBodyB() const
-	{
-		return m_rbB;
-	}
-	
+    btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+
+    btGeneric6DofConstraint();
+
+	//! Calcs global transform of the offsets
+	/*!
+	Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
+	\sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo
+	*/
+    void calculateTransforms();
+
+	//! Gets the global transform of the offset for body A
+    /*!
+    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
+    */
+    const btTransform & getCalculatedTransformA() const
+    {
+    	return m_calculatedTransformA;
+    }
+
+    //! Gets the global transform of the offset for body B
+    /*!
+    \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo.
+    */
+    const btTransform & getCalculatedTransformB() const
+    {
+    	return m_calculatedTransformB;
+    }
+
+    const btTransform & getFrameOffsetA() const
+    {
+    	return m_frameInA;
+    }
+
+    const btTransform & getFrameOffsetB() const
+    {
+    	return m_frameInB;
+    }
+
+
+    btTransform & getFrameOffsetA()
+    {
+    	return m_frameInA;
+    }
+
+    btTransform & getFrameOffsetB()
+    {
+    	return m_frameInB;
+    }
+
+
+	//! performs Jacobian calculation, and also calculates angle differences and axis
+    virtual void	buildJacobian();
+
+    virtual	void	solveConstraint(btScalar	timeStep);
+
+    void	updateRHS(btScalar	timeStep);
+
+	//! Get the rotation axis in global coordinates
+	/*!
+	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	*/
+    btVector3 getAxis(int axis_index) const;
+
+    //! Get the relative Euler angle
+    /*!
+	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	*/
+    btScalar getAngle(int axis_index) const;
+
+	//! Test angular limit.
+	/*!
+	Calculates angular correction and returns true if limit needs to be corrected.
+	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	*/
+    bool testAngularLimitMotor(int axis_index);
+
+    void	setLinearLowerLimit(const btVector3& linearLower)
+    {
+    	m_linearLimits.m_lowerLimit = linearLower;
+    }
+
+    void	setLinearUpperLimit(const btVector3& linearUpper)
+    {
+    	m_linearLimits.m_upperLimit = linearUpper;
+    }
+
+    void	setAngularLowerLimit(const btVector3& angularLower)
+    {
+        m_angularLimits[0].m_loLimit = angularLower.getX();
+        m_angularLimits[1].m_loLimit = angularLower.getY();
+        m_angularLimits[2].m_loLimit = angularLower.getZ();
+    }
+
+    void	setAngularUpperLimit(const btVector3& angularUpper)
+    {
+        m_angularLimits[0].m_hiLimit = angularUpper.getX();
+        m_angularLimits[1].m_hiLimit = angularUpper.getY();
+        m_angularLimits[2].m_hiLimit = angularUpper.getZ();
+    }
+
+	//! Retrieves the angular limit informacion
+    btRotationalLimitMotor * getRotationalLimitMotor(int index)
+    {
+    	return &m_angularLimits[index];
+    }
+
+    //! Retrieves the  limit informacion
+    btTranslationalLimitMotor * getTranslationalLimitMotor()
+    {
+    	return &m_linearLimits;
+    }
+
+    //first 3 are linear, next 3 are angular
+    void setLimit(int axis, btScalar lo, btScalar hi)
+    {
+    	if(axis<3)
+    	{
+    		m_linearLimits.m_lowerLimit[axis] = lo;
+    		m_linearLimits.m_upperLimit[axis] = hi;
+    	}
+    	else
+    	{
+    		m_angularLimits[axis-3].m_loLimit = lo;
+    		m_angularLimits[axis-3].m_hiLimit = hi;
+    	}
+    }
+
+	//! Test limit
+	/*!
+    - free means upper < lower,
+    - locked means upper == lower
+    - limited means upper > lower
+    - limitIndex: first 3 are linear, next 3 are angular
+    */
+    bool	isLimited(int limitIndex)
+    {
+    	if(limitIndex<3)
+    	{
+			return m_linearLimits.isLimited(limitIndex);
+
+    	}
+        return m_angularLimits[limitIndex-3].isLimited();
+    }
+
+    const btRigidBody& getRigidBodyA() const
+    {
+        return m_rbA;
+    }
+    const btRigidBody& getRigidBodyB() const
+    {
+        return m_rbB;
+    }
+
+	virtual void calcAnchorPos(void); // overridable
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
index 27e30987549..114abce24c7 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
@@ -17,58 +17,185 @@ subject to the following restrictions:
 #include "btHingeConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btMinMax.h"
 #include <new>
 
-btHingeConstraint::btHingeConstraint():
+
+btHingeConstraint::btHingeConstraint()
+: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
 m_enableAngularMotor(false)
 {
 }
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-								 btVector3& axisInA,btVector3& axisInB)
-:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
-m_axisInA(axisInA),
-m_axisInB(-axisInB),
-m_angularOnly(false),
-m_enableAngularMotor(false)
+									 btVector3& axisInA,btVector3& axisInB)
+									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
+									 m_angularOnly(false),
+									 m_enableAngularMotor(false)
 {
+	m_rbAFrame.getOrigin() = pivotInA;
+	
+	// since no frame is given, assume this to be zero angle and just pick rb transform axis
+	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
+
+	btVector3 rbAxisA2;
+	btScalar projection = axisInA.dot(rbAxisA1);
+	if (projection >= 1.0f - SIMD_EPSILON) {
+		rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
+		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+	} else if (projection <= -1.0f + SIMD_EPSILON) {
+		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
+		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
+	} else {
+		rbAxisA2 = axisInA.cross(rbAxisA1);
+		rbAxisA1 = rbAxisA2.cross(axisInA);
+	}
+
+	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+	btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);	
+	
+	m_rbBFrame.getOrigin() = pivotInB;
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
+	
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
 
 }
 
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
-m_axisInA(axisInA),
-//fixed axis in worldspace
-m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
+{
+
+	// since no frame is given, assume this to be zero angle and just pick rb transform axis
+	// fixed axis in worldspace
+	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
+	btScalar projection = rbAxisA1.dot(axisInA);
+	if (projection > SIMD_EPSILON)
+		rbAxisA1 = rbAxisA1*projection - axisInA;
+	else
+		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
+
+	btVector3 rbAxisA2 = axisInA.cross(rbAxisA1);
+
+	m_rbAFrame.getOrigin() = pivotInA;
+	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
+									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
+									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
+
+
+	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
+
+	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
+	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
+	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
+
+
+	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
+	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
+									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
+									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
+	
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+}
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
+								     const btTransform& rbAFrame, const btTransform& rbBFrame)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 m_angularOnly(false),
 m_enableAngularMotor(false)
 {
-	
+	// flip axis
+	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
+
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
+}			
+
+
+
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
+:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+m_angularOnly(false),
+m_enableAngularMotor(false)
+{
+	///not providing rigidbody B means implicitly using worldspace for body B
+
+	// flip axis
+	m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
+	m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
+
+	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
+
+	//start with free
+	m_lowerLimit = btScalar(1e30);
+	m_upperLimit = btScalar(-1e30);	
+	m_biasFactor = 0.3f;
+	m_relaxationFactor = 1.0f;
+	m_limitSoftness = 0.9f;
+	m_solveLimit = false;
 }
 
 void	btHingeConstraint::buildJacobian()
 {
 	m_appliedImpulse = btScalar(0.);
 
-	btVector3	normal(0,0,0);
-
 	if (!m_angularOnly)
 	{
+		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+		btVector3 relPos = pivotBInW - pivotAInW;
+
+		btVector3 normal[3];
+		if (relPos.length2() > SIMD_EPSILON)
+		{
+			normal[0] = relPos.normalized();
+		}
+		else
+		{
+			normal[0].setValue(btScalar(1.0),0,0);
+		}
+
+		btPlaneSpace1(normal[0], normal[1], normal[2]);
+
 		for (int i=0;i<3;i++)
 		{
-			normal[i] = 1;
 			new (&m_jac[i]) btJacobianEntry(
 				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
 				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-				m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
-				m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
-				normal,
+				pivotAInW - m_rbA.getCenterOfMassPosition(),
+				pivotBInW - m_rbB.getCenterOfMassPosition(),
+				normal[i],
 				m_rbA.getInvInertiaDiagLocal(),
 				m_rbA.getInvMass(),
 				m_rbB.getInvInertiaDiagLocal(),
 				m_rbB.getInvMass());
-			normal[i] = 0;
 		}
 	}
 
@@ -79,12 +206,12 @@ void	btHingeConstraint::buildJacobian()
 	btVector3 jointAxis0local;
 	btVector3 jointAxis1local;
 	
-	btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);
+	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
 
-	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
 	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
 	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
-	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
 		
 	new (&m_jacAng[0])	btJacobianEntry(jointAxis0,
 		m_rbA.getCenterOfMassTransform().getBasis().transpose(),
@@ -105,44 +232,70 @@ void	btHingeConstraint::buildJacobian()
 		m_rbB.getInvInertiaDiagLocal());
 
 
+	// Compute limit information
+	btScalar hingeAngle = getHingeAngle();  
+
+	//set bias, sign, clear accumulator
+	m_correction = btScalar(0.);
+	m_limitSign = btScalar(0.);
+	m_solveLimit = false;
+	m_accLimitImpulse = btScalar(0.);
+
+	if (m_lowerLimit < m_upperLimit)
+	{
+		if (hingeAngle <= m_lowerLimit*m_limitSoftness)
+		{
+			m_correction = (m_lowerLimit - hingeAngle);
+			m_limitSign = 1.0f;
+			m_solveLimit = true;
+		} 
+		else if (hingeAngle >= m_upperLimit*m_limitSoftness)
+		{
+			m_correction = m_upperLimit - hingeAngle;
+			m_limitSign = -1.0f;
+			m_solveLimit = true;
+		}
+	}
+
+	//Compute K = J*W*J' for hinge axis
+	btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+	m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+			             getRigidBodyB().computeAngularImpulseDenominator(axisA));
 
 }
 
 void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 {
 
-	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
-	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
+	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
+	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
 
-	btVector3 normal(0,0,0);
 	btScalar tau = btScalar(0.3);
-	btScalar damping = btScalar(1.);
 
-//linear part
+	//linear part
 	if (!m_angularOnly)
 	{
+		btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+		btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+
+		btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+		btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+		btVector3 vel = vel1 - vel2;
+
 		for (int i=0;i<3;i++)
 		{		
-			normal[i] = 1;
+			const btVector3& normal = m_jac[i].m_linearJointAxis;
 			btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
 
-			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-			
-			btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-			btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-			btVector3 vel = vel1 - vel2;
 			btScalar rel_vel;
 			rel_vel = normal.dot(vel);
 			//positional error (zeroth order error)
 			btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
+			btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
 			m_appliedImpulse += impulse;
 			btVector3 impulse_vector = normal * impulse;
 			m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
 			m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
-			
-			normal[i] = 0;
 		}
 	}
 
@@ -151,8 +304,8 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 		///solve angular part
 
 		// get axes in world space
-		btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
-		btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
+		btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
+		btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
 
 		const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
 		const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
@@ -174,7 +327,7 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 					getRigidBodyB().computeAngularImpulseDenominator(normal);
 				// scale for mass and relaxation
 				//todo:  expose this 0.9 factor to developer
-				velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9);
+				velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
 			}
 
 			//solve angular positional correction
@@ -190,10 +343,28 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 
 			m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
 			m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
+
+			// solve limit
+			if (m_solveLimit)
+			{
+				btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
+
+				btScalar impulseMag = amplitude * m_kHinge;
+
+				// Clamp the accumulated impulse
+				btScalar temp = m_accLimitImpulse;
+				m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
+				impulseMag = m_accLimitImpulse - temp;
+
+
+				btVector3 impulse = axisA * impulseMag * m_limitSign;
+				m_rbA.applyTorqueImpulse(impulse);
+				m_rbB.applyTorqueImpulse(-impulse);
+			}
 		}
 
 		//apply motor
-		if (m_enableAngularMotor)
+		if (m_enableAngularMotor) 
 		{
 			//todo: add limits too
 			btVector3 angularLimit(0,0,0);
@@ -204,10 +375,7 @@ void	btHingeConstraint::solveConstraint(btScalar	timeStep)
 			btScalar desiredMotorVel = m_motorTargetVelocity;
 			btScalar motor_relvel = desiredMotorVel - projRelVel;
 
-			btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-					getRigidBodyB().computeAngularImpulseDenominator(axisA);
-
-			btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;;
+			btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
 			//todo: should clip against accumulated impulse
 			btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
 			clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
@@ -227,3 +395,12 @@ void	btHingeConstraint::updateRHS(btScalar	timeStep)
 
 }
 
+btScalar btHingeConstraint::getHingeAngle()
+{
+	const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
+	const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
+	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
+
+	return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
+}
+
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
index 5c1ceafbc5b..4fa9972f6d8 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h
@@ -13,39 +13,61 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
+
 #ifndef HINGECONSTRAINT_H
 #define HINGECONSTRAINT_H
 
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
 
 class btRigidBody;
 
-
 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /// axis defines the orientation of the hinge axis
 class btHingeConstraint : public btTypedConstraint
 {
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
 	btJacobianEntry	m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
-	btVector3	m_pivotInA;
-	btVector3	m_pivotInB;
-	btVector3	m_axisInA;
-	btVector3	m_axisInB;
+	btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
+	btTransform m_rbBFrame;
 
-	bool		m_angularOnly;
+	btScalar	m_motorTargetVelocity;
+	btScalar	m_maxMotorImpulse;
+
+	btScalar	m_limitSoftness; 
+	btScalar	m_biasFactor; 
+	btScalar    m_relaxationFactor; 
 
-	btScalar		m_motorTargetVelocity;
-	btScalar		m_maxMotorImpulse;
+	btScalar    m_lowerLimit;	
+	btScalar    m_upperLimit;	
+	
+	btScalar	m_kHinge;
+
+	btScalar	m_limitSign;
+	btScalar	m_correction;
+
+	btScalar	m_accLimitImpulse;
+
+	bool		m_angularOnly;
 	bool		m_enableAngularMotor;
+	bool		m_solveLimit;
+
 	
 public:
 
-	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB);
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
 
 	btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
+	
+	btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
+
+	btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
 
 	btHingeConstraint();
 
@@ -76,6 +98,61 @@ public:
 		m_maxMotorImpulse = maxMotorImpulse;
 	}
 
+	void	setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	{
+		m_lowerLimit = low;
+		m_upperLimit = high;
+
+		m_limitSoftness =  _softness;
+		m_biasFactor = _biasFactor;
+		m_relaxationFactor = _relaxationFactor;
+
+	}
+
+	btScalar	getLowerLimit() const
+	{
+		return m_lowerLimit;
+	}
+
+	btScalar	getUpperLimit() const
+	{
+		return m_upperLimit;
+	}
+
+
+	btScalar getHingeAngle();
+
+
+	const btTransform& getAFrame() { return m_rbAFrame; };	
+	const btTransform& getBFrame() { return m_rbBFrame; };
+
+	inline int getSolveLimit()
+	{
+		return m_solveLimit;
+	}
+
+	inline btScalar getLimitSign()
+	{
+		return m_limitSign;
+	}
+
+	inline bool getAngularOnly() 
+	{ 
+		return m_angularOnly; 
+	}
+	inline bool getEnableAngularMotor() 
+	{ 
+		return m_enableAngularMotor; 
+	}
+	inline btScalar getMotorTargetVelosity() 
+	{ 
+		return m_motorTargetVelocity; 
+	}
+	inline btScalar getMaxMotorImpulse() 
+	{ 
+		return m_maxMotorImpulse; 
+	}
+
 };
 
 #endif //HINGECONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
index aae3ed0373f..bfeb24c2dfb 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h
@@ -16,8 +16,8 @@ subject to the following restrictions:
 #ifndef JACOBIAN_ENTRY_H
 #define JACOBIAN_ENTRY_H
 
-#include "../../LinearMath/btVector3.h"
-#include "../Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
 
 
 //notes:
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp
new file mode 100644
index 00000000000..7d2f19998ac
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.cpp
@@ -0,0 +1,278 @@
+/*
+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);
+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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeContactJoint.h
new file mode 100644
index 00000000000..8dd0282c59e
--- /dev/null
+++ b/extern/bullet2/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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.cpp
new file mode 100644
index 00000000000..46c3783c6a0
--- /dev/null
+++ b/extern/bullet2/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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeJoint.h
new file mode 100644
index 00000000000..50733d1418f
--- /dev/null
+++ b/extern/bullet2/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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeMacros.h
new file mode 100644
index 00000000000..e4bc2628bd4
--- /dev/null
+++ b/extern/bullet2/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) \
+{ \
+  (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]; \
+} 
+
+
+#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; \
+	unsigned int  memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \
+	if (memNeeded_##name < static_cast<size_t>(stackAlloc->getAvailableMemory())) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \
+	else{ btAssert(memNeeded_##name < static_cast<size_t>(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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp
new file mode 100644
index 00000000000..ab90c926559
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.cpp
@@ -0,0 +1,393 @@
+/*
+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>
+#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);
+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++)
+    {
+
+        //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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeQuickstepConstraintSolver.h
new file mode 100644
index 00000000000..e548ea6fc22
--- /dev/null
+++ b/extern/bullet2/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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeSolverBody.h
new file mode 100644
index 00000000000..0c936971b79
--- /dev/null
+++ b/extern/bullet2/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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp
new file mode 100644
index 00000000000..f683bf7d748
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.cpp
@@ -0,0 +1,880 @@
+/*
+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;
+    case SLIDER_CONSTRAINT_TYPE:
+    {
+        OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
+        sliderjoint.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;
+    case SLIDER_CONSTRAINT_TYPE:
+    {
+        OdeSliderJoint sliderjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
+        sliderjoint.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_body1->m_originalBody : NULL,
+				info,row,axis,1);
+		}
+    }
+
+    return row;
+}
+
+void OdeD6Joint::GetInfo2(Info2 *info)
+{
+    int row = setLinearLimits(info);
+    setAngularLimits(info, row);
+}
+
+//----------------------------------------------------------------------------------
+//----------------------------------------------------------------------------------
+//----------------------------------------------------------------------------------
+//----------------------------------------------------------------------------------
+/*
+OdeSliderJoint
+Ported from ODE by Roman Ponomarev (rponom@gmail.com)
+April 24, 2008
+*/
+
+OdeSliderJoint::OdeSliderJoint(
+    btTypedConstraint * constraint,
+    int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1):
+        btOdeTypedJoint(constraint,index,swap,body0,body1)
+{
+} // OdeSliderJoint::OdeSliderJoint()
+
+//----------------------------------------------------------------------------------
+
+void OdeSliderJoint::GetInfo1(Info1* info)
+{
+	info->nub = 4; 
+	info->m = 4; // Fixed 2 linear + 2 angular
+	btSliderConstraint * slider = this->getSliderConstraint();
+	//prepare constraint
+	slider->calculateTransforms();
+	slider->testLinLimits();
+	if(slider->getSolveLinLimit() || slider->getPoweredLinMotor())
+	{
+		info->m++; // limit 3rd linear as well
+	}
+	slider->testAngLimits();
+	if(slider->getSolveAngLimit() || slider->getPoweredAngMotor())
+	{
+		info->m++; // limit 3rd angular as well
+	}
+} // OdeSliderJoint::GetInfo1()
+
+//----------------------------------------------------------------------------------
+
+void OdeSliderJoint::GetInfo2(Info2 *info)
+{
+	int i, s = info->rowskip;
+	btSliderConstraint * slider = this->getSliderConstraint();
+	const btTransform& trA = slider->getCalculatedTransformA();
+	const btTransform& trB = slider->getCalculatedTransformB();
+	// make rotations around Y and Z equal
+	// the slider axis should be the only unconstrained
+	// rotational axis, the angular velocity of the two bodies perpendicular to
+	// the slider 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 slider axis, and w1 and w2
+	// are the angular velocity vectors of the two bodies.
+	// get slider axis (X)
+	btVector3 ax1 = trA.getBasis().getColumn(0);
+	// get 2 orthos to slider axis (Y, Z)
+	btVector3 p = trA.getBasis().getColumn(1);
+	btVector3 q = trA.getBasis().getColumn(2);
+	// set the two slider rows 
+	info->J1a[0] = p[0];
+	info->J1a[1] = p[1];
+	info->J1a[2] = p[2];
+	info->J1a[s+0] = q[0];
+	info->J1a[s+1] = q[1];
+	info->J1a[s+2] = q[2];
+	if(m_body1) 
+	{
+		info->J2a[0] = -p[0];
+		info->J2a[1] = -p[1];
+		info->J2a[2] = -p[2];
+		info->J2a[s+0] = -q[0];
+		info->J2a[s+1] = -q[1];
+		info->J2a[s+2] = -q[2];
+	}
+	// compute the right hand side of the constraint equation. set relative
+	// body velocities along p and q to bring the slider back into alignment.
+	// if ax1,ax2 are the unit length slider 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.
+	btScalar k = info->fps * info->erp * slider->getSoftnessOrthoAng();
+    btVector3 ax2 = trB.getBasis().getColumn(0);
+	btVector3 u;
+	if(m_body1)
+	{
+		u = ax1.cross(ax2);
+	}
+	else
+	{
+		u = ax2.cross(ax1);
+	}
+	info->c[0] = k * u.dot(p);
+	info->c[1] = k * u.dot(q);
+	// pull out pos and R for both bodies. also get the connection
+	// vector c = pos2-pos1.
+	// next 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.
+	btTransform bodyA_trans = m_body0->m_originalBody->getCenterOfMassTransform();
+	btTransform bodyB_trans;
+	if(m_body1)
+	{
+		bodyB_trans = m_body1->m_originalBody->getCenterOfMassTransform();
+	}
+	int s2 = 2 * s, s3 = 3 * s;
+	btVector3 c;
+	if(m_body1)
+	{
+		c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+		btVector3 tmp = btScalar(0.5) * c.cross(p);
+
+		for (i=0; i<3; i++) info->J1a[s2+i] = tmp[i];
+		for (i=0; i<3; i++) info->J2a[s2+i] = tmp[i];
+
+		tmp = btScalar(0.5) * c.cross(q);
+
+		for (i=0; i<3; i++) info->J1a[s3+i] = tmp[i];
+		for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i];
+
+		for (i=0; i<3; i++) info->J2l[s2+i] = -p[i];
+		for (i=0; i<3; i++) info->J2l[s3+i] = -q[i];
+	}
+	for (i=0; i<3; i++) info->J1l[s2+i] = p[i];
+	for (i=0; i<3; i++) info->J1l[s3+i] = q[i];
+	// compute two elements of right hand side. we want to align the offset
+	// point (in body 2's frame) with the center of body 1.
+	btVector3 ofs; // offset point in global coordinates
+	if(m_body1)
+	{
+		ofs = trB.getOrigin() - trA.getOrigin();
+	}
+	else
+	{
+		ofs = trA.getOrigin() - trB.getOrigin();
+	}
+	k = info->fps * info->erp * slider->getSoftnessOrthoLin();
+	info->c[2] = k * p.dot(ofs);
+	info->c[3] = k * q.dot(ofs);
+	int nrow = 3; // last filled row
+	int srow;
+	// check linear limits linear
+	btScalar limit_err = btScalar(0.0);
+	int limit = 0;
+	if(slider->getSolveLinLimit())
+	{
+		limit_err = slider->getLinDepth();
+		if(m_body1) 
+		{
+			limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+		}
+		else
+		{
+			limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+		}
+	}
+	int powered = 0;
+	if(slider->getPoweredLinMotor())
+	{
+		powered = 1;
+	}
+	// if the slider has joint limits, add in the extra row
+	if (limit || powered) 
+	{
+		nrow++;
+		srow = nrow * info->rowskip;
+		info->J1l[srow+0] = ax1[0];
+		info->J1l[srow+1] = ax1[1];
+		info->J1l[srow+2] = ax1[2];
+		if(m_body1)
+		{
+			info->J2l[srow+0] = -ax1[0];
+			info->J2l[srow+1] = -ax1[1];
+			info->J2l[srow+2] = -ax1[2];
+		}
+		// linear torque decoupling step:
+		//
+		// 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 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.
+	    if (m_body1) 
+		{
+			dVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+			c = btScalar(0.5) * c;
+			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];
+		}
+		// right-hand part
+		btScalar lostop = slider->getLowerLinLimit();
+		btScalar histop = slider->getUpperLinLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		if(powered)
+		{
+            info->cfm[nrow] = btScalar(0.0); 
+            if(!limit)
+            {
+				info->c[nrow] = slider->getTargetLinMotorVelocity();
+				info->lo[nrow] = -slider->getMaxLinMotorForce() * info->fps;
+				info->hi[nrow] = slider->getMaxLinMotorForce() * info->fps;
+            }
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			if(m_body1) 
+			{
+				info->c[nrow] = k * limit_err;
+			}
+			else
+			{
+				info->c[nrow] = - k * limit_err;
+			}
+			info->cfm[nrow] = btScalar(0.0); // stop_cfm;
+			if(lostop == histop) 
+			{
+				// limited low and high simultaneously
+				info->lo[nrow] = -SIMD_INFINITY;
+				info->hi[nrow] = SIMD_INFINITY;
+			}
+			else 
+			{
+				if(limit == 1) 
+				{
+					// low limit
+					info->lo[nrow] = 0;
+					info->hi[nrow] = SIMD_INFINITY;
+				}
+				else 
+				{
+					// high limit
+					info->lo[nrow] = -SIMD_INFINITY;
+					info->hi[nrow] = 0;
+				}
+			}
+			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
+			btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimLin());
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = m_body0->m_originalBody->getLinearVelocity().dot(ax1);
+				if(m_body1)
+				{
+					vel -= m_body1->m_originalBody->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 = -bounce * vel;
+						if (newc > info->c[nrow]) info->c[nrow] = newc;
+					}
+				}
+				else
+				{
+					// high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->c[nrow]) info->c[nrow] = newc;
+					}
+				}
+			}
+			info->c[nrow] *= slider->getSoftnessLimLin();
+		} // if(limit)
+	} // if linear limit
+	// check angular limits
+	limit_err = btScalar(0.0);
+	limit = 0;
+	if(slider->getSolveAngLimit())
+	{
+		limit_err = slider->getAngDepth();
+		if(m_body1) 
+		{
+			limit = (limit_err > btScalar(0.0)) ? 1 : 2;
+		}
+		else
+		{
+			limit = (limit_err > btScalar(0.0)) ? 2 : 1;
+		}
+	}
+	// if the slider has joint limits, add in the extra row
+	powered = 0;
+	if(slider->getPoweredAngMotor())
+	{
+		powered = 1;
+	}
+	if(limit || powered) 
+	{
+		nrow++;
+		srow = nrow * info->rowskip;
+		info->J1a[srow+0] = ax1[0];
+		info->J1a[srow+1] = ax1[1];
+		info->J1a[srow+2] = ax1[2];
+		if(m_body1)
+		{
+			info->J2a[srow+0] = -ax1[0];
+			info->J2a[srow+1] = -ax1[1];
+			info->J2a[srow+2] = -ax1[2];
+		}
+		btScalar lostop = slider->getLowerAngLimit();
+		btScalar histop = slider->getUpperAngLimit();
+		if(limit && (lostop == histop))
+		{  // the joint motor is ineffective
+			powered = 0;
+		}
+		if(powered)
+		{
+            info->cfm[nrow] = btScalar(0.0); 
+            if(!limit)
+            {
+				info->c[nrow] = slider->getTargetAngMotorVelocity();
+				info->lo[nrow] = -slider->getMaxAngMotorForce() * info->fps;
+				info->hi[nrow] = slider->getMaxAngMotorForce() * info->fps;
+            }
+		}
+		if(limit)
+		{
+			k = info->fps * info->erp;
+			if (m_body1) 
+			{
+				info->c[nrow] = k * limit_err;
+			}
+			else
+			{
+				info->c[nrow] = -k * limit_err;
+			}
+			info->cfm[nrow] = btScalar(0.0); // stop_cfm;
+			if(lostop == histop) 
+			{
+				// limited low and high simultaneously
+				info->lo[nrow] = -SIMD_INFINITY;
+				info->hi[nrow] = SIMD_INFINITY;
+			}
+			else 
+			{
+				if (limit == 1) 
+				{
+					// low limit
+					info->lo[nrow] = 0;
+					info->hi[nrow] = SIMD_INFINITY;
+				}
+				else 
+				{
+					// high limit
+					info->lo[nrow] = -SIMD_INFINITY;
+					info->hi[nrow] = 0;
+				}
+			}
+			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
+			btScalar bounce = btFabs(btScalar(1.0) - slider->getDampingLimAng());
+			if(bounce > btScalar(0.0))
+			{
+				btScalar vel = m_body0->m_originalBody->getAngularVelocity().dot(ax1);
+				if(m_body1)
+				{
+					vel -= m_body1->m_originalBody->getAngularVelocity().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 = -bounce * vel;
+						if (newc > info->c[nrow]) info->c[nrow] = newc;
+					}
+				}
+				else
+				{
+					// high limit - all those computations are reversed
+					if(vel > 0)
+					{
+						btScalar newc = -bounce * vel;
+						if(newc < info->c[nrow]) info->c[nrow] = newc;
+					}
+				}
+			}
+			info->c[nrow] *= slider->getSoftnessLimAng();
+		} // if(limit)
+	} // if angular limit or powered
+} // OdeSliderJoint::GetInfo2()
+
+//----------------------------------------------------------------------------------
+//----------------------------------------------------------------------------------
+
+
+
+
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h
new file mode 100644
index 00000000000..a2affda382d
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btOdeTypedJoint.h
@@ -0,0 +1,142 @@
+/*
+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"
+#include "BulletDynamics/ConstraintSolver/btSliderConstraint.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);
+
+/*
+OdeSliderJoint
+Ported from ODE by Roman Ponomarev (rponom@gmail.com)
+April 24, 2008
+*/
+class OdeSliderJoint : public btOdeTypedJoint
+{
+protected:
+	inline btSliderConstraint * getSliderConstraint()
+	{
+		return static_cast<btSliderConstraint * >(m_constraint);
+	}
+public:
+
+	OdeSliderJoint() {};
+
+	OdeSliderJoint(btTypedConstraint* constraint,int index,bool swap, btOdeSolverBody* body0, btOdeSolverBody* body1);
+
+	//BU_Joint interface for solver
+
+	virtual void GetInfo1(Info1 *info);
+
+	virtual void GetInfo2(Info2 *info);
+};
+
+
+
+
+#endif //CONTACT_JOINT_H
+
+
+
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
index aacb0a3ea66..2b69ad90438 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp
@@ -21,18 +21,19 @@ subject to the following restrictions:
 
 
 btPoint2PointConstraint::btPoint2PointConstraint()
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE)
 {
 }
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB)
-:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB)
 {
 
 }
 
 
 btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA)
-:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
+:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA))
 {
 	
 }
@@ -99,6 +100,16 @@ void	btPoint2PointConstraint::solveConstraint(btScalar	timeStep)
 		btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
 		
 		btScalar impulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;
+
+		btScalar impulseClamp = m_setting.m_impulseClamp;
+		if (impulseClamp > 0)
+		{
+			if (impulse < -impulseClamp) 
+				impulse = -impulseClamp;
+			if (impulse > impulseClamp) 
+				impulse = impulseClamp;
+		}
+
 		m_appliedImpulse+=impulse;
 		btVector3 impulse_vector = normal * impulse;
 		m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
index 71da8ac0347..c9d5968530c 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h
@@ -16,7 +16,7 @@ subject to the following restrictions:
 #ifndef POINT2POINTCONSTRAINT_H
 #define POINT2POINTCONSTRAINT_H
 
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btVector3.h"
 #include "btJacobianEntry.h"
 #include "btTypedConstraint.h"
 
@@ -26,16 +26,21 @@ struct	btConstraintSetting
 {
 	btConstraintSetting()	:
 		m_tau(btScalar(0.3)),
-		m_damping(btScalar(1.))
+		m_damping(btScalar(1.)),
+		m_impulseClamp(btScalar(0.))
 	{
 	}
 	btScalar		m_tau;
 	btScalar		m_damping;
+	btScalar		m_impulseClamp;
 };
 
 /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
 class btPoint2PointConstraint : public btTypedConstraint
 {
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
 	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
 	
 	btVector3	m_pivotInA;
@@ -70,6 +75,15 @@ public:
 		m_pivotInB = pivotB;
 	}
 
+	const btVector3& getPivotInA() const
+	{
+		return m_pivotInA;
+	}
+
+	const btVector3& getPivotInB() const
+	{
+		return m_pivotInB;
+	}
 
 
 };
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 14b36ad44fd..b8afbd6aac5 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -13,6 +13,9 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+//#define COMPUTE_IMPULSE_DENOM 1
+//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
+//#define FORCE_REFESH_CONTACT_MANIFOLDS 1
 
 #include "btSequentialImpulseConstraintSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -30,11 +33,9 @@ subject to the following restrictions:
 #include "btSolverBody.h"
 #include "btSolverConstraint.h"
 
+
 #include "LinearMath/btAlignedObjectArray.h"
 
-#ifdef USE_PROFILE
-#include "LinearMath/btQuickprof.h"
-#endif //USE_PROFILE
 
 int totalCpd = 0;
 
@@ -64,7 +65,7 @@ unsigned long btSequentialImpulseConstraintSolver::btRand2()
 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 {
   // seems good; xor-fold and modulus
-  const unsigned long un = n;
+  const unsigned long un = static_cast<unsigned long>(n);
   unsigned long r = btRand2();
 
   // note: probably more aggressive than it needs to be -- might be
@@ -91,12 +92,12 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 
 
 
-
+bool  MyContactDestroyedCallback(void* userPersistentData);
 bool  MyContactDestroyedCallback(void* userPersistentData)
 {
 	assert (userPersistentData);
 	btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
-	delete cpd;
+	btAlignedFree(cpd);
 	totalCpd--;
 	//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
 	return true;
@@ -105,8 +106,7 @@ bool  MyContactDestroyedCallback(void* userPersistentData)
 
 
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
-:m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY), //not using SOLVER_USE_WARMSTARTING,
-m_btSeed2(0)
+:m_btSeed2(0)
 {
 	gContactDestroyedCallback = &MyContactDestroyedCallback;
 
@@ -121,27 +121,42 @@ m_btSeed2(0)
 		}
 }
 
-
-void	initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody)
+btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
 {
-/*	int size = sizeof(btSolverBody);
-	int sizeofrb = sizeof(btRigidBody);
-	int sizemanifold = sizeof(btPersistentManifold);
-	int sizeofmp = sizeof(btManifoldPoint);
-	int sizeofPersistData = sizeof (btConstraintPersistentData);
-*/
 
-	solverBody->m_angularVelocity = rigidbody->getAngularVelocity();
-	solverBody->m_centerOfMassPosition = rigidbody->getCenterOfMassPosition();
-	solverBody->m_friction = rigidbody->getFriction();
-//	solverBody->m_invInertiaWorld = rigidbody->getInvInertiaTensorWorld();
-	solverBody->m_invMass = rigidbody->getInvMass();
-	solverBody->m_linearVelocity = rigidbody->getLinearVelocity();
-	solverBody->m_originalBody = rigidbody;
-	solverBody->m_angularFactor = rigidbody->getAngularFactor();
 }
 
-btScalar penetrationResolveFactor = btScalar(0.9);
+void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
+void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
+{
+	btRigidBody* rb = btRigidBody::upcast(collisionObject);
+	if (rb)
+	{
+		solverBody->m_angularVelocity = rb->getAngularVelocity() ;
+		solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
+		solverBody->m_friction = collisionObject->getFriction();
+		solverBody->m_invMass = rb->getInvMass();
+		solverBody->m_linearVelocity = rb->getLinearVelocity();
+		solverBody->m_originalBody = rb;
+		solverBody->m_angularFactor = rb->getAngularFactor();
+	} else
+	{
+		solverBody->m_angularVelocity.setValue(0,0,0);
+		solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
+		solverBody->m_friction = collisionObject->getFriction();
+		solverBody->m_invMass = 0.f;
+		solverBody->m_linearVelocity.setValue(0,0,0);
+		solverBody->m_originalBody = 0;
+		solverBody->m_angularFactor = 1.f;
+	}
+	solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
+	solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
+}
+
+
+int		gNumSplitImpulseRecoveries = 0;
+
+btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
 {
 	btScalar rest = restitution * -rel_vel;
@@ -149,30 +164,95 @@ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
 }
 
 
+void	resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo);
+
+//SIMD_FORCE_INLINE
+void	resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo)
+{
+        (void)solverInfo;
+
+		if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
+        {
 
+				gNumSplitImpulseRecoveries++;
+                btScalar normalImpulse;
 
+                //  Optimized version of projected relative velocity, use precomputed cross products with normal
+                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+                //      btVector3 vel = vel1 - vel2;
+                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+
+                btScalar rel_vel;
+                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
+                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
+                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
+                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
+
+                rel_vel = vel1Dotn-vel2Dotn;
+
+
+				btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
+                //      btScalar positionalError = contactConstraint.m_penetration;
+
+                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
+
+                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
+                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
+                normalImpulse = penetrationImpulse+velocityImpulse;
+
+                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+                btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
+                btScalar sum = oldNormalImpulse + normalImpulse;
+                contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+                normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
+
+				body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
+               
+				body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
+               
+        }
+
+}
 
 
 //velocity + friction
 //response  between two dynamic objects with friction
-SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(
+
+btScalar resolveSingleCollisionCombinedCacheFriendly(
 	btSolverBody& body1,
 	btSolverBody& body2,
-	btSolverConstraint& contactConstraint,
+	const btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo);
+
+//SIMD_FORCE_INLINE 
+btScalar resolveSingleCollisionCombinedCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	const btSolverConstraint& contactConstraint,
 	const btContactSolverInfo& solverInfo)
 {
 	(void)solverInfo;
 
-	btScalar normalImpulse(0.f);
+	btScalar normalImpulse;
+
 	{
-		if (contactConstraint.m_penetration < 0.f)
-			return 0.f;
 
-	//  Optimized version of projected relative velocity, use precomputed cross products with normal
-	//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-	//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-	//	btVector3 vel = vel1 - vel2;
-	//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+		
+		//  Optimized version of projected relative velocity, use precomputed cross products with normal
+		//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+		//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+		//	btVector3 vel = vel1 - vel2;
+		//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
 
 		btScalar rel_vel;
 		btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
@@ -182,50 +262,51 @@ SIMD_FORCE_INLINE btScalar resolveSingleCollisionCombinedCacheFriendly(
 
 		rel_vel = vel1Dotn-vel2Dotn;
 
+		btScalar positionalError = 0.f;
+		if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
+		{
+ 			positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
+		}
 
-		btScalar positionalError = contactConstraint.m_penetration;
 		btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
 
 		btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
 		btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-		btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+		normalImpulse = penetrationImpulse+velocityImpulse;
+		
 		
 		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
 		btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
 		btScalar sum = oldNormalImpulse + normalImpulse;
 		contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
 
-		btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
-		btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
-		contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
-
 		normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
 
-		if (body1.m_invMass)
-		{
-			body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
+		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
 				contactConstraint.m_angularComponentA,normalImpulse);
-		}
-		if (body2.m_invMass)
-		{
-			body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
+		
+		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
 				contactConstraint.m_angularComponentB,-normalImpulse);
-		}
-
 	}
 
-	
-
 	return normalImpulse;
 }
 
 
 #ifndef NO_FRICTION_TANGENTIALS
 
-SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
+btScalar resolveSingleFrictionCacheFriendly(
 	btSolverBody& body1,
 	btSolverBody& body2,
-	btSolverConstraint& contactConstraint,
+	const btSolverConstraint& contactConstraint,
+	const btContactSolverInfo& solverInfo,
+	btScalar appliedNormalImpulse);
+
+//SIMD_FORCE_INLINE 
+btScalar resolveSingleFrictionCacheFriendly(
+	btSolverBody& body1,
+	btSolverBody& body2,
+	const btSolverConstraint& contactConstraint,
 	const btContactSolverInfo& solverInfo,
 	btScalar appliedNormalImpulse)
 {
@@ -252,22 +333,42 @@ SIMD_FORCE_INLINE btScalar resolveSingleFrictionCacheFriendly(
 
 			// calculate j that moves us to zero relative velocity
 			j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
+#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
+#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
 			btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
 			contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
-			GEN_set_min(contactConstraint.m_appliedImpulse, limit);
-			GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+			
+			if (limit < contactConstraint.m_appliedImpulse)
+			{
+				contactConstraint.m_appliedImpulse = limit;
+			} else
+			{
+				if (contactConstraint.m_appliedImpulse < -limit)
+					contactConstraint.m_appliedImpulse = -limit;
+			}
 			j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
+#else
+			if (limit < j1)
+			{
+				j1 = limit;
+			} else
+			{
+				if (j1 < -limit)
+					j1 = -limit;
+			}
+
+#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
+
+			//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
+			//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+
+			
 
 		}
 	
-		if (body1.m_invMass)
-		{
-			body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
-		}
-		if (body2.m_invMass)
-		{
-			body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
-		}
+		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
+		
+		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
 
 	} 
 	return 0.f;
@@ -309,7 +410,6 @@ btScalar resolveSingleFrictionCacheFriendly(
 		const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
 
 
-		//if (contactConstraint.m_appliedVelocityImpulse > 0.f)
 		if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
 		{
 			lat_rel_vel = btSqrt(lat_rel_vel);
@@ -319,7 +419,7 @@ btScalar resolveSingleFrictionCacheFriendly(
 			btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
 			btScalar friction_impulse = lat_rel_vel /
 				(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-			btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction;
+			btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
 
 			GEN_set_min(friction_impulse, normal_impulse);
 			GEN_set_max(friction_impulse, -normal_impulse);
@@ -333,39 +433,111 @@ btScalar resolveSingleFrictionCacheFriendly(
 
 #endif //NO_FRICTION_TANGENTIALS
 
-btAlignedObjectArray<btSolverBody>	tmpSolverBodyPool;
-btAlignedObjectArray<btSolverConstraint>	tmpSolverConstraintPool;
-btAlignedObjectArray<btSolverConstraint>	tmpSolverFrictionConstraintPool;
 
 
-btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+
+
+void	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
+{
+
+	btRigidBody* body0=btRigidBody::upcast(colObj0);
+	btRigidBody* body1=btRigidBody::upcast(colObj1);
+
+	btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
+	solverConstraint.m_contactNormal = normalAxis;
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+	solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
+	solverConstraint.m_frictionIndex = frictionIndex;
+
+	solverConstraint.m_friction = cp.m_combinedFriction;
+	solverConstraint.m_originalContactPoint = 0;
+
+	solverConstraint.m_appliedImpulse = btScalar(0.);
+	solverConstraint.m_appliedPushImpulse = 0.f;
+	solverConstraint.m_penetration = 0.f;
+	{
+		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
+		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+	}
+	{
+		btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
+		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
+	}
+
+#ifdef COMPUTE_IMPULSE_DENOM
+	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
+	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
+#else
+	btVector3 vec;
+	btScalar denom0 = 0.f;
+	btScalar denom1 = 0.f;
+	if (body0)
+	{
+		vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+		denom0 = body0->getInvMass() + normalAxis.dot(vec);
+	}
+	if (body1)
+	{
+		vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
+		denom1 = body1->getInvMass() + normalAxis.dot(vec);
+	}
+
+
+#endif //COMPUTE_IMPULSE_DENOM
+	btScalar denom = relaxation/(denom0+denom1);
+	solverConstraint.m_jacDiagABInv = denom;
+
+
+}
+
+
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 {
+	BT_PROFILE("solveGroupCacheFriendlySetup");
 	(void)stackAlloc;
 	(void)debugDrawer;
 
+
 	if (!(numConstraints + numManifolds))
 	{
 //		printf("empty\n");
 		return 0.f;
 	}
+	btPersistentManifold* manifold = 0;
+	btCollisionObject* colObj0=0,*colObj1=0;
+
+	//btRigidBody* rb0=0,*rb1=0;
+
+
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 
 	BEGIN_PROFILE("refreshManifolds");
 
 	int i;
+	
+	
+
 	for (i=0;i<numManifolds;i++)
 	{
-		btPersistentManifold* manifold = manifoldPtr[i];
-		btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
-		btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
-
+		manifold = manifoldPtr[i];
+		rb1 = (btRigidBody*)manifold->getBody1();
+		rb0 = (btRigidBody*)manifold->getBody0();
+		
 		manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
 
 	}
-	
+
 	END_PROFILE("refreshManifolds");
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS
+
+	
 
 
-	BEGIN_PROFILE("gatherSolverData");
 
 	//int sizeofSB = sizeof(btSolverBody);
 	//int sizeofSC = sizeof(btSolverConstraint);
@@ -382,10 +554,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 
 		
 		//todo: use stack allocator for this temp memory
-		int minReservation = numManifolds*2;
+//		int minReservation = numManifolds*2;
 
-		tmpSolverBodyPool.reserve(minReservation);
+		//m_tmpSolverBodyPool.reserve(minReservation);
 
+		//don't convert all bodies, only the one we need so solver the constraints
+/*
 		{
 			for (int i=0;i<numBodies;i++)
 			{
@@ -393,26 +567,26 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 				if (rb && 	(rb->getIslandTag() >= 0))
 				{
 					btAssert(rb->getCompanionId() < 0);
-					int solverBodyId = tmpSolverBodyPool.size();
-					btSolverBody& solverBody = tmpSolverBodyPool.expand();
+					int solverBodyId = m_tmpSolverBodyPool.size();
+					btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 					initSolverBody(&solverBody,rb);
 					rb->setCompanionId(solverBodyId);
 				} 
 			}
 		}
-
+*/
 		
-		tmpSolverConstraintPool.reserve(minReservation);
-		tmpSolverFrictionConstraintPool.reserve(minReservation);
+		//m_tmpSolverConstraintPool.reserve(minReservation);
+		//m_tmpSolverFrictionConstraintPool.reserve(minReservation);
+
 		{
 			int i;
 
 			for (i=0;i<numManifolds;i++)
 			{
-				btPersistentManifold* manifold = manifoldPtr[i];
-				btRigidBody* rb0 = (btRigidBody*)manifold->getBody0();
-				btRigidBody* rb1 = (btRigidBody*)manifold->getBody1();
-
+				manifold = manifoldPtr[i];
+				colObj0 = (btCollisionObject*)manifold->getBody0();
+				colObj1 = (btCollisionObject*)manifold->getBody1();
 			
 				int solverBodyIdA=-1;
 				int solverBodyIdB=-1;
@@ -422,61 +596,108 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 
 					
 
-					if (rb0->getIslandTag() >= 0)
+					if (colObj0->getIslandTag() >= 0)
 					{
-						solverBodyIdA = rb0->getCompanionId();
+						if (colObj0->getCompanionId() >= 0)
+						{
+							//body has already been converted
+							solverBodyIdA = colObj0->getCompanionId();
+						} else
+						{
+							solverBodyIdA = m_tmpSolverBodyPool.size();
+							btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+							initSolverBody(&solverBody,colObj0);
+							colObj0->setCompanionId(solverBodyIdA);
+						}
 					} else
 					{
 						//create a static body
-						solverBodyIdA = tmpSolverBodyPool.size();
-						btSolverBody& solverBody = tmpSolverBodyPool.expand();
-						initSolverBody(&solverBody,rb0);
+						solverBodyIdA = m_tmpSolverBodyPool.size();
+						btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+						initSolverBody(&solverBody,colObj0);
 					}
 
-					if (rb1->getIslandTag() >= 0)
+					if (colObj1->getIslandTag() >= 0)
 					{
-						solverBodyIdB = rb1->getCompanionId();
+						if (colObj1->getCompanionId() >= 0)
+						{
+							solverBodyIdB = colObj1->getCompanionId();
+						} else
+						{
+							solverBodyIdB = m_tmpSolverBodyPool.size();
+							btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+							initSolverBody(&solverBody,colObj1);
+							colObj1->setCompanionId(solverBodyIdB);
+						}
 					} else
 					{
 						//create a static body
-						solverBodyIdB = tmpSolverBodyPool.size();
-						btSolverBody& solverBody = tmpSolverBodyPool.expand();
-						initSolverBody(&solverBody,rb1);
+						solverBodyIdB = m_tmpSolverBodyPool.size();
+						btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+						initSolverBody(&solverBody,colObj1);
 					}
 				}
 
+				btVector3 rel_pos1;
+				btVector3 rel_pos2;
+				btScalar relaxation;
+
 				for (int j=0;j<manifold->getNumContacts();j++)
 				{
 					
 					btManifoldPoint& cp = manifold->getContactPoint(j);
-
-					int frictionIndex = tmpSolverConstraintPool.size();
-
+					
 					if (cp.getDistance() <= btScalar(0.))
 					{
 						
 						const btVector3& pos1 = cp.getPositionWorldOnA();
 						const btVector3& pos2 = cp.getPositionWorldOnB();
 
-						btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); 
-						btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition();
+						 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
+						 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 
 						
-							btScalar relaxation = 1.f;
+						relaxation = 1.f;
+						btScalar rel_vel;
+						btVector3 vel;
+
+						int frictionIndex = m_tmpSolverConstraintPool.size();
 
 						{
-							btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand();
+							btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
+							btRigidBody* rb0 = btRigidBody::upcast(colObj0);
+							btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 
 							solverConstraint.m_solverBodyIdA = solverBodyIdA;
 							solverConstraint.m_solverBodyIdB = solverBodyIdB;
 							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
 
-					
+							solverConstraint.m_originalContactPoint = &cp;
 
+							btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
+							solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
+							btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
+							solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
 							{
-								//can be optimized, the cross products are already calculated
+#ifdef COMPUTE_IMPULSE_DENOM
 								btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
 								btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+#else							
+								btVector3 vec;
+								btScalar denom0 = 0.f;
+								btScalar denom1 = 0.f;
+								if (rb0)
+								{
+									vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+									denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+								}
+								if (rb1)
+								{
+									vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
+									denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
+								}
+#endif //COMPUTE_IMPULSE_DENOM		
+								
 								btScalar denom = relaxation/(denom0+denom1);
 								solverConstraint.m_jacDiagABInv = denom;
 							}
@@ -486,121 +707,115 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 							solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
 
 
-							btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1);
-							btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2);
+							btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
+							btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
 				
-							btVector3 vel = vel1 - vel2;
-							btScalar rel_vel;
+							vel  = vel1 - vel2;
+							
 							rel_vel = cp.m_normalWorldOnB.dot(vel);
 							
+							solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
+							//solverConstraint.m_penetration = cp.getDistance();
 
-							solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
 							solverConstraint.m_friction = cp.m_combinedFriction;
-							btScalar rest =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
-							if (rest <= btScalar(0.))
+							solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
+							if (solverConstraint.m_restitution <= btScalar(0.))
 							{
-								rest = 0.f;
+								solverConstraint.m_restitution = 0.f;
 							};
+
 							
 							btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
-							if (rest > penVel)
-							{
-								rest = btScalar(0.);
-							}
-							solverConstraint.m_restitution = rest;
-
-							solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep);
 
-							solverConstraint.m_appliedImpulse = 0.f;
-							solverConstraint.m_appliedVelocityImpulse = 0.f;
 							
-					
-							btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
-							solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0;
-							btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);		
-							solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1;
-						}
-
-						//create 2 '1d axis' constraints for 2 tangential friction directions
-			
-						//re-calculate friction direction every frame, todo: check if this is really needed
-						btVector3 frictionTangential0a, frictionTangential1b;
-
-						btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b);
-
-						{
-							btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
-							solverConstraint.m_contactNormal = frictionTangential0a;
-
-							solverConstraint.m_solverBodyIdA = solverBodyIdA;
-							solverConstraint.m_solverBodyIdB = solverBodyIdB;
-							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
-							solverConstraint.m_frictionIndex = frictionIndex;
-
-							solverConstraint.m_friction = cp.m_combinedFriction;
-
-							solverConstraint.m_appliedImpulse = btScalar(0.);
-							solverConstraint.m_appliedVelocityImpulse = 0.f;
-
-							btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
-							btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
-							btScalar denom = relaxation/(denom0+denom1);
-							solverConstraint.m_jacDiagABInv = denom;
 
+							if (solverConstraint.m_restitution > penVel)
 							{
-							btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal);
-							solverConstraint.m_relpos1CrossNormal = ftorqueAxis0;
-							solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0;
+								solverConstraint.m_penetration = btScalar(0.);
 							}
+							 
+							
+							
+							///warm starting (or zero if disabled)
+							if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+							{
+								solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+								if (rb0)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+								if (rb1)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
+							} else
 							{
-							btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal);
-							solverConstraint.m_relpos2CrossNormal = ftorqueAxis0;
-							solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0;
+								solverConstraint.m_appliedImpulse = 0.f;
 							}
 
-						}
-
-
-						{
-
-							btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand();
-							solverConstraint.m_contactNormal = frictionTangential1b;
-
-							solverConstraint.m_solverBodyIdA = solverBodyIdA;
-							solverConstraint.m_solverBodyIdB = solverBodyIdB;
-							solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
-							solverConstraint.m_frictionIndex = frictionIndex;
-
-							solverConstraint.m_friction = cp.m_combinedFriction;
+							solverConstraint.m_appliedPushImpulse = 0.f;
+							
+							solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
+							if (!cp.m_lateralFrictionInitialized)
+							{
+								cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
+								btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
+								if (lat_rel_vel > SIMD_EPSILON)//0.0f)
+								{
+									cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
+									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+									cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+									cp.m_lateralFrictionDir2.normalize();//??
+									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+								} else
+								{
+									//re-calculate friction direction every frame, todo: check if this is really needed
+									
+									btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
+									addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+									addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+								}
+								cp.m_lateralFrictionInitialized = true;
+								
+							} else
+							{
+								addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+								addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+							}
 
-							solverConstraint.m_appliedImpulse = btScalar(0.);
-							solverConstraint.m_appliedVelocityImpulse = 0.f;
-						
-							btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
-							btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
-							btScalar denom = relaxation/(denom0+denom1);
-							solverConstraint.m_jacDiagABInv = denom;
 							{
-								btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
-								solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
-								solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1;
+								btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
+								if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+								{
+									frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
+									if (rb0)
+										m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
+									if (rb1)
+										m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
+								} else
+								{
+									frictionConstraint1.m_appliedImpulse = 0.f;
 								}
+							}
 							{
-								btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
-								solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
-								solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1;
+								btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
+								if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+								{
+									frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
+									if (rb0)
+										m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
+									if (rb1)
+										m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
+								} else
+								{
+									frictionConstraint2.m_appliedImpulse = 0.f;
 								}
 							}
+						}
+
 
 					}
 				}
 			}
 		}
 	}
-	END_PROFILE("gatherSolverData");
-
-	BEGIN_PROFILE("prepareConstraints");
-
+	
 	btContactSolverInfo info = infoGlobal;
 
 	{
@@ -612,57 +827,60 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 		}
 	}
 	
-	btAlignedObjectArray<int>	gOrderTmpConstraintPool;
-	btAlignedObjectArray<int>	gOrderFrictionConstraintPool;
+	
 
-	int numConstraintPool = tmpSolverConstraintPool.size();
-	int numFrictionPool = tmpSolverFrictionConstraintPool.size();
+	int numConstraintPool = m_tmpSolverConstraintPool.size();
+	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
 
 	///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
-	gOrderTmpConstraintPool.resize(numConstraintPool);
-	gOrderFrictionConstraintPool.resize(numFrictionPool);
+	m_orderTmpConstraintPool.resize(numConstraintPool);
+	m_orderFrictionConstraintPool.resize(numFrictionPool);
 	{
 		int i;
 		for (i=0;i<numConstraintPool;i++)
 		{
-			gOrderTmpConstraintPool[i] = i;
+			m_orderTmpConstraintPool[i] = i;
 		}
 		for (i=0;i<numFrictionPool;i++)
 		{
-			gOrderFrictionConstraintPool[i] = i;
+			m_orderFrictionConstraintPool[i] = i;
 		}
 	}
 
 
 
+	return 0.f;
 
-	END_PROFILE("prepareConstraints");
-
+}
 
-	BEGIN_PROFILE("solveConstraints");
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
+{
+	BT_PROFILE("solveGroupCacheFriendlyIterations");
+	int numConstraintPool = m_tmpSolverConstraintPool.size();
+	int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
 
 	//should traverse the contacts random order...
 	int iteration;
 	{
-		for ( iteration = 0;iteration<info.m_numIterations;iteration++)
+		for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
 		{			
 
 			int j;
-			if (m_solverMode & SOLVER_RANDMIZE_ORDER)
+			if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 			{
 				if ((iteration & 7) == 0) {
 					for (j=0; j<numConstraintPool; ++j) {
-						int tmp = gOrderTmpConstraintPool[j];
+						int tmp = m_orderTmpConstraintPool[j];
 						int swapi = btRandInt2(j+1);
-						gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
-						gOrderTmpConstraintPool[swapi] = tmp;
+						m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
+						m_orderTmpConstraintPool[swapi] = tmp;
 					}
 
 					for (j=0; j<numFrictionPool; ++j) {
-						int tmp = gOrderFrictionConstraintPool[j];
+						int tmp = m_orderFrictionConstraintPool[j];
 						int swapi = btRandInt2(j+1);
-						gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
-						gOrderFrictionConstraintPool[swapi] = tmp;
+						m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
+						m_orderFrictionConstraintPool[swapi] = tmp;
 					}
 				}
 			}
@@ -674,86 +892,145 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 
 				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
 				{
-					tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
+					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
 				}
 				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
 				{
-					tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
+					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
 				}
 
-				constraint->solveConstraint(info.m_timeStep);
+				constraint->solveConstraint(infoGlobal.m_timeStep);
 
 				if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
 				{
-					tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
+					m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
 				}
 				if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
 				{
-					tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
+					m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
 				}
 
 			}
 
 			{
-				int numPoolConstraints = tmpSolverConstraintPool.size();
+				int numPoolConstraints = m_tmpSolverConstraintPool.size();
 				for (j=0;j<numPoolConstraints;j++)
 				{
-					btSolverConstraint& solveManifold = tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
-					resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-						tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info);
+					
+					const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
+					resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+						m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
 				}
 			}
 
 			{
-				 int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
-				for (j=0;j<numFrictionPoolConstraints;j++)
+				 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
+				
+				 for (j=0;j<numFrictionPoolConstraints;j++)
 				{
-					btSolverConstraint& solveManifold = tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
-						btScalar appliedNormalImpulse = tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+					const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
+					btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
+								m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;			
 
-						resolveSingleFrictionCacheFriendly(tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
-							tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,info,appliedNormalImpulse);
+						resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
+							totalImpulse);
 				}
 			}
 			
 
 
 		}
+	
+		if (infoGlobal.m_splitImpulse)
+		{
+			
+			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverConstraintPool.size();
+					int j;
+					for (j=0;j<numPoolConstraints;j++)
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
+
+						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
+					}
+				}
+			}
+
+		}
+
 	}
-		
-	for ( i=0;i<tmpSolverBodyPool.size();i++)
+
+	return 0.f;
+}
+
+
+btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+{
+	int i;
+
+	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
+
+	int numPoolConstraints = m_tmpSolverConstraintPool.size();
+	int j;
+	for (j=0;j<numPoolConstraints;j++)
 	{
-		tmpSolverBodyPool[i].writebackVelocity();
+		
+		const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
+		btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
+		btAssert(pt);
+		pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+		pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
+		pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+
+		//do a callback here?
+
 	}
 
-	END_PROFILE("solveConstraints");
+	if (infoGlobal.m_splitImpulse)
+	{		
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+		{
+			m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
+		}
+	} else
+	{
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+        {
+                m_tmpSolverBodyPool[i].writebackVelocity();
+        }
+	}
 
-//	printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
+//	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
 
 /*
-	printf("tmpSolverBodyPool.size() = %i\n",tmpSolverBodyPool.size());
-	printf("tmpSolverConstraintPool.size() = %i\n",tmpSolverConstraintPool.size());
-	printf("tmpSolverFrictionConstraintPool.size() = %i\n",tmpSolverFrictionConstraintPool.size());
+	printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
+	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
+	printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
 
 	
-	printf("tmpSolverBodyPool.capacity() = %i\n",tmpSolverBodyPool.capacity());
-	printf("tmpSolverConstraintPool.capacity() = %i\n",tmpSolverConstraintPool.capacity());
-	printf("tmpSolverFrictionConstraintPool.capacity() = %i\n",tmpSolverFrictionConstraintPool.capacity());
+	printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
+	printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
+	printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
 */
 
-	tmpSolverBodyPool.resize(0);
-	tmpSolverConstraintPool.resize(0);
-	tmpSolverFrictionConstraintPool.resize(0);
+	m_tmpSolverBodyPool.resize(0);
+	m_tmpSolverConstraintPool.resize(0);
+	m_tmpSolverFrictionConstraintPool.resize(0);
 
 
 	return 0.f;
 }
 
 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
-btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
+btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
 {
-
-	if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
+	BT_PROFILE("solveGroup");
+	if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
 	{
 		//you need to provide at least some bodies
 		//btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
@@ -762,16 +1039,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 		return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
 	}
 
-
-	BEGIN_PROFILE("prepareConstraints");
 	
 
 	btContactSolverInfo info = infoGlobal;
 
 	int numiter = infoGlobal.m_numIterations;
-#ifdef USE_PROFILE
-	btProfiler::beginBlock("solve");
-#endif //USE_PROFILE
 
 	int totalPoints = 0;
 
@@ -801,10 +1073,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 		}
 	}
 	
-	END_PROFILE("prepareConstraints");
-
-
-	BEGIN_PROFILE("solveConstraints");
 
 	//should traverse the contacts random order...
 	int iteration;
@@ -813,7 +1081,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 		for ( iteration = 0;iteration<numiter;iteration++)
 		{
 			int j;
-			if (m_solverMode & SOLVER_RANDMIZE_ORDER)
+			if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 			{
 				if ((iteration & 7) == 0) {
 					for (j=0; j<totalPoints; ++j) {
@@ -845,16 +1113,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
 				solveFriction((btRigidBody*)manifold->getBody0(),
 					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
 			}
+			
 		}
 	}
 		
-	END_PROFILE("solveConstraints");
-
-
-#ifdef USE_PROFILE
-	btProfiler::endBlock("solve");
-#endif //USE_PROFILE
-
 
 
 
@@ -878,13 +1140,14 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 
 	//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
 	{
+#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
 		manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
-		
+#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
 		int numpoints = manifoldPtr->getNumContacts();
 
 		gTotalContactPoints += numpoints;
 
-		btVector3 color(0,1,0);
+		
 		for (int i=0;i<numpoints ;i++)
 		{
 			btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
@@ -925,7 +1188,9 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 				} else
 				{
 						
-					cpd = new btConstraintPersistentData;
+					//todo: should this be in a pool?
+					void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
+					cpd = new (mem)btConstraintPersistentData;
 					assert(cpd);
 
 					totalCpd ++;
@@ -972,10 +1237,9 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 					cpd->m_penetration = btScalar(0.);
 				} 				
 				
-				
 
 				btScalar relaxation = info.m_damping;
-				if (m_solverMode & SOLVER_USE_WARMSTARTING)
+				if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
 				{
 					cpd->m_appliedImpulse *= relaxation;
 				} else
@@ -1060,16 +1324,12 @@ btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRig
 
 	{
 
-		btVector3 color(0,1,0);
+		
 		{
 			if (cp.getDistance() <= btScalar(0.))
 			{
 
-				if (iter == 0)
-				{
-					if (debugDrawer)
-						debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
-				}
+				
 
 				{
 
@@ -1098,16 +1358,12 @@ btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBo
 
 	{
 
-		btVector3 color(0,1,0);
+		
 		{
 			if (cp.getDistance() <= btScalar(0.))
 			{
 
-				if (iter == 0)
-				{
-					if (debugDrawer)
-						debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
-				}
+				
 
 				{
 
@@ -1136,7 +1392,7 @@ btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,b
 
 	{
 
-		btVector3 color(0,1,0);
+		
 		{
 			
 			if (cp.getDistance() <= btScalar(0.))
@@ -1156,3 +1412,11 @@ btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,b
 	}
 	return btScalar(0.);
 }
+
+
+void	btSequentialImpulseConstraintSolver::reset()
+{
+	m_btSeed2 = 0;
+}
+
+
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index 13e70c41be4..7143bc41991 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -19,7 +19,8 @@ subject to the following restrictions:
 #include "btConstraintSolver.h"
 class btIDebugDraw;
 #include "btContactConstraint.h"
-	
+#include "btSolverBody.h"
+#include "btSolverConstraint.h"
 
 
 /// btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses
@@ -29,29 +30,29 @@ class btIDebugDraw;
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
 
+	btAlignedObjectArray<btSolverBody>	m_tmpSolverBodyPool;
+	btAlignedObjectArray<btSolverConstraint>	m_tmpSolverConstraintPool;
+	btAlignedObjectArray<btSolverConstraint>	m_tmpSolverFrictionConstraintPool;
+	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
+	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
+
+
 protected:
 	btScalar solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 	btScalar solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 	void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
+	void	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation);
 
 	ContactSolverFunc m_contactDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
 	ContactSolverFunc m_frictionDispatch[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
 
-	//choose between several modes, different friction model etc.
-	int	m_solverMode;
+	
 	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
 	unsigned long	m_btSeed2;
 
 public:
 
-	enum	eSolverMode
-	{
-		SOLVER_RANDMIZE_ORDER = 1,
-		SOLVER_FRICTION_SEPARATE = 2,
-		SOLVER_USE_WARMSTARTING = 4,
-		SOLVER_CACHE_FRIENDLY = 8
-	};
-
+	
 	btSequentialImpulseConstraintSolver();
 
 	///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
@@ -68,25 +69,22 @@ public:
 		m_frictionDispatch[type0][type1] = func;
 	}
 
-	virtual ~btSequentialImpulseConstraintSolver() {}
+	virtual ~btSequentialImpulseConstraintSolver();
 	
-	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc);
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher);
 
 	virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
+	btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc);
 
-	btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 
+	///clear internal cached data and reset random seed
+	virtual	void	reset();
 
-	void	setSolverMode(int mode)
-	{
-		m_solverMode = mode;
-	}
+	btScalar solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 
-	int	getSolverMode() const
-	{
-		return m_solverMode;
-	}
 
+	
 	unsigned long btRand2();
 
 	int btRandInt2 (int n);
@@ -102,7 +100,9 @@ public:
 
 };
 
-
+#ifndef BT_PREFER_SIMD
+typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered;
+#endif
 
 
 #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
new file mode 100644
index 00000000000..e7f07a428eb
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp
@@ -0,0 +1,414 @@
+/*
+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.
+*/
+
+/*
+Added by Roman Ponomarev (rponom@gmail.com)
+April 04, 2008
+*/
+
+//-----------------------------------------------------------------------------
+
+#include "btSliderConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btTransformUtil.h"
+#include <new>
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::initParams()
+{
+    m_lowerLinLimit = btScalar(1.0);
+    m_upperLinLimit = btScalar(-1.0);
+    m_lowerAngLimit = btScalar(0.);
+    m_upperAngLimit = btScalar(0.);
+	m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingDirLin = btScalar(0.);
+	m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingDirAng = btScalar(0.);
+	m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
+	m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
+	m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
+	m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
+
+	m_poweredLinMotor = false;
+    m_targetLinMotorVelocity = btScalar(0.);
+    m_maxLinMotorForce = btScalar(0.);
+	m_accumulatedLinMotorImpulse = btScalar(0.0);
+
+	m_poweredAngMotor = false;
+    m_targetAngMotorVelocity = btScalar(0.);
+    m_maxAngMotorForce = btScalar(0.);
+	m_accumulatedAngMotorImpulse = btScalar(0.0);
+
+} // btSliderConstraint::initParams()
+
+//-----------------------------------------------------------------------------
+
+btSliderConstraint::btSliderConstraint()
+        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
+		m_useLinearReferenceFrameA(true)
+{
+	initParams();
+} // btSliderConstraint::btSliderConstraint()
+
+//-----------------------------------------------------------------------------
+
+btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
+        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
+        , m_frameInA(frameInA)
+        , m_frameInB(frameInB),
+		m_useLinearReferenceFrameA(useLinearReferenceFrameA)
+{
+	initParams();
+} // btSliderConstraint::btSliderConstraint()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::buildJacobian()
+{
+	if(m_useLinearReferenceFrameA)
+	{
+		buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
+	}
+	else
+	{
+		buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
+	}
+} // btSliderConstraint::buildJacobian()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
+{
+	//calculate transforms
+    m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
+    m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
+	m_realPivotAInW = m_calculatedTransformA.getOrigin();
+	m_realPivotBInW = m_calculatedTransformB.getOrigin();
+	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+	m_delta = m_realPivotBInW - m_realPivotAInW;
+	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+	m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
+	m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
+    btVector3 normalWorld;
+    int i;
+    //linear part
+    for(i = 0; i < 3; i++)
+    {
+		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+		new (&m_jacLin[i]) btJacobianEntry(
+			rbA.getCenterOfMassTransform().getBasis().transpose(),
+			rbB.getCenterOfMassTransform().getBasis().transpose(),
+			m_relPosA,
+			m_relPosB,
+			normalWorld,
+			rbA.getInvInertiaDiagLocal(),
+			rbA.getInvMass(),
+			rbB.getInvInertiaDiagLocal(),
+			rbB.getInvMass()
+			);
+		m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
+		m_depth[i] = m_delta.dot(normalWorld);
+    }
+	testLinLimits();
+    // angular part
+    for(i = 0; i < 3; i++)
+    {
+		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+		new (&m_jacAng[i])	btJacobianEntry(
+			normalWorld,
+            rbA.getCenterOfMassTransform().getBasis().transpose(),
+            rbB.getCenterOfMassTransform().getBasis().transpose(),
+            rbA.getInvInertiaDiagLocal(),
+            rbB.getInvInertiaDiagLocal()
+			);
+	}
+	testAngLimits();
+	btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
+	m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
+	// clear accumulator for motors
+	m_accumulatedLinMotorImpulse = btScalar(0.0);
+	m_accumulatedAngMotorImpulse = btScalar(0.0);
+} // btSliderConstraint::buildJacobianInt()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::solveConstraint(btScalar timeStep)
+{
+    m_timeStep = timeStep;
+	if(m_useLinearReferenceFrameA)
+	{
+		solveConstraintInt(m_rbA, m_rbB);
+	}
+	else
+	{
+		solveConstraintInt(m_rbB, m_rbA);
+	}
+} // btSliderConstraint::solveConstraint()
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
+{
+    int i;
+    // linear
+    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
+    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
+    btVector3 vel = velA - velB;
+	for(i = 0; i < 3; i++)
+    {
+		const btVector3& normal = m_jacLin[i].m_linearJointAxis;
+		btScalar rel_vel = normal.dot(vel);
+		// calculate positional error
+		btScalar depth = m_depth[i];
+		// get parameters
+		btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
+		btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
+		btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
+		// calcutate and apply impulse
+		btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
+		btVector3 impulse_vector = normal * normalImpulse;
+		rbA.applyImpulse( impulse_vector, m_relPosA);
+		rbB.applyImpulse(-impulse_vector, m_relPosB);
+		if(m_poweredLinMotor && (!i))
+		{ // apply linear motor
+			if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
+			{
+				btScalar desiredMotorVel = m_targetLinMotorVelocity;
+				btScalar motor_relvel = desiredMotorVel + rel_vel;
+				normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
+				// clamp accumulated impulse
+				btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
+				if(new_acc  > m_maxLinMotorForce)
+				{
+					new_acc = m_maxLinMotorForce;
+				}
+				btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
+				if(normalImpulse < btScalar(0.0))
+				{
+					normalImpulse = -del;
+				}
+				else
+				{
+					normalImpulse = del;
+				}
+				m_accumulatedLinMotorImpulse = new_acc;
+				// apply clamped impulse
+				impulse_vector = normal * normalImpulse;
+				rbA.applyImpulse( impulse_vector, m_relPosA);
+				rbB.applyImpulse(-impulse_vector, m_relPosB);
+			}
+		}
+    }
+	// angular 
+	// get axes in world space
+	btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
+	btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
+
+	const btVector3& angVelA = rbA.getAngularVelocity();
+	const btVector3& angVelB = rbB.getAngularVelocity();
+
+	btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
+	btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
+
+	btVector3 angAorthog = angVelA - angVelAroundAxisA;
+	btVector3 angBorthog = angVelB - angVelAroundAxisB;
+	btVector3 velrelOrthog = angAorthog-angBorthog;
+	//solve orthogonal angular velocity correction
+	btScalar len = velrelOrthog.length();
+	if (len > btScalar(0.00001))
+	{
+		btVector3 normal = velrelOrthog.normalized();
+		btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
+		velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
+	}
+	//solve angular positional correction
+	btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
+	btScalar len2 = angularError.length();
+	if (len2>btScalar(0.00001))
+	{
+		btVector3 normal2 = angularError.normalized();
+		btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
+		angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
+	}
+	// apply impulse
+	rbA.applyTorqueImpulse(-velrelOrthog+angularError);
+	rbB.applyTorqueImpulse(velrelOrthog-angularError);
+	btScalar impulseMag;
+	//solve angular limits
+	if(m_solveAngLim)
+	{
+		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
+		impulseMag *= m_kAngle * m_softnessLimAng;
+	}
+	else
+	{
+		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
+		impulseMag *= m_kAngle * m_softnessDirAng;
+	}
+	btVector3 impulse = axisA * impulseMag;
+	rbA.applyTorqueImpulse(impulse);
+	rbB.applyTorqueImpulse(-impulse);
+	//apply angular motor
+	if(m_poweredAngMotor) 
+	{
+		if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
+		{
+			btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
+			btScalar projRelVel = velrel.dot(axisA);
+
+			btScalar desiredMotorVel = m_targetAngMotorVelocity;
+			btScalar motor_relvel = desiredMotorVel - projRelVel;
+
+			btScalar angImpulse = m_kAngle * motor_relvel;
+			// clamp accumulated impulse
+			btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
+			if(new_acc  > m_maxAngMotorForce)
+			{
+				new_acc = m_maxAngMotorForce;
+			}
+			btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
+			if(angImpulse < btScalar(0.0))
+			{
+				angImpulse = -del;
+			}
+			else
+			{
+				angImpulse = del;
+			}
+			m_accumulatedAngMotorImpulse = new_acc;
+			// apply clamped impulse
+			btVector3 motorImp = angImpulse * axisA;
+			m_rbA.applyTorqueImpulse(motorImp);
+			m_rbB.applyTorqueImpulse(-motorImp);
+		}
+	}
+} // btSliderConstraint::solveConstraint()
+
+//-----------------------------------------------------------------------------
+
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::calculateTransforms(void){
+	if(m_useLinearReferenceFrameA)
+	{
+		m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
+		m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
+	}
+	else
+	{
+		m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
+		m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
+	}
+	m_realPivotAInW = m_calculatedTransformA.getOrigin();
+	m_realPivotBInW = m_calculatedTransformB.getOrigin();
+	m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
+	m_delta = m_realPivotBInW - m_realPivotAInW;
+	m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
+    btVector3 normalWorld;
+    int i;
+    //linear part
+    for(i = 0; i < 3; i++)
+    {
+		normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
+		m_depth[i] = m_delta.dot(normalWorld);
+    }
+} // btSliderConstraint::calculateTransforms()
+ 
+//-----------------------------------------------------------------------------
+
+void btSliderConstraint::testLinLimits(void)
+{
+	m_solveLinLim = false;
+	if(m_lowerLinLimit <= m_upperLinLimit)
+	{
+		if(m_depth[0] > m_upperLinLimit)
+		{
+			m_depth[0] -= m_upperLinLimit;
+			m_solveLinLim = true;
+		}
+		else if(m_depth[0] < m_lowerLinLimit)
+		{
+			m_depth[0] -= m_lowerLinLimit;
+			m_solveLinLim = true;
+		}
+		else
+		{
+			m_depth[0] = btScalar(0.);
+		}
+	}
+	else
+	{
+		m_depth[0] = btScalar(0.);
+	}
+} // btSliderConstraint::testLinLimits()
+
+//-----------------------------------------------------------------------------
+ 
+
+void btSliderConstraint::testAngLimits(void)
+{
+	m_angDepth = btScalar(0.);
+	m_solveAngLim = false;
+	if(m_lowerAngLimit <= m_upperAngLimit)
+	{
+		const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
+		const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
+		const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
+		btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
+		if(rot < m_lowerAngLimit)
+		{
+			m_angDepth = rot - m_lowerAngLimit;
+			m_solveAngLim = true;
+		} 
+		else if(rot > m_upperAngLimit)
+		{
+			m_angDepth = rot - m_upperAngLimit;
+			m_solveAngLim = true;
+		}
+	}
+} // btSliderConstraint::testAngLimits()
+
+	
+//-----------------------------------------------------------------------------
+
+
+
+btVector3 btSliderConstraint::getAncorInA(void)
+{
+	btVector3 ancorInA;
+	ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
+	ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
+	return ancorInA;
+} // btSliderConstraint::getAncorInA()
+
+//-----------------------------------------------------------------------------
+
+btVector3 btSliderConstraint::getAncorInB(void)
+{
+	btVector3 ancorInB;
+	ancorInB = m_frameInB.getOrigin();
+	return ancorInB;
+} // btSliderConstraint::getAncorInB();
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
new file mode 100644
index 00000000000..f69dfcf3aa7
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h
@@ -0,0 +1,215 @@
+/*
+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.
+*/
+
+/*
+Added by Roman Ponomarev (rponom@gmail.com)
+April 04, 2008
+
+TODO:
+ - add clamping od accumulated impulse to improve stability
+ - add conversion for ODE constraint solver
+*/
+
+#ifndef SLIDER_CONSTRAINT_H
+#define SLIDER_CONSTRAINT_H
+
+//-----------------------------------------------------------------------------
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+//-----------------------------------------------------------------------------
+
+class btRigidBody;
+
+//-----------------------------------------------------------------------------
+
+#define SLIDER_CONSTRAINT_DEF_SOFTNESS		(btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_DAMPING		(btScalar(1.0))
+#define SLIDER_CONSTRAINT_DEF_RESTITUTION	(btScalar(0.7))
+
+//-----------------------------------------------------------------------------
+
+class btSliderConstraint : public btTypedConstraint
+{
+protected:
+	btTransform	m_frameInA;
+    btTransform	m_frameInB;
+	// use frameA fo define limits, if true
+	bool m_useLinearReferenceFrameA;
+	// linear limits
+	btScalar m_lowerLinLimit;
+	btScalar m_upperLinLimit;
+	// angular limits
+	btScalar m_lowerAngLimit;
+	btScalar m_upperAngLimit;
+	// softness, restitution and damping for different cases
+	// DirLin - moving inside linear limits
+	// LimLin - hitting linear limit
+	// DirAng - moving inside angular limits
+	// LimAng - hitting angular limit
+	// OrthoLin, OrthoAng - against constraint axis
+	btScalar m_softnessDirLin;
+	btScalar m_restitutionDirLin;
+	btScalar m_dampingDirLin;
+	btScalar m_softnessDirAng;
+	btScalar m_restitutionDirAng;
+	btScalar m_dampingDirAng;
+	btScalar m_softnessLimLin;
+	btScalar m_restitutionLimLin;
+	btScalar m_dampingLimLin;
+	btScalar m_softnessLimAng;
+	btScalar m_restitutionLimAng;
+	btScalar m_dampingLimAng;
+	btScalar m_softnessOrthoLin;
+	btScalar m_restitutionOrthoLin;
+	btScalar m_dampingOrthoLin;
+	btScalar m_softnessOrthoAng;
+	btScalar m_restitutionOrthoAng;
+	btScalar m_dampingOrthoAng;
+	
+	// for interlal use
+	bool m_solveLinLim;
+	bool m_solveAngLim;
+
+	btJacobianEntry	m_jacLin[3];
+	btScalar		m_jacLinDiagABInv[3];
+
+    btJacobianEntry	m_jacAng[3];
+
+	btScalar m_timeStep;
+    btTransform m_calculatedTransformA;
+    btTransform m_calculatedTransformB;
+
+	btVector3 m_sliderAxis;
+	btVector3 m_realPivotAInW;
+	btVector3 m_realPivotBInW;
+	btVector3 m_projPivotInW;
+	btVector3 m_delta;
+	btVector3 m_depth;
+	btVector3 m_relPosA;
+	btVector3 m_relPosB;
+
+	btScalar m_angDepth;
+	btScalar m_kAngle;
+
+	bool	 m_poweredLinMotor;
+    btScalar m_targetLinMotorVelocity;
+    btScalar m_maxLinMotorForce;
+    btScalar m_accumulatedLinMotorImpulse;
+	
+	bool	 m_poweredAngMotor;
+    btScalar m_targetAngMotorVelocity;
+    btScalar m_maxAngMotorForce;
+    btScalar m_accumulatedAngMotorImpulse;
+
+	//------------------------    
+	void initParams();
+public:
+	// constructors
+    btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+    btSliderConstraint();
+	// overrides
+    virtual void	buildJacobian();
+    virtual	void	solveConstraint(btScalar	timeStep);
+	// access
+    const btRigidBody& getRigidBodyA() const { return m_rbA; }
+    const btRigidBody& getRigidBodyB() const { return m_rbB; }
+    const btTransform & getCalculatedTransformA() const { return m_calculatedTransformA; }
+    const btTransform & getCalculatedTransformB() const { return m_calculatedTransformB; }
+    const btTransform & getFrameOffsetA() const { return m_frameInA; }
+    const btTransform & getFrameOffsetB() const { return m_frameInB; }
+    btTransform & getFrameOffsetA() { return m_frameInA; }
+    btTransform & getFrameOffsetB() { return m_frameInB; }
+    btScalar getLowerLinLimit() { return m_lowerLinLimit; }
+    void setLowerLinLimit(btScalar lowerLimit) { m_lowerLinLimit = lowerLimit; }
+    btScalar getUpperLinLimit() { return m_upperLinLimit; }
+    void setUpperLinLimit(btScalar upperLimit) { m_upperLinLimit = upperLimit; }
+    btScalar getLowerAngLimit() { return m_lowerAngLimit; }
+    void setLowerAngLimit(btScalar lowerLimit) { m_lowerAngLimit = lowerLimit; }
+    btScalar getUpperAngLimit() { return m_upperAngLimit; }
+    void setUpperAngLimit(btScalar upperLimit) { m_upperAngLimit = upperLimit; }
+	bool getUseLinearReferenceFrameA() { return m_useLinearReferenceFrameA; }
+	btScalar getSoftnessDirLin() { return m_softnessDirLin; }
+	btScalar getRestitutionDirLin() { return m_restitutionDirLin; }
+	btScalar getDampingDirLin() { return m_dampingDirLin ; }
+	btScalar getSoftnessDirAng() { return m_softnessDirAng; }
+	btScalar getRestitutionDirAng() { return m_restitutionDirAng; }
+	btScalar getDampingDirAng() { return m_dampingDirAng; }
+	btScalar getSoftnessLimLin() { return m_softnessLimLin; }
+	btScalar getRestitutionLimLin() { return m_restitutionLimLin; }
+	btScalar getDampingLimLin() { return m_dampingLimLin; }
+	btScalar getSoftnessLimAng() { return m_softnessLimAng; }
+	btScalar getRestitutionLimAng() { return m_restitutionLimAng; }
+	btScalar getDampingLimAng() { return m_dampingLimAng; }
+	btScalar getSoftnessOrthoLin() { return m_softnessOrthoLin; }
+	btScalar getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
+	btScalar getDampingOrthoLin() { return m_dampingOrthoLin; }
+	btScalar getSoftnessOrthoAng() { return m_softnessOrthoAng; }
+	btScalar getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
+	btScalar getDampingOrthoAng() { return m_dampingOrthoAng; }
+	void setSoftnessDirLin(btScalar softnessDirLin) { m_softnessDirLin = softnessDirLin; }
+	void setRestitutionDirLin(btScalar restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
+	void setDampingDirLin(btScalar dampingDirLin) { m_dampingDirLin = dampingDirLin; }
+	void setSoftnessDirAng(btScalar softnessDirAng) { m_softnessDirAng = softnessDirAng; }
+	void setRestitutionDirAng(btScalar restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
+	void setDampingDirAng(btScalar dampingDirAng) { m_dampingDirAng = dampingDirAng; }
+	void setSoftnessLimLin(btScalar softnessLimLin) { m_softnessLimLin = softnessLimLin; }
+	void setRestitutionLimLin(btScalar restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
+	void setDampingLimLin(btScalar dampingLimLin) { m_dampingLimLin = dampingLimLin; }
+	void setSoftnessLimAng(btScalar softnessLimAng) { m_softnessLimAng = softnessLimAng; }
+	void setRestitutionLimAng(btScalar restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
+	void setDampingLimAng(btScalar dampingLimAng) { m_dampingLimAng = dampingLimAng; }
+	void setSoftnessOrthoLin(btScalar softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
+	void setRestitutionOrthoLin(btScalar restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
+	void setDampingOrthoLin(btScalar dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
+	void setSoftnessOrthoAng(btScalar softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
+	void setRestitutionOrthoAng(btScalar restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
+	void setDampingOrthoAng(btScalar dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
+	void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
+	bool getPoweredLinMotor() { return m_poweredLinMotor; }
+	void setTargetLinMotorVelocity(btScalar targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
+	btScalar getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
+	void setMaxLinMotorForce(btScalar maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
+	btScalar getMaxLinMotorForce() { return m_maxLinMotorForce; }
+	void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
+	bool getPoweredAngMotor() { return m_poweredAngMotor; }
+	void setTargetAngMotorVelocity(btScalar targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
+	btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
+	void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
+	btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; }
+
+	// access for ODE solver
+	bool getSolveLinLimit() { return m_solveLinLim; }
+	btScalar getLinDepth() { return m_depth[0]; }
+	bool getSolveAngLimit() { return m_solveAngLim; }
+	btScalar getAngDepth() { return m_angDepth; }
+	// internal
+    void	buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB);
+    void	solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB);
+	// shared code used by ODE solver
+	void	calculateTransforms(void);
+	void	testLinLimits(void);
+	void	testAngLimits(void);
+	// access for PE Solver
+	btVector3 getAncorInA(void);
+	btVector3 getAncorInB(void);
+};
+
+//-----------------------------------------------------------------------------
+
+#endif //SLIDER_CONSTRAINT_H
+
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
index e7d26645c6a..057d3fac827 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h
@@ -16,8 +16,8 @@ subject to the following restrictions:
 #ifndef SOLVE_2LINEAR_CONSTRAINT_H
 #define SOLVE_2LINEAR_CONSTRAINT_H
 
-#include "../../LinearMath/btMatrix3x3.h"
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btVector3.h"
 
 
 class btRigidBody;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
index 0ab536f42b3..b3f0c9d7444 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h
@@ -19,38 +19,78 @@ subject to the following restrictions:
 class	btRigidBody;
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btMatrix3x3.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btTransformUtil.h"
 
 
-
-
+///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
 ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 {
-	btVector3		m_centerOfMassPosition;
-	btVector3		m_linearVelocity;
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
 	btVector3		m_angularVelocity;
-	btRigidBody*	m_originalBody;
+	float			m_angularFactor;
 	float			m_invMass;
 	float			m_friction;
-	float			m_angularFactor;
-
-	inline void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+	btRigidBody*	m_originalBody;
+	btVector3		m_linearVelocity;
+	btVector3		m_centerOfMassPosition;
+	
+	btVector3		m_pushVelocity;
+	btVector3		m_turnVelocity;
+	
+	
+	SIMD_FORCE_INLINE void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
 	{
 		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
 	}
 
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		if (m_invMass)
+		{
+			m_linearVelocity += linearComponent*impulseMagnitude;
+			m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+	
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
 	{
-		m_linearVelocity += linearComponent*impulseMagnitude;
-		m_angularVelocity += angularComponent*impulseMagnitude*m_angularFactor;
+		if (m_invMass)
+		{
+			m_pushVelocity += linearComponent*impulseMagnitude;
+			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
 	}
 
+	
 	void	writebackVelocity()
 	{
 		if (m_invMass)
 		{
 			m_originalBody->setLinearVelocity(m_linearVelocity);
 			m_originalBody->setAngularVelocity(m_angularVelocity);
+			
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+	
+
+	void	writebackVelocity(btScalar timeStep)
+	{
+		if (m_invMass)
+		{
+			m_originalBody->setLinearVelocity(m_linearVelocity);
+			m_originalBody->setAngularVelocity(m_angularVelocity);
+			
+			//correct the position/orientation based on push/turn recovery
+			btTransform newTransform;
+			btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+			m_originalBody->setWorldTransform(newTransform);
+			
+			//m_originalBody->setCompanionId(-1);
 		}
 	}
 
@@ -69,3 +109,5 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 };
 
 #endif //BT_SOLVER_BODY_H
+
+
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
index f1f40ffdf19..2c71360c5b9 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h
@@ -1,5 +1,3 @@
-
-
 /*
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
@@ -27,24 +25,34 @@ class	btRigidBody;
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
 ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 {
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
 	btVector3	m_relpos1CrossNormal;
-	btVector3	m_relpos2CrossNormal;
 	btVector3	m_contactNormal;
+
+	btVector3	m_relpos2CrossNormal;
 	btVector3	m_angularComponentA;
+
 	btVector3	m_angularComponentB;
 
-	btScalar	m_appliedVelocityImpulse;
+	mutable btScalar	m_appliedPushImpulse;
+	
+	mutable btScalar	m_appliedImpulse;
 	int			m_solverBodyIdA;
 	int			m_solverBodyIdB;
+	
 	btScalar	m_friction;
 	btScalar	m_restitution;
 	btScalar	m_jacDiagABInv;
 	btScalar	m_penetration;
-	btScalar	m_appliedImpulse;
+	
 
+	
 	int			m_constraintType;
 	int			m_frictionIndex;
-	int			m_unusedPadding[2];
+	void*		m_originalContactPoint;
+	int			m_unusedPadding[1];
+
 
 	enum		btSolverConstraintType
 	{
@@ -61,3 +69,4 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 #endif //BT_SOLVER_CONSTRAINT_H
 
 
+
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.cpp
new file mode 100644
index 00000000000..175d15dcfcf
--- /dev/null
+++ b/extern/bullet2/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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSorLcp.h
new file mode 100644
index 00000000000..0d3583d33d9
--- /dev/null
+++ b/extern/bullet2/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/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
index a15b3e026cd..6e8b552dbbc 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
@@ -19,18 +19,20 @@ subject to the following restrictions:
 
 static btRigidBody s_fixed(0, 0,0);
 
-btTypedConstraint::btTypedConstraint()
-: m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
+:m_userConstraintType(-1),
 m_userConstraintId(-1),
+m_constraintType (type),
 m_rbA(s_fixed),
 m_rbB(s_fixed),
 m_appliedImpulse(btScalar(0.))
 {
 	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
 }
-btTypedConstraint::btTypedConstraint(btRigidBody& rbA)
-: m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
+:m_userConstraintType(-1),
 m_userConstraintId(-1),
+m_constraintType (type),
 m_rbA(rbA),
 m_rbB(s_fixed),
 m_appliedImpulse(btScalar(0.))
@@ -40,9 +42,10 @@ m_appliedImpulse(btScalar(0.))
 }
 
 
-btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB)
-: m_userConstraintType(-1),
+btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
+:m_userConstraintType(-1),
 m_userConstraintId(-1),
+m_constraintType (type),
 m_rbA(rbA),
 m_rbB(rbB),
 m_appliedImpulse(btScalar(0.))
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
index dfee6e80d0e..c50ec6ec579 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
@@ -17,7 +17,17 @@ subject to the following restrictions:
 #define TYPED_CONSTRAINT_H
 
 class btRigidBody;
-#include "../../LinearMath/btScalar.h"
+#include "LinearMath/btScalar.h"
+
+enum btTypedConstraintType
+{
+	POINT2POINT_CONSTRAINT_TYPE,
+	HINGE_CONSTRAINT_TYPE,
+	CONETWIST_CONSTRAINT_TYPE,
+	D6_CONSTRAINT_TYPE,
+	VEHICLE_CONSTRAINT_TYPE,
+	SLIDER_CONSTRAINT_TYPE
+};
 
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
 class btTypedConstraint
@@ -25,6 +35,8 @@ class btTypedConstraint
 	int	m_userConstraintType;
 	int	m_userConstraintId;
 
+	btTypedConstraintType m_constraintType;
+
 	btTypedConstraint&	operator=(btTypedConstraint&	other)
 	{
 		btAssert(0);
@@ -40,11 +52,11 @@ protected:
 
 public:
 
-	btTypedConstraint();
+	btTypedConstraint(btTypedConstraintType type);
 	virtual ~btTypedConstraint() {};
-	btTypedConstraint(btRigidBody& rbA);
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA);
 
-	btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB);
+	btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB);
 
 	virtual void	buildJacobian() = 0;
 
@@ -59,7 +71,7 @@ public:
 		return m_rbB;
 	}
 
-	btRigidBody& getRigidBodyA()
+	btRigidBody& getRigidBodyA() 
 	{
 		return m_rbA;
 	}
@@ -83,14 +95,26 @@ public:
 		m_userConstraintId = uid;
 	}
 	
-	int getUserConstraintId()
+	int getUserConstraintId() const
 	{
 		return m_userConstraintId;
 	}
-	btScalar	getAppliedImpulse()
+
+	int getUid() const
+	{
+		return m_userConstraintId;   
+	} 
+
+	btScalar	getAppliedImpulse() const
 	{
 		return m_appliedImpulse;
 	}
+
+	btTypedConstraintType getConstraintType () const
+	{
+		return m_constraintType;
+	}
+
 };
 
 #endif //TYPED_CONSTRAINT_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
index 248c582dcd8..c2fd71d67fe 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
@@ -25,6 +25,7 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedAllocator.h"
 
 
+
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btScalar.h"	
 #include "LinearMath/btMatrix3x3.h"
@@ -39,18 +40,296 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
 #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-
 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 #include "LinearMath/btStackAlloc.h"
 
-extern "C"
+/*
+	Create and Delete a Physics SDK	
+*/
+
+struct	btPhysicsSdk
+{
+
+//	btDispatcher*				m_dispatcher;
+//	btOverlappingPairCache*		m_pairCache;
+//	btConstraintSolver*			m_constraintSolver
+
+	btVector3	m_worldAabbMin;
+	btVector3	m_worldAabbMax;
+
+
+	//todo: version, hardware/optimization settings etc?
+	btPhysicsSdk()
+		:m_worldAabbMin(-1000,-1000,-1000),
+		m_worldAabbMax(1000,1000,1000)
+	{
+
+	}
+
+	
+};
+
+plPhysicsSdkHandle	plNewBulletSdk()
+{
+	void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
+	return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
+}
+
+void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk)
+{
+	btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
+	btAlignedFree(phys);	
+}
+
+
+/* Dynamics World */
+plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
+{
+	btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
+	void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
+	btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
+	mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
+	btDispatcher*				dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
+	mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
+	btBroadphaseInterface*		pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
+	mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+	btConstraintSolver*			constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
+
+	mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
+	return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
+}
+void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
+{
+	//todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	btAlignedFree(dynamicsWorld);
+}
+
+void	plStepSimulation(plDynamicsWorldHandle world,	plReal	timeStep)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	dynamicsWorld->stepSimulation(timeStep);
+}
+
+void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	assert(body);
+
+	dynamicsWorld->addRigidBody(body);
+}
+
+void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	assert(body);
+
+	dynamicsWorld->removeRigidBody(body);
+}
+
+/* Rigid Body  */
+
+plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape )
+{
+	btTransform trans;
+	trans.setIdentity();
+	btVector3 localInertia(0,0,0);
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	if (mass)
+	{
+		shape->calculateLocalInertia(mass,localInertia);
+	}
+	void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
+	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
+	btRigidBody* body = new (mem)btRigidBody(rbci);
+	body->setWorldTransform(trans);
+	body->setUserPointer(user_data);
+	return (plRigidBodyHandle) body;
+}
+
+void plDeleteRigidBody(plRigidBodyHandle cbody)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
+	assert(body);
+	btAlignedFree( body);
+}
+
+
+/* Collision Shape definition */
+
+plCollisionShapeHandle plNewSphereShape(plReal radius)
+{
+	void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
+	return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
+	
+}
+	
+plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
+{
+	void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
+	return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
+}
+
+plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
+{
+	//capsule is convex hull of 2 spheres, so use btMultiSphereShape
+	btVector3 inertiaHalfExtents(radius,height,radius);
+	const int numSpheres = 2;
+	btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
+	btScalar radi[numSpheres] = {radius,radius};
+	void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
+	return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres);
+}
+plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
+{
+	void* mem = btAlignedAlloc(sizeof(btConeShape),16);
+	return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
+}
+
+plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
+{
+	void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
+	return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
+}
+
+/* Convex Meshes */
+plCollisionShapeHandle plNewConvexHullShape()
+{
+	void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
+	return (plCollisionShapeHandle) new (mem)btConvexHullShape();
+}
+
+
+/* Concave static triangle meshes */
+plMeshInterfaceHandle		   plNewMeshInterface()
+{
+	return 0;
+}
+
+plCollisionShapeHandle plNewCompoundShape()
+{
+	void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
+	return (plCollisionShapeHandle) new (mem)btCompoundShape();
+}
+
+void	plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
+{
+	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
+	btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
+	btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
+	btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
+	btTransform	localTrans;
+	localTrans.setIdentity();
+	localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
+	localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
+	compoundShape->addChildShape(localTrans,childShape);
+}
+
+void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
+{
+	btQuaternion orn;
+	orn.setEuler(yaw,pitch,roll);
+	orient[0] = orn.getX();
+	orient[1] = orn.getY();
+	orient[2] = orn.getZ();
+	orient[3] = orn.getW();
+
+}
+
+
+//	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
+//	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
+
+
+void		plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
+{
+	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
+	(void)colShape;
+	btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
+	btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
+	convexHullShape->addPoint(btPoint3(x,y,z));
+
+}
+
+void plDeleteShape(plCollisionShapeHandle cshape)
+{
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	btAlignedFree(shape);
+}
+void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
+{
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
+	shape->setLocalScaling(scaling);	
+}
+
+
+
+void plSetPosition(plRigidBodyHandle object, const plVector3 position)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	btVector3 pos(position[0],position[1],position[2]);
+	btTransform worldTrans = body->getWorldTransform();
+	worldTrans.setOrigin(pos);
+	body->setWorldTransform(worldTrans);
+}
+
+void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
+	btTransform worldTrans = body->getWorldTransform();
+	worldTrans.setRotation(orn);
+	body->setWorldTransform(worldTrans);
+}
+
+void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	body->getWorldTransform().getOpenGLMatrix(matrix);
+
+}
+
+void	plGetPosition(plRigidBodyHandle object,plVector3 position)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	const btVector3& pos = body->getWorldTransform().getOrigin();
+	position[0] = pos.getX();
+	position[1] = pos.getY();
+	position[2] = pos.getZ();
+}
+
+void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	const btQuaternion& orn = body->getWorldTransform().getRotation();
+	orientation[0] = orn.getX();
+	orientation[1] = orn.getY();
+	orientation[2] = orn.getZ();
+	orientation[3] = orn.getW();
+}
+
+
+
+//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
+
+//	extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
+
 double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
 {
 	btVector3 vp(p1[0], p1[1], p1[2]);
@@ -118,3 +397,4 @@ double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float
 	}
 	return -1.0f;	
 }
+
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
new file mode 100644
index 00000000000..19443adc723
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
@@ -0,0 +1,193 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 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 "btContinuousDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+
+
+#include <stdio.h>
+
+btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+}
+
+btContinuousDynamicsWorld::~btContinuousDynamicsWorld()
+{
+}
+
+	
+void	btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
+{
+	
+	startProfiling(timeStep);
+	
+
+	///update aabbs information
+	updateAabbs();
+	//static int frame=0;
+//	printf("frame %d\n",frame++);
+
+	///apply gravity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
+	///perform collision detection
+	performDiscreteCollisionDetection();
+
+	calculateSimulationIslands();
+
+	
+	getSolverInfo().m_timeStep = timeStep;
+	
+
+
+	///solve contact and other joint constraints
+	solveConstraints(getSolverInfo());
+	
+	///CallbackTriggers();
+	calculateTimeOfImpacts(timeStep);
+
+	btScalar toi = dispatchInfo.m_timeOfImpact;
+//	if (toi < 1.f)
+//		printf("toi = %f\n",toi);
+	if (toi < 0.f)
+		printf("toi = %f\n",toi);
+
+
+	///integrate transforms
+	integrateTransforms(timeStep * toi);
+
+	///update vehicle simulation
+	updateVehicles(timeStep);
+
+
+	updateActivationState( timeStep );
+	
+	if(0 != m_internalTickCallback) {
+		(*m_internalTickCallback)(this, timeStep);
+	}
+}
+
+void	btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep)
+{
+		///these should be 'temporal' aabbs!
+		updateTemporalAabbs(timeStep);
+		
+		///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually.
+		///so we handle the case moving versus static properly, and we cheat for moving versus moving
+		btScalar toi = 1.f;
+		
+	
+		btDispatcherInfo& dispatchInfo = getDispatchInfo();
+		dispatchInfo.m_timeStep = timeStep;
+		dispatchInfo.m_timeOfImpact = 1.f;
+		dispatchInfo.m_stepCount = 0;
+		dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS;
+
+		///calculate time of impact for overlapping pairs
+
+
+		btDispatcher* dispatcher = getDispatcher();
+		if (dispatcher)
+			dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
+
+		toi = dispatchInfo.m_timeOfImpact;
+
+		dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE;
+
+}
+
+void	btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep)
+{
+
+	btVector3 temporalAabbMin,temporalAabbMax;
+
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax);
+			const btVector3& linvel = body->getLinearVelocity();
+
+			//make the AABB temporal
+			btScalar temporalAabbMaxx = temporalAabbMax.getX();
+			btScalar temporalAabbMaxy = temporalAabbMax.getY();
+			btScalar temporalAabbMaxz = temporalAabbMax.getZ();
+			btScalar temporalAabbMinx = temporalAabbMin.getX();
+			btScalar temporalAabbMiny = temporalAabbMin.getY();
+			btScalar temporalAabbMinz = temporalAabbMin.getZ();
+
+			// add linear motion
+			btVector3 linMotion = linvel*timeStep;
+		
+			if (linMotion.x() > 0.f)
+				temporalAabbMaxx += linMotion.x(); 
+			else
+				temporalAabbMinx += linMotion.x();
+			if (linMotion.y() > 0.f)
+				temporalAabbMaxy += linMotion.y(); 
+			else
+				temporalAabbMiny += linMotion.y();
+			if (linMotion.z() > 0.f)
+				temporalAabbMaxz += linMotion.z(); 
+			else
+				temporalAabbMinz += linMotion.z();
+
+			//add conservative angular motion
+			btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep;
+			btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
+			temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
+			temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
+
+			temporalAabbMin -= angularMotion3d;
+			temporalAabbMax += angularMotion3d;
+
+			m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1);
+		}
+	}
+
+	//update aabb (of all moved objects)
+
+	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
+	
+
+
+}
+
+
+
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h
new file mode 100644
index 00000000000..61c8dea03eb
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h
@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 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 BT_CONTINUOUS_DYNAMICS_WORLD_H
+#define BT_CONTINUOUS_DYNAMICS_WORLD_H
+
+#include "btDiscreteDynamicsWorld.h"
+
+///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld.
+///This copes with fast moving objects that otherwise would tunnel/miss collisions.
+///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead.
+class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld
+{
+
+	void	updateTemporalAabbs(btScalar timeStep);
+
+	public:
+
+		btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+		virtual ~btContinuousDynamicsWorld();
+		
+		///time stepping with calculation of time of impact for selected fast moving objects
+		virtual void	internalSingleStepSimulation( btScalar timeStep);
+
+		virtual void	calculateTimeOfImpacts(btScalar timeStep);
+
+		virtual btDynamicsWorldType	getWorldType() const
+		{
+			return BT_CONTINUOUS_DYNAMICS_WORLD;
+		}
+
+};
+
+#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 1017c8af6ea..c7b1af245e9 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -21,7 +21,8 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
-#include <LinearMath/btTransformUtil.h>
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btQuickprof.h"
 
 //rigidbody & constraints
 #include "BulletDynamics/Dynamics/btRigidBody.h"
@@ -41,6 +42,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 #include "LinearMath/btIDebugDraw.h"
 
 
@@ -57,17 +59,29 @@ subject to the following restrictions:
 
 
 
-btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
-:btDynamicsWorld(dispatcher,pairCache),
-m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
-m_debugDrawer(0),
+btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
+m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
 m_localTime(btScalar(1.)/btScalar(60.)),
 m_profileTimings(0)
 {
-	m_islandManager = new btSimulationIslandManager();
+	if (!m_constraintSolver)
+	{
+		void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+		m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
+		m_ownsConstraintSolver = true;
+	} else
+	{
+		m_ownsConstraintSolver = false;
+	}
+
+	{
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
+		m_islandManager = new (mem) btSimulationIslandManager();
+	}
+
 	m_ownsIslandManager = true;
-	m_ownsConstraintSolver = (constraintSolver==0);
 }
 
 
@@ -75,9 +89,16 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 {
 	//only delete it when we created it
 	if (m_ownsIslandManager)
-		delete m_islandManager;
+	{
+		m_islandManager->~btSimulationIslandManager();
+		btAlignedFree( m_islandManager);
+	}
 	if (m_ownsConstraintSolver)
-		 delete m_constraintSolver;
+	{
+
+		m_constraintSolver->~btConstraintSolver();
+		btAlignedFree(m_constraintSolver);
+	}
 }
 
 void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
@@ -102,14 +123,35 @@ void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 	}
 }
 
-void	btDiscreteDynamicsWorld::synchronizeMotionStates()
+void	btDiscreteDynamicsWorld::debugDrawWorld()
 {
-	//debug vehicle wheels
-	
-	
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		btVector3 color(0,0,0);
+		for (int i=0;i<numManifolds;i++)
+		{
+			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+			int numContacts = contactManifold->getNumContacts();
+			for (int j=0;j<numContacts;j++)
+			{
+				btManifoldPoint& cp = contactManifold->getContactPoint(j);
+				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+			}
+		}
+	}
+
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
 	{
+		int i;
+
 		//todo: iterate over awake simulation islands!
-		for ( int i=0;i<m_collisionObjects.size();i++)
+		for (  i=0;i<m_collisionObjects.size();i++)
 		{
 			btCollisionObject* colObj = m_collisionObjects[i];
 			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
@@ -135,26 +177,17 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 
 				debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
 			}
-			btRigidBody* body = btRigidBody::upcast(colObj);
-			if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 			{
-				//we need to call the update at least once, even for sleeping objects
-				//otherwise the 'graphics' transform never updates properly
-				//so todo: add 'dirty' flag
-				//if (body->getActivationState() != ISLAND_SLEEPING)
-				{
-					btTransform interpolatedTransform;
-					btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-						body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
-					body->getMotionState()->setWorldTransform(interpolatedTransform);
-				}
+				btPoint3 minAabb,maxAabb;
+				btVector3 colorvec(1,0,0);
+				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+				m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
 			}
-		}
-	}
 
-	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
-	{
-		for ( int i=0;i<this->m_vehicles.size();i++)
+		}
+	
+		for (  i=0;i<this->m_vehicles.size();i++)
 		{
 			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
 			{
@@ -166,10 +199,7 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 				{
 					wheelColor.setValue(255,0,255);
 				}
-
-				//synchronize the wheels with the (interpolated) chassis worldtransform
-				m_vehicles[i]->updateWheelTransform(v,true);
-					
+		
 				btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
 
 				btVector3 axle = btVector3(	
@@ -186,12 +216,87 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 			}
 		}
 	}
+}
+
+void	btDiscreteDynamicsWorld::clearForces()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->clearForces();
+		}
+	}
+}	
+
+///apply gravity, call this once per timestep
+void	btDiscreteDynamicsWorld::applyGravity()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body && body->isActive())
+		{
+			body->applyGravity();
+		}
+	}
+}
+
+
+
+void	btDiscreteDynamicsWorld::synchronizeMotionStates()
+{
+	{
+		//todo: iterate over awake simulation islands!
+		for ( int i=0;i<m_collisionObjects.size();i++)
+		{
+			btCollisionObject* colObj = m_collisionObjects[i];
+			
+			btRigidBody* body = btRigidBody::upcast(colObj);
+			if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
+			{
+				//we need to call the update at least once, even for sleeping objects
+				//otherwise the 'graphics' transform never updates properly
+				//so todo: add 'dirty' flag
+				//if (body->getActivationState() != ISLAND_SLEEPING)
+				{
+					btTransform interpolatedTransform;
+					btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
+						body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
+					body->getMotionState()->setWorldTransform(interpolatedTransform);
+				}
+			}
+		}
+	}
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+	{
+		for ( int i=0;i<this->m_vehicles.size();i++)
+		{
+			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
+			{
+				//synchronize the wheels with the (interpolated) chassis worldtransform
+				m_vehicles[i]->updateWheelTransform(v,true);
+			}
+		}
+	}
 
 }
 
 
 int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
 {
+	startProfiling(timeStep);
+
+	BT_PROFILE("stepSimulation");
+
 	int numSimulationSubSteps = 0;
 
 	if (maxSubSteps)
@@ -229,6 +334,8 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 
 		saveKinematicState(fixedTimeStep);
 
+		applyGravity();
+
 		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
 		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
 
@@ -242,16 +349,19 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 
 	synchronizeMotionStates();
 
+	clearForces();
+
+#ifndef BT_NO_PROFILE
+	CProfileManager::Increment_Frame_Counter();
+#endif //BT_NO_PROFILE
+	
 	return numSimulationSubSteps;
 }
 
 void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
 	
-	startProfiling(timeStep);
-
-	///update aabbs information
-	updateAabbs();
+	BT_PROFILE("internalSingleStepSimulation");
 
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
@@ -286,8 +396,9 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 
 	updateActivationState( timeStep );
 
-	
-
+	if(0 != m_internalTickCallback) {
+		(*m_internalTickCallback)(this, timeStep);
+	}	
 }
 
 void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
@@ -304,6 +415,11 @@ void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
 	}
 }
 
+btVector3 btDiscreteDynamicsWorld::getGravity () const
+{
+	return m_gravity;
+}
+
 
 void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
@@ -343,19 +459,18 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
 
 void	btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
 {
-	BEGIN_PROFILE("updateVehicles");
-
+	BT_PROFILE("updateVehicles");
+	
 	for ( int i=0;i<m_vehicles.size();i++)
 	{
 		btRaycastVehicle* vehicle = m_vehicles[i];
 		vehicle->updateVehicle( timeStep);
 	}
-	END_PROFILE("updateVehicles");
 }
 
 void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
-	BEGIN_PROFILE("updateActivationState");
+	BT_PROFILE("updateActivationState");
 
 	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
@@ -374,6 +489,12 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 				{
 					if (body->getActivationState() == ACTIVE_TAG)
 						body->setActivationState( WANTS_DEACTIVATION );
+					if (body->getActivationState() == ISLAND_SLEEPING) 
+					{
+						body->setAngularVelocity(btVector3(0,0,0));
+						body->setLinearVelocity(btVector3(0,0,0));
+					}
+
 				}
 			} else
 			{
@@ -382,7 +503,6 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 			}
 		}
 	}
-	END_PROFILE("updateActivationState");
 }
 
 void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
@@ -412,7 +532,7 @@ void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
 	m_vehicles.remove(vehicle);
 }
 
-inline	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
 {
 	int islandId;
 	
@@ -442,6 +562,7 @@ class btSortConstraintOnIslandPredicate
 
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
+	BT_PROFILE("solveConstraints");
 	
 	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 	{
@@ -452,7 +573,7 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		int						m_numConstraints;
 		btIDebugDraw*			m_debugDrawer;
 		btStackAlloc*			m_stackAlloc;
-
+		btDispatcher*			m_dispatcher;
 
 		InplaceSolverIslandCallback(
 			btContactSolverInfo& solverInfo,
@@ -460,13 +581,15 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 			btTypedConstraint** sortedConstraints,
 			int	numConstraints,
 			btIDebugDraw*	debugDrawer,
-			btStackAlloc*			stackAlloc)
+			btStackAlloc*			stackAlloc,
+			btDispatcher* dispatcher)
 			:m_solverInfo(solverInfo),
 			m_solver(solver),
 			m_sortedConstraints(sortedConstraints),
 			m_numConstraints(numConstraints),
 			m_debugDrawer(debugDrawer),
-			m_stackAlloc(stackAlloc)
+			m_stackAlloc(stackAlloc),
+			m_dispatcher(dispatcher)
 		{
 
 		}
@@ -479,30 +602,42 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		}
 		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 		{
-			//also add all non-contact constraints/joints for this island
-			btTypedConstraint** startConstraint = 0;
-			int numCurConstraints = 0;
-			int i;
-			
-			//find the first constraint for this island
-			for (i=0;i<m_numConstraints;i++)
+			if (islandId<0)
+			{
+				///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+			} else
 			{
-				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+					//also add all non-contact constraints/joints for this island
+				btTypedConstraint** startConstraint = 0;
+				int numCurConstraints = 0;
+				int i;
+				
+				//find the first constraint for this island
+				for (i=0;i<m_numConstraints;i++)
 				{
-					startConstraint = &m_sortedConstraints[i];
-					break;
+					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+					{
+						startConstraint = &m_sortedConstraints[i];
+						break;
+					}
 				}
-			}
-			//count the number of constraints in this island
-			for (;i<m_numConstraints;i++)
-			{
-				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+				//count the number of constraints in this island
+				for (;i<m_numConstraints;i++)
 				{
-					numCurConstraints++;
+					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+					{
+						numCurConstraints++;
+					}
 				}
-			}
 
-			m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc);
+				///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+				if (numManifolds + numCurConstraints)
+				{
+					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+				}
+		
+			}
 		}
 
 	};
@@ -520,18 +655,18 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		
 	
 
-	sortedConstraints.heapSort(btSortConstraintOnIslandPredicate());
+	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
 	
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
 	
-	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc);
-
+	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc,m_dispatcher1);
 	
+	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 	
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
 
-
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
 }
 
 
@@ -539,7 +674,7 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
 void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 {
-	BEGIN_PROFILE("calculateSimulationIslands");
+	BT_PROFILE("calculateSimulationIslands");
 
 	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
 
@@ -569,66 +704,14 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
 
-	END_PROFILE("calculateSimulationIslands");
-
-}
-
-
-void	btDiscreteDynamicsWorld::updateAabbs()
-{
-	BEGIN_PROFILE("updateAabbs");
 	
-	btVector3 colorvec(1,0,0);
-	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
-		{
-		//	if (body->IsActive() && (!body->IsStatic()))
-			{
-				btPoint3 minAabb,maxAabb;
-				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-				btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
-
-				//moving objects should be moderately sized, probably something wrong if not
-				if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
-				{
-					bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
-				} else
-				{
-					//something went wrong, investigate
-					//this assert is unwanted in 3D modelers (danger of loosing work)
-					body->setActivationState(DISABLE_SIMULATION);
-					
-					static bool reportMe = true;
-					if (reportMe && m_debugDrawer)
-					{
-						reportMe = false;
-						m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
-						m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
-						m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
-						m_debugDrawer->reportErrorWarning("Thanks.\n");
-					}
+}
 
 
-				}
-				if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
-				{
-					m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
-				}
-			}
-		}
-	}
-	
-	END_PROFILE("updateAabbs");
-}
 
 void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
-	BEGIN_PROFILE("integrateTransforms");
+	BT_PROFILE("integrateTransforms");
 	btTransform predictedTrans;
 	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
@@ -643,14 +726,13 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			}
 		}
 	}
-	END_PROFILE("integrateTransforms");
 }
 
 
 
 void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
-	BEGIN_PROFILE("predictUnconstraintMotion");
+	BT_PROFILE("predictUnconstraintMotion");
 	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
@@ -661,51 +743,26 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 			{
 				if (body->isActive())
 				{
-					body->applyForces( timeStep);
 					body->integrateVelocities( timeStep);
+					//damping
+					body->applyDamping(timeStep);
+
 					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
 				}
 			}
 		}
 	}
-	END_PROFILE("predictUnconstraintMotion");
 }
 
 
 void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
 {
 	(void)timeStep;
-	#ifdef USE_QUICKPROF
 
+#ifndef BT_NO_PROFILE
+	CProfileManager::Reset();
+#endif //BT_NO_PROFILE
 
-	//toggle btProfiler
-	if ( m_debugDrawer && m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
-	{
-		if (!m_profileTimings)
-		{
-			m_profileTimings = 1;
-			// To disable profiling, simply comment out the following line.
-			static int counter = 0;
-
-			char filename[128];
-			sprintf(filename,"quickprof_bullet_timings%i.csv",counter++);
-			btProfiler::init(filename, btProfiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS
-		} else
-		{
-			btProfiler::endProfilingCycle();
-		}
-
-	} else
-	{
-		if (m_profileTimings)
-		{
-			btProfiler::endProfilingCycle();
-
-			m_profileTimings = 0;
-			btProfiler::destroy();
-		}
-	}
-#endif //USE_QUICKPROF
 }
 
 
@@ -827,27 +884,52 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 
 				btScalar radius = capsuleShape->getRadius();
 				btScalar halfHeight = capsuleShape->getHalfHeight();
+				
+				int upAxis = capsuleShape->getUpAxis();
+
+				
+				btVector3 capStart(0.f,0.f,0.f);
+				capStart[upAxis] = -halfHeight;
+
+				btVector3 capEnd(0.f,0.f,0.f);
+				capEnd[upAxis] = halfHeight;
 
 				// Draw the ends
 				{
+					
 					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
+					childTransform.getOrigin() = worldTransform * capStart;
 					debugDrawSphere(radius, childTransform, color);
 				}
 
 				{
 					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
+					childTransform.getOrigin() = worldTransform * capEnd;
 					debugDrawSphere(radius, childTransform, color);
 				}
 
 				// Draw some additional lines
 				btVector3 start = worldTransform.getOrigin();
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
 
+				
+				capStart[(upAxis+1)%3] = radius;
+				capEnd[(upAxis+1)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+				capStart[(upAxis+1)%3] = -radius;
+				capEnd[(upAxis+1)%3] = -radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+				capStart[(upAxis+1)%3] = 0.f;
+				capEnd[(upAxis+1)%3] = 0.f;
+
+				capStart[(upAxis+2)%3] = radius;
+				capEnd[(upAxis+2)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+				capStart[(upAxis+2)%3] = -radius;
+				capEnd[(upAxis+2)%3] = -radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+				
 				break;
 			}
 		case CONE_SHAPE_PROXYTYPE:
@@ -856,9 +938,10 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
 				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
 				btVector3 start = worldTransform.getOrigin();
-				// insert here Bullet 2.69 that fixes representation of cone
+
 				int upAxis= coneShape->getConeUpIndex();
 				
+
 				btVector3	offsetHeight(0,0,0);
 				offsetHeight[upAxis] = height * btScalar(0.5);
 				btVector3	offsetRadius(0,0,0);
@@ -871,11 +954,8 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
 				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
 
-				// buggy code that does not take into account the direction of the cone
-				//getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color);
-				//getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color);
-				//getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color);
-				//getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color);
+
+
 				break;
 
 			}
@@ -884,7 +964,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
 				int upAxis = cylinder->getUpAxis();
 				btScalar radius = cylinder->getRadius();
-				btScalar halfHeight = cylinder->getHalfExtents()[upAxis];
+				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
 				btVector3 start = worldTransform.getOrigin();
 				btVector3	offsetHeight(0,0,0);
 				offsetHeight[upAxis] = halfHeight;
@@ -894,6 +974,25 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
 				break;
 			}
+
+			case STATIC_PLANE_PROXYTYPE:
+				{
+					const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+					btScalar planeConst = staticPlaneShape->getPlaneConstant();
+					const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+					btVector3 planeOrigin = planeNormal * planeConst;
+					btVector3 vec0,vec1;
+					btPlaneSpace1(planeNormal,vec0,vec1);
+					btScalar vecLen = 100.f;
+					btVector3 pt0 = planeOrigin + vec0*vecLen;
+					btVector3 pt1 = planeOrigin - vec0*vecLen;
+					btVector3 pt2 = planeOrigin + vec1*vecLen;
+					btVector3 pt3 = planeOrigin - vec1*vecLen;
+					getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
+					getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
+					break;
+
+				}
 		default:
 			{
 
@@ -918,7 +1017,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 					btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
 					//DebugDrawcallback drawCallback;
 					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-					convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+					convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
 				}
 
 
@@ -950,12 +1049,18 @@ void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
 	{
-		delete m_constraintSolver;
+		btAlignedFree( m_constraintSolver);
 	}
 	m_ownsConstraintSolver = false;
 	m_constraintSolver = solver;
 }
 
+btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
+{
+	return m_constraintSolver;
+}
+
+
 int		btDiscreteDynamicsWorld::getNumConstraints() const
 {
 	return int(m_constraints.size());
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 83b90bfeebc..d206a604960 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -23,11 +23,11 @@ class btOverlappingPairCache;
 class btConstraintSolver;
 class btSimulationIslandManager;
 class btTypedConstraint;
-#include "../ConstraintSolver/btContactSolverInfo.h"
+
 
 class btRaycastVehicle;
 class btIDebugDraw;
-#include "../../LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 
 ///btDiscreteDynamicsWorld provides discrete rigid body simulation
@@ -42,7 +42,6 @@ protected:
 
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
-	btIDebugDraw*	m_debugDrawer;
 
 	btVector3	m_gravity;
 
@@ -53,14 +52,12 @@ protected:
 	bool	m_ownsIslandManager;
 	bool	m_ownsConstraintSolver;
 
-	btContactSolverInfo	m_solverInfo;
-
-
+	
 	btAlignedObjectArray<btRaycastVehicle*>	m_vehicles;
 
 	int	m_profileTimings;
 
-	void	predictUnconstraintMotion(btScalar timeStep);
+	virtual void	predictUnconstraintMotion(btScalar timeStep);
 	
 	void	integrateTransforms(btScalar timeStep);
 		
@@ -86,14 +83,13 @@ public:
 
 
 	///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
-	btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver);
+	btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btDiscreteDynamicsWorld();
 
 	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
 	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
 
-	virtual void	updateAabbs();
 
 	void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
 
@@ -118,17 +114,9 @@ public:
 		return this;
 	}
 
-	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer)
-	{
-			m_debugDrawer = debugDrawer;
-	}
-
-	virtual btIDebugDraw*	getDebugDrawer()
-	{
-		return m_debugDrawer;
-	}
 
 	virtual void	setGravity(const btVector3& gravity);
+	virtual btVector3 getGravity () const;
 
 	virtual void	addRigidBody(btRigidBody* body);
 
@@ -138,7 +126,11 @@ public:
 
 	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
+	virtual void	debugDrawWorld();
+
 	virtual void	setConstraintSolver(btConstraintSolver* solver);
+
+	virtual btConstraintSolver* getConstraintSolver();
 	
 	virtual	int		getNumConstraints() const;
 
@@ -146,11 +138,21 @@ public:
 
 	virtual const btTypedConstraint* getConstraint(int index) const;
 
-	btContactSolverInfo& getSolverInfo()
+	
+	virtual btDynamicsWorldType	getWorldType() const
 	{
-		return m_solverInfo;
+		return BT_DISCRETE_DYNAMICS_WORLD;
 	}
+	
+	///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
+	virtual void	clearForces();
+
+	///apply gravity, call this once per timestep
+	virtual void	applyGravity();
 
+	virtual void	setNumTasks(int numTasks)
+	{
+	}
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 65b63fad4b5..dd9dfa71b7f 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -16,20 +16,39 @@ subject to the following restrictions:
 #ifndef BT_DYNAMICS_WORLD_H
 #define BT_DYNAMICS_WORLD_H
 
-#include "../../BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
 class btTypedConstraint;
 class btRaycastVehicle;
 class btConstraintSolver;
+class btDynamicsWorld;
+
+/// Type for the callback for each tick
+typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
 
+enum btDynamicsWorldType
+{
+	BT_SIMPLE_DYNAMICS_WORLD=1,
+	BT_DISCRETE_DYNAMICS_WORLD=2,
+	BT_CONTINUOUS_DYNAMICS_WORLD=3
+};
 
-///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous
+///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
 class btDynamicsWorld : public btCollisionWorld
 {
-	public:
+
+protected:
+		btInternalTickCallback m_internalTickCallback;
+		void*	m_worldUserInfo;
+
+		btContactSolverInfo	m_solverInfo;
+
+public:
 		
 
-		btDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
-		:btCollisionWorld(dispatcher,pairCache)
+		btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
+		:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
 		{
 		}
 
@@ -37,34 +56,37 @@ class btDynamicsWorld : public btCollisionWorld
 		{
 		}
 		
-		///stepSimulation proceeds the simulation over timeStep units
-		///if maxSubSteps > 0, it will interpolate time steps
-		virtual int		stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
+		///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
+		///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
+		///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
+		///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
+		virtual int		stepSimulation( btScalar timeStep,int maxSubSteps=10, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
 			
-		virtual void	updateAabbs() = 0;
+		virtual void	debugDrawWorld() = 0;
 				
-		virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) { (void)constraint;};
-
-		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;};
-
-		virtual void	addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;};
-
-		virtual void	removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;};
+		virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) 
+		{ 
+			(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
+		}
 
+		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
 
-		virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer) = 0;
+		virtual void	addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
 
-		virtual btIDebugDraw*	getDebugDrawer() = 0;
+		virtual void	removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
 
 		//once a rigidbody is added to the dynamics world, it will get this gravity assigned
 		//existing rigidbodies in the world get gravity assigned too, during this method
 		virtual void	setGravity(const btVector3& gravity) = 0;
+		virtual btVector3 getGravity () const = 0;
 
 		virtual void	addRigidBody(btRigidBody* body) = 0;
 
 		virtual void	removeRigidBody(btRigidBody* body) = 0;
 
 		virtual void	setConstraintSolver(btConstraintSolver* solver) = 0;
+
+		virtual btConstraintSolver* getConstraintSolver() = 0;
 		
 		virtual	int		getNumConstraints() const {	return 0;		}
 		
@@ -72,7 +94,35 @@ class btDynamicsWorld : public btCollisionWorld
 		
 		virtual const btTypedConstraint* getConstraint(int index) const	{	(void)index;	return 0;	}
 
+		virtual btDynamicsWorldType	getWorldType() const=0;
+
+		virtual void	clearForces() = 0;
+
+		/// Set the callback for when an internal tick (simulation substep) happens, optional user info
+		void setInternalTickCallback(btInternalTickCallback cb,	void* worldUserInfo=0) 
+		{ 
+			m_internalTickCallback = cb; 
+			m_worldUserInfo = worldUserInfo;
+		}
+
+		void	setWorldUserInfo(void* worldUserInfo)
+		{
+			m_worldUserInfo = worldUserInfo;
+		}
+
+		void*	getWorldUserInfo() const
+		{
+			return m_worldUserInfo;
+		}
+
+		btContactSolverInfo& getSolverInfo()
+		{
+			return m_solverInfo;
+		}
+
+
 };
 
 #endif //BT_DYNAMICS_WORLD_H
 
+
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
index 9ed3579d89c..e2afb687ac6 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -20,36 +20,53 @@ subject to the following restrictions:
 #include "LinearMath/btMotionState.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 
-btScalar gLinearAirDamping = btScalar(1.);
 //'temporarily' global variables
 btScalar	gDeactivationTime = btScalar(2.);
 bool	gDisableDeactivation = false;
-
-btScalar gLinearSleepingThreshold = btScalar(0.8);
-btScalar gAngularSleepingThreshold = btScalar(1.0);
 static int uniqueId = 0;
 
-btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
-: 
-	m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)),
-	m_angularFactor(btScalar(1.)),
-	m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_linearDamping(btScalar(0.)),
-	m_angularDamping(btScalar(0.5)),
-	m_optionalMotionState(motionState),
-	m_contactSolverType(0),
-	m_frictionSolverType(0)
+
+btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+{
+	setupRigidBody(constructionInfo);
+}
+
+btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
+{
+	btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
+	setupRigidBody(cinfo);
+}
+
+void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
 {
 
-	if (motionState)
+	m_internalType=CO_RIGID_BODY;
+
+	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	m_angularFactor = btScalar(1.);
+	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
+	m_linearDamping = btScalar(0.);
+	m_angularDamping = btScalar(0.5);
+	m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
+	m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
+	m_optionalMotionState = constructionInfo.m_motionState;
+	m_contactSolverType = 0;
+	m_frictionSolverType = 0;
+	m_additionalDamping = constructionInfo.m_additionalDamping;
+	m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
+	m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
+	m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
+	m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
+
+	if (m_optionalMotionState)
 	{
-		motionState->getWorldTransform(m_worldTransform);
+		m_optionalMotionState->getWorldTransform(m_worldTransform);
 	} else
 	{
-		m_worldTransform = btTransform::getIdentity();
+		m_worldTransform = constructionInfo.m_startWorldTransform;
 	}
 
 	m_interpolationWorldTransform = m_worldTransform;
@@ -57,90 +74,21 @@ btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionS
 	m_interpolationAngularVelocity.setValue(0,0,0);
 	
 	//moved to btCollisionObject
-	m_friction = friction;
-	m_restitution = restitution;
-
-	m_collisionShape = collisionShape;
-	m_debugBodyId = uniqueId++;
-	
-	//m_internalOwner is to allow upcasting from collision object to rigid body
-	m_internalOwner = this;
-
-	setMassProps(mass, localInertia);
-    setDamping(linearDamping, angularDamping);
-	updateInertiaTensor();
-
-}
-
-#ifdef OBSOLETE_MOTIONSTATE_LESS
-btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
-: 
-	m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)),
-	m_linearDamping(btScalar(0.)),
-	m_angularDamping(btScalar(0.5)),
-	m_optionalMotionState(0),
-	m_contactSolverType(0),
-	m_frictionSolverType(0)
-{
-	
-	m_worldTransform = worldTransform;
-	m_interpolationWorldTransform = m_worldTransform;
-	m_interpolationLinearVelocity.setValue(0,0,0);
-	m_interpolationAngularVelocity.setValue(0,0,0);
-
-	//moved to btCollisionObject
-	m_friction = friction;
-	m_restitution = restitution;
+	m_friction = constructionInfo.m_friction;
+	m_restitution = constructionInfo.m_restitution;
 
-	m_collisionShape = collisionShape;
+	setCollisionShape( constructionInfo.m_collisionShape );
 	m_debugBodyId = uniqueId++;
 	
-	//m_internalOwner is to allow upcasting from collision object to rigid body
-	m_internalOwner = this;
-
-	setMassProps(mass, localInertia);
-    setDamping(linearDamping, angularDamping);
+	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
+    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
 	updateInertiaTensor();
 
 }
 
-#endif //OBSOLETE_MOTIONSTATE_LESS
-
-
-
-
-//#define EXPERIMENTAL_JITTER_REMOVAL 1
-#ifdef EXPERIMENTAL_JITTER_REMOVAL
-//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
-//doesn't work very well yet (value 0 disabled this damping)
-//note there this influences deactivation thresholds!
-btScalar gClippedAngvelThresholdSqr = btScalar(0.01);
-btScalar	gClippedLinearThresholdSqr = btScalar(0.01);
-#endif //EXPERIMENTAL_JITTER_REMOVAL
-
-btScalar	gJitterVelocityDampingFactor = btScalar(0.7);
 
 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
 {
-
-#ifdef EXPERIMENTAL_JITTER_REMOVAL
-	//if (wantsSleeping())
-	{
-		//clip to avoid jitter
-		if ((m_angularVelocity.length2() < gClippedAngvelThresholdSqr) &&
-			(m_linearVelocity.length2() < gClippedLinearThresholdSqr))
-		{
-			m_angularVelocity *= gJitterVelocityDampingFactor;
-			m_linearVelocity *= gJitterVelocityDampingFactor;
-		}
-	}
-	
-#endif //EXPERIMENTAL_JITTER_REMOVAL
-
 	btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
 }
 
@@ -191,50 +139,63 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 
 
 
-#include <stdio.h>
-
 
-void btRigidBody::applyForces(btScalar step)
+///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
+void			btRigidBody::applyDamping(btScalar timeStep)
 {
-	if (isStaticOrKinematicObject())
-		return;
-	
-	applyCentralForce(m_gravity);	
-	
-	m_linearVelocity *= GEN_clamped((btScalar(1.) - step * gLinearAirDamping * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularVelocity *= GEN_clamped((btScalar(1.) - step * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
 
-#define FORCE_VELOCITY_DAMPING 1
-#ifdef FORCE_VELOCITY_DAMPING
-	btScalar speed = m_linearVelocity.length();
-	if (speed < m_linearDamping)
+	if (m_additionalDamping)
 	{
-		btScalar dampVel = btScalar(0.005);
-		if (speed > dampVel)
+		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+		if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
+			(m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
 		{
-			btVector3 dir = m_linearVelocity.normalized();
-			m_linearVelocity -=  dir * dampVel;
-		} else
-		{
-			m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			m_angularVelocity *= m_additionalDampingFactor;
+			m_linearVelocity *= m_additionalDampingFactor;
 		}
-	}
+	
 
-	btScalar angSpeed = m_angularVelocity.length();
-	if (angSpeed < m_angularDamping)
-	{
-		btScalar angDampVel = btScalar(0.005);
-		if (angSpeed > angDampVel)
+		btScalar speed = m_linearVelocity.length();
+		if (speed < m_linearDamping)
 		{
-			btVector3 dir = m_angularVelocity.normalized();
-			m_angularVelocity -=  dir * angDampVel;
-		} else
+			btScalar dampVel = btScalar(0.005);
+			if (speed > dampVel)
+			{
+				btVector3 dir = m_linearVelocity.normalized();
+				m_linearVelocity -=  dir * dampVel;
+			} else
+			{
+				m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			}
+		}
+
+		btScalar angSpeed = m_angularVelocity.length();
+		if (angSpeed < m_angularDamping)
 		{
-			m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			btScalar angDampVel = btScalar(0.005);
+			if (angSpeed > angDampVel)
+			{
+				btVector3 dir = m_angularVelocity.normalized();
+				m_angularVelocity -=  dir * angDampVel;
+			} else
+			{
+				m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			}
 		}
 	}
-#endif //FORCE_VELOCITY_DAMPING
+}
+
+
+void btRigidBody::applyGravity()
+{
+	if (isStaticOrKinematicObject())
+		return;
 	
+	applyCentralForce(m_gravity);	
+
 }
 
 void btRigidBody::proceedToTransform(const btTransform& newTrans)
@@ -285,7 +246,6 @@ void btRigidBody::integrateVelocities(btScalar step)
 		m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
 	}
 
-	clearForces();
 }
 
 btQuaternion btRigidBody::getOrientation() const
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
index 0707595d48e..4596f90a00f 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -16,27 +16,29 @@ subject to the following restrictions:
 #ifndef RIGIDBODY_H
 #define RIGIDBODY_H
 
-#include "../../LinearMath/btAlignedObjectArray.h"
-#include "../../LinearMath/btPoint3.h"
-#include "../../LinearMath/btTransform.h"
-#include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
-#include "../../BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btTransform.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 class btCollisionShape;
 class btMotionState;
 class btTypedConstraint;
 
 
-extern btScalar gLinearAirDamping;
-
 extern btScalar gDeactivationTime;
 extern bool gDisableDeactivation;
-extern btScalar gLinearSleepingThreshold;
-extern btScalar gAngularSleepingThreshold;
 
 
-/// btRigidBody class for btRigidBody Dynamics
-/// 
+///btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
+///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
+///There are 3 types of rigid bodies: 
+///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
+///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
+///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
+///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
+///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
 class btRigidBody  : public btCollisionObject
 {
 
@@ -53,7 +55,16 @@ class btRigidBody  : public btCollisionObject
 	
 	btScalar		m_linearDamping;
 	btScalar		m_angularDamping;
-	
+
+	bool			m_additionalDamping;
+	btScalar		m_additionalDampingFactor;
+	btScalar		m_additionalLinearDampingThresholdSqr;
+	btScalar		m_additionalAngularDampingThresholdSqr;
+	btScalar		m_additionalAngularDampingFactor;
+
+
+	btScalar		m_linearSleepingThreshold;
+	btScalar		m_angularSleepingThreshold;
 
 	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
 	btMotionState*	m_optionalMotionState;
@@ -63,12 +74,85 @@ class btRigidBody  : public btCollisionObject
 
 public:
 
-#ifdef OBSOLETE_MOTIONSTATE_LESS
-	//not supported, please use btMotionState
-	btRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.));
-#endif //OBSOLETE_MOTIONSTATE_LESS
 
-	btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.));
+	///btRigidBodyConstructionInfo provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
+	///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
+	///You can use the motion state to synchronize the world transform between physics and graphics objects. 
+	///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
+	///m_startWorldTransform is only used when you don't provide a motion state.
+	struct	btRigidBodyConstructionInfo
+	{
+		btScalar			m_mass;
+
+		///When a motionState is provided, the rigid body will initialize its world transform from the motion state
+		///In this case, m_startWorldTransform is ignored.
+		btMotionState*		m_motionState;
+		btTransform	m_startWorldTransform;
+
+		btCollisionShape*	m_collisionShape;
+		btVector3			m_localInertia;
+		btScalar			m_linearDamping;
+		btScalar			m_angularDamping;
+
+		///best simulation results when friction is non-zero
+		btScalar			m_friction;
+		///best simulation results using zero restitution.
+		btScalar			m_restitution;
+
+		btScalar			m_linearSleepingThreshold;
+		btScalar			m_angularSleepingThreshold;
+
+		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+		bool				m_additionalDamping;
+		btScalar			m_additionalDampingFactor;
+		btScalar			m_additionalLinearDampingThresholdSqr;
+		btScalar			m_additionalAngularDampingThresholdSqr;
+		btScalar			m_additionalAngularDampingFactor;
+
+		
+		btRigidBodyConstructionInfo(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
+		m_mass(mass),
+			m_motionState(motionState),
+			m_collisionShape(collisionShape),
+			m_localInertia(localInertia),
+			m_linearDamping(btScalar(0.)),
+			m_angularDamping(btScalar(0.)),
+			m_friction(btScalar(0.5)),
+			m_restitution(btScalar(0.)),
+			m_linearSleepingThreshold(btScalar(0.8)),
+			m_angularSleepingThreshold(btScalar(1.f)),
+			m_additionalDamping(false),
+			m_additionalDampingFactor(btScalar(0.005)),
+			m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
+			m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
+			m_additionalAngularDampingFactor(btScalar(0.01))
+		{
+			m_startWorldTransform.setIdentity();
+		}
+	};
+
+	///btRigidBody constructor using construction info
+	btRigidBody(	const btRigidBodyConstructionInfo& constructionInfo);
+
+	///btRigidBody constructor for backwards compatibility. 
+	///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
+	btRigidBody(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
+
+
+	virtual ~btRigidBody()
+        { 
+                //No constraints should point to this rigidbody
+		//Remove constraints from the dynamics world before you delete the related rigidbodies. 
+                btAssert(m_constraintRefs.size()==0); 
+        }
+
+protected:
+
+	///setupRigidBody is only used internally by the constructor
+	void	setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
+
+public:
 
 	void			proceedToTransform(const btTransform& newTrans); 
 	
@@ -76,11 +160,15 @@ public:
 	///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
 	static const btRigidBody*	upcast(const btCollisionObject* colObj)
 	{
-		return (const btRigidBody*)colObj->getInternalOwner();
+		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+			return (const btRigidBody*)colObj;
+		return 0;
 	}
 	static btRigidBody*	upcast(btCollisionObject* colObj)
 	{
-		return (btRigidBody*)colObj->getInternalOwner();
+		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+			return (btRigidBody*)colObj;
+		return 0;
 	}
 	
 	/// continuous collision detection needs prediction
@@ -88,8 +176,7 @@ public:
 	
 	void			saveKinematicState(btScalar step);
 	
-
-	void			applyForces(btScalar step);
+	void			applyGravity();
 	
 	void			setGravity(const btVector3& acceleration);  
 
@@ -99,12 +186,34 @@ public:
 	}
 
 	void			setDamping(btScalar lin_damping, btScalar ang_damping);
-	
-	inline const btCollisionShape*	getCollisionShape() const {
+
+	btScalar getLinearDamping() const
+	{
+		return m_linearDamping;
+	}
+
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+
+	btScalar getLinearSleepingThreshold() const
+	{
+		return m_linearSleepingThreshold;
+	}
+
+	btScalar getAngularSleepingThreshold() const
+	{
+		return m_angularSleepingThreshold;
+	}
+
+	void			applyDamping(btScalar timeStep);
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const {
 		return m_collisionShape;
 	}
 
-	inline btCollisionShape*	getCollisionShape() {
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape() {
 			return m_collisionShape;
 	}
 	
@@ -134,6 +243,12 @@ public:
 		m_invInertiaLocal = diagInvInertia;
 	}
 
+	void	setSleepingThresholds(btScalar linear,btScalar angular)
+	{
+		m_linearSleepingThreshold = linear;
+		m_angularSleepingThreshold = angular;
+	}
+
 	void	applyTorque(const btVector3& torque)
 	{
 		m_totalTorque += torque;
@@ -142,7 +257,7 @@ public:
 	void	applyForce(const btVector3& force, const btVector3& rel_pos) 
 	{
 		applyCentralForce(force);
-		applyTorque(rel_pos.cross(force));
+		applyTorque(rel_pos.cross(force)*m_angularFactor);
 	}
 	
 	void applyCentralImpulse(const btVector3& impulse)
@@ -168,7 +283,7 @@ public:
 	}
 
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
 	{
 		if (m_inverseMass != btScalar(0.))
 		{
@@ -238,7 +353,7 @@ public:
 
 
 	
-	inline btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
+	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
 	{
 		btVector3 r0 = pos - getCenterOfMassPosition();
 
@@ -250,19 +365,19 @@ public:
 
 	}
 
-	inline btScalar computeAngularImpulseDenominator(const btVector3& axis) const
+	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
 	{
 		btVector3 vec = axis * getInvInertiaTensorWorld();
 		return axis.dot(vec);
 	}
 
-	inline void	updateDeactivation(btScalar timeStep)
+	SIMD_FORCE_INLINE void	updateDeactivation(btScalar timeStep)
 	{
 		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
 			return;
 
-		if ((getLinearVelocity().length2() < gLinearSleepingThreshold*gLinearSleepingThreshold) &&
-			(getAngularVelocity().length2() < gAngularSleepingThreshold*gAngularSleepingThreshold))
+		if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
+			(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
 		{
 			m_deactivationTime += timeStep;
 		} else
@@ -273,7 +388,7 @@ public:
 
 	}
 
-	inline bool	wantsSleeping()
+	SIMD_FORCE_INLINE bool	wantsSleeping()
 	{
 
 		if (getActivationState() == DISABLE_DEACTIVATION)
@@ -348,6 +463,16 @@ public:
 	void addConstraintRef(btTypedConstraint* c);
 	void removeConstraintRef(btTypedConstraint* c);
 
+	btTypedConstraint* getConstraintRef(int index)
+	{
+		return m_constraintRefs[index];
+	}
+
+	int getNumConstraintRefs()
+	{
+		return m_constraintRefs.size();
+	}
+
 	int	m_debugBodyId;
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index 4ebcb8e7517..3be04d1a4ad 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -27,16 +27,19 @@ subject to the following restrictions:
   can be used by probes that are checking whether the
   library is actually installed.
 */
-extern "C" void btBulletDynamicsProbe () {}
+extern "C" 
+{
+	void btBulletDynamicsProbe ();
+	void btBulletDynamicsProbe () {}
+}
 
 
 
 
-btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
-:btDynamicsWorld(dispatcher,pairCache),
+btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
 m_constraintSolver(constraintSolver),
 m_ownsConstraintSolver(false),
-m_debugDrawer(0),
 m_gravity(0,0,-10)
 {
 
@@ -46,7 +49,7 @@ m_gravity(0,0,-10)
 btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
 {
 	if (m_ownsConstraintSolver)
-		delete m_constraintSolver;
+		btAlignedFree( m_constraintSolver);
 }
 
 int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
@@ -74,8 +77,9 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 		
 		btContactSolverInfo infoGlobal;
 		infoGlobal.m_timeStep = timeStep;
-		
-		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc);
+		m_constraintSolver->prepareSolve(0,numManifolds);
+		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
+		m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
 	}
 
 	///integrate transforms
@@ -85,10 +89,27 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 
 	synchronizeMotionStates();
 
+	clearForces();
+
 	return 1;
 
 }
 
+void	btSimpleDynamicsWorld::clearForces()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->clearForces();
+		}
+	}
+}	
+
 
 void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 {
@@ -104,6 +125,11 @@ void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 	}
 }
 
+btVector3 btSimpleDynamicsWorld::getGravity () const
+{
+	return m_gravity;
+}
+
 void	btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
 	removeCollisionObject(body);
@@ -133,7 +159,7 @@ void	btSimpleDynamicsWorld::updateAabbs()
 				btPoint3 minAabb,maxAabb;
 				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
 				btBroadphaseInterface* bp = getBroadphase();
-				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
+				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
 			}
 		}
 	}
@@ -171,8 +197,9 @@ void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 			{
 				if (body->isActive())
 				{
-					body->applyForces( timeStep);
+					body->applyGravity();
 					body->integrateVelocities( timeStep);
+					body->applyDamping(timeStep);
 					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
 				}
 			}
@@ -204,8 +231,13 @@ void	btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
 	{
-		delete m_constraintSolver;
+		btAlignedFree(m_constraintSolver);
 	}
 	m_ownsConstraintSolver = false;
 	m_constraintSolver = solver;
 }
+
+btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
+{
+	return m_constraintSolver;
+}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
index 25f4ccd8e68..5c56fdf1327 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
@@ -22,11 +22,8 @@ class btDispatcher;
 class btOverlappingPairCache;
 class btConstraintSolver;
 
-///btSimpleDynamicsWorld demonstrates very basic usage of Bullet rigid body dynamics
-///It can be used for basic simulations, and as a starting point for porting Bullet
-///btSimpleDynamicsWorld lacks object deactivation, island management and other concepts.
-///For more complicated simulations, btDiscreteDynamicsWorld and btContinuousDynamicsWorld are recommended
-///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
+///btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
+///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished).
 class btSimpleDynamicsWorld : public btDynamicsWorld
 {
 protected:
@@ -35,8 +32,6 @@ protected:
 
 	bool	m_ownsConstraintSolver;
 
-	btIDebugDraw*	m_debugDrawer;
-
 	void	predictUnconstraintMotion(btScalar timeStep);
 	
 	void	integrateTransforms(btScalar timeStep);
@@ -48,25 +43,17 @@ public:
 
 
 	///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
-	btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver);
+	btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btSimpleDynamicsWorld();
 		
 	///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
 	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
 
-	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer) 
-	{
-		m_debugDrawer = debugDrawer;
-	};
-
-	virtual btIDebugDraw*	getDebugDrawer()
-	{
-		return m_debugDrawer;
-	}
-
 	virtual void	setGravity(const btVector3& gravity);
 
+	virtual btVector3 getGravity () const;
+
 	virtual void	addRigidBody(btRigidBody* body);
 
 	virtual void	removeRigidBody(btRigidBody* body);
@@ -77,6 +64,15 @@ public:
 
 	virtual void	setConstraintSolver(btConstraintSolver* solver);
 
+	virtual btConstraintSolver* getConstraintSolver();
+
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return BT_SIMPLE_DYNAMICS_WORLD;
+	}
+
+	virtual void	clearForces();
+
 };
 
 #endif //BT_SIMPLE_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index d53de7f3687..fe65245c2a1 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -23,12 +23,11 @@
 
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
-
-
 static btRigidBody s_fixedObject( 0,0,0);
 
 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
-:m_vehicleRaycaster(raycaster),
+: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
+m_vehicleRaycaster(raycaster),
 m_pitchControl(btScalar(0.))
 {
 	m_chassisBody = chassis;
@@ -487,6 +486,7 @@ struct btWheelContactPoint
 
 };
 
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
 btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 {
 
@@ -507,8 +507,8 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 
 	// calculate j that moves us to zero relative velocity
 	j1 = -vrel * contactPoint.m_jacDiagABInv;
-	GEN_set_min(j1, maxImpulse);
-	GEN_set_max(j1, -maxImpulse);
+	btSetMin(j1, maxImpulse);
+	btSetMax(j1, -maxImpulse);
 
 	return j1;
 }
@@ -525,11 +525,10 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 		if (!numWheel)
 			return;
 
-
-		btVector3*	forwardWS = new	btVector3[numWheel];
-		btVector3*	axle = new btVector3[numWheel];
-		btScalar* forwardImpulse = new btScalar[numWheel];
-		btScalar* sideImpulse = new btScalar[numWheel];
+		m_forwardWS.resize(numWheel);
+		m_axle.resize(numWheel);
+		m_forwardImpulse.resize(numWheel);
+		m_sideImpulse.resize(numWheel);
 		
 		int numWheelsOnGround = 0;
 	
@@ -541,8 +540,8 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
 			if (groundObject)
 				numWheelsOnGround++;
-			sideImpulse[i] = btScalar(0.);
-			forwardImpulse[i] = btScalar(0.);
+			m_sideImpulse[i] = btScalar(0.);
+			m_forwardImpulse[i] = btScalar(0.);
 
 		}
 	
@@ -561,25 +560,25 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 					const btTransform& wheelTrans = getWheelTransformWS( i );
 
 					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
-					axle[i] = btVector3(	
+					m_axle[i] = btVector3(	
 						wheelBasis0[0][m_indexRightAxis],
 						wheelBasis0[1][m_indexRightAxis],
 						wheelBasis0[2][m_indexRightAxis]);
 					
 					const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
-					btScalar proj = axle[i].dot(surfNormalWS);
-					axle[i] -= surfNormalWS * proj;
-					axle[i] = axle[i].normalize();
+					btScalar proj = m_axle[i].dot(surfNormalWS);
+					m_axle[i] -= surfNormalWS * proj;
+					m_axle[i] = m_axle[i].normalize();
 					
-					forwardWS[i] = surfNormalWS.cross(axle[i]);
-					forwardWS[i].normalize();
+					m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
+					m_forwardWS[i].normalize();
 
 				
 					resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
 							  *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
-							  btScalar(0.), axle[i],sideImpulse[i],timeStep);
+							  btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
 
-					sideImpulse[i] *= sideFrictionStiffness2;
+					m_sideImpulse[i] *= sideFrictionStiffness2;
 						
 				}
 				
@@ -608,7 +607,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 				{
 					btScalar defaultRollingFrictionImpulse = 0.f;
 					btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
-					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,forwardWS[wheel],maxImpulse);
+					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
 					rollingFriction = calcRollingFriction(contactPt);
 				}
 			}
@@ -618,7 +617,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 
 
 
-			forwardImpulse[wheel] = btScalar(0.);
+			m_forwardImpulse[wheel] = btScalar(0.);
 			m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
 
 			if (groundObject)
@@ -631,10 +630,10 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 				btScalar maximpSquared = maximp * maximpSide;
 			
 
-				forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
+				m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
 
-				btScalar x = (forwardImpulse[wheel] ) * fwdFactor;
-				btScalar y = (sideImpulse[wheel] ) * sideFactor;
+				btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
+				btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
 				
 				btScalar impulseSquared = (x*x + y*y);
 
@@ -658,12 +657,12 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 		{
 			for (int wheel = 0;wheel < getNumWheels(); wheel++)
 			{
-				if (sideImpulse[wheel] != btScalar(0.))
+				if (m_sideImpulse[wheel] != btScalar(0.))
 				{
 					if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
 					{
-						forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
-						sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
+						m_forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
+						m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
 					}
 				}
 			}
@@ -678,11 +677,11 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 				btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
 						m_chassisBody->getCenterOfMassPosition();
 
-				if (forwardImpulse[wheel] != btScalar(0.))
+				if (m_forwardImpulse[wheel] != btScalar(0.))
 				{
-					m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos);
+					m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
 				}
-				if (sideImpulse[wheel] != btScalar(0.))
+				if (m_sideImpulse[wheel] != btScalar(0.))
 				{
 					class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
 
@@ -690,7 +689,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 						groundObject->getCenterOfMassPosition();
 
 					
-					btVector3 sideImp = axle[wheel] * sideImpulse[wheel];
+					btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
 
 					rel_pos[2] *= wheelInfo.m_rollInfluence;
 					m_chassisBody->applyImpulse(sideImp,rel_pos);
@@ -701,10 +700,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 			}
 		}
 
-		delete []forwardWS;
-		delete [] axle;
-		delete[]forwardImpulse;
-		delete[] sideImpulse;
+	
 }
 
 
@@ -716,11 +712,11 @@ void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3&
 
 	m_dynamicsWorld->rayTest(from, to, rayCallback);
 
-	if (rayCallback.HasHit())
+	if (rayCallback.hasHit())
 	{
 		
 		btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
-		if (body)
+        if (body && body->hasContactResponse())
 		{
 			result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
 			result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
index f4249599615..8361dcabe4b 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
@@ -11,11 +11,11 @@
 #ifndef RAYCASTVEHICLE_H
 #define RAYCASTVEHICLE_H
 
-#include "../Dynamics/btRigidBody.h"
-#include "../ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 #include "btVehicleRaycaster.h"
 class btDynamicsWorld;
-#include "../../LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAlignedObjectArray.h"
 #include "btWheelInfo.h"
 
 class btVehicleTuning;
@@ -23,6 +23,12 @@ class btVehicleTuning;
 ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
 class btRaycastVehicle : public btTypedConstraint
 {
+
+		btAlignedObjectArray<btVector3>	m_forwardWS;
+		btAlignedObjectArray<btVector3>	m_axle;
+		btAlignedObjectArray<btScalar>	m_forwardImpulse;
+		btAlignedObjectArray<btScalar>	m_sideImpulse;
+
 public:
 	class btVehicleTuning
 		{
@@ -114,7 +120,7 @@ public:
 	
 	void	updateSuspension(btScalar deltaTime);
 
-	void	updateFriction(btScalar	timeStep);
+	virtual void	updateFriction(btScalar	timeStep);
 
 
 
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
index 64a47fcaada..5112ce6d420 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
@@ -11,7 +11,7 @@
 #ifndef VEHICLE_RAYCASTER_H
 #define VEHICLE_RAYCASTER_H
 
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btVector3.h"
 
 /// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
 struct btVehicleRaycaster
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
index 2e349b3fde4..ac2729f4fd7 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
@@ -11,8 +11,8 @@
 #ifndef WHEEL_INFO_H
 #define WHEEL_INFO_H
 
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
 
 class btRigidBody;