fix: improve the lexico-minimum finder in LemkeAlgorithm.

The following issues in btLemkeAlgorithm::findLexicographicMinimum() are addressed in this commit.

- The z_0 column located at A(*,2*dim), is used for lexico minimum test, which should not be used.
- If the check on the 1st column 'q' produces multiple minimums due to generacy,
  and if it contains z_0, the tie must be broken in favor of z_0 and return,
  which is not performed in the original.

- The 1st part to construct 'Rows' is unnecessary.
- The 2nd part has three nested loops with brute-force search, which is inefficient.

The improved version performs the lexico minimum test column-by-column
from the left-most column of (-B^{-1}q:B^{-1}).
If the test finds a single row, it returns it.
If the test finds multiple rows and if they contain z_0, it returns z_0.
Otherwise, it moves on to the next column.
Please see  2.2.2 "Pivot Step" of
    K. Murty, "Linear Complementarity, Linear and Nonlinear Programming" Heldermann Verlag, Berlin, 1998
for the algorithm details.

Some performance tests are done and non-negligible improvment was observed.
Please see https://github.com/ShoYamanishi/AppleNumericalComputing/tree/main/14_lcp for the test details.

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);