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