From 2b583fcf89cc031d25a08336385251e29ebb87f5 Mon Sep 17 00:00:00 2001
From: Lunkhound <lunkhound@gmail.com>
Date: Wed, 27 May 2015 18:03:18 -0700
Subject: [PATCH] MLCP solver: make it threadsafe

---
 src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp | 14 +++++++-------
 src/BulletDynamics/MLCPSolvers/btMLCPSolver.h   | 11 +++++++++++
 2 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
index 6688694a92..8adcbaa4b2 100644
--- a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
+++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
@@ -216,12 +216,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		jointNodeArray.reserve(2*m_allConstraintPtrArray.size());
 	}
 
-	static btMatrixXu J3;
+    btMatrixXu& J3 = m_scratchJ3;
 	{
 		BT_PROFILE("J3.resize");
 		J3.resize(2*m,8);
 	}
-	static btMatrixXu JinvM3;
+    btMatrixXu& JinvM3 = m_scratchJInvM3;
 	{
 		BT_PROFILE("JinvM3.resize/setZero");
 
@@ -231,7 +231,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 	}
 	int cur=0;
 	int rowOffset = 0;
-	static btAlignedObjectArray<int> ofs;
+    btAlignedObjectArray<int>& ofs = m_scratchOfs;
 	{
 		BT_PROFILE("ofs resize");
 		ofs.resize(0);
@@ -490,7 +490,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 		}
 	}
  
-	static btMatrixXu Minv;
+    btMatrixXu& Minv = m_scratchMInv;
 	Minv.resize(6*numBodies,6*numBodies);
 	Minv.setZero();
 	for (int i=0;i<numBodies;i++)
@@ -507,7 +507,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 				setElem(Minv,i*6+3+r,i*6+3+c,orgBody? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
 	}
  
-	static btMatrixXu J;
+    btMatrixXu& J = m_scratchJ;
 	J.resize(numConstraintRows,6*numBodies);
 	J.setZero();
  
@@ -542,10 +542,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 		}
 	}
  
-	static btMatrixXu J_transpose;
+    btMatrixXu& J_transpose = m_scratchJTranspose;
 	J_transpose= J.transpose();
 
-	static btMatrixXu tmp;
+    btMatrixXu& tmp = m_scratchTmp;
 
 	{
 		{
diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
index 43e85445ba..c020bc2acf 100644
--- a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
+++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
@@ -44,6 +44,17 @@ class btMLCPSolver : public btSequentialImpulseConstraintSolver
 	int m_fallback;
 	btScalar m_cfm;
 
+    /// The following scratch variables are not stateful -- contents are cleared prior to each use.
+    /// They are only cached here to avoid extra memory allocations and deallocations and to ensure
+    /// that multiple instances of the solver can be run in parallel.
+    btMatrixXu m_scratchJ3;
+    btMatrixXu m_scratchJInvM3;
+    btAlignedObjectArray<int> m_scratchOfs;
+    btMatrixXu m_scratchMInv;
+    btMatrixXu m_scratchJ;
+    btMatrixXu m_scratchJTranspose;
+    btMatrixXu m_scratchTmp;
+
 	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);