23 #define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 
   43         return angularDeltaVelocity.
dot(angularJacobian) + contactNormal.
dot(linearJacobian) * invMass;
 
   53         return angularDeltaVelocity.
dot(angularJacobian) + invMass;
 
   60         for (
int i = 0; i < 
size; ++i)
 
   61                 result += deltaVelocity[i] * jacobian[i];
 
   80                 const int ndofA = multiBodyA->
getNumDofs() + 6;
 
   87                 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
 
   99                 const int ndofB = multiBodyB->
getNumDofs() + 6;
 
  106                 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
 
  132         btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
 
  134         if (offDiagMultiBodyA)
 
  138                 if (offDiagMultiBodyA == multiBodyA)
 
  140                         const int ndofA = multiBodyA->
getNumDofs() + 6;
 
  144                 else if (offDiagMultiBodyA == multiBodyB)
 
  146                         const int ndofB = multiBodyB->
getNumDofs() + 6;
 
  157                 btAssert(offDiagSolverBodyIdA != -1);
 
  159                 if (offDiagSolverBodyIdA == solverBodyIdA)
 
  162                         const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
 
  170                 else if (offDiagSolverBodyIdA == solverBodyIdB)
 
  173                         const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
 
  184         if (offDiagMultiBodyB)
 
  188                 if (offDiagMultiBodyB == multiBodyA)
 
  190                         const int ndofA = multiBodyA->
getNumDofs() + 6;
 
  194                 else if (offDiagMultiBodyB == multiBodyB)
 
  196                         const int ndofB = multiBodyB->
getNumDofs() + 6;
 
  207                 btAssert(offDiagSolverBodyIdB != -1);
 
  209                 if (offDiagSolverBodyIdB == solverBodyIdA)
 
  212                         const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
 
  220                 else if (offDiagSolverBodyIdB == solverBodyIdB)
 
  223                         const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
 
  248         if (numConstraintRows == 0)
 
  251         int n = numConstraintRows;
 
  254                 m_b.resize(numConstraintRows);
 
  258                 for (
int i = 0; i < numConstraintRows; i++)
 
  265                                 m_b[i] = rhs / jacDiag;
 
  266                                 m_bSplit[i] = rhsPenetration / jacDiag;
 
  274         m_lo.resize(numConstraintRows);
 
  275         m_hi.resize(numConstraintRows);
 
  280                 for (
int i = 0; i < numConstraintRows; i++)
 
  302                 bodyJointNodeArray.
resize(numBodies, -1);
 
  319                 JinvM3.resize(2 * m, 8);
 
  351                                         slotA = jointNodeArray.
size();
 
  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;
 
  360                                 for (
int row = 0; row < numRows; row++, cur++)
 
  365                                         for (
int r = 0; r < 3; r++)
 
  369                                                 JinvM3.setElem(cur, r, normalInvMass[r]);
 
  370                                                 JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
 
  372                                         J3.setElem(cur, 3, 0);
 
  373                                         JinvM3.setElem(cur, 3, 0);
 
  374                                         J3.setElem(cur, 7, 0);
 
  375                                         JinvM3.setElem(cur, 7, 0);
 
  387                                         slotB = jointNodeArray.
size();
 
  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;
 
  397                                 for (
int row = 0; row < numRows; row++, cur++)
 
  402                                         for (
int r = 0; r < 3; r++)
 
  406                                                 JinvM3.setElem(cur, r, normalInvMassB[r]);
 
  407                                                 JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
 
  409                                         J3.setElem(cur, 3, 0);
 
  410                                         JinvM3.setElem(cur, 3, 0);
 
  411                                         J3.setElem(cur, 7, 0);
 
  412                                         JinvM3.setElem(cur, 7, 0);
 
  419                         rowOffset += numRows;
 
  424         const btScalar* JinvM = JinvM3.getBufferPointer();
 
  426         const btScalar* Jptr = J3.getBufferPointer();
 
  450                         const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
 
  453                                 int startJointNodeA = bodyJointNodeArray[sbA];
 
  454                                 while (startJointNodeA >= 0)
 
  456                                         int j0 = jointNodeArray[startJointNodeA].jointIndex;
 
  457                                         int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
 
  463                                                 m_A.multiplyAdd2_p8r(JinvMrow,
 
  464                                                                                          Jptr + 2 * 8 * (
size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
 
  466                                         startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
 
  471                                 int startJointNodeB = bodyJointNodeArray[sbB];
 
  472                                 while (startJointNodeB >= 0)
 
  474                                         int j1 = jointNodeArray[startJointNodeB].jointIndex;
 
  475                                         int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
 
  481                                                 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (
size_t)numRows,
 
  482                                                                                          Jptr + 2 * 8 * (
size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
 
  484                                         startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
 
  497                         for (; row__ < numJointRows;)
 
  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__);
 
  511                                         m_A.multiplyAdd2_p8r(JinvMrow + 8 * (
size_t)infom, Jrow + 8 * (
size_t)infom, infom, infom, row__, row__);
 
  522                 for (
int i = 0; i < 
m_A.rows(); ++i)
 
  531                 m_A.copyLowerToUpperTriangle();
 
  536                 m_x.resize(numConstraintRows);
 
  560         if (multiBodyNumConstraints == 0)
 
  570                 for (
int i = 0; i < multiBodyNumConstraints; ++i)
 
  591                 for (
int i = 0; i < multiBodyNumConstraints; ++i)
 
  605                         m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
 
  608                 for (
int i = 0; i < multiBodyNumConstraints; ++i)
 
  618                         for (
int j = i + 1; j < multiBodyNumConstraints; ++j)
 
  644                         for (
int i = 0; i < multiBodyNumConstraints; ++i)
 
  703                 bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
  729                 int firstContactConstraintOffset = dindex;
 
  743                                 if (numFrictionPerContact == 2)
 
  785                 firstContactConstraintOffset = dindex;
 
  800                                 const int findex = (frictionContactConstraint1.
m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
 
  804                                 if (numtiBodyNumFrictionPerContact == 2)
 
  899                                 const int ndofA = multiBodyA->
getNumDofs() + 6;
 
  901 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 
  905 #endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 
  917                                 const int ndofB = multiBodyB->
getNumDofs() + 6;
 
  919 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 
  923 #endif  // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS 
  938         : m_solver(solver), m_fallback(0)