Engine/lib/bullet/src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.cpp

   1 /* Copyright (C) 2004-2013 MBSim Development Team
   2
   3 Code was converted for the Bullet Continuous Collision Detection and Physics Library
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 //The original version is here
  17 //https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
  18 //This file is re-distributed under the ZLib license, with permission of the original author
  19 //Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
  20 //STL/std::vector replaced by btAlignedObjectArray
  21
  22
  23
  24 #include "btLemkeAlgorithm.h"
  25
  26 #undef BT_DEBUG_OSTREAM
  27 #ifdef BT_DEBUG_OSTREAM
  28 using namespace std;
  29 #endif //BT_DEBUG_OSTREAM
  30
  31 btScalar btMachEps()
  32 {
  33         static bool calculated=false;
  34         static btScalar machEps = btScalar(1.);
  35         if (!calculated)
  36         {
  37                 do {
  38                         machEps /= btScalar(2.0);
  39                         // If next epsilon yields 1, then break, because current
  40                         // epsilon is the machine epsilon.
  41                 }
  42                 while ((btScalar)(1.0 + (machEps/btScalar(2.0))) != btScalar(1.0));
  43 //              printf( "\nCalculated Machine epsilon: %G\n", machEps );
  44                 calculated=true;
  45         }
  46         return machEps;
  47 }
  48
  49 btScalar btEpsRoot() {
  50
  51         static btScalar epsroot = 0.;
  52         static bool alreadyCalculated = false;
  53
  54         if (!alreadyCalculated) {
  55                 epsroot = btSqrt(btMachEps());
  56                 alreadyCalculated = true;
  57         }
  58         return epsroot;
  59 }
  60
  61
  62
  63   btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
  64 {
  65
  66
  67     steps = 0;
  68
  69     int dim = m_q.size();
  70 #ifdef BT_DEBUG_OSTREAM
  71     if(DEBUGLEVEL >= 1) {
  72       cout << "Dimension = " << dim << endl;
  73     }
  74 #endif //BT_DEBUG_OSTREAM
  75
  76         btVectorXu solutionVector(2 * dim);
  77         solutionVector.setZero();
  78
  79           //, INIT, 0.);
  80
  81         btMatrixXu ident(dim, dim);
  82         ident.setIdentity();
  83 #ifdef BT_DEBUG_OSTREAM
  84         cout << m_M << std::endl;
  85 #endif
  86
  87         btMatrixXu mNeg = m_M.negative();
  88
  89     btMatrixXu A(dim, 2 * dim + 2);
  90         //
  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);
  95
  96 #ifdef BT_DEBUG_OSTREAM
  97         cout << A << std::endl;
  98 #endif //BT_DEBUG_OSTREAM
  99
 100
 101  //   btVectorXu q_;
 102  //   q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
 103
 104     btAlignedObjectArray<int> basis;
 105     //At first, all w-values are in the basis
 106     for (int i = 0; i < dim; i++)
 107       basis.push_back(i);
 108
 109         int pivotRowIndex = -1;
 110         btScalar minValue = 1e30f;
 111         bool greaterZero = true;
 112         for (int i=0;i<dim;i++)
 113         {
 114                 btScalar v =A(i,2*dim+1);
 115                 if (v<minValue)
 116                 {
 117                         minValue=v;
 118                         pivotRowIndex = i;
 119                 }
 120                 if (v<0)
 121                         greaterZero = false;
 122         }
 123
 124
 125
 126   //  int pivotRowIndex = q_.minIndex();//minIndex(q_);     // first row is that with lowest q-value
 127     int z0Row = pivotRowIndex;           // remember the col of z0 for ending algorithm afterwards
 128     int pivotColIndex = 2 * dim;         // first col is that of z0
 129
 130 #ifdef BT_DEBUG_OSTREAM
 131     if (DEBUGLEVEL >= 3)
 132         {
 133     //  cout << "A: " << A << endl;
 134       cout << "pivotRowIndex " << pivotRowIndex << endl;
 135       cout << "pivotColIndex " << pivotColIndex << endl;
 136       cout << "Basis: ";
 137       for (int i = 0; i < basis.size(); i++)
 138         cout << basis[i] << " ";
 139       cout << endl;
 140     }
 141 #endif //BT_DEBUG_OSTREAM
 142
 143         if (!greaterZero)
 144         {
 145
 146       if (maxloops == 0) {
 147                   maxloops = 100;
 148 //        maxloops = UINT_MAX; //TODO: not a really nice way, problem is: maxloops should be 2^dim (=1<<dim), but this could exceed UINT_MAX and thus the result would be 0 and therefore the lemke algorithm wouldn't start but probably would find a solution within less then UINT_MAX steps. Therefore this constant is used as a upper border right now...
 149       }
 150
 151       /*start looping*/
 152       for(steps = 0; steps < maxloops; steps++) {
 153
 154         GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
 155 #ifdef BT_DEBUG_OSTREAM
 156         if (DEBUGLEVEL >= 3) {
 157         //  cout << "A: " << A << endl;
 158           cout << "pivotRowIndex " << pivotRowIndex << endl;
 159           cout << "pivotColIndex " << pivotColIndex << endl;
 160           cout << "Basis: ";
 161           for (int i = 0; i < basis.size(); i++)
 162             cout << basis[i] << " ";
 163           cout << endl;
 164         }
 165 #endif //BT_DEBUG_OSTREAM
 166
 167         int pivotColIndexOld = pivotColIndex;
 168
 169         /*find new column index */
 170         if (basis[pivotRowIndex] < dim) //if a w-value left the basis get in the correspondent z-value
 171           pivotColIndex = basis[pivotRowIndex] + dim;
 172         else
 173           //else do it the other way round and get in the corresponding w-value
 174           pivotColIndex = basis[pivotRowIndex] - dim;
 175
 176         /*the column becomes part of the basis*/
 177         basis[pivotRowIndex] = pivotColIndexOld;
 178
 179         pivotRowIndex = findLexicographicMinimum(A, pivotColIndex);
 180
 181         if(z0Row == pivotRowIndex) { //if z0 leaves the basis the solution is found --> one last elimination step is necessary
 182           GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
 183           basis[pivotRowIndex] = pivotColIndex; //update basis
 184           break;
 185       }
 186
 187       }
 188 #ifdef BT_DEBUG_OSTREAM
 189       if(DEBUGLEVEL >= 1) {
 190         cout << "Number of loops: " << steps << endl;
 191         cout << "Number of maximal loops: " << maxloops << endl;
 192       }
 193 #endif //BT_DEBUG_OSTREAM
 194
 195       if(!validBasis(basis)) {
 196         info = -1;
 197 #ifdef BT_DEBUG_OSTREAM
 198         if(DEBUGLEVEL >= 1)
 199           cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
 200 #endif //BT_DEBUG_OSTREAM
 201
 202         return solutionVector;
 203       }
 204
 205     }
 206 #ifdef BT_DEBUG_OSTREAM
 207     if (DEBUGLEVEL >= 2) {
 208      // cout << "A: " << A << endl;
 209       cout << "pivotRowIndex " << pivotRowIndex << endl;
 210       cout << "pivotColIndex " << pivotColIndex << endl;
 211     }
 212 #endif //BT_DEBUG_OSTREAM
 213
 214     for (int i = 0; i < basis.size(); i++)
 215         {
 216       solutionVector[basis[i]] = A(i,2*dim+1);//q_[i];
 217         }
 218
 219     info = 0;
 220
 221     return solutionVector;
 222   }
 223
 224   int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int & pivotColIndex) {
 225           int RowIndex = 0;
 226           int dim = A.rows();
 227           btAlignedObjectArray<btVectorXu> Rows;
 228           for (int row = 0; row < dim; row++)
 229           {
 230
 231                   btVectorXu vec(dim + 1);
 232                   vec.setZero();//, INIT, 0.)
 233                   Rows.push_back(vec);
 234                   btScalar a = A(row, pivotColIndex);
 235                   if (a > 0) {
 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;
 240
 241 #ifdef BT_DEBUG_OSTREAM
 242                 //              if (DEBUGLEVEL) {
 243                         //        cout << "Rows(" << row << ") = " << Rows[row] << endl;
 244                                 // }
 245 #endif
 246                   }
 247           }
 248
 249           for (int i = 0; i < Rows.size(); i++)
 250           {
 251                   if (Rows[i].nrm2() > 0.) {
 252
 253                           int j = 0;
 254                           for (; j < Rows.size(); j++)
 255                           {
 256                                   if(i != j)
 257                                   {
 258                                           if(Rows[j].nrm2() > 0.)
 259                                           {
 260                                                   btVectorXu test(dim + 1);
 261                                                   for (int ii=0;ii<dim+1;ii++)
 262                                                   {
 263                                                           test[ii] = Rows[j][ii] - Rows[i][ii];
 264                                                   }
 265
 266                                                   //=Rows[j] - Rows[i]
 267                                                   if (! LexicographicPositive(test))
 268                                                           break;
 269                                           }
 270                                   }
 271                           }
 272
 273                           if (j == Rows.size())
 274                           {
 275                                   RowIndex += i;
 276                                   break;
 277                           }
 278                   }
 279           }
 280
 281           return RowIndex;
 282   }
 283
 284   bool btLemkeAlgorithm::LexicographicPositive(const btVectorXu & v)
 285 {
 286     int i = 0;
 287   //  if (DEBUGLEVEL)
 288     //  cout << "v " << v << endl;
 289
 290     while(i < v.size()-1 && fabs(v[i]) < btMachEps())
 291       i++;
 292     if (v[i] > 0)
 293       return true;
 294
 295     return false;
 296   }
 297
 298 void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
 299 {
 300
 301         btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
 302 #ifdef BT_DEBUG_OSTREAM
 303         cout << A << std::endl;
 304 #endif
 305
 306     for (int i = 0; i < A.rows(); i++)
 307         {
 308       if (i != pivotRowIndex)
 309           {
 310         for (int j = 0; j < A.cols(); j++)
 311                 {
 312           if (j != pivotColumnIndex)
 313                   {
 314                           btScalar v = A(i, j);
 315                           v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
 316             A.setElem(i, j, v);
 317                   }
 318                 }
 319           }
 320         }
 321
 322 #ifdef BT_DEBUG_OSTREAM
 323         cout << A << std::endl;
 324 #endif //BT_DEBUG_OSTREAM
 325     for (int i = 0; i < A.cols(); i++)
 326         {
 327       A.mulElem(pivotRowIndex, i,-a);
 328     }
 329 #ifdef BT_DEBUG_OSTREAM
 330         cout << A << std::endl;
 331 #endif //#ifdef BT_DEBUG_OSTREAM
 332
 333     for (int i = 0; i < A.rows(); i++)
 334         {
 335       if (i != pivotRowIndex)
 336           {
 337         A.setElem(i, pivotColumnIndex,0);
 338           }
 339         }
 340 #ifdef BT_DEBUG_OSTREAM
 341         cout << A << std::endl;
 342 #endif //#ifdef BT_DEBUG_OSTREAM
 343   }
 344
 345   bool btLemkeAlgorithm::greaterZero(const btVectorXu & vector)
 346 {
 347     bool isGreater = true;
 348     for (int i = 0; i < vector.size(); i++) {
 349       if (vector[i] < 0) {
 350         isGreater = false;
 351         break;
 352       }
 353     }
 354
 355     return isGreater;
 356   }
 357
 358   bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis)
 359   {
 360     bool isValid = true;
 361     for (int i = 0; i < basis.size(); i++) {
 362       if (basis[i] >= basis.size() * 2) { //then z0 is in the base
 363         isValid = false;
 364         break;
 365       }
 366     }
 367
 368     return isValid;
 369   }
 370
 371