Blender  V3.3
btMultiBodyMLCPConstraintSolver.cpp
Go to the documentation of this file.
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 
17 
22 
23 #define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
24 
25 static bool interleaveContactAndFriction1 = false;
26 
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
33 };
34 
35 // Helper function to compute a delta velocity in the constraint space.
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.
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 
67  const btAlignedObjectArray<btSolverBody>& solverBodyPool,
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;
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;
109  constraint.m_relpos2CrossNormal,
110  invMassB,
111  constraint.m_angularComponentB);
112  }
113 
114  return ret;
115 }
116 
118  const btAlignedObjectArray<btSolverBody>& solverBodyPool,
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;
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;
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;
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;
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 
237 {
240 }
241 
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;
326  {
327  BT_PROFILE("ofs resize");
328  ofs.resize(0);
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 
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 
540  {
541  for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
542  {
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 
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  {
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  {
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)
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  {
634  }
635 
636  // 4. Initialize x
637  {
638  BT_PROFILE("resize/init x");
639 
640  m_multiBodyX.resize(multiBodyNumConstraints);
641 
643  {
644  for (int i = 0; i < multiBodyNumConstraints; ++i)
645  {
647  m_multiBodyX[i] = constraint.m_appliedImpulse;
648  }
649  }
650  else
651  {
652  m_multiBodyX.setZero();
653  }
654  }
655 }
656 
658 {
659  bool result = true;
660 
661  if (m_A.rows() != 0)
662  {
663  // If using split impulse, we solve 2 separate (M)LCPs
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 
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  {
677  }
678  }
679 
680  if (!result)
681  return false;
682 
683  if (m_multiBodyA.rows() != 0)
684  {
686  }
687 
688  return result;
689 }
690 
692  btCollisionObject** bodies,
693  int numBodies,
695  int numManifolds,
697  int numConstraints,
699  btIDebugDraw* debugDrawer)
700 {
701  // 1. Setup for rigid-bodies
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 
714  const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
715 
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  {
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
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 
739  m_limitDependencies[dindex++] = -1;
741  int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
742  m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
743  if (numFrictionPerContact == 2)
744  {
746  m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
747  }
748  }
749  }
750  else
751  {
752  for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
753  {
755  m_limitDependencies[dindex++] = -1;
756  }
757  for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
758  {
760  m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
761  }
762  }
763 
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
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 
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  {
818  m_multiBodyLimitDependencies[dindex++] = -1;
819  }
820  for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
821  {
823  m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
824  }
825  }
826 
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");
841  }
842 
843  return btScalar(0);
844 }
845 
847 {
848  bool result = true;
849  {
850  BT_PROFILE("solveMLCP");
852  }
853 
854  // Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
855  if (!result)
856  {
857  m_fallback++;
859  }
860 
861  {
862  BT_PROFILE("process MLCP results");
863 
864  for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
865  {
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 
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  {
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 
938  : m_solver(solver), m_fallback(0)
939 {
940  // Do nothing
941 }
942 
944 {
945  // Do nothing
946 }
947 
949 {
950  m_solver = solver;
951 }
952 
954 {
955  return m_fallback;
956 }
957 
959 {
960  m_fallback = num;
961 }
962 
964 {
965  return BT_MLCP_SOLVER;
966 }
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble GLdouble r _GL_VOID_RET _GL_VOID GLfloat GLfloat r _GL_VOID_RET _GL_VOID GLint GLint r _GL_VOID_RET _GL_VOID GLshort GLshort r _GL_VOID_RET _GL_VOID GLdouble GLdouble r
btCollisionObject
btConstraintSolverType
btConstraintSolver provides solver interface
@ BT_MLCP_SOLVER
@ SOLVER_USE_WARMSTARTING
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
#define btMatrixXu
Definition: btMatrixX.h:529
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
btAlignedObjectArray< btScalar > m_data
static btScalar computeConstraintMatrixDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint)
static btScalar computeConstraintMatrixOffDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint, const btMultiBodySolverConstraint &offDiagConstraint)
static btScalar computeDeltaVelocityInConstraintSpace(const btVector3 &angularDeltaVelocity, const btVector3 &contactNormal, btScalar invMass, const btVector3 &angularJacobian, const btVector3 &linearJacobian)
static bool interleaveContactAndFriction1
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBody
Definition: btMultiBody.h:51
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btPersistentManifold()
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
#define BT_INFINITY
Definition: btScalar.h:405
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
btSequentialImpulseConstraintSolverMt int numBodies
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btConstraintArray m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverNonContactConstraintPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btVector3 m_relpos2CrossNormal
btVector3 m_contactNormal1
btVector3 m_relpos1CrossNormal
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btVector3 m_contactNormal2
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE void reserve(int _Count)
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE void push_back(const T &_Val)
SIMD_FORCE_INLINE T & expand(const T &fillValue=T())
original version written by Erwin Coumans, October 2013
virtual bool solveMLCP(const btMatrixXu &A, const btVectorXu &b, btVectorXu &x, const btVectorXu &lo, const btVectorXu &hi, const btAlignedObjectArray< int > &limitDependency, int numIterations, bool useSparsity=true)=0
btMatrixXu m_scratchJ3
Cache variable for constraint Jacobian matrix.
btAlignedObjectArray< int > m_multiBodyLimitDependencies
btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
btVectorXu m_xSplit
Split impulse cache vector corresponding to m_x.
btVectorXu m_b
b vector in the MLCP formulation.
btAlignedObjectArray< btMultiBodySolverConstraint * > m_multiBodyAllConstraintPtrArray
Array of all the multibody constraints.
btMLCPSolverInterface * m_solver
MLCP solver.
void setNumFallbacks(int num)
Sets the number of fallbacks. This function may be used to reset the number to zero.
btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btVectorXu m_lo
Lower bound of constraint impulse, m_x.
btMatrixXu m_A
A matrix in the MLCP formulation.
btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface *solver)
int m_fallback
Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver f...
btMatrixXu m_multiBodyA
A matrix in the MLCP formulation.
btAlignedObjectArray< int > m_limitDependencies
btVectorXu m_bSplit
Split impulse Cache vector corresponding to m_b.
void setMLCPSolver(btMLCPSolverInterface *solver)
Sets MLCP solver. Assumed it's not null.
btAlignedObjectArray< int > m_scratchOfs
Cache variable for offsets.
void createMLCPFastRigidBody(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms for constraints of two rigid bodies.
btVectorXu m_hi
Upper bound of constraint impulse, m_x.
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
Solves MLCP and returns the success.
btVectorXu m_x
Constraint impulse, which is an output of MLCP solving.
btVectorXu m_multiBodyLo
Lower bound of constraint impulse, m_x.
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms, which are m_A, m_b, m_lo, and m_hi.
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
Array of all the rigid body constraints.
btMatrixXu m_scratchJInvM3
Cache variable for constraint Jacobian times inverse mass matrix.
btVectorXu m_multiBodyX
Constraint impulse, which is an output of MLCP solving.
void createMLCPFastMultiBody(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody.
btVectorXu m_multiBodyHi
Upper bound of constraint impulse, m_x.
virtual btConstraintSolverType getSolverType() const
Returns the constraint solver type.
btVectorXu m_multiBodyB
b vector in the MLCP formulation.
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
static unsigned c
Definition: RandGen.cpp:83
return ret