dali-physics/third-party/bullet3/src/BulletDynamics/MLCPSolvers/btLemkeSolver.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
   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 ///original version written by Erwin Coumans, October 2013
  16
  17 #ifndef BT_LEMKE_SOLVER_H
  18 #define BT_LEMKE_SOLVER_H
  19
  20 #include "btMLCPSolverInterface.h"
  21 #include "btLemkeAlgorithm.h"
  22
  23 ///The btLemkeSolver is based on "Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
  24 ///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
  25 ///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
  26 class btLemkeSolver : public btMLCPSolverInterface
  27 {
  28 protected:
  29 public:
  30         btScalar m_maxValue;
  31         int m_debugLevel;
  32         int m_maxLoops;
  33         bool m_useLoHighBounds;
  34
  35         btLemkeSolver()
  36                 : m_maxValue(100000),
  37                   m_debugLevel(0),
  38                   m_maxLoops(1000),
  39                   m_useLoHighBounds(true)
  40         {
  41         }
  42         virtual bool solveMLCP(const btMatrixXu& A, const btVectorXu& b, btVectorXu& x, const btVectorXu& lo, const btVectorXu& hi, const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true)
  43         {
  44                 if (m_useLoHighBounds)
  45                 {
  46                         BT_PROFILE("btLemkeSolver::solveMLCP");
  47                         int n = A.rows();
  48                         if (0 == n)
  49                                 return true;
  50
  51                         bool fail = false;
  52
  53                         btVectorXu solution(n);
  54                         btVectorXu q1;
  55                         q1.resize(n);
  56                         for (int row = 0; row < n; row++)
  57                         {
  58                                 q1[row] = -b[row];
  59                         }
  60
  61                         //              cout << "A" << endl;
  62                         //              cout << A << endl;
  63
  64                         /////////////////////////////////////
  65
  66                         //slow matrix inversion, replace with LU decomposition
  67                         btMatrixXu A1;
  68                         btMatrixXu B(n, n);
  69                         {
  70                                 //BT_PROFILE("inverse(slow)");
  71                                 A1.resize(A.rows(), A.cols());
  72                                 for (int row = 0; row < A.rows(); row++)
  73                                 {
  74                                         for (int col = 0; col < A.cols(); col++)
  75                                         {
  76                                                 A1.setElem(row, col, A(row, col));
  77                                         }
  78                                 }
  79
  80                                 btMatrixXu matrix;
  81                                 matrix.resize(n, 2 * n);
  82                                 for (int row = 0; row < n; row++)
  83                                 {
  84                                         for (int col = 0; col < n; col++)
  85                                         {
  86                                                 matrix.setElem(row, col, A1(row, col));
  87                                         }
  88                                 }
  89
  90                                 btScalar ratio, a;
  91                                 int i, j, k;
  92                                 for (i = 0; i < n; i++)
  93                                 {
  94                                         for (j = n; j < 2 * n; j++)
  95                                         {
  96                                                 if (i == (j - n))
  97                                                         matrix.setElem(i, j, 1.0);
  98                                                 else
  99                                                         matrix.setElem(i, j, 0.0);
 100                                         }
 101                                 }
 102                                 for (i = 0; i < n; i++)
 103                                 {
 104                                         for (j = 0; j < n; j++)
 105                                         {
 106                                                 if (i != j)
 107                                                 {
 108                                                         btScalar v = matrix(i, i);
 109                                                         if (btFuzzyZero(v))
 110                                                         {
 111                                                                 a = 0.000001f;
 112                                                         }
 113                                                         ratio = matrix(j, i) / matrix(i, i);
 114                                                         for (k = 0; k < 2 * n; k++)
 115                                                         {
 116                                                                 matrix.addElem(j, k, -ratio * matrix(i, k));
 117                                                         }
 118                                                 }
 119                                         }
 120                                 }
 121                                 for (i = 0; i < n; i++)
 122                                 {
 123                                         a = matrix(i, i);
 124                                         if (btFuzzyZero(a))
 125                                         {
 126                                                 a = 0.000001f;
 127                                         }
 128                                         btScalar invA = 1.f / a;
 129                                         for (j = 0; j < 2 * n; j++)
 130                                         {
 131                                                 matrix.mulElem(i, j, invA);
 132                                         }
 133                                 }
 134
 135                                 for (int row = 0; row < n; row++)
 136                                 {
 137                                         for (int col = 0; col < n; col++)
 138                                         {
 139                                                 B.setElem(row, col, matrix(row, n + col));
 140                                         }
 141                                 }
 142                         }
 143
 144                         btMatrixXu b1(n, 1);
 145
 146                         btMatrixXu M(n * 2, n * 2);
 147                         for (int row = 0; row < n; row++)
 148                         {
 149                                 b1.setElem(row, 0, -b[row]);
 150                                 for (int col = 0; col < n; col++)
 151                                 {
 152                                         btScalar v = B(row, col);
 153                                         M.setElem(row, col, v);
 154                                         M.setElem(n + row, n + col, v);
 155                                         M.setElem(n + row, col, -v);
 156                                         M.setElem(row, n + col, -v);
 157                                 }
 158                         }
 159
 160                         btMatrixXu Bb1 = B * b1;
 161                         //              q = [ (-B*b1 - lo)'   (hi + B*b1)' ]'
 162
 163                         btVectorXu qq;
 164                         qq.resize(n * 2);
 165                         for (int row = 0; row < n; row++)
 166                         {
 167                                 qq[row] = -Bb1(row, 0) - lo[row];
 168                                 qq[n + row] = Bb1(row, 0) + hi[row];
 169                         }
 170
 171                         btVectorXu z1;
 172
 173                         btMatrixXu y1;
 174                         y1.resize(n, 1);
 175                         btLemkeAlgorithm lemke(M, qq, m_debugLevel);
 176                         {
 177                                 //BT_PROFILE("lemke.solve");
 178                                 lemke.setSystem(M, qq);
 179                                 z1 = lemke.solve(m_maxLoops);
 180                         }
 181                         for (int row = 0; row < n; row++)
 182                         {
 183                                 y1.setElem(row, 0, z1[2 * n + row] - z1[3 * n + row]);
 184                         }
 185                         btMatrixXu y1_b1(n, 1);
 186                         for (int i = 0; i < n; i++)
 187                         {
 188                                 y1_b1.setElem(i, 0, y1(i, 0) - b1(i, 0));
 189                         }
 190
 191                         btMatrixXu x1;
 192
 193                         x1 = B * (y1_b1);
 194
 195                         for (int row = 0; row < n; row++)
 196                         {
 197                                 solution[row] = x1(row, 0);  //n];
 198                         }
 199
 200                         int errorIndexMax = -1;
 201                         int errorIndexMin = -1;
 202                         float errorValueMax = -1e30;
 203                         float errorValueMin = 1e30;
 204
 205                         for (int i = 0; i < n; i++)
 206                         {
 207                                 x[i] = solution[i];
 208                                 volatile btScalar check = x[i];
 209                                 if (x[i] != check)
 210                                 {
 211                                         //printf("Lemke result is #NAN\n");
 212                                         x.setZero();
 213                                         return false;
 214                                 }
 215
 216                                 //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
 217                                 //we need to figure out why it happens, and fix it, or detect it properly)
 218                                 if (x[i] > m_maxValue)
 219                                 {
 220                                         if (x[i] > errorValueMax)
 221                                         {
 222                                                 fail = true;
 223                                                 errorIndexMax = i;
 224                                                 errorValueMax = x[i];
 225                                         }
 226                                         ////printf("x[i] = %f,",x[i]);
 227                                 }
 228                                 if (x[i] < -m_maxValue)
 229                                 {
 230                                         if (x[i] < errorValueMin)
 231                                         {
 232                                                 errorIndexMin = i;
 233                                                 errorValueMin = x[i];
 234                                                 fail = true;
 235                                                 //printf("x[i] = %f,",x[i]);
 236                                         }
 237                                 }
 238                         }
 239                         if (fail)
 240                         {
 241                                 int m_errorCountTimes = 0;
 242                                 if (errorIndexMin < 0)
 243                                         errorValueMin = 0.f;
 244                                 if (errorIndexMax < 0)
 245                                         errorValueMax = 0.f;
 246                                 m_errorCountTimes++;
 247                                 //      printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
 248                                 for (int i = 0; i < n; i++)
 249                                 {
 250                                         x[i] = 0.f;
 251                                 }
 252                         }
 253                         return !fail;
 254                 }
 255                 else
 256
 257                 {
 258                         int dimension = A.rows();
 259                         if (0 == dimension)
 260                                 return true;
 261
 262                         //              printf("================ solving using Lemke/Newton/Fixpoint\n");
 263
 264                         btVectorXu q;
 265                         q.resize(dimension);
 266                         for (int row = 0; row < dimension; row++)
 267                         {
 268                                 q[row] = -b[row];
 269                         }
 270
 271                         btLemkeAlgorithm lemke(A, q, m_debugLevel);
 272
 273                         lemke.setSystem(A, q);
 274
 275                         btVectorXu solution = lemke.solve(m_maxLoops);
 276
 277                         //check solution
 278
 279                         bool fail = false;
 280                         int errorIndexMax = -1;
 281                         int errorIndexMin = -1;
 282                         float errorValueMax = -1e30;
 283                         float errorValueMin = 1e30;
 284
 285                         for (int i = 0; i < dimension; i++)
 286                         {
 287                                 x[i] = solution[i + dimension];
 288                                 volatile btScalar check = x[i];
 289                                 if (x[i] != check)
 290                                 {
 291                                         x.setZero();
 292                                         return false;
 293                                 }
 294
 295                                 //this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
 296                                 //we need to figure out why it happens, and fix it, or detect it properly)
 297                                 if (x[i] > m_maxValue)
 298                                 {
 299                                         if (x[i] > errorValueMax)
 300                                         {
 301                                                 fail = true;
 302                                                 errorIndexMax = i;
 303                                                 errorValueMax = x[i];
 304                                         }
 305                                         ////printf("x[i] = %f,",x[i]);
 306                                 }
 307                                 if (x[i] < -m_maxValue)
 308                                 {
 309                                         if (x[i] < errorValueMin)
 310                                         {
 311                                                 errorIndexMin = i;
 312                                                 errorValueMin = x[i];
 313                                                 fail = true;
 314                                                 //printf("x[i] = %f,",x[i]);
 315                                         }
 316                                 }
 317                         }
 318                         if (fail)
 319                         {
 320                                 static int errorCountTimes = 0;
 321                                 if (errorIndexMin < 0)
 322                                         errorValueMin = 0.f;
 323                                 if (errorIndexMax < 0)
 324                                         errorValueMax = 0.f;
 325                                 printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin, errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
 326                                 for (int i = 0; i < dimension; i++)
 327                                 {
 328                                         x[i] = 0.f;
 329                                 }
 330                         }
 331
 332                         return !fail;
 333                 }
 334                 return true;
 335         }
 336 };
 337
 338 #endif  //BT_LEMKE_SOLVER_H