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.

36 files changed, 3810 insertions, 1366 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
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
index 248c582dcd8..b28a46e299e 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp
@@ -24,97 +24,287 @@ subject to the following restrictions:
 #include "btBulletDynamicsCommon.h"
 #include "LinearMath/btAlignedAllocator.h"
 
+/*
+	Create and Delete a Physics SDK	
+*/
+
+struct	btPhysicsSdk
+{
+
+//	btDispatcher*				m_dispatcher;
+//	btOverlappingPairCache*		m_pairCache;
+//	btConstraintSolver*			m_constraintSolver
+
+	btVector3	m_worldAabbMin;
+	btVector3	m_worldAabbMax;
+
+
+	//todo: version, hardware/optimization settings etc?
+	btPhysicsSdk()
+		:m_worldAabbMin(-1000,-1000,-1000),
+		m_worldAabbMax(1000,1000,1000)
+	{
+
+	}
 
-#include "LinearMath/btVector3.h"
-#include "LinearMath/btScalar.h"	
-#include "LinearMath/btMatrix3x3.h"
-#include "LinearMath/btTransform.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/CollisionShapes/btTriangleShape.h"
-
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
-#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
-#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
-#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-
-#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
-#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
-#include "LinearMath/btStackAlloc.h"
-
-extern "C"
-double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3])
-{
-	btVector3 vp(p1[0], p1[1], p1[2]);
-	btTriangleShape trishapeA(vp, 
-				  btVector3(p2[0], p2[1], p2[2]), 
-				  btVector3(p3[0], p3[1], p3[2]));
-	trishapeA.setMargin(0.000001f);
-	btVector3 vq(q1[0], q1[1], q1[2]);
-	btTriangleShape trishapeB(vq, 
-				  btVector3(q2[0], q2[1], q2[2]), 
-				  btVector3(q3[0], q3[1], q3[2]));
-	trishapeB.setMargin(0.000001f);
-	
-	// btVoronoiSimplexSolver sGjkSimplexSolver;
-	// btGjkEpaPenetrationDepthSolver penSolverPtr;	
-	
-	static btSimplexSolverInterface sGjkSimplexSolver;
-	sGjkSimplexSolver.reset();
-	
-	static btGjkEpaPenetrationDepthSolver Solver0;
-	static btMinkowskiPenetrationDepthSolver Solver1;
-		
-	btConvexPenetrationDepthSolver* Solver = NULL;
-	
-	Solver = &Solver1;	
-		
-	btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver);
-	
-	convexConvex.m_catchDegeneracies = 1;
-	
-	// btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0);
-	
-	btPointCollector gjkOutput;
-	btGjkPairDetector::ClosestPointInput input;
-	
-	btStackAlloc gStackAlloc(1024*1024*2);
- 
-	input.m_stackAlloc = &gStackAlloc;
-	
-	btTransform tr;
-	tr.setIdentity();
-	
-	input.m_transformA = tr;
-	input.m_transformB = tr;
-	
-	convexConvex.getClosestPoints(input, gjkOutput, 0);
-	
 	
-	if (gjkOutput.m_hasResult)
+};
+
+plPhysicsSdkHandle	plNewBulletSdk()
+{
+	void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16);
+	return (plPhysicsSdkHandle)new (mem)btPhysicsSdk;
+}
+
+void		plDeletePhysicsSdk(plPhysicsSdkHandle	physicsSdk)
+{
+	btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk);
+	btAlignedFree(phys);	
+}
+
+
+/* Dynamics World */
+plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle)
+{
+	btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle);
+	void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16);
+	btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration();
+	mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16);
+	btDispatcher*				dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration);
+	mem = btAlignedAlloc(sizeof(btAxisSweep3),16);
+	btBroadphaseInterface*		pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax);
+	mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+	btConstraintSolver*			constraintSolver = new(mem) btSequentialImpulseConstraintSolver();
+
+	mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16);
+	return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration);
+}
+void           plDeleteDynamicsWorld(plDynamicsWorldHandle world)
+{
+	//todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	btAlignedFree(dynamicsWorld);
+}
+
+void	plStepSimulation(plDynamicsWorldHandle world,	plReal	timeStep)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	dynamicsWorld->stepSimulation(timeStep);
+}
+
+void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	assert(body);
+
+	dynamicsWorld->addRigidBody(body);
+}
+
+void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object)
+{
+	btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world);
+	assert(dynamicsWorld);
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	assert(body);
+
+	dynamicsWorld->removeRigidBody(body);
+}
+
+/* Rigid Body  */
+
+plRigidBodyHandle plCreateRigidBody(	void* user_data,  float mass, plCollisionShapeHandle cshape )
+{
+	btTransform trans;
+	trans.setIdentity();
+	btVector3 localInertia(0,0,0);
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	if (mass)
 	{
-		
-		pb[0] = pa[0] = gjkOutput.m_pointInWorld[0];
-		pb[1] = pa[1] = gjkOutput.m_pointInWorld[1];
-		pb[2] = pa[2] = gjkOutput.m_pointInWorld[2];
-
-		pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance;
-		pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance;
-		pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance;
-		
-		normal[0] = gjkOutput.m_normalOnBInWorld[0];
-		normal[1] = gjkOutput.m_normalOnBInWorld[1];
-		normal[2] = gjkOutput.m_normalOnBInWorld[2];
-
-		return gjkOutput.m_distance;
+		shape->calculateLocalInertia(mass,localInertia);
 	}
-	return -1.0f;	
+	void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
+	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
+	btRigidBody* body = new (mem)btRigidBody(rbci);
+	body->setWorldTransform(trans);
+	body->setUserPointer(user_data);
+	return (plRigidBodyHandle) body;
 }
+
+void plDeleteRigidBody(plRigidBodyHandle cbody)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody);
+	assert(body);
+	btAlignedFree( body);
+}
+
+
+/* Collision Shape definition */
+
+plCollisionShapeHandle plNewSphereShape(plReal radius)
+{
+	void* mem = btAlignedAlloc(sizeof(btSphereShape),16);
+	return (plCollisionShapeHandle) new (mem)btSphereShape(radius);
+	
+}
+	
+plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z)
+{
+	void* mem = btAlignedAlloc(sizeof(btBoxShape),16);
+	return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z));
+}
+
+plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height)
+{
+	//capsule is convex hull of 2 spheres, so use btMultiSphereShape
+	btVector3 inertiaHalfExtents(radius,height,radius);
+	const int numSpheres = 2;
+	btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)};
+	btScalar radi[numSpheres] = {radius,radius};
+	void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16);
+	return (plCollisionShapeHandle) new (mem)btMultiSphereShape(inertiaHalfExtents,positions,radi,numSpheres);
+}
+plCollisionShapeHandle plNewConeShape(plReal radius, plReal height)
+{
+	void* mem = btAlignedAlloc(sizeof(btConeShape),16);
+	return (plCollisionShapeHandle) new (mem)btConeShape(radius,height);
+}
+
+plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height)
+{
+	void* mem = btAlignedAlloc(sizeof(btCylinderShape),16);
+	return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius));
+}
+
+/* Convex Meshes */
+plCollisionShapeHandle plNewConvexHullShape()
+{
+	void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16);
+	return (plCollisionShapeHandle) new (mem)btConvexHullShape();
+}
+
+
+/* Concave static triangle meshes */
+plMeshInterfaceHandle		   plNewMeshInterface()
+{
+	return 0;
+}
+
+plCollisionShapeHandle plNewCompoundShape()
+{
+	void* mem = btAlignedAlloc(sizeof(btCompoundShape),16);
+	return (plCollisionShapeHandle) new (mem)btCompoundShape();
+}
+
+void	plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn)
+{
+	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle);
+	btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE);
+	btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape);
+	btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle);
+	btTransform	localTrans;
+	localTrans.setIdentity();
+	localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2]));
+	localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3]));
+	compoundShape->addChildShape(localTrans,childShape);
+}
+
+void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient)
+{
+	btQuaternion orn;
+	orn.setEuler(yaw,pitch,roll);
+	orient[0] = orn.getX();
+	orient[1] = orn.getY();
+	orient[2] = orn.getZ();
+	orient[3] = orn.getW();
+
+}
+
+
+//	extern  void		plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2);
+//	extern  plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle);
+
+
+void		plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z)
+{
+	btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape);
+	(void)colShape;
+	btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE);
+	btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape);
+	convexHullShape->addPoint(btPoint3(x,y,z));
+
+}
+
+void plDeleteShape(plCollisionShapeHandle cshape)
+{
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	btAlignedFree(shape);
+}
+void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling)
+{
+	btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
+	assert(shape);
+	btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]);
+	shape->setLocalScaling(scaling);	
+}
+
+
+
+void plSetPosition(plRigidBodyHandle object, const plVector3 position)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	btVector3 pos(position[0],position[1],position[2]);
+	btTransform worldTrans = body->getWorldTransform();
+	worldTrans.setOrigin(pos);
+	body->setWorldTransform(worldTrans);
+}
+
+void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]);
+	btTransform worldTrans = body->getWorldTransform();
+	worldTrans.setRotation(orn);
+	body->setWorldTransform(worldTrans);
+}
+
+void	plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	body->getWorldTransform().getOpenGLMatrix(matrix);
+
+}
+
+void	plGetPosition(plRigidBodyHandle object,plVector3 position)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	const btVector3& pos = body->getWorldTransform().getOrigin();
+	position[0] = pos.getX();
+	position[1] = pos.getY();
+	position[2] = pos.getZ();
+}
+
+void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation)
+{
+	btRigidBody* body = reinterpret_cast< btRigidBody* >(object);
+	btAssert(body);
+	const btQuaternion& orn = body->getWorldTransform().getRotation();
+	orientation[0] = orn.getX();
+	orientation[1] = orn.getY();
+	orientation[2] = orn.getZ();
+	orientation[3] = orn.getW();
+}
+
+
+
+//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
+
+//	extern  plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal);
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
new file mode 100644
index 00000000000..19443adc723
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp
@@ -0,0 +1,193 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "btContinuousDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//collision detection
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
+#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
+#include "BulletCollision/CollisionShapes/btCollisionShape.h"
+#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
+
+//rigidbody & constraints
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
+
+
+
+#include <stdio.h>
+
+btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration)
+{
+}
+
+btContinuousDynamicsWorld::~btContinuousDynamicsWorld()
+{
+}
+
+	
+void	btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
+{
+	
+	startProfiling(timeStep);
+	
+
+	///update aabbs information
+	updateAabbs();
+	//static int frame=0;
+//	printf("frame %d\n",frame++);
+
+	///apply gravity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	btDispatcherInfo& dispatchInfo = getDispatchInfo();
+
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
+	///perform collision detection
+	performDiscreteCollisionDetection();
+
+	calculateSimulationIslands();
+
+	
+	getSolverInfo().m_timeStep = timeStep;
+	
+
+
+	///solve contact and other joint constraints
+	solveConstraints(getSolverInfo());
+	
+	///CallbackTriggers();
+	calculateTimeOfImpacts(timeStep);
+
+	btScalar toi = dispatchInfo.m_timeOfImpact;
+//	if (toi < 1.f)
+//		printf("toi = %f\n",toi);
+	if (toi < 0.f)
+		printf("toi = %f\n",toi);
+
+
+	///integrate transforms
+	integrateTransforms(timeStep * toi);
+
+	///update vehicle simulation
+	updateVehicles(timeStep);
+
+
+	updateActivationState( timeStep );
+	
+	if(0 != m_internalTickCallback) {
+		(*m_internalTickCallback)(this, timeStep);
+	}
+}
+
+void	btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep)
+{
+		///these should be 'temporal' aabbs!
+		updateTemporalAabbs(timeStep);
+		
+		///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually.
+		///so we handle the case moving versus static properly, and we cheat for moving versus moving
+		btScalar toi = 1.f;
+		
+	
+		btDispatcherInfo& dispatchInfo = getDispatchInfo();
+		dispatchInfo.m_timeStep = timeStep;
+		dispatchInfo.m_timeOfImpact = 1.f;
+		dispatchInfo.m_stepCount = 0;
+		dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS;
+
+		///calculate time of impact for overlapping pairs
+
+
+		btDispatcher* dispatcher = getDispatcher();
+		if (dispatcher)
+			dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
+
+		toi = dispatchInfo.m_timeOfImpact;
+
+		dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE;
+
+}
+
+void	btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep)
+{
+
+	btVector3 temporalAabbMin,temporalAabbMax;
+
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax);
+			const btVector3& linvel = body->getLinearVelocity();
+
+			//make the AABB temporal
+			btScalar temporalAabbMaxx = temporalAabbMax.getX();
+			btScalar temporalAabbMaxy = temporalAabbMax.getY();
+			btScalar temporalAabbMaxz = temporalAabbMax.getZ();
+			btScalar temporalAabbMinx = temporalAabbMin.getX();
+			btScalar temporalAabbMiny = temporalAabbMin.getY();
+			btScalar temporalAabbMinz = temporalAabbMin.getZ();
+
+			// add linear motion
+			btVector3 linMotion = linvel*timeStep;
+		
+			if (linMotion.x() > 0.f)
+				temporalAabbMaxx += linMotion.x(); 
+			else
+				temporalAabbMinx += linMotion.x();
+			if (linMotion.y() > 0.f)
+				temporalAabbMaxy += linMotion.y(); 
+			else
+				temporalAabbMiny += linMotion.y();
+			if (linMotion.z() > 0.f)
+				temporalAabbMaxz += linMotion.z(); 
+			else
+				temporalAabbMinz += linMotion.z();
+
+			//add conservative angular motion
+			btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep;
+			btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
+			temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
+			temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
+
+			temporalAabbMin -= angularMotion3d;
+			temporalAabbMax += angularMotion3d;
+
+			m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1);
+		}
+	}
+
+	//update aabb (of all moved objects)
+
+	m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
+	
+
+
+}
+
+
+
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h
new file mode 100644
index 00000000000..61c8dea03eb
--- /dev/null
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h
@@ -0,0 +1,46 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef BT_CONTINUOUS_DYNAMICS_WORLD_H
+#define BT_CONTINUOUS_DYNAMICS_WORLD_H
+
+#include "btDiscreteDynamicsWorld.h"
+
+///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld.
+///This copes with fast moving objects that otherwise would tunnel/miss collisions.
+///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead.
+class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld
+{
+
+	void	updateTemporalAabbs(btScalar timeStep);
+
+	public:
+
+		btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
+		virtual ~btContinuousDynamicsWorld();
+		
+		///time stepping with calculation of time of impact for selected fast moving objects
+		virtual void	internalSingleStepSimulation( btScalar timeStep);
+
+		virtual void	calculateTimeOfImpacts(btScalar timeStep);
+
+		virtual btDynamicsWorldType	getWorldType() const
+		{
+			return BT_CONTINUOUS_DYNAMICS_WORLD;
+		}
+
+};
+
+#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 1017c8af6ea..c7b1af245e9 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -21,7 +21,8 @@ subject to the following restrictions:
 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
-#include <LinearMath/btTransformUtil.h>
+#include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btQuickprof.h"
 
 //rigidbody & constraints
 #include "BulletDynamics/Dynamics/btRigidBody.h"
@@ -41,6 +42,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btSphereShape.h"
 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
+#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
 #include "LinearMath/btIDebugDraw.h"
 
 
@@ -57,17 +59,29 @@ subject to the following restrictions:
 
 
 
-btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
-:btDynamicsWorld(dispatcher,pairCache),
-m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
-m_debugDrawer(0),
+btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
+m_constraintSolver(constraintSolver),
 m_gravity(0,-10,0),
 m_localTime(btScalar(1.)/btScalar(60.)),
 m_profileTimings(0)
 {
-	m_islandManager = new btSimulationIslandManager();
+	if (!m_constraintSolver)
+	{
+		void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
+		m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
+		m_ownsConstraintSolver = true;
+	} else
+	{
+		m_ownsConstraintSolver = false;
+	}
+
+	{
+		void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
+		m_islandManager = new (mem) btSimulationIslandManager();
+	}
+
 	m_ownsIslandManager = true;
-	m_ownsConstraintSolver = (constraintSolver==0);
 }
 
 
@@ -75,9 +89,16 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
 {
 	//only delete it when we created it
 	if (m_ownsIslandManager)
-		delete m_islandManager;
+	{
+		m_islandManager->~btSimulationIslandManager();
+		btAlignedFree( m_islandManager);
+	}
 	if (m_ownsConstraintSolver)
-		 delete m_constraintSolver;
+	{
+
+		m_constraintSolver->~btConstraintSolver();
+		btAlignedFree(m_constraintSolver);
+	}
 }
 
 void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
@@ -102,14 +123,35 @@ void	btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
 	}
 }
 
-void	btDiscreteDynamicsWorld::synchronizeMotionStates()
+void	btDiscreteDynamicsWorld::debugDrawWorld()
 {
-	//debug vehicle wheels
-	
-	
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
+	{
+		int numManifolds = getDispatcher()->getNumManifolds();
+		btVector3 color(0,0,0);
+		for (int i=0;i<numManifolds;i++)
+		{
+			btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
+			//btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+			//btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+
+			int numContacts = contactManifold->getNumContacts();
+			for (int j=0;j<numContacts;j++)
+			{
+				btManifoldPoint& cp = contactManifold->getContactPoint(j);
+				getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
+			}
+		}
+	}
+
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
 	{
+		int i;
+
 		//todo: iterate over awake simulation islands!
-		for ( int i=0;i<m_collisionObjects.size();i++)
+		for (  i=0;i<m_collisionObjects.size();i++)
 		{
 			btCollisionObject* colObj = m_collisionObjects[i];
 			if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
@@ -135,26 +177,17 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 
 				debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
 			}
-			btRigidBody* body = btRigidBody::upcast(colObj);
-			if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 			{
-				//we need to call the update at least once, even for sleeping objects
-				//otherwise the 'graphics' transform never updates properly
-				//so todo: add 'dirty' flag
-				//if (body->getActivationState() != ISLAND_SLEEPING)
-				{
-					btTransform interpolatedTransform;
-					btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-						body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
-					body->getMotionState()->setWorldTransform(interpolatedTransform);
-				}
+				btPoint3 minAabb,maxAabb;
+				btVector3 colorvec(1,0,0);
+				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
+				m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
 			}
-		}
-	}
 
-	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
-	{
-		for ( int i=0;i<this->m_vehicles.size();i++)
+		}
+	
+		for (  i=0;i<this->m_vehicles.size();i++)
 		{
 			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
 			{
@@ -166,10 +199,7 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 				{
 					wheelColor.setValue(255,0,255);
 				}
-
-				//synchronize the wheels with the (interpolated) chassis worldtransform
-				m_vehicles[i]->updateWheelTransform(v,true);
-					
+		
 				btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
 
 				btVector3 axle = btVector3(	
@@ -186,12 +216,87 @@ void	btDiscreteDynamicsWorld::synchronizeMotionStates()
 			}
 		}
 	}
+}
+
+void	btDiscreteDynamicsWorld::clearForces()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->clearForces();
+		}
+	}
+}	
+
+///apply gravity, call this once per timestep
+void	btDiscreteDynamicsWorld::applyGravity()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body && body->isActive())
+		{
+			body->applyGravity();
+		}
+	}
+}
+
+
+
+void	btDiscreteDynamicsWorld::synchronizeMotionStates()
+{
+	{
+		//todo: iterate over awake simulation islands!
+		for ( int i=0;i<m_collisionObjects.size();i++)
+		{
+			btCollisionObject* colObj = m_collisionObjects[i];
+			
+			btRigidBody* body = btRigidBody::upcast(colObj);
+			if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
+			{
+				//we need to call the update at least once, even for sleeping objects
+				//otherwise the 'graphics' transform never updates properly
+				//so todo: add 'dirty' flag
+				//if (body->getActivationState() != ISLAND_SLEEPING)
+				{
+					btTransform interpolatedTransform;
+					btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
+						body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
+					body->getMotionState()->setWorldTransform(interpolatedTransform);
+				}
+			}
+		}
+	}
+
+	if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
+	{
+		for ( int i=0;i<this->m_vehicles.size();i++)
+		{
+			for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
+			{
+				//synchronize the wheels with the (interpolated) chassis worldtransform
+				m_vehicles[i]->updateWheelTransform(v,true);
+			}
+		}
+	}
 
 }
 
 
 int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
 {
+	startProfiling(timeStep);
+
+	BT_PROFILE("stepSimulation");
+
 	int numSimulationSubSteps = 0;
 
 	if (maxSubSteps)
@@ -229,6 +334,8 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 
 		saveKinematicState(fixedTimeStep);
 
+		applyGravity();
+
 		//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
 		int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
 
@@ -242,16 +349,19 @@ int	btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps,
 
 	synchronizeMotionStates();
 
+	clearForces();
+
+#ifndef BT_NO_PROFILE
+	CProfileManager::Increment_Frame_Counter();
+#endif //BT_NO_PROFILE
+	
 	return numSimulationSubSteps;
 }
 
 void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
 	
-	startProfiling(timeStep);
-
-	///update aabbs information
-	updateAabbs();
+	BT_PROFILE("internalSingleStepSimulation");
 
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
@@ -286,8 +396,9 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 
 	updateActivationState( timeStep );
 
-	
-
+	if(0 != m_internalTickCallback) {
+		(*m_internalTickCallback)(this, timeStep);
+	}	
 }
 
 void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
@@ -304,6 +415,11 @@ void	btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
 	}
 }
 
+btVector3 btDiscreteDynamicsWorld::getGravity () const
+{
+	return m_gravity;
+}
+
 
 void	btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
@@ -343,19 +459,18 @@ void	btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short
 
 void	btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
 {
-	BEGIN_PROFILE("updateVehicles");
-
+	BT_PROFILE("updateVehicles");
+	
 	for ( int i=0;i<m_vehicles.size();i++)
 	{
 		btRaycastVehicle* vehicle = m_vehicles[i];
 		vehicle->updateVehicle( timeStep);
 	}
-	END_PROFILE("updateVehicles");
 }
 
 void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 {
-	BEGIN_PROFILE("updateActivationState");
+	BT_PROFILE("updateActivationState");
 
 	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
@@ -374,6 +489,12 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 				{
 					if (body->getActivationState() == ACTIVE_TAG)
 						body->setActivationState( WANTS_DEACTIVATION );
+					if (body->getActivationState() == ISLAND_SLEEPING) 
+					{
+						body->setAngularVelocity(btVector3(0,0,0));
+						body->setLinearVelocity(btVector3(0,0,0));
+					}
+
 				}
 			} else
 			{
@@ -382,7 +503,6 @@ void	btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
 			}
 		}
 	}
-	END_PROFILE("updateActivationState");
 }
 
 void	btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
@@ -412,7 +532,7 @@ void	btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
 	m_vehicles.remove(vehicle);
 }
 
-inline	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
+SIMD_FORCE_INLINE	int	btGetConstraintIslandId(const btTypedConstraint* lhs)
 {
 	int islandId;
 	
@@ -442,6 +562,7 @@ class btSortConstraintOnIslandPredicate
 
 void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 {
+	BT_PROFILE("solveConstraints");
 	
 	struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 	{
@@ -452,7 +573,7 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		int						m_numConstraints;
 		btIDebugDraw*			m_debugDrawer;
 		btStackAlloc*			m_stackAlloc;
-
+		btDispatcher*			m_dispatcher;
 
 		InplaceSolverIslandCallback(
 			btContactSolverInfo& solverInfo,
@@ -460,13 +581,15 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 			btTypedConstraint** sortedConstraints,
 			int	numConstraints,
 			btIDebugDraw*	debugDrawer,
-			btStackAlloc*			stackAlloc)
+			btStackAlloc*			stackAlloc,
+			btDispatcher* dispatcher)
 			:m_solverInfo(solverInfo),
 			m_solver(solver),
 			m_sortedConstraints(sortedConstraints),
 			m_numConstraints(numConstraints),
 			m_debugDrawer(debugDrawer),
-			m_stackAlloc(stackAlloc)
+			m_stackAlloc(stackAlloc),
+			m_dispatcher(dispatcher)
 		{
 
 		}
@@ -479,30 +602,42 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		}
 		virtual	void	ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**	manifolds,int numManifolds, int islandId)
 		{
-			//also add all non-contact constraints/joints for this island
-			btTypedConstraint** startConstraint = 0;
-			int numCurConstraints = 0;
-			int i;
-			
-			//find the first constraint for this island
-			for (i=0;i<m_numConstraints;i++)
+			if (islandId<0)
+			{
+				///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+				m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+			} else
 			{
-				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+					//also add all non-contact constraints/joints for this island
+				btTypedConstraint** startConstraint = 0;
+				int numCurConstraints = 0;
+				int i;
+				
+				//find the first constraint for this island
+				for (i=0;i<m_numConstraints;i++)
 				{
-					startConstraint = &m_sortedConstraints[i];
-					break;
+					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+					{
+						startConstraint = &m_sortedConstraints[i];
+						break;
+					}
 				}
-			}
-			//count the number of constraints in this island
-			for (;i<m_numConstraints;i++)
-			{
-				if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+				//count the number of constraints in this island
+				for (;i<m_numConstraints;i++)
 				{
-					numCurConstraints++;
+					if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
+					{
+						numCurConstraints++;
+					}
 				}
-			}
 
-			m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc);
+				///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
+				if (numManifolds + numCurConstraints)
+				{
+					m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
+				}
+		
+			}
 		}
 
 	};
@@ -520,18 +655,18 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 		
 	
 
-	sortedConstraints.heapSort(btSortConstraintOnIslandPredicate());
+	sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
 	
 	btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
 	
-	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc);
-
+	InplaceSolverIslandCallback	solverCallback(	solverInfo,	m_constraintSolver, constraintsPtr,sortedConstraints.size(),	m_debugDrawer,m_stackAlloc,m_dispatcher1);
 	
+	m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 	
 	/// solve all the constraints for this island
 	m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
 
-
+	m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
 }
 
 
@@ -539,7 +674,7 @@ void	btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
 void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 {
-	BEGIN_PROFILE("calculateSimulationIslands");
+	BT_PROFILE("calculateSimulationIslands");
 
 	getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
 
@@ -569,66 +704,14 @@ void	btDiscreteDynamicsWorld::calculateSimulationIslands()
 	//Store the island id in each body
 	getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
 
-	END_PROFILE("calculateSimulationIslands");
-
-}
-
-
-void	btDiscreteDynamicsWorld::updateAabbs()
-{
-	BEGIN_PROFILE("updateAabbs");
 	
-	btVector3 colorvec(1,0,0);
-	btTransform predictedTrans;
-	for ( int i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		
-		btRigidBody* body = btRigidBody::upcast(colObj);
-		if (body)
-		{
-		//	if (body->IsActive() && (!body->IsStatic()))
-			{
-				btPoint3 minAabb,maxAabb;
-				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
-				btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
-
-				//moving objects should be moderately sized, probably something wrong if not
-				if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
-				{
-					bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
-				} else
-				{
-					//something went wrong, investigate
-					//this assert is unwanted in 3D modelers (danger of loosing work)
-					body->setActivationState(DISABLE_SIMULATION);
-					
-					static bool reportMe = true;
-					if (reportMe && m_debugDrawer)
-					{
-						reportMe = false;
-						m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
-						m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
-						m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
-						m_debugDrawer->reportErrorWarning("Thanks.\n");
-					}
+}
 
 
-				}
-				if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
-				{
-					m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
-				}
-			}
-		}
-	}
-	
-	END_PROFILE("updateAabbs");
-}
 
 void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
-	BEGIN_PROFILE("integrateTransforms");
+	BT_PROFILE("integrateTransforms");
 	btTransform predictedTrans;
 	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
@@ -643,14 +726,13 @@ void	btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
 			}
 		}
 	}
-	END_PROFILE("integrateTransforms");
 }
 
 
 
 void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 {
-	BEGIN_PROFILE("predictUnconstraintMotion");
+	BT_PROFILE("predictUnconstraintMotion");
 	for ( int i=0;i<m_collisionObjects.size();i++)
 	{
 		btCollisionObject* colObj = m_collisionObjects[i];
@@ -661,51 +743,26 @@ void	btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 			{
 				if (body->isActive())
 				{
-					body->applyForces( timeStep);
 					body->integrateVelocities( timeStep);
+					//damping
+					body->applyDamping(timeStep);
+
 					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
 				}
 			}
 		}
 	}
-	END_PROFILE("predictUnconstraintMotion");
 }
 
 
 void	btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
 {
 	(void)timeStep;
-	#ifdef USE_QUICKPROF
 
+#ifndef BT_NO_PROFILE
+	CProfileManager::Reset();
+#endif //BT_NO_PROFILE
 
-	//toggle btProfiler
-	if ( m_debugDrawer && m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
-	{
-		if (!m_profileTimings)
-		{
-			m_profileTimings = 1;
-			// To disable profiling, simply comment out the following line.
-			static int counter = 0;
-
-			char filename[128];
-			sprintf(filename,"quickprof_bullet_timings%i.csv",counter++);
-			btProfiler::init(filename, btProfiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS
-		} else
-		{
-			btProfiler::endProfilingCycle();
-		}
-
-	} else
-	{
-		if (m_profileTimings)
-		{
-			btProfiler::endProfilingCycle();
-
-			m_profileTimings = 0;
-			btProfiler::destroy();
-		}
-	}
-#endif //USE_QUICKPROF
 }
 
 
@@ -827,27 +884,52 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 
 				btScalar radius = capsuleShape->getRadius();
 				btScalar halfHeight = capsuleShape->getHalfHeight();
+				
+				int upAxis = capsuleShape->getUpAxis();
+
+				
+				btVector3 capStart(0.f,0.f,0.f);
+				capStart[upAxis] = -halfHeight;
+
+				btVector3 capEnd(0.f,0.f,0.f);
+				capEnd[upAxis] = halfHeight;
 
 				// Draw the ends
 				{
+					
 					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
+					childTransform.getOrigin() = worldTransform * capStart;
 					debugDrawSphere(radius, childTransform, color);
 				}
 
 				{
 					btTransform childTransform = worldTransform;
-					childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
+					childTransform.getOrigin() = worldTransform * capEnd;
 					debugDrawSphere(radius, childTransform, color);
 				}
 
 				// Draw some additional lines
 				btVector3 start = worldTransform.getOrigin();
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
-				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
 
+				
+				capStart[(upAxis+1)%3] = radius;
+				capEnd[(upAxis+1)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+				capStart[(upAxis+1)%3] = -radius;
+				capEnd[(upAxis+1)%3] = -radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+				capStart[(upAxis+1)%3] = 0.f;
+				capEnd[(upAxis+1)%3] = 0.f;
+
+				capStart[(upAxis+2)%3] = radius;
+				capEnd[(upAxis+2)%3] = radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+				capStart[(upAxis+2)%3] = -radius;
+				capEnd[(upAxis+2)%3] = -radius;
+				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
+
+				
 				break;
 			}
 		case CONE_SHAPE_PROXYTYPE:
@@ -856,9 +938,10 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
 				btScalar height = coneShape->getHeight();//+coneShape->getMargin();
 				btVector3 start = worldTransform.getOrigin();
-				// insert here Bullet 2.69 that fixes representation of cone
+
 				int upAxis= coneShape->getConeUpIndex();
 				
+
 				btVector3	offsetHeight(0,0,0);
 				offsetHeight[upAxis] = height * btScalar(0.5);
 				btVector3	offsetRadius(0,0,0);
@@ -871,11 +954,8 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
 				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
 
-				// buggy code that does not take into account the direction of the cone
-				//getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color);
-				//getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color);
-				//getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color);
-				//getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color);
+
+
 				break;
 
 			}
@@ -884,7 +964,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
 				int upAxis = cylinder->getUpAxis();
 				btScalar radius = cylinder->getRadius();
-				btScalar halfHeight = cylinder->getHalfExtents()[upAxis];
+				btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
 				btVector3 start = worldTransform.getOrigin();
 				btVector3	offsetHeight(0,0,0);
 				offsetHeight[upAxis] = halfHeight;
@@ -894,6 +974,25 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 				getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
 				break;
 			}
+
+			case STATIC_PLANE_PROXYTYPE:
+				{
+					const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
+					btScalar planeConst = staticPlaneShape->getPlaneConstant();
+					const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
+					btVector3 planeOrigin = planeNormal * planeConst;
+					btVector3 vec0,vec1;
+					btPlaneSpace1(planeNormal,vec0,vec1);
+					btScalar vecLen = 100.f;
+					btVector3 pt0 = planeOrigin + vec0*vecLen;
+					btVector3 pt1 = planeOrigin - vec0*vecLen;
+					btVector3 pt2 = planeOrigin + vec1*vecLen;
+					btVector3 pt3 = planeOrigin - vec1*vecLen;
+					getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
+					getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
+					break;
+
+				}
 		default:
 			{
 
@@ -918,7 +1017,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 					btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
 					//DebugDrawcallback drawCallback;
 					DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
-					convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+					convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
 				}
 
 
@@ -950,12 +1049,18 @@ void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
 	{
-		delete m_constraintSolver;
+		btAlignedFree( m_constraintSolver);
 	}
 	m_ownsConstraintSolver = false;
 	m_constraintSolver = solver;
 }
 
+btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
+{
+	return m_constraintSolver;
+}
+
+
 int		btDiscreteDynamicsWorld::getNumConstraints() const
 {
 	return int(m_constraints.size());
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 83b90bfeebc..d206a604960 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -23,11 +23,11 @@ class btOverlappingPairCache;
 class btConstraintSolver;
 class btSimulationIslandManager;
 class btTypedConstraint;
-#include "../ConstraintSolver/btContactSolverInfo.h"
+
 
 class btRaycastVehicle;
 class btIDebugDraw;
-#include "../../LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 
 ///btDiscreteDynamicsWorld provides discrete rigid body simulation
@@ -42,7 +42,6 @@ protected:
 
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
-	btIDebugDraw*	m_debugDrawer;
 
 	btVector3	m_gravity;
 
@@ -53,14 +52,12 @@ protected:
 	bool	m_ownsIslandManager;
 	bool	m_ownsConstraintSolver;
 
-	btContactSolverInfo	m_solverInfo;
-
-
+	
 	btAlignedObjectArray<btRaycastVehicle*>	m_vehicles;
 
 	int	m_profileTimings;
 
-	void	predictUnconstraintMotion(btScalar timeStep);
+	virtual void	predictUnconstraintMotion(btScalar timeStep);
 	
 	void	integrateTransforms(btScalar timeStep);
 		
@@ -86,14 +83,13 @@ public:
 
 
 	///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
-	btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver);
+	btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btDiscreteDynamicsWorld();
 
 	///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
 	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
 
-	virtual void	updateAabbs();
 
 	void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false);
 
@@ -118,17 +114,9 @@ public:
 		return this;
 	}
 
-	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer)
-	{
-			m_debugDrawer = debugDrawer;
-	}
-
-	virtual btIDebugDraw*	getDebugDrawer()
-	{
-		return m_debugDrawer;
-	}
 
 	virtual void	setGravity(const btVector3& gravity);
+	virtual btVector3 getGravity () const;
 
 	virtual void	addRigidBody(btRigidBody* body);
 
@@ -138,7 +126,11 @@ public:
 
 	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
+	virtual void	debugDrawWorld();
+
 	virtual void	setConstraintSolver(btConstraintSolver* solver);
+
+	virtual btConstraintSolver* getConstraintSolver();
 	
 	virtual	int		getNumConstraints() const;
 
@@ -146,11 +138,21 @@ public:
 
 	virtual const btTypedConstraint* getConstraint(int index) const;
 
-	btContactSolverInfo& getSolverInfo()
+	
+	virtual btDynamicsWorldType	getWorldType() const
 	{
-		return m_solverInfo;
+		return BT_DISCRETE_DYNAMICS_WORLD;
 	}
+	
+	///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep.
+	virtual void	clearForces();
+
+	///apply gravity, call this once per timestep
+	virtual void	applyGravity();
 
+	virtual void	setNumTasks(int numTasks)
+	{
+	}
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 65b63fad4b5..dd9dfa71b7f 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -16,20 +16,39 @@ subject to the following restrictions:
 #ifndef BT_DYNAMICS_WORLD_H
 #define BT_DYNAMICS_WORLD_H
 
-#include "../../BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
+
 class btTypedConstraint;
 class btRaycastVehicle;
 class btConstraintSolver;
+class btDynamicsWorld;
+
+/// Type for the callback for each tick
+typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep);
 
+enum btDynamicsWorldType
+{
+	BT_SIMPLE_DYNAMICS_WORLD=1,
+	BT_DISCRETE_DYNAMICS_WORLD=2,
+	BT_CONTINUOUS_DYNAMICS_WORLD=3
+};
 
-///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous
+///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
 class btDynamicsWorld : public btCollisionWorld
 {
-	public:
+
+protected:
+		btInternalTickCallback m_internalTickCallback;
+		void*	m_worldUserInfo;
+
+		btContactSolverInfo	m_solverInfo;
+
+public:
 		
 
-		btDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache)
-		:btCollisionWorld(dispatcher,pairCache)
+		btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration)
+		:btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0)
 		{
 		}
 
@@ -37,34 +56,37 @@ class btDynamicsWorld : public btCollisionWorld
 		{
 		}
 		
-		///stepSimulation proceeds the simulation over timeStep units
-		///if maxSubSteps > 0, it will interpolate time steps
-		virtual int		stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
+		///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds.
+		///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'.
+		///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'.
+		///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant.
+		virtual int		stepSimulation( btScalar timeStep,int maxSubSteps=10, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0;
 			
-		virtual void	updateAabbs() = 0;
+		virtual void	debugDrawWorld() = 0;
 				
-		virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) { (void)constraint;};
-
-		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;};
-
-		virtual void	addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;};
-
-		virtual void	removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;};
+		virtual void	addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) 
+		{ 
+			(void)constraint; (void)disableCollisionsBetweenLinkedBodies;
+		}
 
+		virtual void	removeConstraint(btTypedConstraint* constraint) {(void)constraint;}
 
-		virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer) = 0;
+		virtual void	addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
 
-		virtual btIDebugDraw*	getDebugDrawer() = 0;
+		virtual void	removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}
 
 		//once a rigidbody is added to the dynamics world, it will get this gravity assigned
 		//existing rigidbodies in the world get gravity assigned too, during this method
 		virtual void	setGravity(const btVector3& gravity) = 0;
+		virtual btVector3 getGravity () const = 0;
 
 		virtual void	addRigidBody(btRigidBody* body) = 0;
 
 		virtual void	removeRigidBody(btRigidBody* body) = 0;
 
 		virtual void	setConstraintSolver(btConstraintSolver* solver) = 0;
+
+		virtual btConstraintSolver* getConstraintSolver() = 0;
 		
 		virtual	int		getNumConstraints() const {	return 0;		}
 		
@@ -72,7 +94,35 @@ class btDynamicsWorld : public btCollisionWorld
 		
 		virtual const btTypedConstraint* getConstraint(int index) const	{	(void)index;	return 0;	}
 
+		virtual btDynamicsWorldType	getWorldType() const=0;
+
+		virtual void	clearForces() = 0;
+
+		/// Set the callback for when an internal tick (simulation substep) happens, optional user info
+		void setInternalTickCallback(btInternalTickCallback cb,	void* worldUserInfo=0) 
+		{ 
+			m_internalTickCallback = cb; 
+			m_worldUserInfo = worldUserInfo;
+		}
+
+		void	setWorldUserInfo(void* worldUserInfo)
+		{
+			m_worldUserInfo = worldUserInfo;
+		}
+
+		void*	getWorldUserInfo() const
+		{
+			return m_worldUserInfo;
+		}
+
+		btContactSolverInfo& getSolverInfo()
+		{
+			return m_solverInfo;
+		}
+
+
 };
 
 #endif //BT_DYNAMICS_WORLD_H
 
+
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
index 9ed3579d89c..e2afb687ac6 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp
@@ -20,36 +20,53 @@ subject to the following restrictions:
 #include "LinearMath/btMotionState.h"
 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 
-btScalar gLinearAirDamping = btScalar(1.);
 //'temporarily' global variables
 btScalar	gDeactivationTime = btScalar(2.);
 bool	gDisableDeactivation = false;
-
-btScalar gLinearSleepingThreshold = btScalar(0.8);
-btScalar gAngularSleepingThreshold = btScalar(1.0);
 static int uniqueId = 0;
 
-btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
-: 
-	m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)),
-	m_angularFactor(btScalar(1.)),
-	m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_linearDamping(btScalar(0.)),
-	m_angularDamping(btScalar(0.5)),
-	m_optionalMotionState(motionState),
-	m_contactSolverType(0),
-	m_frictionSolverType(0)
+
+btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
+{
+	setupRigidBody(constructionInfo);
+}
+
+btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
+{
+	btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
+	setupRigidBody(cinfo);
+}
+
+void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
 {
 
-	if (motionState)
+	m_internalType=CO_RIGID_BODY;
+
+	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+	m_angularFactor = btScalar(1.);
+	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
+	m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
+	m_linearDamping = btScalar(0.);
+	m_angularDamping = btScalar(0.5);
+	m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
+	m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
+	m_optionalMotionState = constructionInfo.m_motionState;
+	m_contactSolverType = 0;
+	m_frictionSolverType = 0;
+	m_additionalDamping = constructionInfo.m_additionalDamping;
+	m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
+	m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
+	m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
+	m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
+
+	if (m_optionalMotionState)
 	{
-		motionState->getWorldTransform(m_worldTransform);
+		m_optionalMotionState->getWorldTransform(m_worldTransform);
 	} else
 	{
-		m_worldTransform = btTransform::getIdentity();
+		m_worldTransform = constructionInfo.m_startWorldTransform;
 	}
 
 	m_interpolationWorldTransform = m_worldTransform;
@@ -57,90 +74,21 @@ btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionS
 	m_interpolationAngularVelocity.setValue(0,0,0);
 	
 	//moved to btCollisionObject
-	m_friction = friction;
-	m_restitution = restitution;
-
-	m_collisionShape = collisionShape;
-	m_debugBodyId = uniqueId++;
-	
-	//m_internalOwner is to allow upcasting from collision object to rigid body
-	m_internalOwner = this;
-
-	setMassProps(mass, localInertia);
-    setDamping(linearDamping, angularDamping);
-	updateInertiaTensor();
-
-}
-
-#ifdef OBSOLETE_MOTIONSTATE_LESS
-btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
-: 
-	m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
-	m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)),
-	m_linearDamping(btScalar(0.)),
-	m_angularDamping(btScalar(0.5)),
-	m_optionalMotionState(0),
-	m_contactSolverType(0),
-	m_frictionSolverType(0)
-{
-	
-	m_worldTransform = worldTransform;
-	m_interpolationWorldTransform = m_worldTransform;
-	m_interpolationLinearVelocity.setValue(0,0,0);
-	m_interpolationAngularVelocity.setValue(0,0,0);
-
-	//moved to btCollisionObject
-	m_friction = friction;
-	m_restitution = restitution;
+	m_friction = constructionInfo.m_friction;
+	m_restitution = constructionInfo.m_restitution;
 
-	m_collisionShape = collisionShape;
+	setCollisionShape( constructionInfo.m_collisionShape );
 	m_debugBodyId = uniqueId++;
 	
-	//m_internalOwner is to allow upcasting from collision object to rigid body
-	m_internalOwner = this;
-
-	setMassProps(mass, localInertia);
-    setDamping(linearDamping, angularDamping);
+	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
+    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
 	updateInertiaTensor();
 
 }
 
-#endif //OBSOLETE_MOTIONSTATE_LESS
-
-
-
-
-//#define EXPERIMENTAL_JITTER_REMOVAL 1
-#ifdef EXPERIMENTAL_JITTER_REMOVAL
-//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
-//doesn't work very well yet (value 0 disabled this damping)
-//note there this influences deactivation thresholds!
-btScalar gClippedAngvelThresholdSqr = btScalar(0.01);
-btScalar	gClippedLinearThresholdSqr = btScalar(0.01);
-#endif //EXPERIMENTAL_JITTER_REMOVAL
-
-btScalar	gJitterVelocityDampingFactor = btScalar(0.7);
 
 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
 {
-
-#ifdef EXPERIMENTAL_JITTER_REMOVAL
-	//if (wantsSleeping())
-	{
-		//clip to avoid jitter
-		if ((m_angularVelocity.length2() < gClippedAngvelThresholdSqr) &&
-			(m_linearVelocity.length2() < gClippedLinearThresholdSqr))
-		{
-			m_angularVelocity *= gJitterVelocityDampingFactor;
-			m_linearVelocity *= gJitterVelocityDampingFactor;
-		}
-	}
-	
-#endif //EXPERIMENTAL_JITTER_REMOVAL
-
 	btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
 }
 
@@ -191,50 +139,63 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 
 
 
-#include <stdio.h>
-
 
-void btRigidBody::applyForces(btScalar step)
+///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
+void			btRigidBody::applyDamping(btScalar timeStep)
 {
-	if (isStaticOrKinematicObject())
-		return;
-	
-	applyCentralForce(m_gravity);	
-	
-	m_linearVelocity *= GEN_clamped((btScalar(1.) - step * gLinearAirDamping * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
-	m_angularVelocity *= GEN_clamped((btScalar(1.) - step * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
+	m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
 
-#define FORCE_VELOCITY_DAMPING 1
-#ifdef FORCE_VELOCITY_DAMPING
-	btScalar speed = m_linearVelocity.length();
-	if (speed < m_linearDamping)
+	if (m_additionalDamping)
 	{
-		btScalar dampVel = btScalar(0.005);
-		if (speed > dampVel)
+		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+		if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
+			(m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
 		{
-			btVector3 dir = m_linearVelocity.normalized();
-			m_linearVelocity -=  dir * dampVel;
-		} else
-		{
-			m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			m_angularVelocity *= m_additionalDampingFactor;
+			m_linearVelocity *= m_additionalDampingFactor;
 		}
-	}
+	
 
-	btScalar angSpeed = m_angularVelocity.length();
-	if (angSpeed < m_angularDamping)
-	{
-		btScalar angDampVel = btScalar(0.005);
-		if (angSpeed > angDampVel)
+		btScalar speed = m_linearVelocity.length();
+		if (speed < m_linearDamping)
 		{
-			btVector3 dir = m_angularVelocity.normalized();
-			m_angularVelocity -=  dir * angDampVel;
-		} else
+			btScalar dampVel = btScalar(0.005);
+			if (speed > dampVel)
+			{
+				btVector3 dir = m_linearVelocity.normalized();
+				m_linearVelocity -=  dir * dampVel;
+			} else
+			{
+				m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			}
+		}
+
+		btScalar angSpeed = m_angularVelocity.length();
+		if (angSpeed < m_angularDamping)
 		{
-			m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			btScalar angDampVel = btScalar(0.005);
+			if (angSpeed > angDampVel)
+			{
+				btVector3 dir = m_angularVelocity.normalized();
+				m_angularVelocity -=  dir * angDampVel;
+			} else
+			{
+				m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
+			}
 		}
 	}
-#endif //FORCE_VELOCITY_DAMPING
+}
+
+
+void btRigidBody::applyGravity()
+{
+	if (isStaticOrKinematicObject())
+		return;
 	
+	applyCentralForce(m_gravity);	
+
 }
 
 void btRigidBody::proceedToTransform(const btTransform& newTrans)
@@ -285,7 +246,6 @@ void btRigidBody::integrateVelocities(btScalar step)
 		m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
 	}
 
-	clearForces();
 }
 
 btQuaternion btRigidBody::getOrientation() const
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
index 0707595d48e..4596f90a00f 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h
@@ -16,27 +16,29 @@ subject to the following restrictions:
 #ifndef RIGIDBODY_H
 #define RIGIDBODY_H
 
-#include "../../LinearMath/btAlignedObjectArray.h"
-#include "../../LinearMath/btPoint3.h"
-#include "../../LinearMath/btTransform.h"
-#include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
-#include "../../BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btPoint3.h"
+#include "LinearMath/btTransform.h"
+#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
 class btCollisionShape;
 class btMotionState;
 class btTypedConstraint;
 
 
-extern btScalar gLinearAirDamping;
-
 extern btScalar gDeactivationTime;
 extern bool gDisableDeactivation;
-extern btScalar gLinearSleepingThreshold;
-extern btScalar gAngularSleepingThreshold;
 
 
-/// btRigidBody class for btRigidBody Dynamics
-/// 
+///btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
+///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
+///There are 3 types of rigid bodies: 
+///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
+///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
+///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
+///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
+///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
 class btRigidBody  : public btCollisionObject
 {
 
@@ -53,7 +55,16 @@ class btRigidBody  : public btCollisionObject
 	
 	btScalar		m_linearDamping;
 	btScalar		m_angularDamping;
-	
+
+	bool			m_additionalDamping;
+	btScalar		m_additionalDampingFactor;
+	btScalar		m_additionalLinearDampingThresholdSqr;
+	btScalar		m_additionalAngularDampingThresholdSqr;
+	btScalar		m_additionalAngularDampingFactor;
+
+
+	btScalar		m_linearSleepingThreshold;
+	btScalar		m_angularSleepingThreshold;
 
 	//m_optionalMotionState allows to automatic synchronize the world transform for active objects
 	btMotionState*	m_optionalMotionState;
@@ -63,12 +74,85 @@ class btRigidBody  : public btCollisionObject
 
 public:
 
-#ifdef OBSOLETE_MOTIONSTATE_LESS
-	//not supported, please use btMotionState
-	btRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.));
-#endif //OBSOLETE_MOTIONSTATE_LESS
 
-	btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.));
+	///btRigidBodyConstructionInfo provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
+	///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
+	///You can use the motion state to synchronize the world transform between physics and graphics objects. 
+	///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
+	///m_startWorldTransform is only used when you don't provide a motion state.
+	struct	btRigidBodyConstructionInfo
+	{
+		btScalar			m_mass;
+
+		///When a motionState is provided, the rigid body will initialize its world transform from the motion state
+		///In this case, m_startWorldTransform is ignored.
+		btMotionState*		m_motionState;
+		btTransform	m_startWorldTransform;
+
+		btCollisionShape*	m_collisionShape;
+		btVector3			m_localInertia;
+		btScalar			m_linearDamping;
+		btScalar			m_angularDamping;
+
+		///best simulation results when friction is non-zero
+		btScalar			m_friction;
+		///best simulation results using zero restitution.
+		btScalar			m_restitution;
+
+		btScalar			m_linearSleepingThreshold;
+		btScalar			m_angularSleepingThreshold;
+
+		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+		bool				m_additionalDamping;
+		btScalar			m_additionalDampingFactor;
+		btScalar			m_additionalLinearDampingThresholdSqr;
+		btScalar			m_additionalAngularDampingThresholdSqr;
+		btScalar			m_additionalAngularDampingFactor;
+
+		
+		btRigidBodyConstructionInfo(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
+		m_mass(mass),
+			m_motionState(motionState),
+			m_collisionShape(collisionShape),
+			m_localInertia(localInertia),
+			m_linearDamping(btScalar(0.)),
+			m_angularDamping(btScalar(0.)),
+			m_friction(btScalar(0.5)),
+			m_restitution(btScalar(0.)),
+			m_linearSleepingThreshold(btScalar(0.8)),
+			m_angularSleepingThreshold(btScalar(1.f)),
+			m_additionalDamping(false),
+			m_additionalDampingFactor(btScalar(0.005)),
+			m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
+			m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
+			m_additionalAngularDampingFactor(btScalar(0.01))
+		{
+			m_startWorldTransform.setIdentity();
+		}
+	};
+
+	///btRigidBody constructor using construction info
+	btRigidBody(	const btRigidBodyConstructionInfo& constructionInfo);
+
+	///btRigidBody constructor for backwards compatibility. 
+	///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
+	btRigidBody(	btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
+
+
+	virtual ~btRigidBody()
+        { 
+                //No constraints should point to this rigidbody
+		//Remove constraints from the dynamics world before you delete the related rigidbodies. 
+                btAssert(m_constraintRefs.size()==0); 
+        }
+
+protected:
+
+	///setupRigidBody is only used internally by the constructor
+	void	setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
+
+public:
 
 	void			proceedToTransform(const btTransform& newTrans); 
 	
@@ -76,11 +160,15 @@ public:
 	///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
 	static const btRigidBody*	upcast(const btCollisionObject* colObj)
 	{
-		return (const btRigidBody*)colObj->getInternalOwner();
+		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+			return (const btRigidBody*)colObj;
+		return 0;
 	}
 	static btRigidBody*	upcast(btCollisionObject* colObj)
 	{
-		return (btRigidBody*)colObj->getInternalOwner();
+		if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY)
+			return (btRigidBody*)colObj;
+		return 0;
 	}
 	
 	/// continuous collision detection needs prediction
@@ -88,8 +176,7 @@ public:
 	
 	void			saveKinematicState(btScalar step);
 	
-
-	void			applyForces(btScalar step);
+	void			applyGravity();
 	
 	void			setGravity(const btVector3& acceleration);  
 
@@ -99,12 +186,34 @@ public:
 	}
 
 	void			setDamping(btScalar lin_damping, btScalar ang_damping);
-	
-	inline const btCollisionShape*	getCollisionShape() const {
+
+	btScalar getLinearDamping() const
+	{
+		return m_linearDamping;
+	}
+
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+
+	btScalar getLinearSleepingThreshold() const
+	{
+		return m_linearSleepingThreshold;
+	}
+
+	btScalar getAngularSleepingThreshold() const
+	{
+		return m_angularSleepingThreshold;
+	}
+
+	void			applyDamping(btScalar timeStep);
+
+	SIMD_FORCE_INLINE const btCollisionShape*	getCollisionShape() const {
 		return m_collisionShape;
 	}
 
-	inline btCollisionShape*	getCollisionShape() {
+	SIMD_FORCE_INLINE btCollisionShape*	getCollisionShape() {
 			return m_collisionShape;
 	}
 	
@@ -134,6 +243,12 @@ public:
 		m_invInertiaLocal = diagInvInertia;
 	}
 
+	void	setSleepingThresholds(btScalar linear,btScalar angular)
+	{
+		m_linearSleepingThreshold = linear;
+		m_angularSleepingThreshold = angular;
+	}
+
 	void	applyTorque(const btVector3& torque)
 	{
 		m_totalTorque += torque;
@@ -142,7 +257,7 @@ public:
 	void	applyForce(const btVector3& force, const btVector3& rel_pos) 
 	{
 		applyCentralForce(force);
-		applyTorque(rel_pos.cross(force));
+		applyTorque(rel_pos.cross(force)*m_angularFactor);
 	}
 	
 	void applyCentralImpulse(const btVector3& impulse)
@@ -168,7 +283,7 @@ public:
 	}
 
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-	inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
 	{
 		if (m_inverseMass != btScalar(0.))
 		{
@@ -238,7 +353,7 @@ public:
 
 
 	
-	inline btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
+	SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const
 	{
 		btVector3 r0 = pos - getCenterOfMassPosition();
 
@@ -250,19 +365,19 @@ public:
 
 	}
 
-	inline btScalar computeAngularImpulseDenominator(const btVector3& axis) const
+	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
 	{
 		btVector3 vec = axis * getInvInertiaTensorWorld();
 		return axis.dot(vec);
 	}
 
-	inline void	updateDeactivation(btScalar timeStep)
+	SIMD_FORCE_INLINE void	updateDeactivation(btScalar timeStep)
 	{
 		if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
 			return;
 
-		if ((getLinearVelocity().length2() < gLinearSleepingThreshold*gLinearSleepingThreshold) &&
-			(getAngularVelocity().length2() < gAngularSleepingThreshold*gAngularSleepingThreshold))
+		if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
+			(getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
 		{
 			m_deactivationTime += timeStep;
 		} else
@@ -273,7 +388,7 @@ public:
 
 	}
 
-	inline bool	wantsSleeping()
+	SIMD_FORCE_INLINE bool	wantsSleeping()
 	{
 
 		if (getActivationState() == DISABLE_DEACTIVATION)
@@ -348,6 +463,16 @@ public:
 	void addConstraintRef(btTypedConstraint* c);
 	void removeConstraintRef(btTypedConstraint* c);
 
+	btTypedConstraint* getConstraintRef(int index)
+	{
+		return m_constraintRefs[index];
+	}
+
+	int getNumConstraintRefs()
+	{
+		return m_constraintRefs.size();
+	}
+
 	int	m_debugBodyId;
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index 4ebcb8e7517..3be04d1a4ad 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -27,16 +27,19 @@ subject to the following restrictions:
   can be used by probes that are checking whether the
   library is actually installed.
 */
-extern "C" void btBulletDynamicsProbe () {}
+extern "C" 
+{
+	void btBulletDynamicsProbe ();
+	void btBulletDynamicsProbe () {}
+}
 
 
 
 
-btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
-:btDynamicsWorld(dispatcher,pairCache),
+btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
+:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
 m_constraintSolver(constraintSolver),
 m_ownsConstraintSolver(false),
-m_debugDrawer(0),
 m_gravity(0,0,-10)
 {
 
@@ -46,7 +49,7 @@ m_gravity(0,0,-10)
 btSimpleDynamicsWorld::~btSimpleDynamicsWorld()
 {
 	if (m_ownsConstraintSolver)
-		delete m_constraintSolver;
+		btAlignedFree( m_constraintSolver);
 }
 
 int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
@@ -74,8 +77,9 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 		
 		btContactSolverInfo infoGlobal;
 		infoGlobal.m_timeStep = timeStep;
-		
-		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc);
+		m_constraintSolver->prepareSolve(0,numManifolds);
+		m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1);
+		m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc);
 	}
 
 	///integrate transforms
@@ -85,10 +89,27 @@ int		btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b
 
 	synchronizeMotionStates();
 
+	clearForces();
+
 	return 1;
 
 }
 
+void	btSimpleDynamicsWorld::clearForces()
+{
+	//todo: iterate over awake simulation islands!
+	for ( int i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		
+		btRigidBody* body = btRigidBody::upcast(colObj);
+		if (body)
+		{
+			body->clearForces();
+		}
+	}
+}	
+
 
 void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 {
@@ -104,6 +125,11 @@ void	btSimpleDynamicsWorld::setGravity(const btVector3& gravity)
 	}
 }
 
+btVector3 btSimpleDynamicsWorld::getGravity () const
+{
+	return m_gravity;
+}
+
 void	btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body)
 {
 	removeCollisionObject(body);
@@ -133,7 +159,7 @@ void	btSimpleDynamicsWorld::updateAabbs()
 				btPoint3 minAabb,maxAabb;
 				colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
 				btBroadphaseInterface* bp = getBroadphase();
-				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
+				bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
 			}
 		}
 	}
@@ -171,8 +197,9 @@ void	btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 			{
 				if (body->isActive())
 				{
-					body->applyForces( timeStep);
+					body->applyGravity();
 					body->integrateVelocities( timeStep);
+					body->applyDamping(timeStep);
 					body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
 				}
 			}
@@ -204,8 +231,13 @@ void	btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
 	{
-		delete m_constraintSolver;
+		btAlignedFree(m_constraintSolver);
 	}
 	m_ownsConstraintSolver = false;
 	m_constraintSolver = solver;
 }
+
+btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver()
+{
+	return m_constraintSolver;
+}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
index 25f4ccd8e68..5c56fdf1327 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h
@@ -22,11 +22,8 @@ class btDispatcher;
 class btOverlappingPairCache;
 class btConstraintSolver;
 
-///btSimpleDynamicsWorld demonstrates very basic usage of Bullet rigid body dynamics
-///It can be used for basic simulations, and as a starting point for porting Bullet
-///btSimpleDynamicsWorld lacks object deactivation, island management and other concepts.
-///For more complicated simulations, btDiscreteDynamicsWorld and btContinuousDynamicsWorld are recommended
-///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
+///btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
+///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished).
 class btSimpleDynamicsWorld : public btDynamicsWorld
 {
 protected:
@@ -35,8 +32,6 @@ protected:
 
 	bool	m_ownsConstraintSolver;
 
-	btIDebugDraw*	m_debugDrawer;
-
 	void	predictUnconstraintMotion(btScalar timeStep);
 	
 	void	integrateTransforms(btScalar timeStep);
@@ -48,25 +43,17 @@ public:
 
 
 	///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
-	btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver);
+	btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
 
 	virtual ~btSimpleDynamicsWorld();
 		
 	///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead
 	virtual int	stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
 
-	virtual void	setDebugDrawer(btIDebugDraw*	debugDrawer) 
-	{
-		m_debugDrawer = debugDrawer;
-	};
-
-	virtual btIDebugDraw*	getDebugDrawer()
-	{
-		return m_debugDrawer;
-	}
-
 	virtual void	setGravity(const btVector3& gravity);
 
+	virtual btVector3 getGravity () const;
+
 	virtual void	addRigidBody(btRigidBody* body);
 
 	virtual void	removeRigidBody(btRigidBody* body);
@@ -77,6 +64,15 @@ public:
 
 	virtual void	setConstraintSolver(btConstraintSolver* solver);
 
+	virtual btConstraintSolver* getConstraintSolver();
+
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return BT_SIMPLE_DYNAMICS_WORLD;
+	}
+
+	virtual void	clearForces();
+
 };
 
 #endif //BT_SIMPLE_DYNAMICS_WORLD_H
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
index d53de7f3687..fe65245c2a1 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp
@@ -23,12 +23,11 @@
 
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
-
-
 static btRigidBody s_fixedObject( 0,0,0);
 
 btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis,	btVehicleRaycaster* raycaster )
-:m_vehicleRaycaster(raycaster),
+: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE),
+m_vehicleRaycaster(raycaster),
 m_pitchControl(btScalar(0.))
 {
 	m_chassisBody = chassis;
@@ -487,6 +486,7 @@ struct btWheelContactPoint
 
 };
 
+btScalar calcRollingFriction(btWheelContactPoint& contactPoint);
 btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 {
 
@@ -507,8 +507,8 @@ btScalar calcRollingFriction(btWheelContactPoint& contactPoint)
 
 	// calculate j that moves us to zero relative velocity
 	j1 = -vrel * contactPoint.m_jacDiagABInv;
-	GEN_set_min(j1, maxImpulse);
-	GEN_set_max(j1, -maxImpulse);
+	btSetMin(j1, maxImpulse);
+	btSetMax(j1, -maxImpulse);
 
 	return j1;
 }
@@ -525,11 +525,10 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 		if (!numWheel)
 			return;
 
-
-		btVector3*	forwardWS = new	btVector3[numWheel];
-		btVector3*	axle = new btVector3[numWheel];
-		btScalar* forwardImpulse = new btScalar[numWheel];
-		btScalar* sideImpulse = new btScalar[numWheel];
+		m_forwardWS.resize(numWheel);
+		m_axle.resize(numWheel);
+		m_forwardImpulse.resize(numWheel);
+		m_sideImpulse.resize(numWheel);
 		
 		int numWheelsOnGround = 0;
 	
@@ -541,8 +540,8 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 			class btRigidBody* groundObject = (class btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject;
 			if (groundObject)
 				numWheelsOnGround++;
-			sideImpulse[i] = btScalar(0.);
-			forwardImpulse[i] = btScalar(0.);
+			m_sideImpulse[i] = btScalar(0.);
+			m_forwardImpulse[i] = btScalar(0.);
 
 		}
 	
@@ -561,25 +560,25 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 					const btTransform& wheelTrans = getWheelTransformWS( i );
 
 					btMatrix3x3 wheelBasis0 = wheelTrans.getBasis();
-					axle[i] = btVector3(	
+					m_axle[i] = btVector3(	
 						wheelBasis0[0][m_indexRightAxis],
 						wheelBasis0[1][m_indexRightAxis],
 						wheelBasis0[2][m_indexRightAxis]);
 					
 					const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
-					btScalar proj = axle[i].dot(surfNormalWS);
-					axle[i] -= surfNormalWS * proj;
-					axle[i] = axle[i].normalize();
+					btScalar proj = m_axle[i].dot(surfNormalWS);
+					m_axle[i] -= surfNormalWS * proj;
+					m_axle[i] = m_axle[i].normalize();
 					
-					forwardWS[i] = surfNormalWS.cross(axle[i]);
-					forwardWS[i].normalize();
+					m_forwardWS[i] = surfNormalWS.cross(m_axle[i]);
+					m_forwardWS[i].normalize();
 
 				
 					resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS,
 							  *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS,
-							  btScalar(0.), axle[i],sideImpulse[i],timeStep);
+							  btScalar(0.), m_axle[i],m_sideImpulse[i],timeStep);
 
-					sideImpulse[i] *= sideFrictionStiffness2;
+					m_sideImpulse[i] *= sideFrictionStiffness2;
 						
 				}
 				
@@ -608,7 +607,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 				{
 					btScalar defaultRollingFrictionImpulse = 0.f;
 					btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
-					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,forwardWS[wheel],maxImpulse);
+					btWheelContactPoint contactPt(m_chassisBody,groundObject,wheelInfo.m_raycastInfo.m_contactPointWS,m_forwardWS[wheel],maxImpulse);
 					rollingFriction = calcRollingFriction(contactPt);
 				}
 			}
@@ -618,7 +617,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 
 
 
-			forwardImpulse[wheel] = btScalar(0.);
+			m_forwardImpulse[wheel] = btScalar(0.);
 			m_wheelInfo[wheel].m_skidInfo= btScalar(1.);
 
 			if (groundObject)
@@ -631,10 +630,10 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 				btScalar maximpSquared = maximp * maximpSide;
 			
 
-				forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
+				m_forwardImpulse[wheel] = rollingFriction;//wheelInfo.m_engineForce* timeStep;
 
-				btScalar x = (forwardImpulse[wheel] ) * fwdFactor;
-				btScalar y = (sideImpulse[wheel] ) * sideFactor;
+				btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor;
+				btScalar y = (m_sideImpulse[wheel] ) * sideFactor;
 				
 				btScalar impulseSquared = (x*x + y*y);
 
@@ -658,12 +657,12 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 		{
 			for (int wheel = 0;wheel < getNumWheels(); wheel++)
 			{
-				if (sideImpulse[wheel] != btScalar(0.))
+				if (m_sideImpulse[wheel] != btScalar(0.))
 				{
 					if (m_wheelInfo[wheel].m_skidInfo< btScalar(1.))
 					{
-						forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
-						sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
+						m_forwardImpulse[wheel] *=	m_wheelInfo[wheel].m_skidInfo;
+						m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
 					}
 				}
 			}
@@ -678,11 +677,11 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 				btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - 
 						m_chassisBody->getCenterOfMassPosition();
 
-				if (forwardImpulse[wheel] != btScalar(0.))
+				if (m_forwardImpulse[wheel] != btScalar(0.))
 				{
-					m_chassisBody->applyImpulse(forwardWS[wheel]*(forwardImpulse[wheel]),rel_pos);
+					m_chassisBody->applyImpulse(m_forwardWS[wheel]*(m_forwardImpulse[wheel]),rel_pos);
 				}
-				if (sideImpulse[wheel] != btScalar(0.))
+				if (m_sideImpulse[wheel] != btScalar(0.))
 				{
 					class btRigidBody* groundObject = (class btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject;
 
@@ -690,7 +689,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 						groundObject->getCenterOfMassPosition();
 
 					
-					btVector3 sideImp = axle[wheel] * sideImpulse[wheel];
+					btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
 
 					rel_pos[2] *= wheelInfo.m_rollInfluence;
 					m_chassisBody->applyImpulse(sideImp,rel_pos);
@@ -701,10 +700,7 @@ void	btRaycastVehicle::updateFriction(btScalar	timeStep)
 			}
 		}
 
-		delete []forwardWS;
-		delete [] axle;
-		delete[]forwardImpulse;
-		delete[] sideImpulse;
+	
 }
 
 
@@ -716,11 +712,11 @@ void* btDefaultVehicleRaycaster::castRay(const btVector3& from,const btVector3&
 
 	m_dynamicsWorld->rayTest(from, to, rayCallback);
 
-	if (rayCallback.HasHit())
+	if (rayCallback.hasHit())
 	{
 		
 		btRigidBody* body = btRigidBody::upcast(rayCallback.m_collisionObject);
-		if (body)
+        if (body && body->hasContactResponse())
 		{
 			result.m_hitPointInWorld = rayCallback.m_hitPointWorld;
 			result.m_hitNormalInWorld = rayCallback.m_hitNormalWorld;
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
index f4249599615..8361dcabe4b 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h
@@ -11,11 +11,11 @@
 #ifndef RAYCASTVEHICLE_H
 #define RAYCASTVEHICLE_H
 
-#include "../Dynamics/btRigidBody.h"
-#include "../ConstraintSolver/btTypedConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
 #include "btVehicleRaycaster.h"
 class btDynamicsWorld;
-#include "../../LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btAlignedObjectArray.h"
 #include "btWheelInfo.h"
 
 class btVehicleTuning;
@@ -23,6 +23,12 @@ class btVehicleTuning;
 ///rayCast vehicle, very special constraint that turn a rigidbody into a vehicle.
 class btRaycastVehicle : public btTypedConstraint
 {
+
+		btAlignedObjectArray<btVector3>	m_forwardWS;
+		btAlignedObjectArray<btVector3>	m_axle;
+		btAlignedObjectArray<btScalar>	m_forwardImpulse;
+		btAlignedObjectArray<btScalar>	m_sideImpulse;
+
 public:
 	class btVehicleTuning
 		{
@@ -114,7 +120,7 @@ public:
 	
 	void	updateSuspension(btScalar deltaTime);
 
-	void	updateFriction(btScalar	timeStep);
+	virtual void	updateFriction(btScalar	timeStep);
 
 
 
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
index 64a47fcaada..5112ce6d420 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h
@@ -11,7 +11,7 @@
 #ifndef VEHICLE_RAYCASTER_H
 #define VEHICLE_RAYCASTER_H
 
-#include "../../LinearMath/btVector3.h"
+#include "LinearMath/btVector3.h"
 
 /// btVehicleRaycaster is provides interface for between vehicle simulation and raycasting
 struct btVehicleRaycaster
diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
index 2e349b3fde4..ac2729f4fd7 100644
--- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
+++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h
@@ -11,8 +11,8 @@
 #ifndef WHEEL_INFO_H
 #define WHEEL_INFO_H
 
-#include "../../LinearMath/btVector3.h"
-#include "../../LinearMath/btTransform.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btTransform.h"
 
 class btRigidBody;