Remove a temporary global static work matrix in the BussIK, since it conflicts with multithreaded applications.

Instead, let the user pass it in explicitly.

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -243,7 +243,7 @@ void Jacobian::UpdateThetaDot()
 	m_tree->Compute();
 }
 
-void Jacobian::CalcDeltaThetas()
+void Jacobian::CalcDeltaThetas(MatrixRmn& AugMat)
 {
 	switch (CurrentUpdateMode)
 	{
@@ -257,7 +257,7 @@ void Jacobian::CalcDeltaThetas()
 			CalcDeltaThetasPseudoinverse();
 			break;
 		case JACOB_DLS:
-			CalcDeltaThetasDLS();
+			CalcDeltaThetasDLS(AugMat);
 			break;
 		case JACOB_SDLS:
 			CalcDeltaThetasSDLS();
@@ -327,7 +327,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse()
 	}
 }
 
-void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
+void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat)
 {
 	const MatrixRmn& J = ActiveJacobian();
 
@@ -341,7 +341,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
 	// J.MultiplyTranspose( dTextra, dTheta );
 
 	// Use these two lines for the traditional DLS method
-	U.Solve(dS, &dT1);
+	U.Solve(dS, &dT1, AugMat);
 	J.MultiplyTranspose(dT1, dTheta);
 
 	// Compute JInv in damped least square form
@@ -379,7 +379,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
 	}
 }
 
-void Jacobian::CalcDeltaThetasDLS()
+void Jacobian::CalcDeltaThetasDLS(MatrixRmn& AugMat)
 {
 	const MatrixRmn& J = ActiveJacobian();
 
@@ -393,7 +393,7 @@ void Jacobian::CalcDeltaThetasDLS()
 	// J.MultiplyTranspose( dTextra, dTheta );
 
 	// Use these two lines for the traditional DLS method
-	U.Solve(dS, &dT1);
+	U.Solve(dS, &dT1, AugMat);
 	J.MultiplyTranspose(dT1, dTheta);
 
 	// Scale back to not exceed maximum angle changes
@@ -404,7 +404,7 @@ void Jacobian::CalcDeltaThetasDLS()
 	}
 }
 
-void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
+void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat)
 {
 	const MatrixRmn& J = ActiveJacobian();
 
@@ -414,7 +414,7 @@ void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
 
 	dT1.SetLength(J.GetNumColumns());
 	J.MultiplyTranspose(dS, dT1);
-	U.Solve(dT1, &dTheta);
+	U.Solve(dT1, &dTheta, AugMat);
 
 	// Scale back to not exceed maximum angle changes
 	double maxChange = dTheta.MaxAbs();

examples/ThirdPartyLibs/BussIK/Jacobian.h

@@ -66,15 +66,15 @@ public:
 	void SetJendTrans(MatrixRmn& J);
 	void SetDeltaS(VectorRn& S);
 
-	void CalcDeltaThetas();  // Use this only if the Current Mode has been set.
+	void CalcDeltaThetas(MatrixRmn& AugMat);  // Use this only if the Current Mode has been set.
 	void ZeroDeltaThetas();
 	void CalcDeltaThetasTranspose();
 	void CalcDeltaThetasPseudoinverse();
-	void CalcDeltaThetasDLS();
-	void CalcDeltaThetasDLS2(const VectorRn& dVec);
+	void CalcDeltaThetasDLS(MatrixRmn& AugMat);
+	void CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat);
 	void CalcDeltaThetasDLSwithSVD();
 	void CalcDeltaThetasSDLS();
-	void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV);
+	void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat);
 
 	void UpdateThetas();
 	void UpdateThetaDot();

examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp

@@ -30,7 +30,6 @@ subject to the following restrictions:
 
 #include "MatrixRmn.h"
 
-MatrixRmn MatrixRmn::WorkMatrix;  // Temporary work matrix
 
 // Fill the diagonal entries with the value d.  The rest of the matrix is unchanged.
 void MatrixRmn::SetDiagonalEntries(double d)
@@ -354,12 +353,14 @@ MatrixRmn& MatrixRmn::MultiplyTranspose(const MatrixRmn& A, const MatrixRmn& B,
 // Solves the equation   (*this)*xVec = b;
 // Uses row operations.  Assumes *this is square and invertible.
 // No error checking for divide by zero or instability (except with asserts)
-void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec) const
+void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec, MatrixRmn& AugMat) const
 {
+	B3_PROFILE("MatrixRmn::Solve");
 	assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength());
 
 	// Copy this matrix and b into an Augmented Matrix
-	MatrixRmn& AugMat = GetWorkMatrix(NumRows, NumCols + 1);
+	
+	AugMat.SetSize(NumRows, NumCols + 1);
 	AugMat.LoadAsSubmatrix(*this);
 	AugMat.SetColumn(NumRows, b);
 

examples/ThirdPartyLibs/BussIK/MatrixRmn.h

@@ -117,7 +117,7 @@ public:
 	MatrixRmn& AddToDiagonal(const VectorRn& dVec);
 
 	// Solving systems of linear equations
-	void Solve(const VectorRn& b, VectorRn* x) const;  // Solves the equation   (*this)*x = b;    Uses row operations.  Assumes *this is invertible.
+	void Solve(const VectorRn& b, VectorRn* x, MatrixRmn& AugMat) const;  // Solves the equation   (*this)*x = b;    Uses row operations.  Assumes *this is invertible.
 
 	// Row Echelon Form and Reduced Row Echelon Form routines
 	// Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables.
@@ -150,13 +150,6 @@ private:
 	double* x;       // Array of vector entries - stored in column order
 	long AllocSize;  // Allocated size of the x array
 
-	static MatrixRmn WorkMatrix;  // Temporary work matrix
-	static MatrixRmn& GetWorkMatrix() { return WorkMatrix; }
-	static MatrixRmn& GetWorkMatrix(long numRows, long numCols)
-	{
-		WorkMatrix.SetSize(numRows, numCols);
-		return WorkMatrix;
-	}
 
 	// Internal helper routines for SVD calculations
 	static void CalcBidiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag);