24#undef BT_DEBUG_OSTREAM
25#ifdef BT_DEBUG_OSTREAM
31 static bool calculated =
false;
50 static bool alreadyCalculated =
false;
52 if (!alreadyCalculated)
55 alreadyCalculated =
true;
65#ifdef BT_DEBUG_OSTREAM
68 cout <<
"Dimension = " << dim << endl;
73 solutionVector.setZero();
79#ifdef BT_DEBUG_OSTREAM
80 cout <<
m_M << std::endl;
87 A.setSubMatrix(0, 0, dim - 1, dim - 1, ident);
88 A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1, mNeg);
89 A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
90 A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,
m_q);
92#ifdef BT_DEBUG_OSTREAM
93 cout << A << std::endl;
101 for (
int i = 0; i < dim; i++)
104 int pivotRowIndex = -1;
107 for (
int i = 0; i < dim; i++)
120 int z0Row = pivotRowIndex;
121 int pivotColIndex = 2 * dim;
123#ifdef BT_DEBUG_OSTREAM
127 cout <<
"pivotRowIndex " << pivotRowIndex << endl;
128 cout <<
"pivotColIndex " << pivotColIndex << endl;
130 for (
int i = 0; i < basis.
size(); i++)
131 cout << basis[i] <<
" ";
148#ifdef BT_DEBUG_OSTREAM
152 cout <<
"pivotRowIndex " << pivotRowIndex << endl;
153 cout <<
"pivotColIndex " << pivotColIndex << endl;
155 for (
int i = 0; i < basis.
size(); i++)
156 cout << basis[i] <<
" ";
161 int pivotColIndexOld = pivotColIndex;
164 if (basis[pivotRowIndex] < dim)
165 pivotColIndex = basis[pivotRowIndex] + dim;
168 pivotColIndex = basis[pivotRowIndex] - dim;
171 basis[pivotRowIndex] = pivotColIndexOld;
172 bool isRayTermination =
false;
174 if (isRayTermination)
178 if (z0Row == pivotRowIndex)
181 basis[pivotRowIndex] = pivotColIndex;
185#ifdef BT_DEBUG_OSTREAM
188 cout <<
"Number of loops: " <<
steps << endl;
189 cout <<
"Number of maximal loops: " << maxloops << endl;
196#ifdef BT_DEBUG_OSTREAM
198 cerr <<
"Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
201 return solutionVector;
204#ifdef BT_DEBUG_OSTREAM
208 cout <<
"pivotRowIndex " << pivotRowIndex << endl;
209 cout <<
"pivotColIndex " << pivotColIndex << endl;
213 for (
int i = 0; i < basis.
size(); i++)
215 solutionVector[basis[i]] = A(i, 2 * dim + 1);
220 return solutionVector;
225 isRayTermination =
false;
228 bool firstRow =
true;
233 for (
int row = 0; row < dim; row++)
235 const btScalar denom = A(row, pivotColIndex);
239 const btScalar q = A(row, dim + dim + 1) / denom;
246 else if (fabs(currentMin - q) <
btMachEps())
250 else if (currentMin > q)
259 if (activeRows.
size() == 0)
261 isRayTermination =
true;
264 else if (activeRows.
size() == 1)
266 return activeRows[0];
270 for (
int i = 0; i < activeRows.
size(); i++)
272 if (activeRows[i] == z0Row)
279 for (
int col = 0; col < dim ; col++)
284 for (
int i = 0; i<activeRowsCopy.
size();i++)
286 const int row = activeRowsCopy[i];
289 const btScalar denom = A(row, pivotColIndex);
290 const btScalar ratio = A(row, col) / denom;
297 else if (fabs(currentMin - ratio) <
btMachEps())
301 else if (currentMin > ratio)
309 if (activeRows.
size() == 1)
311 return activeRows[0];
315 isRayTermination =
true;
321 btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
322#ifdef BT_DEBUG_OSTREAM
323 cout << A << std::endl;
326 for (
int i = 0; i < A.rows(); i++)
328 if (i != pivotRowIndex)
330 for (
int j = 0; j < A.cols(); j++)
332 if (j != pivotColumnIndex)
335 v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
342#ifdef BT_DEBUG_OSTREAM
343 cout << A << std::endl;
345 for (
int i = 0; i < A.cols(); i++)
347 A.mulElem(pivotRowIndex, i, -a);
349#ifdef BT_DEBUG_OSTREAM
350 cout << A << std::endl;
353 for (
int i = 0; i < A.rows(); i++)
355 if (i != pivotRowIndex)
357 A.setElem(i, pivotColumnIndex, 0);
360#ifdef BT_DEBUG_OSTREAM
361 cout << A << std::endl;
367 bool isGreater =
true;
368 for (
int i = 0; i < vector.size(); i++)
383 for (
int i = 0; i < basis.
size(); i++)
385 if (basis[i] >= basis.
size() * 2)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btSqrt(btScalar y)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
void push_back(const T &_Val)
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemkeās Algorithm for Rigid Body Contact Simula...
bool validBasis(const btAlignedObjectArray< int > &basis)
int info
did the algorithm find a solution
int findLexicographicMinimum(const btMatrixXu &A, const int &pivotColIndex, const int &z0Row, bool &isRayTermination)
bool greaterZero(const btVectorXu &vector)
int DEBUGLEVEL
define level of debug output
unsigned int steps
number of steps until the Lemke algorithm found a solution
void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray< int > &basis)