dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2018 Google Inc. 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
  16 #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
  17
  18 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
  19 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
  20 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
  21 #include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
  22
  23 #define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
  24
  25 static bool interleaveContactAndFriction1 = false;
  26
  27 struct btJointNode1
  28 {
  29         int jointIndex;          // pointer to enclosing dxJoint object
  30         int otherBodyIndex;      // *other* body this joint is connected to
  31         int nextJointNodeIndex;  //-1 for null
  32         int constraintRowIndex;
  33 };
  34
  35 // Helper function to compute a delta velocity in the constraint space.
  36 static btScalar computeDeltaVelocityInConstraintSpace(
  37         const btVector3& angularDeltaVelocity,
  38         const btVector3& contactNormal,
  39         btScalar invMass,
  40         const btVector3& angularJacobian,
  41         const btVector3& linearJacobian)
  42 {
  43         return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass;
  44 }
  45
  46 // Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are
  47 // identical.
  48 static btScalar computeDeltaVelocityInConstraintSpace(
  49         const btVector3& angularDeltaVelocity,
  50         btScalar invMass,
  51         const btVector3& angularJacobian)
  52 {
  53         return angularDeltaVelocity.dot(angularJacobian) + invMass;
  54 }
  55
  56 // Helper function to compute a delta velocity in the constraint space.
  57 static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size)
  58 {
  59         btScalar result = 0;
  60         for (int i = 0; i < size; ++i)
  61                 result += deltaVelocity[i] * jacobian[i];
  62
  63         return result;
  64 }
  65
  66 static btScalar computeConstraintMatrixDiagElementMultiBody(
  67         const btAlignedObjectArray<btSolverBody>& solverBodyPool,
  68         const btMultiBodyJacobianData& data,
  69         const btMultiBodySolverConstraint& constraint)
  70 {
  71         btScalar ret = 0;
  72
  73         const btMultiBody* multiBodyA = constraint.m_multiBodyA;
  74         const btMultiBody* multiBodyB = constraint.m_multiBodyB;
  75
  76         if (multiBodyA)
  77         {
  78                 const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex];
  79                 const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
  80                 const int ndofA = multiBodyA->getNumDofs() + 6;
  81                 ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA);
  82         }
  83         else
  84         {
  85                 const int solverBodyIdA = constraint.m_solverBodyIdA;
  86                 btAssert(solverBodyIdA != -1);
  87                 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
  88                 const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
  89                 ret += computeDeltaVelocityInConstraintSpace(
  90                         constraint.m_relpos1CrossNormal,
  91                         invMassA,
  92                         constraint.m_angularComponentA);
  93         }
  94
  95         if (multiBodyB)
  96         {
  97                 const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex];
  98                 const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
  99                 const int ndofB = multiBodyB->getNumDofs() + 6;
 100                 ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB);
 101         }
 102         else
 103         {
 104                 const int solverBodyIdB = constraint.m_solverBodyIdB;
 105                 btAssert(solverBodyIdB != -1);
 106                 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
 107                 const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
 108                 ret += computeDeltaVelocityInConstraintSpace(
 109                         constraint.m_relpos2CrossNormal,
 110                         invMassB,
 111                         constraint.m_angularComponentB);
 112         }
 113
 114         return ret;
 115 }
 116
 117 static btScalar computeConstraintMatrixOffDiagElementMultiBody(
 118         const btAlignedObjectArray<btSolverBody>& solverBodyPool,
 119         const btMultiBodyJacobianData& data,
 120         const btMultiBodySolverConstraint& constraint,
 121         const btMultiBodySolverConstraint& offDiagConstraint)
 122 {
 123         btScalar offDiagA = btScalar(0);
 124
 125         const btMultiBody* multiBodyA = constraint.m_multiBodyA;
 126         const btMultiBody* multiBodyB = constraint.m_multiBodyB;
 127         const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
 128         const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
 129
 130         // Assumed at least one system is multibody
 131         btAssert(multiBodyA || multiBodyB);
 132         btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
 133
 134         if (offDiagMultiBodyA)
 135         {
 136                 const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex];
 137
 138                 if (offDiagMultiBodyA == multiBodyA)
 139                 {
 140                         const int ndofA = multiBodyA->getNumDofs() + 6;
 141                         const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
 142                         offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA);
 143                 }
 144                 else if (offDiagMultiBodyA == multiBodyB)
 145                 {
 146                         const int ndofB = multiBodyB->getNumDofs() + 6;
 147                         const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
 148                         offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB);
 149                 }
 150         }
 151         else
 152         {
 153                 const int solverBodyIdA = constraint.m_solverBodyIdA;
 154                 const int solverBodyIdB = constraint.m_solverBodyIdB;
 155
 156                 const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
 157                 btAssert(offDiagSolverBodyIdA != -1);
 158
 159                 if (offDiagSolverBodyIdA == solverBodyIdA)
 160                 {
 161                         btAssert(solverBodyIdA != -1);
 162                         const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
 163                         const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
 164                         offDiagA += computeDeltaVelocityInConstraintSpace(
 165                                 offDiagConstraint.m_relpos1CrossNormal,
 166                                 offDiagConstraint.m_contactNormal1,
 167                                 invMassA, constraint.m_angularComponentA,
 168                                 constraint.m_contactNormal1);
 169                 }
 170                 else if (offDiagSolverBodyIdA == solverBodyIdB)
 171                 {
 172                         btAssert(solverBodyIdB != -1);
 173                         const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
 174                         const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
 175                         offDiagA += computeDeltaVelocityInConstraintSpace(
 176                                 offDiagConstraint.m_relpos1CrossNormal,
 177                                 offDiagConstraint.m_contactNormal1,
 178                                 invMassB,
 179                                 constraint.m_angularComponentB,
 180                                 constraint.m_contactNormal2);
 181                 }
 182         }
 183
 184         if (offDiagMultiBodyB)
 185         {
 186                 const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex];
 187
 188                 if (offDiagMultiBodyB == multiBodyA)
 189                 {
 190                         const int ndofA = multiBodyA->getNumDofs() + 6;
 191                         const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
 192                         offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA);
 193                 }
 194                 else if (offDiagMultiBodyB == multiBodyB)
 195                 {
 196                         const int ndofB = multiBodyB->getNumDofs() + 6;
 197                         const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
 198                         offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB);
 199                 }
 200         }
 201         else
 202         {
 203                 const int solverBodyIdA = constraint.m_solverBodyIdA;
 204                 const int solverBodyIdB = constraint.m_solverBodyIdB;
 205
 206                 const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
 207                 btAssert(offDiagSolverBodyIdB != -1);
 208
 209                 if (offDiagSolverBodyIdB == solverBodyIdA)
 210                 {
 211                         btAssert(solverBodyIdA != -1);
 212                         const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
 213                         const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
 214                         offDiagA += computeDeltaVelocityInConstraintSpace(
 215                                 offDiagConstraint.m_relpos2CrossNormal,
 216                                 offDiagConstraint.m_contactNormal2,
 217                                 invMassA, constraint.m_angularComponentA,
 218                                 constraint.m_contactNormal1);
 219                 }
 220                 else if (offDiagSolverBodyIdB == solverBodyIdB)
 221                 {
 222                         btAssert(solverBodyIdB != -1);
 223                         const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
 224                         const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
 225                         offDiagA += computeDeltaVelocityInConstraintSpace(
 226                                 offDiagConstraint.m_relpos2CrossNormal,
 227                                 offDiagConstraint.m_contactNormal2,
 228                                 invMassB, constraint.m_angularComponentB,
 229                                 constraint.m_contactNormal2);
 230                 }
 231         }
 232
 233         return offDiagA;
 234 }
 235
 236 void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 237 {
 238         createMLCPFastRigidBody(infoGlobal);
 239         createMLCPFastMultiBody(infoGlobal);
 240 }
 241
 242 void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
 243 {
 244         int numContactRows = interleaveContactAndFriction1 ? 3 : 1;
 245
 246         int numConstraintRows = m_allConstraintPtrArray.size();
 247
 248         if (numConstraintRows == 0)
 249                 return;
 250
 251         int n = numConstraintRows;
 252         {
 253                 BT_PROFILE("init b (rhs)");
 254                 m_b.resize(numConstraintRows);
 255                 m_bSplit.resize(numConstraintRows);
 256                 m_b.setZero();
 257                 m_bSplit.setZero();
 258                 for (int i = 0; i < numConstraintRows; i++)
 259                 {
 260                         btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
 261                         if (!btFuzzyZero(jacDiag))
 262                         {
 263                                 btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
 264                                 btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
 265                                 m_b[i] = rhs / jacDiag;
 266                                 m_bSplit[i] = rhsPenetration / jacDiag;
 267                         }
 268                 }
 269         }
 270
 271         //      btScalar* w = 0;
 272         //      int nub = 0;
 273
 274         m_lo.resize(numConstraintRows);
 275         m_hi.resize(numConstraintRows);
 276
 277         {
 278                 BT_PROFILE("init lo/ho");
 279
 280                 for (int i = 0; i < numConstraintRows; i++)
 281                 {
 282                         if (0)  //m_limitDependencies[i]>=0)
 283                         {
 284                                 m_lo[i] = -BT_INFINITY;
 285                                 m_hi[i] = BT_INFINITY;
 286                         }
 287                         else
 288                         {
 289                                 m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
 290                                 m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
 291                         }
 292                 }
 293         }
 294
 295         //
 296         int m = m_allConstraintPtrArray.size();
 297
 298         int numBodies = m_tmpSolverBodyPool.size();
 299         btAlignedObjectArray<int> bodyJointNodeArray;
 300         {
 301                 BT_PROFILE("bodyJointNodeArray.resize");
 302                 bodyJointNodeArray.resize(numBodies, -1);
 303         }
 304         btAlignedObjectArray<btJointNode1> jointNodeArray;
 305         {
 306                 BT_PROFILE("jointNodeArray.reserve");
 307                 jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
 308         }
 309
 310         btMatrixXu& J3 = m_scratchJ3;
 311         {
 312                 BT_PROFILE("J3.resize");
 313                 J3.resize(2 * m, 8);
 314         }
 315         btMatrixXu& JinvM3 = m_scratchJInvM3;
 316         {
 317                 BT_PROFILE("JinvM3.resize/setZero");
 318
 319                 JinvM3.resize(2 * m, 8);
 320                 JinvM3.setZero();
 321                 J3.setZero();
 322         }
 323         int cur = 0;
 324         int rowOffset = 0;
 325         btAlignedObjectArray<int>& ofs = m_scratchOfs;
 326         {
 327                 BT_PROFILE("ofs resize");
 328                 ofs.resize(0);
 329                 ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
 330         }
 331         {
 332                 BT_PROFILE("Compute J and JinvM");
 333                 int c = 0;
 334
 335                 int numRows = 0;
 336
 337                 for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
 338                 {
 339                         ofs[c] = rowOffset;
 340                         int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
 341                         int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 342                         btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 343                         btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 344
 345                         numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
 346                         if (orgBodyA)
 347                         {
 348                                 {
 349                                         int slotA = -1;
 350                                         //find free jointNode slot for sbA
 351                                         slotA = jointNodeArray.size();
 352                                         jointNodeArray.expand();  //NonInitializing();
 353                                         int prevSlot = bodyJointNodeArray[sbA];
 354                                         bodyJointNodeArray[sbA] = slotA;
 355                                         jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
 356                                         jointNodeArray[slotA].jointIndex = c;
 357                                         jointNodeArray[slotA].constraintRowIndex = i;
 358                                         jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
 359                                 }
 360                                 for (int row = 0; row < numRows; row++, cur++)
 361                                 {
 362                                         btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
 363                                         btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
 364
 365                                         for (int r = 0; r < 3; r++)
 366                                         {
 367                                                 J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
 368                                                 J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
 369                                                 JinvM3.setElem(cur, r, normalInvMass[r]);
 370                                                 JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
 371                                         }
 372                                         J3.setElem(cur, 3, 0);
 373                                         JinvM3.setElem(cur, 3, 0);
 374                                         J3.setElem(cur, 7, 0);
 375                                         JinvM3.setElem(cur, 7, 0);
 376                                 }
 377                         }
 378                         else
 379                         {
 380                                 cur += numRows;
 381                         }
 382                         if (orgBodyB)
 383                         {
 384                                 {
 385                                         int slotB = -1;
 386                                         //find free jointNode slot for sbA
 387                                         slotB = jointNodeArray.size();
 388                                         jointNodeArray.expand();  //NonInitializing();
 389                                         int prevSlot = bodyJointNodeArray[sbB];
 390                                         bodyJointNodeArray[sbB] = slotB;
 391                                         jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
 392                                         jointNodeArray[slotB].jointIndex = c;
 393                                         jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
 394                                         jointNodeArray[slotB].constraintRowIndex = i;
 395                                 }
 396
 397                                 for (int row = 0; row < numRows; row++, cur++)
 398                                 {
 399                                         btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
 400                                         btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
 401
 402                                         for (int r = 0; r < 3; r++)
 403                                         {
 404                                                 J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
 405                                                 J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
 406                                                 JinvM3.setElem(cur, r, normalInvMassB[r]);
 407                                                 JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
 408                                         }
 409                                         J3.setElem(cur, 3, 0);
 410                                         JinvM3.setElem(cur, 3, 0);
 411                                         J3.setElem(cur, 7, 0);
 412                                         JinvM3.setElem(cur, 7, 0);
 413                                 }
 414                         }
 415                         else
 416                         {
 417                                 cur += numRows;
 418                         }
 419                         rowOffset += numRows;
 420                 }
 421         }
 422
 423         //compute JinvM = J*invM.
 424         const btScalar* JinvM = JinvM3.getBufferPointer();
 425
 426         const btScalar* Jptr = J3.getBufferPointer();
 427         {
 428                 BT_PROFILE("m_A.resize");
 429                 m_A.resize(n, n);
 430         }
 431
 432         {
 433                 BT_PROFILE("m_A.setZero");
 434                 m_A.setZero();
 435         }
 436         int c = 0;
 437         {
 438                 int numRows = 0;
 439                 BT_PROFILE("Compute A");
 440                 for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
 441                 {
 442                         int row__ = ofs[c];
 443                         int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
 444                         int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
 445                         //      btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 446                         //      btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 447
 448                         numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
 449
 450                         const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
 451
 452                         {
 453                                 int startJointNodeA = bodyJointNodeArray[sbA];
 454                                 while (startJointNodeA >= 0)
 455                                 {
 456                                         int j0 = jointNodeArray[startJointNodeA].jointIndex;
 457                                         int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
 458                                         if (j0 < c)
 459                                         {
 460                                                 int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
 461                                                 size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
 462                                                 //printf("%d joint i %d and j0: %d: ",count++,i,j0);
 463                                                 m_A.multiplyAdd2_p8r(JinvMrow,
 464                                                                                          Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
 465                                         }
 466                                         startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
 467                                 }
 468                         }
 469
 470                         {
 471                                 int startJointNodeB = bodyJointNodeArray[sbB];
 472                                 while (startJointNodeB >= 0)
 473                                 {
 474                                         int j1 = jointNodeArray[startJointNodeB].jointIndex;
 475                                         int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
 476
 477                                         if (j1 < c)
 478                                         {
 479                                                 int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
 480                                                 size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
 481                                                 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
 482                                                                                          Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
 483                                         }
 484                                         startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
 485                                 }
 486                         }
 487                 }
 488
 489                 {
 490                         BT_PROFILE("compute diagonal");
 491                         // compute diagonal blocks of m_A
 492
 493                         int row__ = 0;
 494                         int numJointRows = m_allConstraintPtrArray.size();
 495
 496                         int jj = 0;
 497                         for (; row__ < numJointRows;)
 498                         {
 499                                 //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
 500                                 int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
 501                                 //      btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
 502                                 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
 503
 504                                 const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
 505
 506                                 const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
 507                                 const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
 508                                 m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
 509                                 if (orgBodyB)
 510                                 {
 511                                         m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
 512                                 }
 513                                 row__ += infom;
 514                                 jj++;
 515                         }
 516                 }
 517         }
 518
 519         if (1)
 520         {
 521                 // add cfm to the diagonal of m_A
 522                 for (int i = 0; i < m_A.rows(); ++i)
 523                 {
 524                         m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 525                 }
 526         }
 527
 528         ///fill the upper triangle of the matrix, to make it symmetric
 529         {
 530                 BT_PROFILE("fill the upper triangle ");
 531                 m_A.copyLowerToUpperTriangle();
 532         }
 533
 534         {
 535                 BT_PROFILE("resize/init x");
 536                 m_x.resize(numConstraintRows);
 537                 m_xSplit.resize(numConstraintRows);
 538
 539                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 540                 {
 541                         for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
 542                         {
 543                                 const btSolverConstraint& c = *m_allConstraintPtrArray[i];
 544                                 m_x[i] = c.m_appliedImpulse;
 545                                 m_xSplit[i] = c.m_appliedPushImpulse;
 546                         }
 547                 }
 548                 else
 549                 {
 550                         m_x.setZero();
 551                         m_xSplit.setZero();
 552                 }
 553         }
 554 }
 555
 556 void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
 557 {
 558         const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
 559
 560         if (multiBodyNumConstraints == 0)
 561                 return;
 562
 563         // 1. Compute b
 564         {
 565                 BT_PROFILE("init b (rhs)");
 566
 567                 m_multiBodyB.resize(multiBodyNumConstraints);
 568                 m_multiBodyB.setZero();
 569
 570                 for (int i = 0; i < multiBodyNumConstraints; ++i)
 571                 {
 572                         const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
 573                         const btScalar jacDiag = constraint.m_jacDiagABInv;
 574
 575                         if (!btFuzzyZero(jacDiag))
 576                         {
 577                                 // Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
 578                                 const btScalar rhs = constraint.m_rhs;
 579                                 m_multiBodyB[i] = rhs / jacDiag;
 580                         }
 581                 }
 582         }
 583
 584         // 2. Compute lo and hi
 585         {
 586                 BT_PROFILE("init lo/ho");
 587
 588                 m_multiBodyLo.resize(multiBodyNumConstraints);
 589                 m_multiBodyHi.resize(multiBodyNumConstraints);
 590
 591                 for (int i = 0; i < multiBodyNumConstraints; ++i)
 592                 {
 593                         const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
 594                         m_multiBodyLo[i] = constraint.m_lowerLimit;
 595                         m_multiBodyHi[i] = constraint.m_upperLimit;
 596                 }
 597         }
 598
 599         // 3. Construct A matrix by using the impulse testing
 600         {
 601                 BT_PROFILE("Compute A");
 602
 603                 {
 604                         BT_PROFILE("m_A.resize");
 605                         m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
 606                 }
 607
 608                 for (int i = 0; i < multiBodyNumConstraints; ++i)
 609                 {
 610                         // Compute the diagonal of A, which is A(i, i)
 611                         const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
 612                         const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
 613                         m_multiBodyA.setElem(i, i, diagA);
 614
 615                         // Computes the off-diagonals of A:
 616                         //   a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
 617                         //   b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
 618                         for (int j = i + 1; j < multiBodyNumConstraints; ++j)
 619                         {
 620                                 const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
 621                                 const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
 622
 623                                 // Set the off-diagonal values of A. Note that A is symmetric.
 624                                 m_multiBodyA.setElem(i, j, offDiagA);
 625                                 m_multiBodyA.setElem(j, i, offDiagA);
 626                         }
 627                 }
 628         }
 629
 630         // Add CFM to the diagonal of m_A
 631         for (int i = 0; i < m_multiBodyA.rows(); ++i)
 632         {
 633                 m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 634         }
 635
 636         // 4. Initialize x
 637         {
 638                 BT_PROFILE("resize/init x");
 639
 640                 m_multiBodyX.resize(multiBodyNumConstraints);
 641
 642                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 643                 {
 644                         for (int i = 0; i < multiBodyNumConstraints; ++i)
 645                         {
 646                                 const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
 647                                 m_multiBodyX[i] = constraint.m_appliedImpulse;
 648                         }
 649                 }
 650                 else
 651                 {
 652                         m_multiBodyX.setZero();
 653                 }
 654         }
 655 }
 656
 657 bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
 658 {
 659         bool result = true;
 660
 661         if (m_A.rows() != 0)
 662         {
 663                 // If using split impulse, we solve 2 separate (M)LCPs
 664                 if (infoGlobal.m_splitImpulse)
 665                 {
 666                         const btMatrixXu Acopy = m_A;
 667                         const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
 668                         // TODO(JS): Do we really need these copies when solveMLCP takes them as const?
 669
 670                         result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
 671                         if (result)
 672                                 result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
 673                 }
 674                 else
 675                 {
 676                         result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
 677                 }
 678         }
 679
 680         if (!result)
 681                 return false;
 682
 683         if (m_multiBodyA.rows() != 0)
 684         {
 685                 result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations);
 686         }
 687
 688         return result;
 689 }
 690
 691 btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
 692         btCollisionObject** bodies,
 693         int numBodies,
 694         btPersistentManifold** manifoldPtr,
 695         int numManifolds,
 696         btTypedConstraint** constraints,
 697         int numConstraints,
 698         const btContactSolverInfo& infoGlobal,
 699         btIDebugDraw* debugDrawer)
 700 {
 701         // 1. Setup for rigid-bodies
 702         btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
 703                 bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 704
 705         // 2. Setup for multi-bodies
 706         //   a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
 707         //   b. Set the index array for frictional contact constraints, m_limitDependencies
 708         {
 709                 BT_PROFILE("gather constraint data");
 710
 711                 int dindex = 0;
 712
 713                 const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
 714                 const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
 715
 716                 m_allConstraintPtrArray.resize(0);
 717                 m_multiBodyAllConstraintPtrArray.resize(0);
 718
 719                 // i. Setup for rigid bodies
 720
 721                 m_limitDependencies.resize(numRigidBodyConstraints);
 722
 723                 for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
 724                 {
 725                         m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
 726                         m_limitDependencies[dindex++] = -1;
 727                 }
 728
 729                 int firstContactConstraintOffset = dindex;
 730
 731                 // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
 732                 if (interleaveContactAndFriction1)
 733                 {
 734                         for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
 735                         {
 736                                 const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
 737
 738                                 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 739                                 m_limitDependencies[dindex++] = -1;
 740                                 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
 741                                 int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
 742                                 m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
 743                                 if (numFrictionPerContact == 2)
 744                                 {
 745                                         m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
 746                                         m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
 747                                 }
 748                         }
 749                 }
 750                 else
 751                 {
 752                         for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
 753                         {
 754                                 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
 755                                 m_limitDependencies[dindex++] = -1;
 756                         }
 757                         for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
 758                         {
 759                                 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
 760                                 m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
 761                         }
 762                 }
 763
 764                 if (!m_allConstraintPtrArray.size())
 765                 {
 766                         m_A.resize(0, 0);
 767                         m_b.resize(0);
 768                         m_x.resize(0);
 769                         m_lo.resize(0);
 770                         m_hi.resize(0);
 771                 }
 772
 773                 // ii. Setup for multibodies
 774
 775                 dindex = 0;
 776
 777                 m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
 778
 779                 for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
 780                 {
 781                         m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
 782                         m_multiBodyLimitDependencies[dindex++] = -1;
 783                 }
 784
 785                 firstContactConstraintOffset = dindex;
 786
 787                 // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
 788                 if (interleaveContactAndFriction1)
 789                 {
 790                         for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
 791                         {
 792                                 const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
 793
 794                                 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
 795                                 m_multiBodyLimitDependencies[dindex++] = -1;
 796
 797                                 btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
 798                                 m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
 799
 800                                 const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
 801
 802                                 m_multiBodyLimitDependencies[dindex++] = findex;
 803
 804                                 if (numtiBodyNumFrictionPerContact == 2)
 805                                 {
 806                                         btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
 807                                         m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
 808
 809                                         m_multiBodyLimitDependencies[dindex++] = findex;
 810                                 }
 811                         }
 812                 }
 813                 else
 814                 {
 815                         for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
 816                         {
 817                                 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
 818                                 m_multiBodyLimitDependencies[dindex++] = -1;
 819                         }
 820                         for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
 821                         {
 822                                 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
 823                                 m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
 824                         }
 825                 }
 826
 827                 if (!m_multiBodyAllConstraintPtrArray.size())
 828                 {
 829                         m_multiBodyA.resize(0, 0);
 830                         m_multiBodyB.resize(0);
 831                         m_multiBodyX.resize(0);
 832                         m_multiBodyLo.resize(0);
 833                         m_multiBodyHi.resize(0);
 834                 }
 835         }
 836
 837         // Construct MLCP terms
 838         {
 839                 BT_PROFILE("createMLCPFast");
 840                 createMLCPFast(infoGlobal);
 841         }
 842
 843         return btScalar(0);
 844 }
 845
 846 btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 847 {
 848         bool result = true;
 849         {
 850                 BT_PROFILE("solveMLCP");
 851                 result = solveMLCP(infoGlobal);
 852         }
 853
 854         // Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
 855         if (!result)
 856         {
 857                 m_fallback++;
 858                 return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 859         }
 860
 861         {
 862                 BT_PROFILE("process MLCP results");
 863
 864                 for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
 865                 {
 866                         const btSolverConstraint& c = *m_allConstraintPtrArray[i];
 867
 868                         const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
 869                         c.m_appliedImpulse = m_x[i];
 870
 871                         int sbA = c.m_solverBodyIdA;
 872                         int sbB = c.m_solverBodyIdB;
 873
 874                         btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
 875                         btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
 876
 877                         solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 878                         solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 879
 880                         if (infoGlobal.m_splitImpulse)
 881                         {
 882                                 const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
 883                                 solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse);
 884                                 solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse);
 885                                 c.m_appliedPushImpulse = m_xSplit[i];
 886                         }
 887                 }
 888
 889                 for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
 890                 {
 891                         btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i];
 892
 893                         const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse;
 894                         c.m_appliedImpulse = m_multiBodyX[i];
 895
 896                         btMultiBody* multiBodyA = c.m_multiBodyA;
 897                         if (multiBodyA)
 898                         {
 899                                 const int ndofA = multiBodyA->getNumDofs() + 6;
 900                                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
 901 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 902                                 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 903                                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 904                                 multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
 905 #endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 906                         }
 907                         else
 908                         {
 909                                 const int sbA = c.m_solverBodyIdA;
 910                                 btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
 911                                 solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
 912                         }
 913
 914                         btMultiBody* multiBodyB = c.m_multiBodyB;
 915                         if (multiBodyB)
 916                         {
 917                                 const int ndofB = multiBodyB->getNumDofs() + 6;
 918                                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
 919 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 920                                 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
 921                                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
 922                                 multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
 923 #endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
 924                         }
 925                         else
 926                         {
 927                                 const int sbB = c.m_solverBodyIdB;
 928                                 btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
 929                                 solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
 930                         }
 931                 }
 932         }
 933
 934         return btScalar(0);
 935 }
 936
 937 btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver)
 938         : m_solver(solver), m_fallback(0)
 939 {
 940         // Do nothing
 941 }
 942
 943 btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver()
 944 {
 945         // Do nothing
 946 }
 947
 948 void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver)
 949 {
 950         m_solver = solver;
 951 }
 952
 953 int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const
 954 {
 955         return m_fallback;
 956 }
 957
 958 void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num)
 959 {
 960         m_fallback = num;
 961 }
 962
 963 btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const
 964 {
 965         return BT_MLCP_SOLVER;
 966 }