dali-physics/third-party/bullet3/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 #include "btLemkeAlgorithm.h"
  23
  24 #undef BT_DEBUG_OSTREAM
  25 #ifdef BT_DEBUG_OSTREAM
  26 using namespace std;
  27 #endif  //BT_DEBUG_OSTREAM
  28
  29 btScalar btMachEps()
  30 {
  31         static bool calculated = false;
  32         static btScalar machEps = btScalar(1.);
  33         if (!calculated)
  34         {
  35                 do
  36                 {
  37                         machEps /= btScalar(2.0);
  38                         // If next epsilon yields 1, then break, because current
  39                         // epsilon is the machine epsilon.
  40                 } while ((btScalar)(1.0 + (machEps / btScalar(2.0))) != btScalar(1.0));
  41                 //              printf( "\nCalculated Machine epsilon: %G\n", machEps );
  42                 calculated = true;
  43         }
  44         return machEps;
  45 }
  46
  47 btScalar btEpsRoot()
  48 {
  49         static btScalar epsroot = 0.;
  50         static bool alreadyCalculated = false;
  51
  52         if (!alreadyCalculated)
  53         {
  54                 epsroot = btSqrt(btMachEps());
  55                 alreadyCalculated = true;
  56         }
  57         return epsroot;
  58 }
  59
  60 btVectorXu btLemkeAlgorithm::solve(unsigned int maxloops /* = 0*/)
  61 {
  62         steps = 0;
  63
  64         int dim = m_q.size();
  65 #ifdef BT_DEBUG_OSTREAM
  66         if (DEBUGLEVEL >= 1)
  67         {
  68                 cout << "Dimension = " << dim << endl;
  69         }
  70 #endif  //BT_DEBUG_OSTREAM
  71
  72         btVectorXu solutionVector(2 * dim);
  73         solutionVector.setZero();
  74
  75         //, INIT, 0.);
  76
  77         btMatrixXu ident(dim, dim);
  78         ident.setIdentity();
  79 #ifdef BT_DEBUG_OSTREAM
  80         cout << m_M << std::endl;
  81 #endif
  82
  83         btMatrixXu mNeg = m_M.negative();
  84
  85         btMatrixXu A(dim, 2 * dim + 2);
  86         //
  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);
  91
  92 #ifdef BT_DEBUG_OSTREAM
  93         cout << A << std::endl;
  94 #endif  //BT_DEBUG_OSTREAM
  95
  96         //   btVectorXu q_;
  97         //   q_ >> A(0, 2 * dim + 1, dim - 1, 2 * dim + 1);
  98
  99         btAlignedObjectArray<int> basis;
 100         //At first, all w-values are in the basis
 101         for (int i = 0; i < dim; i++)
 102                 basis.push_back(i);
 103
 104         int pivotRowIndex = -1;
 105         btScalar minValue = 1e30f;
 106         bool greaterZero = true;
 107         for (int i = 0; i < dim; i++)
 108         {
 109                 btScalar v = A(i, 2 * dim + 1);
 110                 if (v < minValue)
 111                 {
 112                         minValue = v;
 113                         pivotRowIndex = i;
 114                 }
 115                 if (v < 0)
 116                         greaterZero = false;
 117         }
 118
 119         //  int pivotRowIndex = q_.minIndex();//minIndex(q_);     // first row is that with lowest q-value
 120         int z0Row = pivotRowIndex;    // remember the col of z0 for ending algorithm afterwards
 121         int pivotColIndex = 2 * dim;  // first col is that of z0
 122
 123 #ifdef BT_DEBUG_OSTREAM
 124         if (DEBUGLEVEL >= 3)
 125         {
 126                 //  cout << "A: " << A << endl;
 127                 cout << "pivotRowIndex " << pivotRowIndex << endl;
 128                 cout << "pivotColIndex " << pivotColIndex << endl;
 129                 cout << "Basis: ";
 130                 for (int i = 0; i < basis.size(); i++)
 131                         cout << basis[i] << " ";
 132                 cout << endl;
 133         }
 134 #endif  //BT_DEBUG_OSTREAM
 135
 136         if (!greaterZero)
 137         {
 138                 if (maxloops == 0)
 139                 {
 140                         maxloops = 100;
 141                         //        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...
 142                 }
 143
 144                 /*start looping*/
 145                 for (steps = 0; steps < maxloops; steps++)
 146                 {
 147                         GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
 148 #ifdef BT_DEBUG_OSTREAM
 149                         if (DEBUGLEVEL >= 3)
 150                         {
 151                                 //  cout << "A: " << A << endl;
 152                                 cout << "pivotRowIndex " << pivotRowIndex << endl;
 153                                 cout << "pivotColIndex " << pivotColIndex << endl;
 154                                 cout << "Basis: ";
 155                                 for (int i = 0; i < basis.size(); i++)
 156                                         cout << basis[i] << " ";
 157                                 cout << endl;
 158                         }
 159 #endif  //BT_DEBUG_OSTREAM
 160
 161                         int pivotColIndexOld = pivotColIndex;
 162
 163                         /*find new column index */
 164                         if (basis[pivotRowIndex] < dim)  //if a w-value left the basis get in the correspondent z-value
 165                                 pivotColIndex = basis[pivotRowIndex] + dim;
 166                         else
 167                                 //else do it the other way round and get in the corresponding w-value
 168                                 pivotColIndex = basis[pivotRowIndex] - dim;
 169
 170                         /*the column becomes part of the basis*/
 171                         basis[pivotRowIndex] = pivotColIndexOld;
 172                         bool isRayTermination = false;
 173                         pivotRowIndex = findLexicographicMinimum(A, pivotColIndex, z0Row, isRayTermination);
 174                         if (isRayTermination)
 175                         {
 176                                 break; // ray termination
 177                         }
 178                         if (z0Row == pivotRowIndex)
 179                         {  //if z0 leaves the basis the solution is found --> one last elimination step is necessary
 180                                 GaussJordanEliminationStep(A, pivotRowIndex, pivotColIndex, basis);
 181                                 basis[pivotRowIndex] = pivotColIndex;  //update basis
 182                                 break;
 183                         }
 184                 }
 185 #ifdef BT_DEBUG_OSTREAM
 186                 if (DEBUGLEVEL >= 1)
 187                 {
 188                         cout << "Number of loops: " << steps << endl;
 189                         cout << "Number of maximal loops: " << maxloops << endl;
 190                 }
 191 #endif  //BT_DEBUG_OSTREAM
 192
 193                 if (!validBasis(basis))
 194                 {
 195                         info = -1;
 196 #ifdef BT_DEBUG_OSTREAM
 197                         if (DEBUGLEVEL >= 1)
 198                                 cerr << "Lemke-Algorithm ended with Ray-Termination (no valid solution)." << endl;
 199 #endif  //BT_DEBUG_OSTREAM
 200
 201                         return solutionVector;
 202                 }
 203         }
 204 #ifdef BT_DEBUG_OSTREAM
 205         if (DEBUGLEVEL >= 2)
 206         {
 207                 // cout << "A: " << A << endl;
 208                 cout << "pivotRowIndex " << pivotRowIndex << endl;
 209                 cout << "pivotColIndex " << pivotColIndex << endl;
 210         }
 211 #endif  //BT_DEBUG_OSTREAM
 212
 213         for (int i = 0; i < basis.size(); i++)
 214         {
 215                 solutionVector[basis[i]] = A(i, 2 * dim + 1);  //q_[i];
 216         }
 217
 218         info = 0;
 219
 220         return solutionVector;
 221 }
 222
 223 int btLemkeAlgorithm::findLexicographicMinimum(const btMatrixXu& A, const int& pivotColIndex, const int& z0Row, bool& isRayTermination)
 224 {
 225         isRayTermination = false;
 226         btAlignedObjectArray<int> activeRows;
 227
 228         bool firstRow = true;
 229         btScalar currentMin = 0.0;
 230
 231         int dim = A.rows();
 232
 233         for (int row = 0; row < dim; row++)
 234         {
 235                 const btScalar denom = A(row, pivotColIndex);
 236
 237                 if (denom > btMachEps())
 238                 {
 239                         const btScalar q = A(row, dim + dim + 1) / denom;
 240                         if (firstRow)
 241                         {
 242                                 currentMin = q;
 243                                 activeRows.push_back(row);
 244                                 firstRow = false;
 245                         }
 246                         else if (fabs(currentMin - q) < btMachEps())
 247                         {
 248                                 activeRows.push_back(row);
 249                         }
 250                         else if (currentMin > q)
 251                         {
 252                                 currentMin = q;
 253                                 activeRows.clear();
 254                                 activeRows.push_back(row);
 255                         }
 256                 }
 257         }
 258
 259         if (activeRows.size() == 0)
 260         {
 261                 isRayTermination = true;
 262                 return 0;
 263         }
 264         else if (activeRows.size() == 1)
 265         {
 266                 return activeRows[0];
 267         }
 268
 269         // if there are multiple rows, check if they contain the row for z_0.
 270         for (int i = 0; i < activeRows.size(); i++)
 271         {
 272                 if (activeRows[i] == z0Row)
 273                 {
 274                         return z0Row;
 275                 }
 276         }
 277
 278         // look through the columns of the inverse of the basic matrix from left to right until the tie is broken.
 279         for (int col = 0; col < dim ; col++)
 280         {
 281                 btAlignedObjectArray<int> activeRowsCopy(activeRows);
 282                 activeRows.clear();
 283                 firstRow = true;
 284                 for (int i = 0; i<activeRowsCopy.size();i++)
 285                 {
 286                         const int row = activeRowsCopy[i];
 287
 288                         // denom is positive here as an invariant.
 289                         const btScalar denom = A(row, pivotColIndex);
 290                         const btScalar ratio = A(row, col) / denom;
 291                         if (firstRow)
 292                         {
 293                                 currentMin = ratio;
 294                                 activeRows.push_back(row);
 295                                 firstRow = false;
 296                         }
 297                         else if (fabs(currentMin - ratio) < btMachEps())
 298                         {
 299                                 activeRows.push_back(row);
 300                         }
 301                         else if (currentMin > ratio)
 302                         {
 303                                 currentMin = ratio;
 304                                 activeRows.clear();
 305                                 activeRows.push_back(row);
 306                         }
 307                 }
 308
 309                 if (activeRows.size() == 1)
 310                 {
 311                         return activeRows[0];
 312                 }
 313         }
 314         // must not reach here.
 315         isRayTermination = true;
 316         return 0;
 317 }
 318
 319 void btLemkeAlgorithm::GaussJordanEliminationStep(btMatrixXu& A, int pivotRowIndex, int pivotColumnIndex, const btAlignedObjectArray<int>& basis)
 320 {
 321         btScalar a = -1 / A(pivotRowIndex, pivotColumnIndex);
 322 #ifdef BT_DEBUG_OSTREAM
 323         cout << A << std::endl;
 324 #endif
 325
 326         for (int i = 0; i < A.rows(); i++)
 327         {
 328                 if (i != pivotRowIndex)
 329                 {
 330                         for (int j = 0; j < A.cols(); j++)
 331                         {
 332                                 if (j != pivotColumnIndex)
 333                                 {
 334                                         btScalar v = A(i, j);
 335                                         v += A(pivotRowIndex, j) * A(i, pivotColumnIndex) * a;
 336                                         A.setElem(i, j, v);
 337                                 }
 338                         }
 339                 }
 340         }
 341
 342 #ifdef BT_DEBUG_OSTREAM
 343         cout << A << std::endl;
 344 #endif  //BT_DEBUG_OSTREAM
 345         for (int i = 0; i < A.cols(); i++)
 346         {
 347                 A.mulElem(pivotRowIndex, i, -a);
 348         }
 349 #ifdef BT_DEBUG_OSTREAM
 350         cout << A << std::endl;
 351 #endif  //#ifdef BT_DEBUG_OSTREAM
 352
 353         for (int i = 0; i < A.rows(); i++)
 354         {
 355                 if (i != pivotRowIndex)
 356                 {
 357                         A.setElem(i, pivotColumnIndex, 0);
 358                 }
 359         }
 360 #ifdef BT_DEBUG_OSTREAM
 361         cout << A << std::endl;
 362 #endif  //#ifdef BT_DEBUG_OSTREAM
 363 }
 364
 365 bool btLemkeAlgorithm::greaterZero(const btVectorXu& vector)
 366 {
 367         bool isGreater = true;
 368         for (int i = 0; i < vector.size(); i++)
 369         {
 370                 if (vector[i] < 0)
 371                 {
 372                         isGreater = false;
 373                         break;
 374                 }
 375         }
 376
 377         return isGreater;
 378 }
 379
 380 bool btLemkeAlgorithm::validBasis(const btAlignedObjectArray<int>& basis)
 381 {
 382         bool isValid = true;
 383         for (int i = 0; i < basis.size(); i++)
 384         {
 385                 if (basis[i] >= basis.size() * 2)
 386                 {  //then z0 is in the base
 387                         isValid = false;
 388                         break;
 389                 }
 390         }
 391
 392         return isValid;
 393 }