26 #undef BT_DEBUG_OSTREAM 27 #ifdef BT_DEBUG_OSTREAM 29 #endif //BT_DEBUG_OSTREAM 33 static bool calculated=
false;
52 static bool alreadyCalculated =
false;
54 if (!alreadyCalculated) {
56 alreadyCalculated =
true;
70 #ifdef BT_DEBUG_OSTREAM 72 cout <<
"Dimension = " << dim << endl;
74 #endif //BT_DEBUG_OSTREAM 77 solutionVector.setZero();
83 #ifdef BT_DEBUG_OSTREAM 84 cout << m_M << std::endl;
91 A.setSubMatrix(0, 0, dim - 1, dim - 1,ident);
92 A.setSubMatrix(0, dim, dim - 1, 2 * dim - 1,mNeg);
93 A.setSubMatrix(0, 2 * dim, dim - 1, 2 * dim, -1.f);
94 A.setSubMatrix(0, 2 * dim + 1, dim - 1, 2 * dim + 1,m_q);
96 #ifdef BT_DEBUG_OSTREAM 97 cout << A << std::endl;
98 #endif //BT_DEBUG_OSTREAM 106 for (
int i = 0; i < dim; i++)
109 int pivotRowIndex = -1;
111 bool greaterZero =
true;
112 for (
int i=0;i<dim;i++)
127 int z0Row = pivotRowIndex;
128 int pivotColIndex = 2 * dim;
130 #ifdef BT_DEBUG_OSTREAM 134 cout <<
"pivotRowIndex " << pivotRowIndex << endl;
135 cout <<
"pivotColIndex " << pivotColIndex << endl;
137 for (
int i = 0; i < basis.
size(); i++)
138 cout << basis[i] <<
" ";
141 #endif //BT_DEBUG_OSTREAM 152 for(steps = 0; steps < maxloops; steps++) {
154 GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
155 #ifdef BT_DEBUG_OSTREAM 156 if (DEBUGLEVEL >= 3) {
158 cout <<
"pivotRowIndex " << pivotRowIndex << endl;
159 cout <<
"pivotColIndex " << pivotColIndex << endl;
161 for (
int i = 0; i < basis.
size(); i++)
162 cout << basis[i] <<
" ";
165 #endif //BT_DEBUG_OSTREAM 167 int pivotColIndexOld = pivotColIndex;
170 if (basis[pivotRowIndex] < dim)
171 pivotColIndex = basis[pivotRowIndex] + dim;
174 pivotColIndex = basis[pivotRowIndex] - dim;
177 basis[pivotRowIndex] = pivotColIndexOld;
179 pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
181 if(z0Row == pivotRowIndex) {
182 GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
183 basis[pivotRowIndex] = pivotColIndex;
188 #ifdef BT_DEBUG_OSTREAM 189 if(DEBUGLEVEL >= 1) {
190 cout <<
"Number of loops: " << steps << endl;
191 cout <<
"Number of maximal loops: " << maxloops << endl;
193 #endif //BT_DEBUG_OSTREAM 195 if(!validBasis(basis)) {
197 #ifdef BT_DEBUG_OSTREAM 199 cerr <<
"Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
200 #endif //BT_DEBUG_OSTREAM 202 return solutionVector;
206 #ifdef BT_DEBUG_OSTREAM 207 if (DEBUGLEVEL >= 2) {
209 cout <<
"pivotRowIndex " << pivotRowIndex << endl;
210 cout <<
"pivotColIndex " << pivotColIndex << endl;
212 #endif //BT_DEBUG_OSTREAM 214 for (
int i = 0; i < basis.
size(); i++)
216 solutionVector[basis[i]] = A(i,2*dim+1);
221 return solutionVector;
228 for (
int row = 0; row < dim; row++)
236 Rows[row][0] = A(row, 2 * dim + 1) / a;
237 Rows[row][1] = A(row, 2 * dim) / a;
238 for (
int j = 2; j < dim + 1; j++)
239 Rows[row][j] = A(row, j - 1) / a;
241 #ifdef BT_DEBUG_OSTREAM 249 for (
int i = 0; i < Rows.
size(); i++)
251 if (Rows[i].nrm2() > 0.) {
254 for (; j < Rows.
size(); j++)
258 if(Rows[j].nrm2() > 0.)
261 for (
int ii=0;ii<dim+1;ii++)
263 test[ii] = Rows[j][ii] - Rows[i][ii];
267 if (! LexicographicPositive(test))
273 if (j == Rows.
size())
290 while(i < v.size()-1 && fabs(v[i]) <
btMachEps())
301 btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
302 #ifdef BT_DEBUG_OSTREAM 303 cout << A << std::endl;
306 for (
int i = 0; i < A.rows(); i++)
308 if (i != pivotRowIndex)
310 for (
int j = 0; j < A.cols(); j++)
312 if (j != pivotColumnIndex)
315 v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
322 #ifdef BT_DEBUG_OSTREAM 323 cout << A << std::endl;
324 #endif //BT_DEBUG_OSTREAM 325 for (
int i = 0; i < A.cols(); i++)
327 A.mulElem(pivotRowIndex, i,-a);
329 #ifdef BT_DEBUG_OSTREAM 330 cout << A << std::endl;
331 #endif //#ifdef BT_DEBUG_OSTREAM 333 for (
int i = 0; i < A.rows(); i++)
335 if (i != pivotRowIndex)
337 A.setElem(i, pivotColumnIndex,0);
340 #ifdef BT_DEBUG_OSTREAM 341 cout << A << std::endl;
342 #endif //#ifdef BT_DEBUG_OSTREAM 347 bool isGreater =
true;
348 for (
int i = 0; i < vector.size(); i++) {
361 for (
int i = 0; i < basis.
size(); i++) {
362 if (basis[i] >= basis.
size() * 2) {
void push_back(const T &_Val)
btScalar btSqrt(btScalar y)
int findLexicographicMinimum(const btMatrixXu &A, const int &pivotColIndex)
int size() const
return the number of elements in the array
btVectorXu solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemkeās Algorithm for Rigid Body Contact Simul...
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...