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)
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...