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)