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