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);
The btRigidBody is the main class for rigid body objects.
const btMatrix3x3 & getInvInertiaTensorWorld() const
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
const btVector3 & internalGetInvMass() const