Merge pull request #4295 from ShoYamanishi/improvement_lemke_leaving_variable

fix: improve the lexico-minimum finder in LemkeAlgorithm.

2 files changed, 84 insertions, 61 deletions

diff --git a/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp b/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
index 954ffaed7..1007d04e3 100644
--- a/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
+++ b/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp
@@ -169,9 +169,12 @@ btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
 
 			/*the column becomes part of the basis*/
 			basis[pivotRowIndex] = pivotColIndexOld;
-
-			pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
-
+			bool isRayTermination = false;
+			pivotRowIndex = findLexicographicMinimum(A, pivotColIndex, z0Row, isRayTermination);
+			if (isRayTermination)
+			{
+				break; // ray termination
+			}
 			if (z0Row == pivotRowIndex)
 			{  //if z0 leaves the basis the solution is found --> one last elimination step is necessary
 				GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
@@ -217,79 +220,100 @@ btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
 	return solutionVector;
 }
 
-int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex)
+int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex, const int& z0Row, bool& isRayTermination)
 {
-	int RowIndex = 0;
+	isRayTermination = false;
+	btAlignedObjectArray<int> activeRows;
+
+        bool firstRow = true;
+	btScalar currentMin = 0.0;
+
 	int dim = A.rows();
-	btAlignedObjectArray<btVectorXu> Rows;
+
 	for (int row = 0; row < dim; row++)
 	{
-		btVectorXu vec(dim + 1);
-		vec.setZero();  //, INIT, 0.)
-		Rows.push_back(vec);
-		btScalar a = A(row, pivotColIndex);
-		if (a > 0)
+		const btScalar denom = A(row, pivotColIndex);
+
+		if (denom > btMachEps())
 		{
-			Rows[row][0] = A(row, 2 * dim + 1) / a;
-			Rows[row][1] = A(row, 2 * dim) / a;
-			for (int j = 2; j < dim + 1; j++)
-				Rows[row][j] = A(row, j - 1) / a;
+			const btScalar q = A(row, dim + dim + 1) / denom;
+			if (firstRow)
+			{
+				currentMin = q;
+				activeRows.push_back(row);
+				firstRow = false;
+			}
+			else if (fabs(currentMin - q) < btMachEps())
+			{
+				activeRows.push_back(row);
+			}
+			else if (currentMin > q)
+			{
+				currentMin = q;
+				activeRows.clear();
+				activeRows.push_back(row);
+			}
+		}
+	}
 
-#ifdef BT_DEBUG_OSTREAM
-				//		if (DEBUGLEVEL) {
-				//	  cout << "Rows(" << row << ") = " << Rows[row] << endl;
-				// }
-#endif
+	if (activeRows.size() == 0)
+	{
+		isRayTermination = true;
+		return 0;
+	}
+	else if (activeRows.size() == 1)
+	{
+		return activeRows[0];
+	}
+
+	// if there are multiple rows, check if they contain the row for z_0.
+	for (int i = 0; i < activeRows.size(); i++)
+	{
+		if (activeRows[i] == z0Row)
+		{
+			return z0Row;
 		}
 	}
 
-	for (int i = 0; i < Rows.size(); i++)
+	// look through the columns of the inverse of the basic matrix from left to right until the tie is broken.
+	for (int col = 0; col < dim ; col++)
 	{
-		if (Rows[i].nrm2() > 0.)
+		btAlignedObjectArray<int> activeRowsCopy(activeRows);
+		activeRows.clear();
+		firstRow = true;
+		for (int i = 0; i<activeRowsCopy.size();i++)
 		{
-			int j = 0;
-			for (; j < Rows.size(); j++)
+			const int row = activeRowsCopy[i];
+
+			// denom is positive here as an invariant.
+			const btScalar denom = A(row, pivotColIndex);
+			const btScalar ratio = A(row, col) / denom;
+			if (firstRow)
 			{
-				if (i != j)
-				{
-					if (Rows[j].nrm2() > 0.)
-					{
-						btVectorXu test(dim + 1);
-						for (int ii = 0; ii < dim + 1; ii++)
-						{
-							test[ii] = Rows[j][ii] - Rows[i][ii];
-						}
-
-						//=Rows[j] - Rows[i]
-						if (!LexicographicPositive(test))
-							break;
-					}
-				}
+				currentMin = ratio;
+				activeRows.push_back(row);
+				firstRow = false;
 			}
-
-			if (j == Rows.size())
+			else if (fabs(currentMin - ratio) < btMachEps())
 			{
-				RowIndex += i;
-				break;
+				activeRows.push_back(row);
+			}
+			else if (currentMin > ratio)
+			{
+				currentMin = ratio;
+				activeRows.clear();
+				activeRows.push_back(row);
 			}
 		}
-	}
-
-	return RowIndex;
-}
-
-bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu& v)
-{
-	int i = 0;
-	//  if (DEBUGLEVEL)
-	//  cout << "v " << v << endl;
-
-	while (i < v.size() - 1 && fabs(v[i]) < btMachEps())
-		i++;
-	if (v[i] > 0)
-		return true;
 
-	return false;
+		if (activeRows.size() == 1)
+		{
+			return activeRows[0];
+		}
+	}
+	// must not reach here.
+	isRayTermination = true;
+	return 0;
 }
 
 void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
diff --git a/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h b/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
index 3c6bf72a2..6c498dd0a 100644
--- a/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
+++ b/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
@@ -71,8 +71,7 @@ public:
 	}
 
 protected:
-	int findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex);
-	bool LexicographicPositive(const btVectorXu& v);
+	int findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex, const int& z0Row, bool& isRayTermination);
 	void GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis);
 	bool greaterZero(const btVectorXu& vector);
 	bool validBasis(const btAlignedObjectArray<int>& basis);