59                 int firstContactConstraintOffset = dindex;
 
   70                                 if (numFrictionPerContact == 2)
 
  153         int n = numConstraintRows;
 
  156                 m_b.resize(numConstraintRows);
 
  160                 for (
int i = 0; i < numConstraintRows; i++)
 
  167                                 m_b[i] = rhs / jacDiag;
 
  168                                 m_bSplit[i] = rhsPenetration / jacDiag;
 
  176         m_lo.resize(numConstraintRows);
 
  177         m_hi.resize(numConstraintRows);
 
  182                 for (
int i = 0; i < numConstraintRows; i++)
 
  204                 bodyJointNodeArray.
resize(numBodies, -1);
 
  221                 JinvM3.resize(2 * m, 8);
 
  253                                         slotA = jointNodeArray.
size();
 
  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;
 
  262                                 for (
int row = 0; row < numRows; row++, cur++)
 
  267                                         for (
int r = 0; r < 3; r++)
 
  271                                                 JinvM3.setElem(cur, r, normalInvMass[r]);
 
  272                                                 JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
 
  274                                         J3.setElem(cur, 3, 0);
 
  275                                         JinvM3.setElem(cur, 3, 0);
 
  276                                         J3.setElem(cur, 7, 0);
 
  277                                         JinvM3.setElem(cur, 7, 0);
 
  289                                         slotB = jointNodeArray.
size();
 
  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;
 
  299                                 for (
int row = 0; row < numRows; row++, cur++)
 
  304                                         for (
int r = 0; r < 3; r++)
 
  308                                                 JinvM3.setElem(cur, r, normalInvMassB[r]);
 
  309                                                 JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
 
  311                                         J3.setElem(cur, 3, 0);
 
  312                                         JinvM3.setElem(cur, 3, 0);
 
  313                                         J3.setElem(cur, 7, 0);
 
  314                                         JinvM3.setElem(cur, 7, 0);
 
  321                         rowOffset += numRows;
 
  326         const btScalar* JinvM = JinvM3.getBufferPointer();
 
  328         const btScalar* Jptr = J3.getBufferPointer();
 
  352                         const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
 
  355                                 int startJointNodeA = bodyJointNodeArray[sbA];
 
  356                                 while (startJointNodeA >= 0)
 
  358                                         int j0 = jointNodeArray[startJointNodeA].jointIndex;
 
  359                                         int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
 
  365                                                 m_A.multiplyAdd2_p8r(JinvMrow,
 
  366                                                                                          Jptr + 2 * 8 * (
size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
 
  368                                         startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
 
  373                                 int startJointNodeB = bodyJointNodeArray[sbB];
 
  374                                 while (startJointNodeB >= 0)
 
  376                                         int j1 = jointNodeArray[startJointNodeB].jointIndex;
 
  377                                         int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
 
  383                                                 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (
size_t)numRows,
 
  384                                                                                          Jptr + 2 * 8 * (
size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
 
  386                                         startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
 
  399                         for (; row__ < numJointRows;)
 
  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__);
 
  413                                         m_A.multiplyAdd2_p8r(JinvMrow + 8 * (
size_t)infom, Jrow + 8 * (
size_t)infom, infom, infom, row__, row__);
 
  424                 for (
int i = 0; i < 
m_A.rows(); ++i)
 
  433                 m_A.copyLowerToUpperTriangle();
 
  438                 m_x.resize(numConstraintRows);
 
  463         m_b.resize(numConstraintRows);
 
  470         for (
int i = 0; i < numConstraintRows; i++)
 
  481         Minv.resize(6 * numBodies, 6 * numBodies);
 
  483         for (
int i = 0; i < numBodies; i++)
 
  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]);
 
  492                 for (
int r = 0; r < 3; r++)
 
  493                         for (
int c = 0; c < 3; c++)
 
  498         J.resize(numConstraintRows, 6 * numBodies);
 
  501         m_lo.resize(numConstraintRows);
 
  502         m_hi.resize(numConstraintRows);
 
  504         for (
int i = 0; i < numConstraintRows; i++)
 
  532         J_transpose = J.transpose();
 
  543                         m_A = tmp * J_transpose;
 
  550                 for (
int i = 0; i < 
m_A.rows(); ++i)
 
  556         m_x.resize(numConstraintRows);