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;