dali-physics/third-party/bullet3/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

   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 #include "btMLCPSolver.h"
  18 #include "LinearMath/btMatrixX.h"
  19 #include "LinearMath/btQuickprof.h"
  20 #include "btSolveProjectedGaussSeidel.h"
  21
  22 btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver)
  23         : m_solver(solver),
  24           m_fallback(0)
  25 {
  26 }
  27
  28 btMLCPSolver::~btMLCPSolver()
  29 {
  30 }
  31
  32 bool gUseMatrixMultiply = false;
  33 bool interleaveContactAndFriction = false;
  34
  35 btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
  36 {
  37         btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodiesUnUsed, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
  38
  39         {
  40                 BT_PROFILE("gather constraint data");
  41
  42                 int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
  43
  44                 //      int numBodies = m_tmpSolverBodyPool.size();
  45                 m_allConstraintPtrArray.resize(0);
  46                 m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size());
  47                 btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size());
  48                 //      printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
  49
  50                 int dindex = 0;
  51                 for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); i++)
  52                 {
  53                         m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
  54                         m_limitDependencies[dindex++] = -1;
  55                 }
  56
  57                 ///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
  58
  59                 int firstContactConstraintOffset = dindex;
  60
  61                 if (interleaveContactAndFriction)
  62                 {
  63                         for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
  64                         {
  65                                 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
  66                                 m_limitDependencies[dindex++] = -1;
  67                                 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
  68                                 int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
  69                                 m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
  70                                 if (numFrictionPerContact == 2)
  71                                 {
  72                                         m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
  73                                         m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
  74                                 }
  75                         }
  76                 }
  77                 else
  78                 {
  79                         for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
  80                         {
  81                                 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
  82                                 m_limitDependencies[dindex++] = -1;
  83                         }
  84                         for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
  85                         {
  86                                 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
  87                                 m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
  88                         }
  89                 }
  90
  91                 if (!m_allConstraintPtrArray.size())
  92                 {
  93                         m_A.resize(0, 0);
  94                         m_b.resize(0);
  95                         m_x.resize(0);
  96                         m_lo.resize(0);
  97                         m_hi.resize(0);
  98                         return 0.f;
  99                 }
 100         }
 101
 102         if (gUseMatrixMultiply)
 103         {
 104                 BT_PROFILE("createMLCP");
 105                 createMLCP(infoGlobal);
 106         }
 107         else
 108         {
 109                 BT_PROFILE("createMLCPFast");
 110                 createMLCPFast(infoGlobal);
 111         }
 112
 113         return 0.f;
 114 }
 115
 116 bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
 117 {
 118         bool result = true;
 119
 120         if (m_A.rows() == 0)
 121                 return true;
 122
 123         //if using split impulse, we solve 2 separate (M)LCPs
 124         if (infoGlobal.m_splitImpulse)
 125         {
 126                 btMatrixXu Acopy = m_A;
 127                 btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
 128                 //              printf("solve first LCP\n");
 129                 result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
 130                 if (result)
 131                         result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
 132         }
 133         else
 134         {
 135                 result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
 136         }
 137         return result;
 138 }
 139
 140 struct btJointNode
 141 {
 142         int jointIndex;          // pointer to enclosing dxJoint object
 143         int otherBodyIndex;      // *other* body this joint is connected to
 144         int nextJointNodeIndex;  //-1 for null
 145         int constraintRowIndex;
 146 };
 147
 148 void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 149 {
 150         int numContactRows = interleaveContactAndFriction ? 3 : 1;
 151
 152         int numConstraintRows = m_allConstraintPtrArray.size();
 153         int n = numConstraintRows;
 154         {
 155                 BT_PROFILE("init b (rhs)");
 156                 m_b.resize(numConstraintRows);
 157                 m_bSplit.resize(numConstraintRows);
 158                 m_b.setZero();
 159                 m_bSplit.setZero();
 160                 for (int i = 0; i < numConstraintRows; i++)
 161                 {
 162                         btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
 163                         if (!btFuzzyZero(jacDiag))
 164                         {
 165                                 btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
 166                                 btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
 167                                 m_b[i] = rhs / jacDiag;
 168                                 m_bSplit[i] = rhsPenetration / jacDiag;
 169                         }
 170                 }
 171         }
 172
 173         //      btScalar* w = 0;
 174         //      int nub = 0;
 175
 176         m_lo.resize(numConstraintRows);
 177         m_hi.resize(numConstraintRows);
 178
 179         {
 180                 BT_PROFILE("init lo/ho");
 181
 182                 for (int i = 0; i < numConstraintRows; i++)
 183                 {
 184                         if (0)  //m_limitDependencies[i]>=0)
 185                         {
 186                                 m_lo[i] = -BT_INFINITY;
 187                                 m_hi[i] = BT_INFINITY;
 188                         }
 189                         else
 190                         {
 191                                 m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
 192                                 m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
 193                         }
 194                 }
 195         }
 196
 197         //
 198         int m = m_allConstraintPtrArray.size();
 199
 200         int numBodies = m_tmpSolverBodyPool.size();
 201         btAlignedObjectArray<int> bodyJointNodeArray;
 202         {
 203                 BT_PROFILE("bodyJointNodeArray.resize");
 204                 bodyJointNodeArray.resize(numBodies, -1);
 205         }
 206         btAlignedObjectArray<btJointNode> jointNodeArray;
 207         {
 208                 BT_PROFILE("jointNodeArray.reserve");
 209                 jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
 210         }
 211
 212         btMatrixXu& J3 = m_scratchJ3;
 213         {
 214                 BT_PROFILE("J3.resize");
 215                 J3.resize(2 * m, 8);
 216         }
 217         btMatrixXu& JinvM3 = m_scratchJInvM3;
 218         {
 219                 BT_PROFILE("JinvM3.resize/setZero");
 220
 221                 JinvM3.resize(2 * m, 8);
 222                 JinvM3.setZero();
 223                 J3.setZero();
 224         }
 225         int cur = 0;
 226         int rowOffset = 0;
 227         btAlignedObjectArray<int>& ofs = m_scratchOfs;
 228         {
 229                 BT_PROFILE("ofs resize");
 230                 ofs.resize(0);
 231                 ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
 232         }
 233         {
 234                 BT_PROFILE("Compute J and JinvM");
 235                 int c = 0;
 236
 237                 int numRows = 0;
 238
 239                 for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
 240                 {
 241                         ofs[c] = rowOffset;
 242                         int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
 243                         int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 244                         btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 245                         btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 246
 247                         numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
 248                         if (orgBodyA)
 249                         {
 250                                 {
 251                                         int slotA = -1;
 252                                         //find free jointNode slot for sbA
 253                                         slotA = jointNodeArray.size();
 254                                         jointNodeArray.expand();  //NonInitializing();
 255                                         int prevSlot = bodyJointNodeArray[sbA];
 256                                         bodyJointNodeArray[sbA] = slotA;
 257                                         jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
 258                                         jointNodeArray[slotA].jointIndex = c;
 259                                         jointNodeArray[slotA].constraintRowIndex = i;
 260                                         jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
 261                                 }
 262                                 for (int row = 0; row < numRows; row++, cur++)
 263                                 {
 264                                         btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
 265                                         btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
 266
 267                                         for (int r = 0; r < 3; r++)
 268                                         {
 269                                                 J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
 270                                                 J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
 271                                                 JinvM3.setElem(cur, r, normalInvMass[r]);
 272                                                 JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
 273                                         }
 274                                         J3.setElem(cur, 3, 0);
 275                                         JinvM3.setElem(cur, 3, 0);
 276                                         J3.setElem(cur, 7, 0);
 277                                         JinvM3.setElem(cur, 7, 0);
 278                                 }
 279                         }
 280                         else
 281                         {
 282                                 cur += numRows;
 283                         }
 284                         if (orgBodyB)
 285                         {
 286                                 {
 287                                         int slotB = -1;
 288                                         //find free jointNode slot for sbA
 289                                         slotB = jointNodeArray.size();
 290                                         jointNodeArray.expand();  //NonInitializing();
 291                                         int prevSlot = bodyJointNodeArray[sbB];
 292                                         bodyJointNodeArray[sbB] = slotB;
 293                                         jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
 294                                         jointNodeArray[slotB].jointIndex = c;
 295                                         jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
 296                                         jointNodeArray[slotB].constraintRowIndex = i;
 297                                 }
 298
 299                                 for (int row = 0; row < numRows; row++, cur++)
 300                                 {
 301                                         btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
 302                                         btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
 303
 304                                         for (int r = 0; r < 3; r++)
 305                                         {
 306                                                 J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
 307                                                 J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
 308                                                 JinvM3.setElem(cur, r, normalInvMassB[r]);
 309                                                 JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
 310                                         }
 311                                         J3.setElem(cur, 3, 0);
 312                                         JinvM3.setElem(cur, 3, 0);
 313                                         J3.setElem(cur, 7, 0);
 314                                         JinvM3.setElem(cur, 7, 0);
 315                                 }
 316                         }
 317                         else
 318                         {
 319                                 cur += numRows;
 320                         }
 321                         rowOffset += numRows;
 322                 }
 323         }
 324
 325         //compute JinvM = J*invM.
 326         const btScalar* JinvM = JinvM3.getBufferPointer();
 327
 328         const btScalar* Jptr = J3.getBufferPointer();
 329         {
 330                 BT_PROFILE("m_A.resize");
 331                 m_A.resize(n, n);
 332         }
 333
 334         {
 335                 BT_PROFILE("m_A.setZero");
 336                 m_A.setZero();
 337         }
 338         int c = 0;
 339         {
 340                 int numRows = 0;
 341                 BT_PROFILE("Compute A");
 342                 for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
 343                 {
 344                         int row__ = ofs[c];
 345                         int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
 346                         int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 347                         //      btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 348                         //      btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 349
 350                         numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
 351
 352                         const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
 353
 354                         {
 355                                 int startJointNodeA = bodyJointNodeArray[sbA];
 356                                 while (startJointNodeA >= 0)
 357                                 {
 358                                         int j0 = jointNodeArray[startJointNodeA].jointIndex;
 359                                         int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
 360                                         if (j0 < c)
 361                                         {
 362                                                 int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
 363                                                 size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
 364                                                 //printf("%d joint i %d and j0: %d: ",count++,i,j0);
 365                                                 m_A.multiplyAdd2_p8r(JinvMrow,
 366                                                                                          Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
 367                                         }
 368                                         startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
 369                                 }
 370                         }
 371
 372                         {
 373                                 int startJointNodeB = bodyJointNodeArray[sbB];
 374                                 while (startJointNodeB >= 0)
 375                                 {
 376                                         int j1 = jointNodeArray[startJointNodeB].jointIndex;
 377                                         int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
 378
 379                                         if (j1 < c)
 380                                         {
 381                                                 int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
 382                                                 size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
 383                                                 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
 384                                                                                          Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
 385                                         }
 386                                         startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
 387                                 }
 388                         }
 389                 }
 390
 391                 {
 392                         BT_PROFILE("compute diagonal");
 393                         // compute diagonal blocks of m_A
 394
 395                         int row__ = 0;
 396                         int numJointRows = m_allConstraintPtrArray.size();
 397
 398                         int jj = 0;
 399                         for (; row__ < numJointRows;)
 400                         {
 401                                 //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
 402                                 int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
 403                                 //      btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 404                                 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 405
 406                                 const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
 407
 408                                 const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
 409                                 const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
 410                                 m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
 411                                 if (orgBodyB)
 412                                 {
 413                                         m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
 414                                 }
 415                                 row__ += infom;
 416                                 jj++;
 417                         }
 418                 }
 419         }
 420
 421         if (1)
 422         {
 423                 // add cfm to the diagonal of m_A
 424                 for (int i = 0; i < m_A.rows(); ++i)
 425                 {
 426                         m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 427                 }
 428         }
 429
 430         ///fill the upper triangle of the matrix, to make it symmetric
 431         {
 432                 BT_PROFILE("fill the upper triangle ");
 433                 m_A.copyLowerToUpperTriangle();
 434         }
 435
 436         {
 437                 BT_PROFILE("resize/init x");
 438                 m_x.resize(numConstraintRows);
 439                 m_xSplit.resize(numConstraintRows);
 440
 441                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 442                 {
 443                         for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 444                         {
 445                                 const btSolverConstraint& c = *m_allConstraintPtrArray[i];
 446                                 m_x[i] = c.m_appliedImpulse;
 447                                 m_xSplit[i] = c.m_appliedPushImpulse;
 448                         }
 449                 }
 450                 else
 451                 {
 452                         m_x.setZero();
 453                         m_xSplit.setZero();
 454                 }
 455         }
 456 }
 457
 458 void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 459 {
 460         int numBodies = this->m_tmpSolverBodyPool.size();
 461         int numConstraintRows = m_allConstraintPtrArray.size();
 462
 463         m_b.resize(numConstraintRows);
 464         if (infoGlobal.m_splitImpulse)
 465                 m_bSplit.resize(numConstraintRows);
 466
 467         m_bSplit.setZero();
 468         m_b.setZero();
 469
 470         for (int i = 0; i < numConstraintRows; i++)
 471         {
 472                 if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
 473                 {
 474                         m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv;
 475                         if (infoGlobal.m_splitImpulse)
 476                                 m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv;
 477                 }
 478         }
 479
 480         btMatrixXu& Minv = m_scratchMInv;
 481         Minv.resize(6 * numBodies, 6 * numBodies);
 482         Minv.setZero();
 483         for (int i = 0; i < numBodies; i++)
 484         {
 485                 const btSolverBody& rb = m_tmpSolverBodyPool[i];
 486                 const btVector3& invMass = rb.m_invMass;
 487                 setElem(Minv, i * 6 + 0, i * 6 + 0, invMass[0]);
 488                 setElem(Minv, i * 6 + 1, i * 6 + 1, invMass[1]);
 489                 setElem(Minv, i * 6 + 2, i * 6 + 2, invMass[2]);
 490                 btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
 491
 492                 for (int r = 0; r < 3; r++)
 493                         for (int c = 0; c < 3; c++)
 494                                 setElem(Minv, i * 6 + 3 + r, i * 6 + 3 + c, orgBody ? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
 495         }
 496
 497         btMatrixXu& J = m_scratchJ;
 498         J.resize(numConstraintRows, 6 * numBodies);
 499         J.setZero();
 500
 501         m_lo.resize(numConstraintRows);
 502         m_hi.resize(numConstraintRows);
 503
 504         for (int i = 0; i < numConstraintRows; i++)
 505         {
 506                 m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
 507                 m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
 508
 509                 int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA;
 510                 int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 511                 if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
 512                 {
 513                         setElem(J, i, 6 * bodyIndex0 + 0, m_allConstraintPtrArray[i]->m_contactNormal1[0]);
 514                         setElem(J, i, 6 * bodyIndex0 + 1, m_allConstraintPtrArray[i]->m_contactNormal1[1]);
 515                         setElem(J, i, 6 * bodyIndex0 + 2, m_allConstraintPtrArray[i]->m_contactNormal1[2]);
 516                         setElem(J, i, 6 * bodyIndex0 + 3, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]);
 517                         setElem(J, i, 6 * bodyIndex0 + 4, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]);
 518                         setElem(J, i, 6 * bodyIndex0 + 5, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]);
 519                 }
 520                 if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
 521                 {
 522                         setElem(J, i, 6 * bodyIndex1 + 0, m_allConstraintPtrArray[i]->m_contactNormal2[0]);
 523                         setElem(J, i, 6 * bodyIndex1 + 1, m_allConstraintPtrArray[i]->m_contactNormal2[1]);
 524                         setElem(J, i, 6 * bodyIndex1 + 2, m_allConstraintPtrArray[i]->m_contactNormal2[2]);
 525                         setElem(J, i, 6 * bodyIndex1 + 3, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]);
 526                         setElem(J, i, 6 * bodyIndex1 + 4, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]);
 527                         setElem(J, i, 6 * bodyIndex1 + 5, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]);
 528                 }
 529         }
 530
 531         btMatrixXu& J_transpose = m_scratchJTranspose;
 532         J_transpose = J.transpose();
 533
 534         btMatrixXu& tmp = m_scratchTmp;
 535         //Minv.printMatrix("Minv=");
 536         {
 537                 {
 538                         BT_PROFILE("J*Minv");
 539                         tmp = J * Minv;
 540                 }
 541                 {
 542                         BT_PROFILE("J*tmp");
 543                         m_A = tmp * J_transpose;
 544                 }
 545         }
 546         //J.printMatrix("J");
 547         if (1)
 548         {
 549                 // add cfm to the diagonal of m_A
 550                 for (int i = 0; i < m_A.rows(); ++i)
 551                 {
 552                         m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 553                 }
 554         }
 555
 556         m_x.resize(numConstraintRows);
 557         if (infoGlobal.m_splitImpulse)
 558                 m_xSplit.resize(numConstraintRows);
 559         //      m_x.setZero();
 560
 561         for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 562         {
 563                 const btSolverConstraint& c = *m_allConstraintPtrArray[i];
 564                 m_x[i] = c.m_appliedImpulse;
 565                 if (infoGlobal.m_splitImpulse)
 566                         m_xSplit[i] = c.m_appliedPushImpulse;
 567         }
 568 }
 569
 570 btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 571 {
 572         bool result = true;
 573         {
 574                 BT_PROFILE("solveMLCP");
 575                 //              printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
 576                 result = solveMLCP(infoGlobal);
 577         }
 578
 579         //check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
 580         if (result)
 581         {
 582                 BT_PROFILE("process MLCP results");
 583                 for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 584                 {
 585                         {
 586                                 btSolverConstraint& c = *m_allConstraintPtrArray[i];
 587                                 int sbA = c.m_solverBodyIdA;
 588                                 int sbB = c.m_solverBodyIdB;
 589                                 //btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 590                                 //      btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 591
 592                                 btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
 593                                 btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
 594
 595                                 {
 596                                         btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
 597                                         c.m_appliedImpulse = m_x[i];
 598                                         solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 599                                         solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 600                                 }
 601
 602                                 if (infoGlobal.m_splitImpulse)
 603                                 {
 604                                         btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
 605                                         solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 606                                         solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 607                                         c.m_appliedPushImpulse = m_xSplit[i];
 608                                 }
 609                         }
 610                 }
 611         }
 612         else
 613         {
 614                 //      printf("m_fallback = %d\n",m_fallback);
 615                 m_fallback++;
 616                 btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 617         }
 618
 619         return 0.f;
 620 }