1 files changed, 274 insertions, 60 deletions

diff --git a/examples/BlockSolver/btBlockSolver.cpp b/examples/BlockSolver/btBlockSolver.cpp
index f908cc487..8cf144c14 100644
--- a/examples/BlockSolver/btBlockSolver.cpp
+++ b/examples/BlockSolver/btBlockSolver.cpp
@@ -1,7 +1,17 @@
 #include "btBlockSolver.h"
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+
 #include "LinearMath/btQuickprof.h"
 
+void setupHelper(btSISolverSingleIterationData& siData,
+				 btCollisionObject** bodies, int numBodies,
+				 const btContactSolverInfo& info,
+				 btTypedConstraint** constraintStart, int constrainNums,
+				 btPersistentManifold** manifoldPtr, int numManifolds);
+
 struct btBlockSolverInternalData
 {
 	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
@@ -13,86 +23,273 @@ struct btBlockSolverInternalData
 	btAlignedObjectArray<int> m_orderTmpConstraintPool;
 	btAlignedObjectArray<int> m_orderNonContactConstraintPool;
 	btAlignedObjectArray<int> m_orderFrictionConstraintPool;
-	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
-	
-	
+	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>
+		m_tmpConstraintSizesPool;
+
 	unsigned long m_btSeed2;
 	int m_fixedBodyId;
 	int m_maxOverrideNumSolverIterations;
-	btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
+	btAlignedObjectArray<int>
+		m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
 
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
 	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
 	btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
 
 	btBlockSolverInternalData()
-		:m_btSeed2(0),
-		m_fixedBodyId(-1),
-		m_maxOverrideNumSolverIterations(0),
-		m_resolveSingleConstraintRowGeneric(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()),
-		m_resolveSingleConstraintRowLowerLimit(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()),
-		m_resolveSplitPenetrationImpulse(btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric())
-	{
-	}
+		: m_btSeed2(0),
+		  m_fixedBodyId(-1),
+		  m_maxOverrideNumSolverIterations(0),
+		  m_resolveSingleConstraintRowGeneric(
+			  btSequentialImpulseConstraintSolver::
+				  getScalarConstraintRowSolverGeneric()),
+		  m_resolveSingleConstraintRowLowerLimit(
+			  btSequentialImpulseConstraintSolver::
+				  getScalarConstraintRowSolverLowerLimit()),
+		  m_resolveSplitPenetrationImpulse(
+			  btSequentialImpulseConstraintSolver::
+				  getScalarSplitPenetrationImpulseGeneric()) {}
 };
 
-
-
-
 btBlockSolver::btBlockSolver()
 {
-	m_data2 = new btBlockSolverInternalData;
+	m_data21 = new btBlockSolverInternalData;
+	m_data22 = new btBlockSolverInternalData;
 }
 
 btBlockSolver::~btBlockSolver()
 {
-	delete m_data2;
+	delete m_data21;
+	delete m_data22;
 }
 
+btScalar btBlockSolver::solveGroupInternalBlock(
+	btCollisionObject** bodies, int numBodies,
+	btPersistentManifold** manifoldPtr, int numManifolds,
+	btTypedConstraint** constraints, int numConstraints,
+	const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
+	btDispatcher* dispatcher)
+{
+	// initialize data for two children solvers
+	btSISolverSingleIterationData siData1(
+		m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool,
+		m_data21->m_tmpSolverNonContactConstraintPool,
+		m_data21->m_tmpSolverContactFrictionConstraintPool,
+		m_data21->m_tmpSolverContactRollingFrictionConstraintPool,
+		m_data21->m_orderTmpConstraintPool,
+		m_data21->m_orderNonContactConstraintPool,
+		m_data21->m_orderFrictionConstraintPool,
+		m_data21->m_tmpConstraintSizesPool,
+		m_data21->m_resolveSingleConstraintRowGeneric,
+		m_data21->m_resolveSingleConstraintRowLowerLimit,
+		m_data21->m_resolveSplitPenetrationImpulse,
+		m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2,
+		m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations);
+
+	btSISolverSingleIterationData siData2(
+		m_data22->m_tmpSolverBodyPool, m_data22->m_tmpSolverContactConstraintPool,
+		m_data22->m_tmpSolverNonContactConstraintPool,
+		m_data22->m_tmpSolverContactFrictionConstraintPool,
+		m_data22->m_tmpSolverContactRollingFrictionConstraintPool,
+		m_data22->m_orderTmpConstraintPool,
+		m_data22->m_orderNonContactConstraintPool,
+		m_data22->m_orderFrictionConstraintPool,
+		m_data22->m_tmpConstraintSizesPool,
+		m_data22->m_resolveSingleConstraintRowGeneric,
+		m_data22->m_resolveSingleConstraintRowLowerLimit,
+		m_data22->m_resolveSplitPenetrationImpulse,
+		m_data22->m_kinematicBodyUniqueIdToSolverBodyTable, m_data22->m_btSeed2,
+		m_data22->m_fixedBodyId, m_data22->m_maxOverrideNumSolverIterations);
+
+	m_data21->m_fixedBodyId = -1;
+	m_data22->m_fixedBodyId = -1;
+
+	// set up
+	int halfNumConstraints1 = numConstraints / 2;
+	int halfNumConstraints2 = numConstraints - halfNumConstraints1;
+
+	int halfNumManifolds1 = numConstraints / 2;
+	int halfNumManifolds2 = numManifolds - halfNumManifolds1;
+
+	setupHelper(siData1, bodies, numBodies, info, constraints,
+				halfNumConstraints1, manifoldPtr, halfNumManifolds1);
+
+	setupHelper(siData2, bodies, numBodies, info,
+				constraints + halfNumConstraints1, halfNumConstraints2,
+				manifoldPtr + halfNumManifolds1, halfNumManifolds2);
+	// set up complete
+
+	// begin solve
+	btScalar leastSquaresResidual = 0;
+	{
+		BT_PROFILE("solveGroupCacheFriendlyIterations");
+		/// this is a special step to resolve penetrations (just for contacts)
+		btSequentialImpulseConstraintSolver::
+			solveGroupCacheFriendlySplitImpulseIterationsInternal(
+				siData1, bodies, numBodies, manifoldPtr, halfNumManifolds1,
+				constraints, halfNumConstraints1, info, debugDrawer);
+
+		btSequentialImpulseConstraintSolver::
+			solveGroupCacheFriendlySplitImpulseIterationsInternal(
+				siData2, bodies, numBodies, manifoldPtr + halfNumManifolds1,
+				halfNumManifolds2, constraints + halfNumConstraints1,
+				halfNumConstraints2, info, debugDrawer);
+
+		int maxIterations =
+			siData1.m_maxOverrideNumSolverIterations > info.m_numIterations
+				? siData1.m_maxOverrideNumSolverIterations
+				: info.m_numIterations;
+
+		for (int iteration = 0; iteration < maxIterations; iteration++)
+		{
+			btScalar res1 =
+				btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
+					siData1, iteration, constraints, halfNumConstraints1, info);
+
+			btScalar res2 =
+				btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
+					siData2, iteration, constraints + halfNumConstraints1,
+					halfNumConstraints2, info);
+			leastSquaresResidual = btMax(res1, res2);
+
+			if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold ||
+				(iteration >= (maxIterations - 1)))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual,
+					   iteration);
+#endif
+				break;
+			}
+		}
+	}
 
-btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+	btScalar res = btSequentialImpulseConstraintSolver::
+		solveGroupCacheFriendlyFinishInternal(siData1, bodies, numBodies, info);
+	+btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(
+		siData2, bodies, numBodies, info);
+
+	return res;
+}
+
+void setupHelper(btSISolverSingleIterationData& siData,
+				 btCollisionObject** bodies, int numBodies,
+				 const btContactSolverInfo& info,
+				 btTypedConstraint** constraintStart, int constrainNums,
+				 btPersistentManifold** manifoldPtr, int numManifolds)
 {
+	btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies,
+															   numBodies, info);
+	btSequentialImpulseConstraintSolver::convertJointsInternal(
+		siData, constraintStart, constrainNums, info);
 
-	btSISolverSingleIterationData siData(m_data2->m_tmpSolverBodyPool,
-		m_data2->m_tmpSolverContactConstraintPool,
-		m_data2->m_tmpSolverNonContactConstraintPool,
-		m_data2->m_tmpSolverContactFrictionConstraintPool,
-		m_data2->m_tmpSolverContactRollingFrictionConstraintPool,
-		m_data2->m_orderTmpConstraintPool,
-		m_data2->m_orderNonContactConstraintPool,
-		m_data2->m_orderFrictionConstraintPool,
-		m_data2->m_tmpConstraintSizesPool,
-		m_data2->m_resolveSingleConstraintRowGeneric,
-		m_data2->m_resolveSingleConstraintRowLowerLimit,
-		m_data2->m_resolveSplitPenetrationImpulse,
-		m_data2->m_kinematicBodyUniqueIdToSolverBodyTable,
-		m_data2->m_btSeed2,
-		m_data2->m_fixedBodyId,
-		m_data2->m_maxOverrideNumSolverIterations);
-	
-	m_data2->m_fixedBodyId = -1;
-	//todo: setup sse2/4 constraint row methods
-
-	btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies, numBodies, info);
-	btSequentialImpulseConstraintSolver::convertJointsInternal(siData, constraints, numConstraints, info);
-	
 	int i;
 	btPersistentManifold* manifold = 0;
-	//			btCollisionObject* colObj0=0,*colObj1=0;
 
 	for (i = 0; i < numManifolds; i++)
 	{
 		manifold = manifoldPtr[i];
-		btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, info);
+		btSequentialImpulseConstraintSolver::convertContactInternal(siData,
+																	manifold, info);
+
+		int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
+		int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
+		int numFrictionPool =
+			siData.m_tmpSolverContactFrictionConstraintPool.size();
+
+		siData.m_orderNonContactConstraintPool.resizeNoInitialize(
+			numNonContactPool);
+		if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+			siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
+		else
+			siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
+
+		siData.m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
+		{
+			int i;
+			for (i = 0; i < numNonContactPool; i++)
+			{
+				siData.m_orderNonContactConstraintPool[i] = i;
+			}
+			for (i = 0; i < numConstraintPool; i++)
+			{
+				siData.m_orderTmpConstraintPool[i] = i;
+			}
+			for (i = 0; i < numFrictionPool; i++)
+			{
+				siData.m_orderFrictionConstraintPool[i] = i;
+			}
+		}
 	}
+}
 
+btScalar btBlockSolver::solveGroup(btCollisionObject** bodies, int numBodies,
+								   btPersistentManifold** manifoldPtr,
+								   int numManifolds,
+								   btTypedConstraint** constraints,
+								   int numConstraints,
+								   const btContactSolverInfo& info,
+								   btIDebugDraw* debugDrawer,
+								   btDispatcher* dispatcher)
+{
+	// if (m_childSolvers.size())
+	// hard code to use block solver for now
+	return solveGroupInternalBlock(bodies, numBodies, manifoldPtr, numManifolds,
+								   constraints, numConstraints, info, debugDrawer,
+								   dispatcher);
+	//  else
+	//    return solveGroupInternal(bodies, numBodies, manifoldPtr, numManifolds,
+	//                             constraints, numConstraints, info, debugDrawer,
+	//                             dispatcher);
+}
+
+btScalar btBlockSolver::solveGroupInternal(
+	btCollisionObject** bodies, int numBodies,
+	btPersistentManifold** manifoldPtr, int numManifolds,
+	btTypedConstraint** constraints, int numConstraints,
+	const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
+	btDispatcher* dispatcher)
+{
+	btSISolverSingleIterationData siData(
+		m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool,
+		m_data21->m_tmpSolverNonContactConstraintPool,
+		m_data21->m_tmpSolverContactFrictionConstraintPool,
+		m_data21->m_tmpSolverContactRollingFrictionConstraintPool,
+		m_data21->m_orderTmpConstraintPool,
+		m_data21->m_orderNonContactConstraintPool,
+		m_data21->m_orderFrictionConstraintPool,
+		m_data21->m_tmpConstraintSizesPool,
+		m_data21->m_resolveSingleConstraintRowGeneric,
+		m_data21->m_resolveSingleConstraintRowLowerLimit,
+		m_data21->m_resolveSplitPenetrationImpulse,
+		m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2,
+		m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations);
+
+	m_data21->m_fixedBodyId = -1;
+	// todo: setup sse2/4 constraint row methods
+
+	btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies,
+															   numBodies, info);
+	btSequentialImpulseConstraintSolver::convertJointsInternal(
+		siData, constraints, numConstraints, info);
+
+	int i;
+	btPersistentManifold* manifold = 0;
+	// btCollisionObject* colObj0=0,*colObj1=0;
 
+	for (i = 0; i < numManifolds; i++)
+	{
+		manifold = manifoldPtr[i];
+		btSequentialImpulseConstraintSolver::convertContactInternal(siData,
+																	manifold, info);
+	}
 
 	int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
 	int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size();
 
-	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+	// @todo: use stack allocator for such temporarily memory, same for solver
+	// bodies/constraints
 	siData.m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
 	if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 		siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
@@ -118,43 +315,60 @@ btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, b
 
 	btScalar leastSquaresResidual = 0;
 
-	
-
 	{
 		BT_PROFILE("solveGroupCacheFriendlyIterations");
-		///this is a special step to resolve penetrations (just for contacts)
-		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, info, debugDrawer);
+		/// this is a special step to resolve penetrations (just for contacts)
+		btSequentialImpulseConstraintSolver::
+			solveGroupCacheFriendlySplitImpulseIterationsInternal(
+				siData, bodies, numBodies, manifoldPtr, numManifolds, constraints,
+				numConstraints, info, debugDrawer);
 
-		int maxIterations = siData.m_maxOverrideNumSolverIterations > info.m_numIterations ? siData.m_maxOverrideNumSolverIterations : info.m_numIterations;
+		int maxIterations =
+			siData.m_maxOverrideNumSolverIterations > info.m_numIterations
+				? siData.m_maxOverrideNumSolverIterations
+				: info.m_numIterations;
 
 		for (int iteration = 0; iteration < maxIterations; iteration++)
 		{
-			leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, info);
+			leastSquaresResidual =
+				btSequentialImpulseConstraintSolver::solveSingleIterationInternal(
+					siData, iteration, constraints, numConstraints, info);
 
-			if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+			if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold ||
+				(iteration >= (maxIterations - 1)))
 			{
 #ifdef VERBOSE_RESIDUAL_PRINTF
-				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual,
+					   iteration);
 #endif
 				break;
 			}
 		}
 	}
 
-	btScalar res = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info);
+	btScalar res = btSequentialImpulseConstraintSolver::
+		solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info);
 	return res;
 }
 
-
-
-void btBlockSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+void btBlockSolver::solveMultiBodyGroup(
+	btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold,
+	int numManifolds, btTypedConstraint** constraints, int numConstraints,
+	btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints,
+	const btContactSolverInfo& info, btIDebugDraw* debugDrawer,
+	btDispatcher* dispatcher)
 {
-	btMultiBodyConstraintSolver::solveMultiBodyGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, dispatcher);
+	btMultiBodyConstraintSolver::solveMultiBodyGroup(
+		bodies, numBodies, manifold, numManifolds, constraints, numConstraints,
+		multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer,
+		dispatcher);
 }
 
 void btBlockSolver::reset()
 {
-	//or just set m_data2->m_btSeed2=0?
-	delete m_data2;
-	m_data2 = new btBlockSolverInternalData;
+	// or just set m_data2->m_btSeed2=0?
+	delete m_data21;
+	delete m_data22;
+	m_data21 = new btBlockSolverInternalData;
+	m_data22 = new btBlockSolverInternalData;
 }