Bullet: Update to svn r2719

Fixes part of T37905, fixed constraint didn't work correctly.

18 files changed, 455 insertions, 300 deletions

diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index 093c6f9b200..d739a2a08d7 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -770,7 +770,7 @@ void	btCollisionWorld::objectQuerySingleInternal(const btConvexShape* castShape,
 									hitPointLocal,
 									hitFraction);
 
-								bool	normalInWorldSpace = false;
+								bool	normalInWorldSpace = true;
 
 								return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
 							}
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
index 991841ee20b..286e6c31f2f 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp
@@ -229,6 +229,7 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap
 		removeChildAlgorithms();
 		
 		preallocateChildAlgorithms(body0Wrap,body1Wrap);
+		m_compoundShapeRevision = compoundShape->getUpdateRevision();
 	}
 
 
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
index 53675145637..7d792c18d34 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h
@@ -36,6 +36,7 @@ extern btShapePairCallback gCompoundChildShapePairCallback;
 /// btCompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes
 class btCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
 {
+protected:
 	btAlignedObjectArray<btCollisionAlgorithm*> m_childCollisionAlgorithms;
 	bool m_isSwapped;
 
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
index a52dd34fe91..95780fb2d27 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
@@ -27,10 +27,8 @@ subject to the following restrictions:
 btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
 
 btCompoundCompoundCollisionAlgorithm::btCompoundCompoundCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
-:btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
-m_sharedManifold(ci.m_manifold)
+:btCompoundCollisionAlgorithm(ci,body0Wrap,body1Wrap,isSwapped)
 {
-	m_ownsManifold = false;
 
 	void* ptr = btAlignedAlloc(sizeof(btHashedSimplePairCache),16);
 	m_childCollisionAlgorithmCache= new(ptr) btHashedSimplePairCache();
@@ -292,12 +290,21 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	const btCompoundShape* compoundShape0 = static_cast<const btCompoundShape*>(col0ObjWrap->getCollisionShape());
 	const btCompoundShape* compoundShape1 = static_cast<const btCompoundShape*>(col1ObjWrap->getCollisionShape());
 
+	const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
+	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+	if (!tree0 || !tree1)
+	{
+		return btCompoundCollisionAlgorithm::processCollision(body0Wrap,body1Wrap,dispatchInfo,resultOut);
+	}
 	///btCompoundShape might have changed:
 	////make sure the internal child collision algorithm caches are still valid
 	if ((compoundShape0->getUpdateRevision() != m_compoundShapeRevision0) || (compoundShape1->getUpdateRevision() != m_compoundShapeRevision1))
 	{
 		///clear all
 		removeChildAlgorithms();
+		m_compoundShapeRevision0 = compoundShape0->getUpdateRevision();
+		m_compoundShapeRevision1 = compoundShape1->getUpdateRevision();
+
 	}
 
 
@@ -329,8 +336,7 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
 	}
 
 
-	const btDbvt* tree0 = compoundShape0->getDynamicAabbTree();
-	const btDbvt* tree1 = compoundShape1->getDynamicAabbTree();
+	
 
 	btCompoundCompoundLeafCallback callback(col0ObjWrap,col1ObjWrap,this->m_dispatcher,dispatchInfo,resultOut,this->m_childCollisionAlgorithmCache,m_sharedManifold);
 
diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
index 7e2d7ad7085..06a762f209d 100644
--- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
+++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.h
@@ -17,6 +17,8 @@ subject to the following restrictions:
 #ifndef BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
 #define BT_COMPOUND_COMPOUND_COLLISION_ALGORITHM_H
 
+#include "btCompoundCollisionAlgorithm.h"
+
 #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
@@ -35,15 +37,12 @@ typedef bool (*btShapePairCallback)(const btCollisionShape* pShape0, const btCol
 extern btShapePairCallback gCompoundCompoundChildShapePairCallback;
 
 /// btCompoundCompoundCollisionAlgorithm  supports collision between two btCompoundCollisionShape shapes
-class btCompoundCompoundCollisionAlgorithm  : public btActivatingCollisionAlgorithm
+class btCompoundCompoundCollisionAlgorithm  : public btCompoundCollisionAlgorithm
 {
 
 	class btHashedSimplePairCache*	m_childCollisionAlgorithmCache;
 	btSimplePairArray m_removePairs;
 
-	class btPersistentManifold*	m_sharedManifold;
-	bool					m_ownsManifold;
-
 
 	int	m_compoundShapeRevision0;//to keep track of changes, so that childAlgorithm array can be updated
 	int	m_compoundShapeRevision1;
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
index 1ba1cd1e839..890afe6da42 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
@@ -33,7 +33,8 @@ class	btDispatcher;
 enum btConstraintSolverType
 {
 	BT_SEQUENTIAL_IMPULSE_SOLVER=1,
-	BT_MLCP_SOLVER=2
+	BT_MLCP_SOLVER=2,
+	BT_NNCG_SOLVER=4
 };
 
 class btConstraintSolver
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
index f93a3280f35..3428e0b069d 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.cpp
@@ -23,9 +23,8 @@ subject to the following restrictions:
 btFixedConstraint::btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB)
 :btTypedConstraint(FIXED_CONSTRAINT_TYPE,rbA,rbB)
 {
-	m_pivotInA = frameInA.getOrigin();
-	m_pivotInB = frameInB.getOrigin();
-	m_relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse();
+	m_frameInA = frameInA;
+	m_frameInB = frameInB;
 
 }
 
@@ -37,14 +36,16 @@ btFixedConstraint::~btFixedConstraint ()
 void btFixedConstraint::getInfo1 (btConstraintInfo1* info)
 {
 	info->m_numConstraintRows = 6;
-	info->nub = 6;
+	info->nub = 0;
 }
 
 void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
 {
 	//fix the 3 linear degrees of freedom
 
-	
+	const btTransform& transA = m_rbA.getCenterOfMassTransform();
+	const btTransform& transB = m_rbB.getCenterOfMassTransform();
+
 	const btVector3& worldPosA = m_rbA.getCenterOfMassTransform().getOrigin();
 	const btMatrix3x3& worldOrnA = m_rbA.getCenterOfMassTransform().getBasis();
 	const btVector3& worldPosB= m_rbB.getCenterOfMassTransform().getOrigin();
@@ -55,15 +56,15 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
 	info->m_J1linearAxis[info->rowskip+1] = 1;
 	info->m_J1linearAxis[2*info->rowskip+2] = 1;
 
-	btVector3 a1 = worldOrnA*m_pivotInA;
-	{
+	btVector3 a1 = worldOrnA * m_frameInA.getOrigin();
+    {
 		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
 		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
 		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
 		btVector3 a1neg = -a1;
 		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
 	}
-    
+
 	if (info->m_J2linearAxis)
 	{
 		info->m_J2linearAxis[0] = -1;
@@ -71,10 +72,8 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
 		info->m_J2linearAxis[2*info->rowskip+2] = -1;
 	}
 	
-	btVector3 a2 = worldOrnB*m_pivotInB;
-   
-	{
-	//	btVector3 a2n = -a2;
+	btVector3 a2 = worldOrnB*m_frameInB.getOrigin();
+   {
 		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
 		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
 		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
@@ -88,42 +87,100 @@ void btFixedConstraint::getInfo2 (btConstraintInfo2* info)
     int j;
 	for (j=0; j<3; j++)
     {
-
-
-
         info->m_constraintError[j*info->rowskip] = linearError[j];
 		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
     }
 
-		//fix the 3 angular degrees of freedom
-
-	int start_row = 3;
-	int s = info->rowskip;
-    int start_index = start_row * s;
-
-    // 3 rows to make body rotations equal
-	info->m_J1angularAxis[start_index] = 1;
-    info->m_J1angularAxis[start_index + s + 1] = 1;
-    info->m_J1angularAxis[start_index + s*2+2] = 1;
-    if ( info->m_J2angularAxis)
-    {
-        info->m_J2angularAxis[start_index] = -1;
-        info->m_J2angularAxis[start_index + s+1] = -1;
-        info->m_J2angularAxis[start_index + s*2+2] = -1;
-    }
-
-    // set right hand side for the angular dofs
-
-	btVector3 diff;
-	btScalar angle;
-	btMatrix3x3 mrelCur = worldOrnA *worldOrnB.inverse();
-	btQuaternion qrelCur;
-	mrelCur.getRotation(qrelCur);
-	btTransformUtil::calculateDiffAxisAngleQuaternion(m_relTargetAB,qrelCur,diff,angle);
-	diff*=-angle;
-	for (j=0; j<3; j++)
-    {
-        info->m_constraintError[(3+j)*info->rowskip] = k * diff[j];
-    }
-
+	btVector3 ivA = transA.getBasis() * m_frameInA.getBasis().getColumn(0);
+	btVector3 jvA = transA.getBasis() * m_frameInA.getBasis().getColumn(1);
+	btVector3 kvA = transA.getBasis() * m_frameInA.getBasis().getColumn(2);
+	btVector3 ivB = transB.getBasis() * m_frameInB.getBasis().getColumn(0);
+	btVector3 target;
+	btScalar x = ivB.dot(ivA);
+	btScalar y = ivB.dot(jvA);
+	btScalar z = ivB.dot(kvA);
+	btVector3 swingAxis(0,0,0);
+	{ 
+		if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
+		{
+			swingAxis = -ivB.cross(ivA);
+		}
+	}
+	btVector3 vTwist(1,0,0);
+
+	// compute rotation of A wrt B (in constraint space)
+	btQuaternion qA = transA.getRotation() * m_frameInA.getRotation();
+	btQuaternion qB = transB.getRotation() * m_frameInB.getRotation();
+	btQuaternion qAB = qB.inverse() * qA;
+	// split rotation into cone and twist
+	// (all this is done from B's perspective. Maybe I should be averaging axes...)
+	btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
+	btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
+	btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
+
+	int row = 3;
+    int srow = row * info->rowskip;
+	btVector3 ax1;
+	// angular limits
+	{
+		btScalar *J1 = info->m_J1angularAxis;
+		btScalar *J2 = info->m_J2angularAxis;
+		btTransform trA = transA*m_frameInA;
+		btVector3 twistAxis = trA.getBasis().getColumn(0);
+
+		btVector3 p = trA.getBasis().getColumn(1);
+		btVector3 q = trA.getBasis().getColumn(2);
+		int srow1 = srow + info->rowskip;
+		J1[srow+0] = p[0];
+		J1[srow+1] = p[1];
+		J1[srow+2] = p[2];
+		J1[srow1+0] = q[0];
+		J1[srow1+1] = q[1];
+		J1[srow1+2] = q[2];
+		J2[srow+0] = -p[0];
+		J2[srow+1] = -p[1];
+		J2[srow+2] = -p[2];
+		J2[srow1+0] = -q[0];
+		J2[srow1+1] = -q[1];
+		J2[srow1+2] = -q[2];
+		btScalar fact = info->fps;
+		info->m_constraintError[srow] =   fact * swingAxis.dot(p);
+		info->m_constraintError[srow1] =  fact * swingAxis.dot(q);
+		info->m_lowerLimit[srow] = -SIMD_INFINITY;
+		info->m_upperLimit[srow] = SIMD_INFINITY;
+		info->m_lowerLimit[srow1] = -SIMD_INFINITY;
+		info->m_upperLimit[srow1] = SIMD_INFINITY;
+		srow = srow1 + info->rowskip;
+
+		{
+			btQuaternion qMinTwist = qABTwist;
+			btScalar twistAngle = qABTwist.getAngle();
+
+			if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
+			{
+				qMinTwist = -(qABTwist);
+				twistAngle = qMinTwist.getAngle();
+			}
+
+			if (twistAngle > SIMD_EPSILON)
+			{
+				twistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+				twistAxis.normalize();
+				twistAxis = quatRotate(qB, -twistAxis);
+			}
+			ax1 = twistAxis;
+			btScalar *J1 = info->m_J1angularAxis;
+			btScalar *J2 = info->m_J2angularAxis;
+			J1[srow+0] = ax1[0];
+			J1[srow+1] = ax1[1];
+			J1[srow+2] = ax1[2];
+			J2[srow+0] = -ax1[0];
+			J2[srow+1] = -ax1[1];
+			J2[srow+2] = -ax1[2];
+			btScalar k = info->fps;
+			info->m_constraintError[srow] = k * twistAngle;
+			info->m_lowerLimit[srow] = -SIMD_INFINITY;
+			info->m_upperLimit[srow] = SIMD_INFINITY;
+		}
+	}
 }
\ No newline at end of file
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
index 697e319e2ce..e1c9430fd3c 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btFixedConstraint.h
@@ -20,10 +20,9 @@ subject to the following restrictions:
 
 ATTRIBUTE_ALIGNED16(class) btFixedConstraint : public btTypedConstraint
 {
-	btVector3 m_pivotInA;
-	btVector3 m_pivotInB;
-	btQuaternion m_relTargetAB;
 
+	btTransform m_frameInA;
+	btTransform m_frameInB;
 public:
 	btFixedConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& frameInA,const btTransform& frameInB);
 	
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index be93e35434c..f855581c712 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -58,13 +58,13 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
 #endif//USE_SIMD
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
-void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
 	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
 	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
-	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+	btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
 	__m128 deltaVel1Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
 	__m128 deltaVel2Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
 	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
@@ -86,13 +86,15 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
 	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
 	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+
+	return deltaImpulse;
 #else
-	resolveSingleConstraintRowGeneric(body1,body2,c);
+	return resolveSingleConstraintRowGeneric(body1,body2,c);
 #endif
 }
 
 // Project Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
 	const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) 	+ c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@@ -120,15 +122,17 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 
 	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+
+	return deltaImpulse;
 }
 
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 #ifdef USE_SIMD
 	__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
 	__m128	lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
 	__m128	upperLimit1 = _mm_set1_ps(c.m_upperLimit);
-	__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
+	btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
 	__m128 deltaVel1Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
 	__m128 deltaVel2Dotn	=	_mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128));
 	deltaImpulse	=	_mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
@@ -147,13 +151,14 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
 	body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
 	body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
+	return deltaImpulse;
 #else
-	resolveSingleConstraintRowLowerLimit(body1,body2,c);
+	return resolveSingleConstraintRowLowerLimit(body1,body2,c);
 #endif
 }
 
-// Projected Gauss Seidel or the equivalent Sequential Impulse
- void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
+
+btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
 {
 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
 	const btScalar deltaVel1Dotn	=	c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@@ -173,6 +178,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	}
 	body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 	body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
+
+	return deltaImpulse;
 }
 
 
@@ -430,6 +437,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		btSimdScalar velocityError =  desiredVelocity - rel_vel;
 		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_rhsPenetration = 0.f;
 		solverConstraint.m_cfm = cfmSlip;
 		solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
 		solverConstraint.m_upperLimit = solverConstraint.m_friction;
@@ -812,7 +820,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 
 	///avoid collision response between two static objects
-	if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
+	if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero())))
 		return;
 
 	int rollingFriction=1;
@@ -1452,8 +1460,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 				for (j=0;j<numPoolConstraints;j++)
 				{
 					const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
-					//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-					resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+					resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 
 				}
 		
@@ -1472,8 +1479,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
 						solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
 
-						//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
-						resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
+						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					}
 				}
 
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index 180d2a385d3..47dbbe3393d 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -88,13 +88,10 @@ protected:
 	int		getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
 	void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
 
-	void	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-
-	void	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
-	
-	void	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
+	btSimdScalar	resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
 		
 protected:
 	
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
index 8888d3961f5..0635b958bed 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
@@ -19,9 +19,11 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "btSolveProjectedGaussSeidel.h"
 
+
 btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
 :m_solver(solver),
-m_fallback(0)
+m_fallback(0),
+m_cfm(0.000001)//0.0000001
 {
 }
 
@@ -160,13 +162,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		BT_PROFILE("init b (rhs)");
 		m_b.resize(numConstraintRows);
 		m_bSplit.resize(numConstraintRows);
-		//m_b.setZero();
+		m_b.setZero();
+		m_bSplit.setZero();
 		for (int i=0;i<numConstraintRows ;i++)
 		{
-			if (m_allConstraintArray[i].m_jacDiagABInv)
+			btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv;
+			if (!btFuzzyZero(jacDiag))
 			{
-				m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
-				m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
+				btScalar rhs = m_allConstraintArray[i].m_rhs;
+				btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration;
+				m_b[i]=rhs/jacDiag;
+				m_bSplit[i] = rhsPenetration/jacDiag;
 			}
 
 		}
@@ -425,14 +431,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		}
 	}
 
-	///todo: use proper cfm values from the constraints (getInfo2)
 	if (1)
 	{
 		// add cfm to the diagonal of m_A
 		for ( int i=0; i<m_A.rows(); ++i) 
 		{
-			float cfm = 0.00001f;
-			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
 		}
 	}
 				   
@@ -473,6 +477,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 	if (infoGlobal.m_splitImpulse)
 		m_bSplit.resize(numConstraintRows);
  
+	m_bSplit.setZero();
+	m_b.setZero();
+
 	for (int i=0;i<numConstraintRows ;i++)
 	{
 		if (m_allConstraintArray[i].m_jacDiagABInv)
@@ -554,12 +561,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 
 	if (1)
 	{
-		///todo: use proper cfm values from the constraints (getInfo2)
 		// add cfm to the diagonal of m_A
 		for ( int i=0; i<m_A.rows(); ++i) 
 		{
-			float cfm = 0.0001f;
-			m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
 		}
 	}
 
@@ -618,6 +623,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
 	}
 	else
 	{
+	//	printf("m_fallback = %d\n",m_fallback);
 		m_fallback++;
 		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 	}
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
index a5dae2d28f6..31e8eb88ba5 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
@@ -42,6 +42,7 @@ protected:
 	btConstraintArray m_allConstraintArray;
 	btMLCPSolverInterface* m_solver;
 	int m_fallback;
+	btScalar m_cfm;
 
 	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
 	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
@@ -70,6 +71,15 @@ public:
 		m_fallback = num;
 	}
 
+	btScalar	getCfm() const
+	{
+		return m_cfm;
+	}
+	void setCfm(btScalar cfm)
+	{
+		m_cfm = cfm;
+	}
+
 	virtual btConstraintSolverType	getSolverType() const
 	{
 		return BT_MLCP_SOLVER;
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
index 44fbfeadddc..d2ad54d21a8 100644
--- a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
+++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
@@ -20,11 +20,17 @@ subject to the following restrictions:
 
 #include "btMLCPSolverInterface.h"
 
+///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix)
 class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
 {
 public:
 	virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
 	{
+		if (!A.rows())
+			return true;
+		//the A matrix is sparse, so compute the non-zero elements
+		A.rowComputeNonZeroElements();
+
 		//A is a m-n matrix, m rows, n columns
 		btAssert(A.rows() == b.rows());
 
diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
index 9429d56e05a..2bfd62f2a2f 100644
--- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
+++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp
@@ -3027,7 +3027,8 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti)
 	{
 		const RContact&		c = psb->m_rcontacts[i];
 		const sCti&			cti = c.m_cti;	
-		if (cti.m_colObj->hasContactResponse()) {
+		if (cti.m_colObj->hasContactResponse()) 
+		{
 			btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
 			const btVector3		va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0);
 			const btVector3		vb = c.m_node->m_x-c.m_node->m_q;	
diff --git a/extern/bullet2/src/LinearMath/btMatrixX.h b/extern/bullet2/src/LinearMath/btMatrixX.h
index 1c29632c536..865d77967a9 100644
--- a/extern/bullet2/src/LinearMath/btMatrixX.h
+++ b/extern/bullet2/src/LinearMath/btMatrixX.h
@@ -20,6 +20,12 @@ subject to the following restrictions:
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
+//#define BT_DEBUG_OSTREAM
+#ifdef BT_DEBUG_OSTREAM
+#include <iostream>
+#include <iomanip>      // std::setw
+#endif //BT_DEBUG_OSTREAM
+
 class btIntSortPredicate
 {
 	public:
@@ -30,6 +36,121 @@ class btIntSortPredicate
 };
 
 
+template <typename T>
+struct btVectorX
+{
+	btAlignedObjectArray<T>	m_storage;
+	
+	btVectorX()
+	{
+	}
+	btVectorX(int numRows)
+	{
+		m_storage.resize(numRows);
+	}
+	
+	void resize(int rows)
+	{
+		m_storage.resize(rows);
+	}
+	int cols() const
+	{
+		return 1;
+	}
+	int rows() const
+	{
+		return m_storage.size();
+	}
+	int size() const
+	{
+		return rows();
+	}
+	
+	T nrm2() const
+	{
+		T norm = T(0);
+		
+		int nn = rows();
+		
+		{
+			if (nn == 1)
+			{
+				norm = btFabs((*this)[0]);
+			}
+			else
+			{
+				T scale = 0.0;
+				T ssq = 1.0;
+				
+				/* The following loop is equivalent to this call to the LAPACK
+				 auxiliary routine:   CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */
+				
+				for (int ix=0;ix<nn;ix++)
+				{
+					if ((*this)[ix] != 0.0)
+					{
+						T absxi = btFabs((*this)[ix]);
+						if (scale < absxi)
+						{
+							T temp;
+							temp = scale / absxi;
+							ssq = ssq * (temp * temp) + 1.0;
+							scale = absxi;
+						}
+						else
+						{
+							T temp;
+							temp = absxi / scale;
+							ssq += temp * temp;
+						}
+					}
+				}
+				norm = scale * sqrt(ssq);
+			}
+		}
+		return norm;
+		
+	}
+	void	setZero()
+	{
+		if (m_storage.size())
+		{
+			//	for (int i=0;i<m_storage.size();i++)
+			//		m_storage[i]=0;
+			//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
+			btSetZero(&m_storage[0],m_storage.size());
+		}
+	}
+	const T& operator[] (int index) const
+	{
+		return m_storage[index];
+	}
+	
+	T& operator[] (int index)
+	{
+		return m_storage[index];
+	}
+	
+	T* getBufferPointerWritable()
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+	
+	const T* getBufferPointer() const
+	{
+		return m_storage.size() ? &m_storage[0] : 0;
+	}
+	
+};
+/*
+ template <typename T>
+ void setElem(btMatrixX<T>& mat, int row, int col, T val)
+ {
+ mat.setElem(row,col,val);
+ }
+ */
+
+
 template <typename T> 
 struct btMatrixX
 {
@@ -40,8 +161,7 @@ struct btMatrixX
 	int m_setElemOperations;
 
 	btAlignedObjectArray<T>	m_storage;
-	btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
-	btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
+	mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
 
 	T* getBufferPointerWritable() 
 	{
@@ -78,7 +198,6 @@ struct btMatrixX
 			BT_PROFILE("m_storage.resize");
 			m_storage.resize(rows*cols);
 		}
-		clearSparseInfo();
 	}
 	int cols() const
 	{
@@ -109,49 +228,44 @@ struct btMatrixX
 		}
 	}
 	
+	
+	void setElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		m_storage[row*m_cols+col] = val;
+	}
+	
+	void mulElem(int row,int col, T val)
+	{
+		m_setElemOperations++;
+		//mul doesn't change sparsity info
+
+		m_storage[row*m_cols+col] *= val;
+	}
+	
+	
+	
+	
 	void copyLowerToUpperTriangle()
 	{
 		int count=0;
-		for (int row=0;row<m_rowNonZeroElements1.size();row++)
+		for (int row=0;row<rows();row++)
 		{
-			for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
+			for (int col=0;col<row;col++)
 			{
-				int col = m_rowNonZeroElements1[row][j];
 				setElem(col,row, (*this)(row,col));
 				count++;
-
+				
 			}
 		}
 		//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
 	}
-	void setElem(int row,int col, T val)
-	{
-		m_setElemOperations++;
-		if (val)
-		{
-			if (m_storage[col+row*m_cols]==0.f)
-			{
-				m_rowNonZeroElements1[row].push_back(col);
-				m_colNonZeroElements[col].push_back(row);
-			}
-			m_storage[row*m_cols+col] = val;
-		}
-	}
+	
 	const T& operator() (int row,int col) const
 	{
 		return m_storage[col+row*m_cols];
 	}
 
-	void clearSparseInfo()
-	{
-		BT_PROFILE("clearSparseInfo=0");
-		m_rowNonZeroElements1.resize(m_rows);
-		m_colNonZeroElements.resize(m_cols);
-		for (int i=0;i<m_rows;i++)
-			m_rowNonZeroElements1[i].resize(0);
-		for (int j=0;j<m_cols;j++)
-			m_colNonZeroElements[j].resize(0);
-	}
 
 	void setZero()
 	{
@@ -162,12 +276,21 @@ struct btMatrixX
 			//for (int i=0;i<m_storage.size();i++)
 	//			m_storage[i]=0;
 		}
+	}
+	
+	void setIdentity()
+	{
+		btAssert(rows() == cols());
+		
+		setZero();
+		for (int row=0;row<rows();row++)
 		{
-			BT_PROFILE("clearSparseInfo=0");
-			clearSparseInfo();
+			setElem(row,row,1);
 		}
 	}
 
+	
+
 	void	printMatrix(const char* msg)
 	{
 		printf("%s ---------------------\n",msg);
@@ -182,28 +305,9 @@ struct btMatrixX
 		printf("\n---------------------\n");
 
 	}
-	void	printNumZeros(const char* msg)
-	{
-		printf("%s: ",msg);
-		int numZeros = 0;
-		for (int i=0;i<m_storage.size();i++)
-			if (m_storage[i]==0)
-				numZeros++;
-		int total = m_cols*m_rows;
-		int computedNonZero = total-numZeros;
-		int nonZero = 0;
-		for (int i=0;i<m_colNonZeroElements.size();i++)
-			nonZero += m_colNonZeroElements[i].size();
-		btAssert(computedNonZero==nonZero);
-		if(computedNonZero!=nonZero)
-		{
-			printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero);
-		}
-		//printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows));
-		printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total));
-	}
-	/*
-	void rowComputeNonZeroElements()
+
+
+	void rowComputeNonZeroElements() const
 	{
 		m_rowNonZeroElements1.resize(rows());
 		for (int i=0;i<rows();i++)
@@ -218,13 +322,11 @@ struct btMatrixX
 			}
 		}
 	}
-	*/
 	btMatrixX transpose() const
 	{
 		//transpose is optimized for sparse matrices
 		btMatrixX tr(m_cols,m_rows);
 		tr.setZero();
-#if 0
 		for (int i=0;i<m_cols;i++)
 			for (int j=0;j<m_rows;j++)
 			{
@@ -234,37 +336,13 @@ struct btMatrixX
 					tr.setElem(i,j,v);
 				}
 			}
-#else		
-		for (int i=0;i<m_colNonZeroElements.size();i++)
-			for (int h=0;h<m_colNonZeroElements[i].size();h++)
-			{
-				int j = m_colNonZeroElements[i][h];
-				T v = (*this)(j,i);
-				tr.setElem(i,j,v);
-			}
-#endif
 		return tr;
 	}
 
-	void sortRowIndexArrays()
-	{
-		for (int i=0;i<m_rowNonZeroElements1[i].size();i++)
-		{
-			m_rowNonZeroElements1[i].quickSort(btIntSortPredicate());
-		}
-	}
-
-	void sortColIndexArrays()
-	{
-		for (int i=0;i<m_colNonZeroElements[i].size();i++)
-		{
-			m_colNonZeroElements[i].quickSort(btIntSortPredicate());
-		}
-	}
 
 	btMatrixX operator*(const btMatrixX& other)
 	{
-		//btMatrixX*btMatrixX implementation, optimized for sparse matrices
+		//btMatrixX*btMatrixX implementation, brute force
 		btAssert(cols() == other.rows());
 
 		btMatrixX res(rows(),other.cols());
@@ -272,76 +350,18 @@ struct btMatrixX
 //		BT_PROFILE("btMatrixX mul");
 		for (int j=0; j < res.cols(); ++j)
 		{
-			//int numZero=other.m_colNonZeroElements[j].size();
-			//if (numZero)
 			{
 				for (int i=0; i < res.rows(); ++i)
-				//for (int g = 0;g<m_colNonZeroElements[j].size();g++)
 				{
 					T dotProd=0;
 					T dotProd2=0;
 					int waste=0,waste2=0;
 
-					bool doubleWalk = false;
-					if (doubleWalk)
-					{
-						int numRows = m_rowNonZeroElements1[i].size();
-						int numOtherCols = other.m_colNonZeroElements[j].size();
-						for (int ii=0;ii<numRows;ii++)
-						{
-							int vThis=m_rowNonZeroElements1[i][ii];
-						}
-
-						for (int ii=0;ii<numOtherCols;ii++)
-						{
-							int vOther = other.m_colNonZeroElements[j][ii];
-						}
-
-
-						int indexRow = 0;
-						int indexOtherCol = 0;
-						while (indexRow < numRows && indexOtherCol < numOtherCols)
-						{
-							int vThis=m_rowNonZeroElements1[i][indexRow];
-							int vOther = other.m_colNonZeroElements[j][indexOtherCol];
-							if (vOther==vThis)
-							{
-								dotProd += (*this)(i,vThis) * other(vThis,j);
-							}
-							if (vThis<vOther)
-							{
-								indexRow++;
-							} else
-							{
-								indexOtherCol++;
-							}
-						}
-
-					} else
 					{
 						bool useOtherCol = true;
-						if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size())
-						{
-						useOtherCol=true;
-						}
-						if (!useOtherCol )
 						{
-							for (int q=0;q<other.m_colNonZeroElements[j].size();q++)
+							for (int v=0;v<rows();v++)
 							{
-								int v = other.m_colNonZeroElements[j][q];
-								T w = (*this)(i,v);
-								if (w!=0.f)
-								{
-									dotProd+=w*other(v,j);
-								}
-						
-							}
-						}
-						else
-						{
-							for (int q=0;q<m_rowNonZeroElements1[i].size();q++)
-							{
-								int v=m_rowNonZeroElements1[i][q];
 								T w = (*this)(i,v);
 								if (other(v,j)!=0.f)
 								{
@@ -404,73 +424,61 @@ struct btMatrixX
 			bb += 8;
 		}
 	}
-
-};
-
-template <typename T> 
-struct btVectorX
-{
-	btAlignedObjectArray<T>	m_storage;
-
-	btVectorX()
-	{
-	}
-	btVectorX(int numRows)
-	{
-		m_storage.resize(numRows);
-	}
-
-	void resize(int rows)
-	{
-		m_storage.resize(rows);
-	}
-	int cols() const
-	{
-		return 1;
-	}
-	int rows() const
-	{
-		return m_storage.size();
-	}
-	int size() const
-	{
-		return rows();
-	}
-	void	setZero()
-	{
-	//	for (int i=0;i<m_storage.size();i++)
-	//		m_storage[i]=0;
-		//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
-		btSetZero(&m_storage[0],m_storage.size());
-	}
-	const T& operator[] (int index) const
+	
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
 	{
-		return m_storage[index];
+		int numRows = rowend+1-rowstart;
+		int numCols = colend+1-colstart;
+		
+		for (int row=0;row<numRows;row++)
+		{
+			for (int col=0;col<numCols;col++)
+			{
+				setElem(rowstart+row,colstart+col,value);
+			}
+		}
 	}
-
-	T& operator[] (int index)
+	
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
 	{
-		return m_storage[index];
+		btAssert(rowend+1-rowstart == block.rows());
+		btAssert(colend+1-colstart == block.cols());
+		for (int row=0;row<block.rows();row++)
+		{
+			for (int col=0;col<block.cols();col++)
+			{
+				setElem(rowstart+row,colstart+col,block(row,col));
+			}
+		}
 	}
-
-	T* getBufferPointerWritable() 
+	void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
 	{
-		return m_storage.size() ? &m_storage[0] : 0;
+		btAssert(rowend+1-rowstart == block.rows());
+		btAssert(colend+1-colstart == block.cols());
+		for (int row=0;row<block.rows();row++)
+		{
+			for (int col=0;col<block.cols();col++)
+			{
+				setElem(rowstart+row,colstart+col,block[row]);
+			}
+		}
 	}
-
-	const T* getBufferPointer() const
+	
+	
+	btMatrixX negative()
 	{
-		return m_storage.size() ? &m_storage[0] : 0;
+		btMatrixX neg(rows(),cols());
+		for (int i=0;i<rows();i++)
+			for (int j=0;j<cols();j++)
+			{
+				T v = (*this)(i,j);
+				neg.setElem(i,j,-v);
+			}
+		return neg;
 	}
 
 };
-/*
-template <typename T> 
-void setElem(btMatrixX<T>& mat, int row, int col, T val)
-{
-	mat.setElem(row,col,val);
-}
-*/
+
 
 
 typedef btMatrixX<float> btMatrixXf;
@@ -480,6 +488,47 @@ typedef btMatrixX<double> btMatrixXd;
 typedef btVectorX<double> btVectorXd;
 
 
+#ifdef BT_DEBUG_OSTREAM
+template <typename T> 
+std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
+	{
+		
+		os << " [";
+		//printf("%s ---------------------\n",msg);
+		for (int i=0;i<mat.rows();i++)
+		{
+			for (int j=0;j<mat.cols();j++)
+			{
+				os << std::setw(12) << mat(i,j);
+			}
+			if (i!=mat.rows()-1)
+				os << std::endl << "  ";
+		}
+		os << " ]";
+		//printf("\n---------------------\n");
+
+		return os;
+	}
+template <typename T> 
+std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
+	{
+		
+		os << " [";
+		//printf("%s ---------------------\n",msg);
+		for (int i=0;i<mat.rows();i++)
+		{
+				os << std::setw(12) << mat[i];
+			if (i!=mat.rows()-1)
+				os << std::endl << "  ";
+		}
+		os << " ]";
+		//printf("\n---------------------\n");
+
+		return os;
+	}
+
+#endif //BT_DEBUG_OSTREAM
+
 
 inline void setElem(btMatrixXd& mat, int row, int col, double val)
 {
diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h
index 37c6dec191d..401e11eaaa8 100644
--- a/extern/bullet2/src/LinearMath/btScalar.h
+++ b/extern/bullet2/src/LinearMath/btScalar.h
@@ -284,6 +284,10 @@ static int btNanMask = 0x7F800001;
 #ifndef BT_INFINITY
 static  int btInfinityMask = 0x7F800000;
 #define BT_INFINITY (*(float*)&btInfinityMask)
+inline int btGetInfinityMask()//suppress stupid compiler warning
+{
+	return btInfinityMask;
+}
 #endif
 
 //use this, in case there are clashes (such as xnamath.h)
@@ -336,6 +340,10 @@ inline __m128 operator * (const __m128 A, const __m128 B)
 	#ifndef BT_INFINITY
 	static  int btInfinityMask = 0x7F800000;
 	#define BT_INFINITY (*(float*)&btInfinityMask)
+	inline int btGetInfinityMask()//suppress stupid compiler warning
+	{
+		return btInfinityMask;
+	}
 	#endif
 #endif//BT_USE_NEON
 
@@ -432,7 +440,7 @@ SIMD_FORCE_INLINE btScalar btFmod(btScalar x,btScalar y) { return fmodf(x,y); }
 #endif
 
 #define SIMD_PI           btScalar(3.1415926535897932384626433832795029)
-#define SIMD_2_PI         btScalar(2.0) * SIMD_PI
+#define SIMD_2_PI         (btScalar(2.0) * SIMD_PI)
 #define SIMD_HALF_PI      (SIMD_PI * btScalar(0.5))
 #define SIMD_RADS_PER_DEG (SIMD_2_PI / btScalar(360.0))
 #define SIMD_DEGS_PER_RAD  (btScalar(360.0) / SIMD_2_PI)
@@ -728,4 +736,5 @@ template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
 	return converter.ptr;
 }
 
+
 #endif //BT_SCALAR_H
diff --git a/extern/bullet2/src/LinearMath/btVector3.cpp b/extern/bullet2/src/LinearMath/btVector3.cpp
index a84f7baee2e..2ba7029c9be 100644
--- a/extern/bullet2/src/LinearMath/btVector3.cpp
+++ b/extern/bullet2/src/LinearMath/btVector3.cpp
@@ -821,6 +821,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
 
 
 #elif defined BT_USE_NEON
+
 #define ARM_NEON_GCC_COMPATIBILITY  1
 #include <arm_neon.h>
 #include <sys/types.h>
@@ -884,7 +885,12 @@ static long _mindot_large_sel( const float *vv, const float *vec, unsigned long
 
 
 
-#define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32  {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
+#if defined __arm__
+# define vld1q_f32_aligned_postincrement( _ptr ) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
+#else
+//support 64bit arm
+# define vld1q_f32_aligned_postincrement( _ptr) ({ float32x4_t _r = ((float32x4_t*)(_ptr))[0]; (_ptr) = (const float*) ((const char*)(_ptr) + 16L); /*return*/ _r; })
+#endif
 
 
 long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, float *dotResult )
diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h
index 89685929288..112b70dd66b 100644
--- a/extern/bullet2/src/LinearMath/btVector3.h
+++ b/extern/bullet2/src/LinearMath/btVector3.h
@@ -297,7 +297,7 @@ public:
 	SIMD_FORCE_INLINE btVector3& normalize() 
 	{
 		
-		btAssert(length() != btScalar(0));
+		btAssert(!fuzzyZero());
 
 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)		
         // dot product first
@@ -685,9 +685,10 @@ public:
 		return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
 	}
 
+
 	SIMD_FORCE_INLINE bool fuzzyZero() const 
 	{
-		return length2() < SIMD_EPSILON;
+		return length2() < SIMD_EPSILON*SIMD_EPSILON;
 	}
 
 	SIMD_FORCE_INLINE	void	serialize(struct	btVector3Data& dataOut) const;
@@ -950,9 +951,9 @@ SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
 
 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
 {
-	btVector3 norm = *this;
+	btVector3 nrm = *this;
 
-	return norm.normalize();
+	return nrm.normalize();
 } 
 
 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
@@ -1010,21 +1011,21 @@ SIMD_FORCE_INLINE   long    btVector3::maxDot( const btVector3 *array, long arra
     if( array_count < scalar_cutoff )	
 #endif
     {
-        btScalar maxDot = -SIMD_INFINITY;
+        btScalar maxDot1 = -SIMD_INFINITY;
         int i = 0;
         int ptIndex = -1;
         for( i = 0; i < array_count; i++ )
         {
             btScalar dot = array[i].dot(*this);
             
-            if( dot > maxDot )
+            if( dot > maxDot1 )
             {
-                maxDot = dot;
+                maxDot1 = dot;
                 ptIndex = i;
             }
         }
         
-        dotOut = maxDot;
+        dotOut = maxDot1;
         return ptIndex;
     }
 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)