24 #undef BT_DEBUG_OSTREAM 25 #ifdef BT_DEBUG_OSTREAM 27 #endif //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;
70 #endif //BT_DEBUG_OSTREAM 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;
94 #endif //BT_DEBUG_OSTREAM 101 for (
int i = 0; i < dim; i++)
104 int pivotRowIndex = -1;
106 bool greaterZero =
true;
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] <<
" ";
134 #endif //BT_DEBUG_OSTREAM 145 for (steps = 0; steps < maxloops; steps++)
147 GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
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] <<
" ";
159 #endif //BT_DEBUG_OSTREAM 161 int pivotColIndexOld = pivotColIndex;
164 if (basis[pivotRowIndex] < dim)
165 pivotColIndex = basis[pivotRowIndex] + dim;
168 pivotColIndex = basis[pivotRowIndex] - dim;
171 basis[pivotRowIndex] = pivotColIndexOld;
173 pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
175 if (z0Row == pivotRowIndex)
177 GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
178 basis[pivotRowIndex] = pivotColIndex;
182 #ifdef BT_DEBUG_OSTREAM 185 cout <<
"Number of loops: " << steps << endl;
186 cout <<
"Number of maximal loops: " << maxloops << endl;
188 #endif //BT_DEBUG_OSTREAM 190 if (!validBasis(basis))
193 #ifdef BT_DEBUG_OSTREAM 195 cerr <<
"Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
196 #endif //BT_DEBUG_OSTREAM 198 return solutionVector;
201 #ifdef BT_DEBUG_OSTREAM 205 cout <<
"pivotRowIndex " << pivotRowIndex << endl;
206 cout <<
"pivotColIndex " << pivotColIndex << endl;
208 #endif //BT_DEBUG_OSTREAM 210 for (
int i = 0; i < basis.
size(); i++)
212 solutionVector[basis[i]] = A(i, 2 * dim + 1);
217 return solutionVector;
225 for (
int row = 0; row < dim; row++)
233 Rows[row][0] = A(row, 2 * dim + 1) / a;
234 Rows[row][1] = A(row, 2 * dim) / a;
235 for (
int j = 2; j < dim + 1; j++)
236 Rows[row][j] = A(row, j - 1) / a;
238 #ifdef BT_DEBUG_OSTREAM 246 for (
int i = 0; i < Rows.
size(); i++)
248 if (Rows[i].nrm2() > 0.)
251 for (; j < Rows.
size(); j++)
255 if (Rows[j].nrm2() > 0.)
258 for (
int ii = 0; ii < dim + 1; ii++)
260 test[ii] = Rows[j][ii] - Rows[i][ii];
264 if (!LexicographicPositive(test))
270 if (j == Rows.
size())
287 while (i < v.size() - 1 && fabs(v[i]) <
btMachEps())
297 btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
298 #ifdef BT_DEBUG_OSTREAM 299 cout << A << std::endl;
302 for (
int i = 0; i < A.rows(); i++)
304 if (i != pivotRowIndex)
306 for (
int j = 0; j < A.cols(); j++)
308 if (j != pivotColumnIndex)
311 v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
318 #ifdef BT_DEBUG_OSTREAM 319 cout << A << std::endl;
320 #endif //BT_DEBUG_OSTREAM 321 for (
int i = 0; i < A.cols(); i++)
323 A.mulElem(pivotRowIndex, i, -a);
325 #ifdef BT_DEBUG_OSTREAM 326 cout << A << std::endl;
327 #endif //#ifdef BT_DEBUG_OSTREAM 329 for (
int i = 0; i < A.rows(); i++)
331 if (i != pivotRowIndex)
333 A.setElem(i, pivotColumnIndex, 0);
336 #ifdef BT_DEBUG_OSTREAM 337 cout << A << std::endl;
338 #endif //#ifdef BT_DEBUG_OSTREAM 343 bool isGreater =
true;
344 for (
int i = 0; i < vector.size(); i++)
359 for (
int i = 0; i < basis.
size(); i++)
361 if (basis[i] >= basis.
size() * 2)
void push_back(const T &_Val)
btScalar btSqrt(btScalar y)
int findLexicographicMinimum(const btMatrixXu &A, const int &pivotColIndex)
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemkeās Algorithm for Rigid Body Contact Simu...
int size() const
return the number of elements in the array
bool LexicographicPositive(const btVectorXu &v)
void GaussJordanEliminationStep(btMatrixXu &A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray< int > &basis)
bool validBasis(const btAlignedObjectArray< int > &basis)
bool greaterZero(const btVectorXu &vector)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...