Bullet Collision Detection & Physics Library
btMLCPSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2013 Erwin Coumans 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 */
16 
17 #include "btMLCPSolver.h"
18 #include "LinearMath/btMatrixX.h"
19 #include "LinearMath/btQuickprof.h"
21 
23  : m_solver(solver),
24  m_fallback(0)
25 {
26 }
27 
29 {
30 }
31 
32 bool gUseMatrixMultiply = false;
34 
35 btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
36 {
37  btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodiesUnUsed, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
38 
39  {
40  BT_PROFILE("gather constraint data");
41 
43 
44  // int numBodies = m_tmpSolverBodyPool.size();
48  // printf("m_limitDependencies.size() = %d\n",m_limitDependencies.size());
49 
50  int dindex = 0;
51  for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); i++)
52  {
54  m_limitDependencies[dindex++] = -1;
55  }
56 
58 
59  int firstContactConstraintOffset = dindex;
60 
62  {
63  for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
64  {
66  m_limitDependencies[dindex++] = -1;
68  int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
69  m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
70  if (numFrictionPerContact == 2)
71  {
73  m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
74  }
75  }
76  }
77  else
78  {
79  for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
80  {
82  m_limitDependencies[dindex++] = -1;
83  }
84  for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
85  {
87  m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
88  }
89  }
90 
92  {
93  m_A.resize(0, 0);
94  m_b.resize(0);
95  m_x.resize(0);
96  m_lo.resize(0);
97  m_hi.resize(0);
98  return 0.f;
99  }
100  }
101 
102  if (gUseMatrixMultiply)
103  {
104  BT_PROFILE("createMLCP");
105  createMLCP(infoGlobal);
106  }
107  else
108  {
109  BT_PROFILE("createMLCPFast");
110  createMLCPFast(infoGlobal);
111  }
112 
113  return 0.f;
114 }
115 
117 {
118  bool result = true;
119 
120  if (m_A.rows() == 0)
121  return true;
122 
123  //if using split impulse, we solve 2 separate (M)LCPs
124  if (infoGlobal.m_splitImpulse)
125  {
126  btMatrixXu Acopy = m_A;
127  btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
128  // printf("solve first LCP\n");
130  if (result)
131  result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
132  }
133  else
134  {
136  }
137  return result;
138 }
139 
141 {
142  int jointIndex; // pointer to enclosing dxJoint object
143  int otherBodyIndex; // *other* body this joint is connected to
144  int nextJointNodeIndex; //-1 for null
146 };
147 
149 {
150  int numContactRows = interleaveContactAndFriction ? 3 : 1;
151 
152  int numConstraintRows = m_allConstraintPtrArray.size();
153  int n = numConstraintRows;
154  {
155  BT_PROFILE("init b (rhs)");
156  m_b.resize(numConstraintRows);
157  m_bSplit.resize(numConstraintRows);
158  m_b.setZero();
159  m_bSplit.setZero();
160  for (int i = 0; i < numConstraintRows; i++)
161  {
162  btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
163  if (!btFuzzyZero(jacDiag))
164  {
165  btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
166  btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
167  m_b[i] = rhs / jacDiag;
168  m_bSplit[i] = rhsPenetration / jacDiag;
169  }
170  }
171  }
172 
173  // btScalar* w = 0;
174  // int nub = 0;
175 
176  m_lo.resize(numConstraintRows);
177  m_hi.resize(numConstraintRows);
178 
179  {
180  BT_PROFILE("init lo/ho");
181 
182  for (int i = 0; i < numConstraintRows; i++)
183  {
184  if (0) //m_limitDependencies[i]>=0)
185  {
186  m_lo[i] = -BT_INFINITY;
187  m_hi[i] = BT_INFINITY;
188  }
189  else
190  {
191  m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
192  m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
193  }
194  }
195  }
196 
197  //
198  int m = m_allConstraintPtrArray.size();
199 
200  int numBodies = m_tmpSolverBodyPool.size();
201  btAlignedObjectArray<int> bodyJointNodeArray;
202  {
203  BT_PROFILE("bodyJointNodeArray.resize");
204  bodyJointNodeArray.resize(numBodies, -1);
205  }
206  btAlignedObjectArray<btJointNode> jointNodeArray;
207  {
208  BT_PROFILE("jointNodeArray.reserve");
209  jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
210  }
211 
212  btMatrixXu& J3 = m_scratchJ3;
213  {
214  BT_PROFILE("J3.resize");
215  J3.resize(2 * m, 8);
216  }
217  btMatrixXu& JinvM3 = m_scratchJInvM3;
218  {
219  BT_PROFILE("JinvM3.resize/setZero");
220 
221  JinvM3.resize(2 * m, 8);
222  JinvM3.setZero();
223  J3.setZero();
224  }
225  int cur = 0;
226  int rowOffset = 0;
228  {
229  BT_PROFILE("ofs resize");
230  ofs.resize(0);
232  }
233  {
234  BT_PROFILE("Compute J and JinvM");
235  int c = 0;
236 
237  int numRows = 0;
238 
239  for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
240  {
241  ofs[c] = rowOffset;
242  int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
243  int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
244  btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
245  btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
246 
247  numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
248  if (orgBodyA)
249  {
250  {
251  int slotA = -1;
252  //find free jointNode slot for sbA
253  slotA = jointNodeArray.size();
254  jointNodeArray.expand(); //NonInitializing();
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;
261  }
262  for (int row = 0; row < numRows; row++, cur++)
263  {
264  btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
265  btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
266 
267  for (int r = 0; r < 3; r++)
268  {
269  J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
270  J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
271  JinvM3.setElem(cur, r, normalInvMass[r]);
272  JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
273  }
274  J3.setElem(cur, 3, 0);
275  JinvM3.setElem(cur, 3, 0);
276  J3.setElem(cur, 7, 0);
277  JinvM3.setElem(cur, 7, 0);
278  }
279  }
280  else
281  {
282  cur += numRows;
283  }
284  if (orgBodyB)
285  {
286  {
287  int slotB = -1;
288  //find free jointNode slot for sbA
289  slotB = jointNodeArray.size();
290  jointNodeArray.expand(); //NonInitializing();
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;
297  }
298 
299  for (int row = 0; row < numRows; row++, cur++)
300  {
301  btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
302  btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
303 
304  for (int r = 0; r < 3; r++)
305  {
306  J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
307  J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
308  JinvM3.setElem(cur, r, normalInvMassB[r]);
309  JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
310  }
311  J3.setElem(cur, 3, 0);
312  JinvM3.setElem(cur, 3, 0);
313  J3.setElem(cur, 7, 0);
314  JinvM3.setElem(cur, 7, 0);
315  }
316  }
317  else
318  {
319  cur += numRows;
320  }
321  rowOffset += numRows;
322  }
323  }
324 
325  //compute JinvM = J*invM.
326  const btScalar* JinvM = JinvM3.getBufferPointer();
327 
328  const btScalar* Jptr = J3.getBufferPointer();
329  {
330  BT_PROFILE("m_A.resize");
331  m_A.resize(n, n);
332  }
333 
334  {
335  BT_PROFILE("m_A.setZero");
336  m_A.setZero();
337  }
338  int c = 0;
339  {
340  int numRows = 0;
341  BT_PROFILE("Compute A");
342  for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
343  {
344  int row__ = ofs[c];
345  int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
346  int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
347  // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
348  // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
349 
350  numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
351 
352  const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
353 
354  {
355  int startJointNodeA = bodyJointNodeArray[sbA];
356  while (startJointNodeA >= 0)
357  {
358  int j0 = jointNodeArray[startJointNodeA].jointIndex;
359  int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
360  if (j0 < c)
361  {
362  int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
363  size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
364  //printf("%d joint i %d and j0: %d: ",count++,i,j0);
365  m_A.multiplyAdd2_p8r(JinvMrow,
366  Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
367  }
368  startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
369  }
370  }
371 
372  {
373  int startJointNodeB = bodyJointNodeArray[sbB];
374  while (startJointNodeB >= 0)
375  {
376  int j1 = jointNodeArray[startJointNodeB].jointIndex;
377  int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
378 
379  if (j1 < c)
380  {
381  int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
382  size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
383  m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
384  Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
385  }
386  startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
387  }
388  }
389  }
390 
391  {
392  BT_PROFILE("compute diagonal");
393  // compute diagonal blocks of m_A
394 
395  int row__ = 0;
396  int numJointRows = m_allConstraintPtrArray.size();
397 
398  int jj = 0;
399  for (; row__ < numJointRows;)
400  {
401  //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
402  int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
403  // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
404  btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
405 
406  const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
407 
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__);
411  if (orgBodyB)
412  {
413  m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
414  }
415  row__ += infom;
416  jj++;
417  }
418  }
419  }
420 
421  if (1)
422  {
423  // add cfm to the diagonal of m_A
424  for (int i = 0; i < m_A.rows(); ++i)
425  {
426  m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
427  }
428  }
429 
431  {
432  BT_PROFILE("fill the upper triangle ");
433  m_A.copyLowerToUpperTriangle();
434  }
435 
436  {
437  BT_PROFILE("resize/init x");
438  m_x.resize(numConstraintRows);
439  m_xSplit.resize(numConstraintRows);
440 
441  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
442  {
443  for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
444  {
446  m_x[i] = c.m_appliedImpulse;
448  }
449  }
450  else
451  {
452  m_x.setZero();
453  m_xSplit.setZero();
454  }
455  }
456 }
457 
459 {
460  int numBodies = this->m_tmpSolverBodyPool.size();
461  int numConstraintRows = m_allConstraintPtrArray.size();
462 
463  m_b.resize(numConstraintRows);
464  if (infoGlobal.m_splitImpulse)
465  m_bSplit.resize(numConstraintRows);
466 
467  m_bSplit.setZero();
468  m_b.setZero();
469 
470  for (int i = 0; i < numConstraintRows; i++)
471  {
472  if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
473  {
474  m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv;
475  if (infoGlobal.m_splitImpulse)
476  m_bSplit[i] = m_allConstraintPtrArray[i]->m_rhsPenetration / m_allConstraintPtrArray[i]->m_jacDiagABInv;
477  }
478  }
479 
480  btMatrixXu& Minv = m_scratchMInv;
481  Minv.resize(6 * numBodies, 6 * numBodies);
482  Minv.setZero();
483  for (int i = 0; i < numBodies; i++)
484  {
485  const btSolverBody& rb = m_tmpSolverBodyPool[i];
486  const btVector3& invMass = rb.m_invMass;
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]);
490  btRigidBody* orgBody = m_tmpSolverBodyPool[i].m_originalBody;
491 
492  for (int r = 0; r < 3; r++)
493  for (int c = 0; c < 3; c++)
494  setElem(Minv, i * 6 + 3 + r, i * 6 + 3 + c, orgBody ? orgBody->getInvInertiaTensorWorld()[r][c] : 0);
495  }
496 
497  btMatrixXu& J = m_scratchJ;
498  J.resize(numConstraintRows, 6 * numBodies);
499  J.setZero();
500 
501  m_lo.resize(numConstraintRows);
502  m_hi.resize(numConstraintRows);
503 
504  for (int i = 0; i < numConstraintRows; i++)
505  {
506  m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
507  m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
508 
509  int bodyIndex0 = m_allConstraintPtrArray[i]->m_solverBodyIdA;
510  int bodyIndex1 = m_allConstraintPtrArray[i]->m_solverBodyIdB;
511  if (m_tmpSolverBodyPool[bodyIndex0].m_originalBody)
512  {
513  setElem(J, i, 6 * bodyIndex0 + 0, m_allConstraintPtrArray[i]->m_contactNormal1[0]);
514  setElem(J, i, 6 * bodyIndex0 + 1, m_allConstraintPtrArray[i]->m_contactNormal1[1]);
515  setElem(J, i, 6 * bodyIndex0 + 2, m_allConstraintPtrArray[i]->m_contactNormal1[2]);
516  setElem(J, i, 6 * bodyIndex0 + 3, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[0]);
517  setElem(J, i, 6 * bodyIndex0 + 4, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[1]);
518  setElem(J, i, 6 * bodyIndex0 + 5, m_allConstraintPtrArray[i]->m_relpos1CrossNormal[2]);
519  }
520  if (m_tmpSolverBodyPool[bodyIndex1].m_originalBody)
521  {
522  setElem(J, i, 6 * bodyIndex1 + 0, m_allConstraintPtrArray[i]->m_contactNormal2[0]);
523  setElem(J, i, 6 * bodyIndex1 + 1, m_allConstraintPtrArray[i]->m_contactNormal2[1]);
524  setElem(J, i, 6 * bodyIndex1 + 2, m_allConstraintPtrArray[i]->m_contactNormal2[2]);
525  setElem(J, i, 6 * bodyIndex1 + 3, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[0]);
526  setElem(J, i, 6 * bodyIndex1 + 4, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[1]);
527  setElem(J, i, 6 * bodyIndex1 + 5, m_allConstraintPtrArray[i]->m_relpos2CrossNormal[2]);
528  }
529  }
530 
531  btMatrixXu& J_transpose = m_scratchJTranspose;
532  J_transpose = J.transpose();
533 
534  btMatrixXu& tmp = m_scratchTmp;
535 
536  {
537  {
538  BT_PROFILE("J*Minv");
539  tmp = J * Minv;
540  }
541  {
542  BT_PROFILE("J*tmp");
543  m_A = tmp * J_transpose;
544  }
545  }
546 
547  if (1)
548  {
549  // add cfm to the diagonal of m_A
550  for (int i = 0; i < m_A.rows(); ++i)
551  {
552  m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
553  }
554  }
555 
556  m_x.resize(numConstraintRows);
557  if (infoGlobal.m_splitImpulse)
558  m_xSplit.resize(numConstraintRows);
559  // m_x.setZero();
560 
561  for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
562  {
564  m_x[i] = c.m_appliedImpulse;
565  if (infoGlobal.m_splitImpulse)
567  }
568 }
569 
570 btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
571 {
572  bool result = true;
573  {
574  BT_PROFILE("solveMLCP");
575  // printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
576  result = solveMLCP(infoGlobal);
577  }
578 
579  //check if solution is valid, and otherwise fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
580  if (result)
581  {
582  BT_PROFILE("process MLCP results");
583  for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
584  {
585  {
587  int sbA = c.m_solverBodyIdA;
588  int sbB = c.m_solverBodyIdB;
589  //btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
590  // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
591 
592  btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
593  btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
594 
595  {
596  btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
597  c.m_appliedImpulse = m_x[i];
598  solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
599  solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
600  }
601 
602  if (infoGlobal.m_splitImpulse)
603  {
604  btScalar deltaImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
605  solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
606  solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
608  }
609  }
610  }
611  }
612  else
613  {
614  // printf("m_fallback = %d\n",m_fallback);
615  m_fallback++;
616  btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
617  }
618 
619  return 0.f;
620 }
btMLCPSolver::m_scratchJ3
btMatrixXu m_scratchJ3
The following scratch variables are not stateful – contents are cleared prior to each use.
Definition: btMLCPSolver.h:47
btSequentialImpulseConstraintSolver::m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:59
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:103
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:74
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:48
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
BT_INFINITY
#define BT_INFINITY
Definition: btScalar.h:405
btMLCPSolver::solveMLCP
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
Definition: btMLCPSolver.cpp:116
btSolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btSolverConstraint.h:43
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btMLCPSolver::m_scratchJ
btMatrixXu m_scratchJ
Definition: btMLCPSolver.h:51
btContactSolverInfo
Definition: btContactSolverInfo.h:72
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btSolverConstraint.h:29
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1726
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btMLCPSolverInterface::solveMLCP
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
gUseMatrixMultiply
bool gUseMatrixMultiply
Definition: btMLCPSolver.cpp:32
btSolveProjectedGaussSeidel.h
btSolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btSolverConstraint.h:38
SOLVER_USE_WARMSTARTING
Definition: btContactSolverInfo.h:25
btMLCPSolver::m_solver
btMLCPSolverInterface * m_solver
Definition: btMLCPSolver.h:41
btMatrixX.h
btMLCPSolver::m_scratchOfs
btAlignedObjectArray< int > m_scratchOfs
Definition: btMLCPSolver.h:49
btMLCPSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btMLCPSolver.cpp:35
btJointNode::constraintRowIndex
int constraintRowIndex
Definition: btMLCPSolver.cpp:145
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:243
btMLCPSolver::createMLCP
virtual void createMLCP(const btContactSolverInfo &infoGlobal)
Definition: btMLCPSolver.cpp:458
btSolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btSolverConstraint.h:41
btJointNode::nextJointNodeIndex
int nextJointNodeIndex
Definition: btMLCPSolver.cpp:144
btSolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btSolverConstraint.h:35
btSolverBody::m_invMass
btVector3 m_invMass
Definition: btSolverBody.h:112
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btMLCPSolver::btMLCPSolver
btMLCPSolver(btMLCPSolverInterface *solver)
original version written by Erwin Coumans, October 2013
Definition: btMLCPSolver.cpp:22
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:26
btMLCPSolver::m_x
btVectorXu m_x
Definition: btMLCPSolver.h:29
btMLCPSolverInterface
original version written by Erwin Coumans, October 2013
Definition: btMLCPSolverInterface.h:22
btMLCPSolver.h
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btSolverBody::internalApplyPushImpulse
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
Definition: btSolverBody.h:165
btJointNode::otherBodyIndex
int otherBodyIndex
Definition: btMLCPSolver.cpp:143
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btMLCPSolver::m_bSplit
btVectorXu m_bSplit
when using 'split impulse' we solve two separate (M)LCPs
Definition: btMLCPSolver.h:34
btMatrixXu
#define btMatrixXu
Definition: btMatrixX.h:530
btMLCPSolver::m_b
btVectorXu m_b
Definition: btMLCPSolver.h:28
btMLCPSolver::m_limitDependencies
btAlignedObjectArray< int > m_limitDependencies
Definition: btMLCPSolver.h:39
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:54
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition: btContactSolverInfo.h:50
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:60
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:63
btJointNode
Definition: btMLCPSolver.cpp:140
btMLCPSolver::m_allConstraintPtrArray
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
Definition: btMLCPSolver.h:40
btFuzzyZero
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
btAlignedObjectArray< int >
btSolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btSolverConstraint.h:62
btSequentialImpulseConstraintSolver::m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:57
btMLCPSolver::m_lo
btVectorXu m_lo
Definition: btMLCPSolver.h:30
btAlignedObjectArray::expand
T & expand(const T &fillValue=T())
Definition: btAlignedObjectArray.h:242
btSequentialImpulseConstraintSolver::m_tmpConstraintSizesPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
Definition: btSequentialImpulseConstraintSolver.h:65
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:56
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1399
btAlignedObjectArray::reserve
void reserve(int _Count)
Definition: btAlignedObjectArray.h:280
btSolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btSolverConstraint.h:63
btQuickprof.h
btMLCPSolver::m_A
btMatrixXu m_A
Definition: btMLCPSolver.h:27
btMLCPSolver::~btMLCPSolver
virtual ~btMLCPSolver()
Definition: btMLCPSolver.cpp:28
btSequentialImpulseConstraintSolver::m_tmpSolverNonContactConstraintPool
btConstraintArray m_tmpSolverNonContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:58
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btMLCPSolver::createMLCPFast
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
Definition: btMLCPSolver.cpp:148
btMLCPSolver::m_scratchJInvM3
btMatrixXu m_scratchJInvM3
Definition: btMLCPSolver.h:48
btJointNode::jointIndex
int jointIndex
Definition: btMLCPSolver.cpp:142
btMLCPSolver::m_scratchTmp
btMatrixXu m_scratchTmp
Definition: btMLCPSolver.h:53
interleaveContactAndFriction
bool interleaveContactAndFriction
Definition: btMLCPSolver.cpp:33
btMLCPSolver::m_xSplit
btVectorXu m_xSplit
Definition: btMLCPSolver.h:35
btAlignedObjectArray::resizeNoInitialize
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
Definition: btAlignedObjectArray.h:194
setElem
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:515
btMLCPSolver::m_fallback
int m_fallback
Definition: btMLCPSolver.h:42
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btSolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btSolverConstraint.h:44
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:197
btMLCPSolver::solveGroupCacheFriendlyIterations
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btMLCPSolver.cpp:570
btMLCPSolver::m_scratchJTranspose
btMatrixXu m_scratchJTranspose
Definition: btMLCPSolver.h:52
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btMLCPSolver::m_scratchMInv
btMatrixXu m_scratchMInv
Definition: btMLCPSolver.h:50
btSolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btSolverConstraint.h:40
btMLCPSolver::m_hi
btVectorXu m_hi
Definition: btMLCPSolver.h:31