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