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)