Finally upgraded to latest Bullet subversion, about to release 2.71. Some recent changes in extern/bullet2 need to be re-applied, will check with Benoit. Ray tests in 0_FPS_Template.blend is broken, didn't figure out why yet.

HELP BUILD SYSTEM MAINTAINERS: Please help with updating all build systems: the newly added files need to be added. Note that the src/SoftBody has been added for future extension of real-time soft bodies.

22 files changed, 2575 insertions, 832 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/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/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