catch up with latest Bullet 2.x (not in use yet)

7 files changed, 183 insertions, 30 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
index 8d22689ec53..1b85a0eea42 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp
@@ -104,7 +104,7 @@ float resolveSingleCollision(
 	
 	btScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
-	float damping = solverInfo.m_damping ;
+	// float damping = solverInfo.m_damping ;
 	float Kerp = solverInfo.m_erp;
 	float Kcor = Kerp *Kfps;
 
@@ -136,7 +136,7 @@ float resolveSingleCollision(
 	{
 		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
 	}
-#else USE_INTERNAL_APPLY_IMPULSE
+#else //USE_INTERNAL_APPLY_IMPULSE
 	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
 	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
 #endif //USE_INTERNAL_APPLY_IMPULSE
@@ -215,7 +215,7 @@ float resolveSingleFriction(
 		body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
 		body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);	
 	}
-#else USE_INTERNAL_APPLY_IMPULSE
+#else //USE_INTERNAL_APPLY_IMPULSE
 	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
 	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
 #endif //USE_INTERNAL_APPLY_IMPULSE
@@ -317,7 +317,7 @@ float resolveSingleCollisionCombined(
 	
 	btScalar Kfps = 1.f / solverInfo.m_timeStep ;
 
-	float damping = solverInfo.m_damping ;
+	//float damping = solverInfo.m_damping ;
 	float Kerp = solverInfo.m_erp;
 	float Kcor = Kerp *Kfps;
 
@@ -350,7 +350,7 @@ float resolveSingleCollisionCombined(
 	{
 		body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
 	}
-#else USE_INTERNAL_APPLY_IMPULSE
+#else //USE_INTERNAL_APPLY_IMPULSE
 	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
 	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
 #endif //USE_INTERNAL_APPLY_IMPULSE
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 568bcc40939..cf33b1d674e 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -63,8 +63,14 @@ bool  MyContactDestroyedCallback(void* userPersistentData)
 	return true;
 }
 
+btSequentialImpulseConstraintSolver2::btSequentialImpulseConstraintSolver2()
+{
+	setSolverMode(SOLVER_USE_WARMSTARTING);
+}
+
 
 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
+:m_solverMode(SOLVER_RANDMIZE_ORDER)
 {
 	gContactDestroyedCallback = &MyContactDestroyedCallback;
 
@@ -92,57 +98,127 @@ float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** man
 
 	int totalPoints = 0;
 
+
 	{
 		int j;
 		for (j=0;j<numManifolds;j++)
 		{
-			prepareConstraints(manifoldPtr[j],info,debugDrawer);
+			btPersistentManifold* manifold = manifoldPtr[j];
+			prepareConstraints(manifold,info,debugDrawer);
+			for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
+			{
+				gOrder[totalPoints].m_manifoldIndex = j;
+				gOrder[totalPoints].m_pointIndex = p;
+				totalPoints++;
+			}
 		}
 	}
+	
+	//should traverse the contacts random order...
+	int iteration;
+
+	{
+		for ( iteration = 0;iteration<numiter-1;iteration++)
+		{
+			int j;
+			if (m_solverMode & SOLVER_RANDMIZE_ORDER)
+			{
+				if ((iteration & 7) == 0) {
+					for (j=0; j<totalPoints; ++j) {
+						btOrderIndex tmp = gOrder[j];
+						int swapi = btRandInt2(j+1);
+						gOrder[j] = gOrder[swapi];
+						gOrder[swapi] = tmp;
+					}
+				}
+			}
+
+			for (j=0;j<totalPoints;j++)
+			{
+				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			}
+		
+			for (j=0;j<totalPoints;j++)
+			{
+				btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
+				solveFriction((btRigidBody*)manifold->getBody0(),
+					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			}
+		}
+	}
+		
+#ifdef USE_PROFILE
+	btProfiler::endBlock("solve");
+#endif //USE_PROFILE
+
+	return 0.f;
+}
+
+
+/// btSequentialImpulseConstraintSolver Sequentially applies impulses
+float btSequentialImpulseConstraintSolver2::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
+{
+	
+	btContactSolverInfo info = infoGlobal;
+
+	int numiter = infoGlobal.m_numIterations;
+#ifdef USE_PROFILE
+	btProfiler::beginBlock("solve");
+#endif //USE_PROFILE
 
 	{
 		int j;
 		for (j=0;j<numManifolds;j++)
 		{
+			btPersistentManifold* manifold = manifoldPtr[j];
+			prepareConstraints(manifold,info,debugDrawer);
 			for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
 			{
-				gOrder[totalPoints].m_manifoldIndex = j;
-				gOrder[totalPoints].m_pointIndex = p;
-				totalPoints++;
+				//interleaving here gives better results
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer);
 			}
 		}
 	}
 	
-	
 	//should traverse the contacts random order...
 	int iteration;
+
 	for ( iteration = 0;iteration<numiter-1;iteration++)
 	{
 		int j;
-		if ((iteration & 7) == 0) {
-			for (j=0; j<totalPoints; ++j) {
-				btOrderIndex tmp = gOrder[j];
-				int swapi = btRandInt2(j+1);
-				gOrder[j] = gOrder[swapi];
-				gOrder[swapi] = tmp;
-			}
-		}
 
-		for (j=0;j<totalPoints;j++)
+		for (j=0;j<numManifolds;j++)
 		{
-			btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-			solve( (btRigidBody*)manifold->getBody0(),
-								(btRigidBody*)manifold->getBody1()
-			,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			btPersistentManifold* manifold = manifoldPtr[j];
+			for (int p=0;p<manifold->getNumContacts();p++)
+			{
+				solve( (btRigidBody*)manifold->getBody0(),
+									(btRigidBody*)manifold->getBody1()
+				,manifold->getContactPoint(p),info,iteration,debugDrawer);
+			}
 		}
 	
-		for (j=0;j<totalPoints;j++)
+	}
+
+	for ( iteration = 0;iteration<numiter-1;iteration++)
+	{
+		int j;
+		for (j=0;j<numManifolds;j++)
 		{
-			btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
-			solveFriction((btRigidBody*)manifold->getBody0(),
-				(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
+			btPersistentManifold* manifold = manifoldPtr[j];
+			for (int p=0;p<manifold->getNumContacts();p++)
+			{
+				solveFriction((btRigidBody*)manifold->getBody0(),
+					(btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer);
+			}
 		}
 	}
+
 		
 #ifdef USE_PROFILE
 	btProfiler::endBlock("solve");
@@ -264,7 +340,14 @@ void	btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifol
 				
 
 				float relaxation = info.m_damping;
-				cpd->m_appliedImpulse =0.f;//*= relaxation;
+				if (m_solverMode & SOLVER_USE_WARMSTARTING)
+				{
+					cpd->m_appliedImpulse *= relaxation;
+				} else
+				{
+					cpd->m_appliedImpulse =0.f;
+				}
+	
 				//for friction
 				cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
 				
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index 8264f5071dd..bcc3806b923 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -30,6 +30,7 @@ class btIDebugDraw;
 class btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
 
+protected:
 	float solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 	float solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer);
 	void  prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer);
@@ -37,8 +38,18 @@ class btSequentialImpulseConstraintSolver : public btConstraintSolver
 	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;
+
 public:
 
+	enum	eSolverMode
+	{
+		SOLVER_RANDMIZE_ORDER = 1,
+		SOLVER_FRICTION_SEPARATE = 2,
+		SOLVER_USE_WARMSTARTING = 4
+	};
+
 	btSequentialImpulseConstraintSolver();
 
 	///Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
@@ -59,7 +70,29 @@ public:
 	
 	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
 
+	void	setSolverMode(int mode)
+	{
+		m_solverMode = mode;
+	}
+
+	int	getSolverMode() const
+	{
+		return m_solverMode;
+	}
+};
+
+/// Small variation on btSequentialImpulseConstraintSolver: warmstarting, separate friction, non-randomized ordering
+class btSequentialImpulseConstraintSolver2 : public btSequentialImpulseConstraintSolver
+{
+public:
+
+	btSequentialImpulseConstraintSolver2();
+
+	virtual float solveGroup(btPersistentManifold** manifold,int numManifolds,const btContactSolverInfo& info, btIDebugDraw* debugDrawer=0);
+
+
 };
 
+
 #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
index 125e933ad90..bdde013aadb 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
@@ -267,8 +267,14 @@ void	btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
 
+	btDispatcherInfo	dispatchInfo;
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
+
 	///perform collision detection
-	performDiscreteCollisionDetection();
+	performDiscreteCollisionDetection(dispatchInfo);
 
 	calculateSimulationIslands();
 
@@ -828,6 +834,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform,
 	}
 }
 
+
 void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 {
 	if (m_ownsConstraintSolver)
@@ -838,3 +845,15 @@ void	btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
 	m_constraintSolver = solver;
 }
 
+int		btDiscreteDynamicsWorld::getNumConstraints() const
+{
+	return int(m_constraints.size());
+}
+btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
+{
+	return m_constraints[index];
+}
+const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
+{
+	return m_constraints[index];
+}
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
index 12939753442..4aa83683cd5 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
@@ -137,6 +137,13 @@ public:
 	void	debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color);
 
 	virtual void	setConstraintSolver(btConstraintSolver* solver);
+	
+	virtual	int		getNumConstraints() const;
+
+	virtual btTypedConstraint* getConstraint(int index)	;
+
+	virtual const btTypedConstraint* getConstraint(int index) const;
+
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 6023f29b4fb..a991fdca79d 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -68,6 +68,12 @@ class btDynamicsWorld : public btCollisionWorld
 		virtual void	removeRigidBody(btRigidBody* body) = 0;
 
 		virtual void	setConstraintSolver(btConstraintSolver* solver) = 0;
+		
+		virtual	int		getNumConstraints() const {	return 0;		}
+		
+		virtual btTypedConstraint* getConstraint(int index)		{			return 0;		}
+		
+		virtual const btTypedConstraint* getConstraint(int index) const	{		return 0;	}
 
 };
 
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
index dabf291669d..fe6a73939a7 100644
--- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
+++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp
@@ -52,8 +52,13 @@ int		btSimpleDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, floa
 	///apply gravity, predict motion
 	predictUnconstraintMotion(timeStep);
 
+	btDispatcherInfo	dispatchInfo;
+	dispatchInfo.m_timeStep = timeStep;
+	dispatchInfo.m_stepCount = 0;
+	dispatchInfo.m_debugDraw = getDebugDrawer();
+
 	///perform collision detection
-	performDiscreteCollisionDetection();
+	performDiscreteCollisionDetection(dispatchInfo );
 
 	///solve contact constraints
 	int numManifolds = m_dispatcher1->getNumManifolds();