Blender  V3.3
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 
36 {
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");
106  }
107  else
108  {
109  BT_PROFILE("createMLCPFast");
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
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 
442  {
443  for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
444  {
446  m_x[i] = c.m_appliedImpulse;
447  m_xSplit[i] = c.m_appliedPushImpulse;
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);
465  m_bSplit.resize(numConstraintRows);
466 
467  m_bSplit.setZero();
468  m_b.setZero();
469 
470  for (int i = 0; i < numConstraintRows; i++)
471  {
473  {
474  m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv;
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);
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;
566  m_xSplit[i] = c.m_appliedPushImpulse;
567  }
568 }
569 
571 {
572  bool result = true;
573  {
574  BT_PROFILE("solveMLCP");
575  // printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
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 
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);
607  c.m_appliedPushImpulse = m_xSplit[i];
608  }
609  }
610  }
611  }
612  else
613  {
614  // printf("m_fallback = %d\n",m_fallback);
615  m_fallback++;
617  }
618 
619  return 0.f;
620 }
_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
@ SOLVER_USE_WARMSTARTING
bool interleaveContactAndFriction
bool gUseMatrixMultiply
#define btMatrixXu
Definition: btMatrixX.h:529
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:514
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
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
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...
btScalar m_jacDiagABInv
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_scratchTmp
Definition: btMLCPSolver.h:53
btVectorXu m_b
Definition: btMLCPSolver.h:28
btMatrixXu m_scratchJTranspose
Definition: btMLCPSolver.h:52
virtual ~btMLCPSolver()
btMatrixXu m_scratchMInv
Definition: btMLCPSolver.h:50
btAlignedObjectArray< int > m_limitDependencies
Definition: btMLCPSolver.h:39
btMatrixXu m_scratchJ
Definition: btMLCPSolver.h:51
btVectorXu m_x
Definition: btMLCPSolver.h:29
virtual void createMLCP(const btContactSolverInfo &infoGlobal)
btMatrixXu m_scratchJ3
Definition: btMLCPSolver.h:47
btVectorXu m_xSplit
Definition: btMLCPSolver.h:35
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
btVectorXu m_bSplit
when using 'split impulse' we solve two separate (M)LCPs
Definition: btMLCPSolver.h:34
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btVectorXu m_hi
Definition: btMLCPSolver.h:31
btMatrixXu m_A
Definition: btMLCPSolver.h:27
btAlignedObjectArray< int > m_scratchOfs
Definition: btMLCPSolver.h:49
btMatrixXu m_scratchJInvM3
Definition: btMLCPSolver.h:48
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btMLCPSolver(btMLCPSolverInterface *solver)
original version written by Erwin Coumans, October 2013
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
Definition: btMLCPSolver.h:40
btMLCPSolverInterface * m_solver
Definition: btMLCPSolver.h:41
btVectorXu m_lo
Definition: btMLCPSolver.h:30
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
static unsigned c
Definition: RandGen.cpp:83
int nextJointNodeIndex
int constraintRowIndex