Bullet Collision Detection & Physics Library: btMultiBodyMLCPConstraintSolver.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodyMLCPConstraintSolver.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2018 Google Inc. 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*/
15
16#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
17
18#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
19#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
20#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
21#include "BulletDynamics/MLCPSolvers/btMLCPSolverInterface.h"
22
23#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
24
25 static bool interleaveContactAndFriction1 = false;
26
27 struct btJointNode1
28{
29 int jointIndex; // pointer to enclosing dxJoint object
30 int otherBodyIndex; // *other* body this joint is connected to
31 int nextJointNodeIndex; //-1 for null
32 int constraintRowIndex;
33};
34
35// Helper function to compute a delta velocity in the constraint space.
36 static btScalar computeDeltaVelocityInConstraintSpace(
37 const btVector3& angularDeltaVelocity,
38 const btVector3& contactNormal,
39 btScalar invMass,
40 const btVector3& angularJacobian,
41 const btVector3& linearJacobian)
42{
43 return angularDeltaVelocity.dot(angularJacobian) + contactNormal.dot(linearJacobian) * invMass;
44}
45
46// Faster version of computeDeltaVelocityInConstraintSpace that can be used when contactNormal and linearJacobian are
47// identical.
48 static btScalar computeDeltaVelocityInConstraintSpace(
49 const btVector3& angularDeltaVelocity,
50 btScalar invMass,
51 const btVector3& angularJacobian)
52{
53 return angularDeltaVelocity.dot(angularJacobian) + invMass;
54}
55
56// Helper function to compute a delta velocity in the constraint space.
57 static btScalar computeDeltaVelocityInConstraintSpace(const btScalar* deltaVelocity, const btScalar* jacobian, int size)
58{
59 btScalar result = 0;
60 for (int i = 0; i < size; ++i)
61 result += deltaVelocity[i] * jacobian[i];
62
63 return result;
64}
65
66 static btScalar computeConstraintMatrixDiagElementMultiBody(
67 const btAlignedObjectArray<btSolverBody>& solverBodyPool,
68 const btMultiBodyJacobianData& data,
69 const btMultiBodySolverConstraint& constraint)
70{
71 btScalar ret = 0;
72
73 const btMultiBody* multiBodyA = constraint.m_multiBodyA;
74 const btMultiBody* multiBodyB = constraint.m_multiBodyB;
75
76 if (multiBodyA)
77 {
78 const btScalar* jacA = &data.m_jacobians[constraint.m_jacAindex];
79 const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
80 const int ndofA = multiBodyA->getNumDofs() + 6;
81 ret += computeDeltaVelocityInConstraintSpace(deltaA, jacA, ndofA);
82 }
83 else
84 {
85 const int solverBodyIdA = constraint.m_solverBodyIdA;
86 btAssert(solverBodyIdA != -1);
87 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
88 const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
89 ret += computeDeltaVelocityInConstraintSpace(
90 constraint.m_relpos1CrossNormal,
91 invMassA,
92 constraint.m_angularComponentA);
93 }
94
95 if (multiBodyB)
96 {
97 const btScalar* jacB = &data.m_jacobians[constraint.m_jacBindex];
98 const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
99 const int ndofB = multiBodyB->getNumDofs() + 6;
100 ret += computeDeltaVelocityInConstraintSpace(deltaB, jacB, ndofB);
101 }
102 else
103 {
104 const int solverBodyIdB = constraint.m_solverBodyIdB;
105 btAssert(solverBodyIdB != -1);
106 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
107 const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
108 ret += computeDeltaVelocityInConstraintSpace(
109 constraint.m_relpos2CrossNormal,
110 invMassB,
111 constraint.m_angularComponentB);
112 }
113
114 return ret;
115}
116
117 static btScalar computeConstraintMatrixOffDiagElementMultiBody(
118 const btAlignedObjectArray<btSolverBody>& solverBodyPool,
119 const btMultiBodyJacobianData& data,
120 const btMultiBodySolverConstraint& constraint,
121 const btMultiBodySolverConstraint& offDiagConstraint)
122{
123 btScalar offDiagA = btScalar(0);
124
125 const btMultiBody* multiBodyA = constraint.m_multiBodyA;
126 const btMultiBody* multiBodyB = constraint.m_multiBodyB;
127 const btMultiBody* offDiagMultiBodyA = offDiagConstraint.m_multiBodyA;
128 const btMultiBody* offDiagMultiBodyB = offDiagConstraint.m_multiBodyB;
129
130 // Assumed at least one system is multibody
131 btAssert(multiBodyA || multiBodyB);
132 btAssert(offDiagMultiBodyA || offDiagMultiBodyB);
133
134 if (offDiagMultiBodyA)
135 {
136 const btScalar* offDiagJacA = &data.m_jacobians[offDiagConstraint.m_jacAindex];
137
138 if (offDiagMultiBodyA == multiBodyA)
139 {
140 const int ndofA = multiBodyA->getNumDofs() + 6;
141 const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
142 offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacA, ndofA);
143 }
144 else if (offDiagMultiBodyA == multiBodyB)
145 {
146 const int ndofB = multiBodyB->getNumDofs() + 6;
147 const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
148 offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacA, ndofB);
149 }
150 }
151 else
152 {
153 const int solverBodyIdA = constraint.m_solverBodyIdA;
154 const int solverBodyIdB = constraint.m_solverBodyIdB;
155
156 const int offDiagSolverBodyIdA = offDiagConstraint.m_solverBodyIdA;
157 btAssert(offDiagSolverBodyIdA != -1);
158
159 if (offDiagSolverBodyIdA == solverBodyIdA)
160 {
161 btAssert(solverBodyIdA != -1);
162 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
163 const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
164 offDiagA += computeDeltaVelocityInConstraintSpace(
165 offDiagConstraint.m_relpos1CrossNormal,
166 offDiagConstraint.m_contactNormal1,
167 invMassA, constraint.m_angularComponentA,
168 constraint.m_contactNormal1);
169 }
170 else if (offDiagSolverBodyIdA == solverBodyIdB)
171 {
172 btAssert(solverBodyIdB != -1);
173 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
174 const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
175 offDiagA += computeDeltaVelocityInConstraintSpace(
176 offDiagConstraint.m_relpos1CrossNormal,
177 offDiagConstraint.m_contactNormal1,
178 invMassB,
179 constraint.m_angularComponentB,
180 constraint.m_contactNormal2);
181 }
182 }
183
184 if (offDiagMultiBodyB)
185 {
186 const btScalar* offDiagJacB = &data.m_jacobians[offDiagConstraint.m_jacBindex];
187
188 if (offDiagMultiBodyB == multiBodyA)
189 {
190 const int ndofA = multiBodyA->getNumDofs() + 6;
191 const btScalar* deltaA = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacAindex];
192 offDiagA += computeDeltaVelocityInConstraintSpace(deltaA, offDiagJacB, ndofA);
193 }
194 else if (offDiagMultiBodyB == multiBodyB)
195 {
196 const int ndofB = multiBodyB->getNumDofs() + 6;
197 const btScalar* deltaB = &data.m_deltaVelocitiesUnitImpulse[constraint.m_jacBindex];
198 offDiagA += computeDeltaVelocityInConstraintSpace(deltaB, offDiagJacB, ndofB);
199 }
200 }
201 else
202 {
203 const int solverBodyIdA = constraint.m_solverBodyIdA;
204 const int solverBodyIdB = constraint.m_solverBodyIdB;
205
206 const int offDiagSolverBodyIdB = offDiagConstraint.m_solverBodyIdB;
207 btAssert(offDiagSolverBodyIdB != -1);
208
209 if (offDiagSolverBodyIdB == solverBodyIdA)
210 {
211 btAssert(solverBodyIdA != -1);
212 const btSolverBody* solverBodyA = &solverBodyPool[solverBodyIdA];
213 const btScalar invMassA = solverBodyA->m_originalBody ? solverBodyA->m_originalBody->getInvMass() : 0.0;
214 offDiagA += computeDeltaVelocityInConstraintSpace(
215 offDiagConstraint.m_relpos2CrossNormal,
216 offDiagConstraint.m_contactNormal2,
217 invMassA, constraint.m_angularComponentA,
218 constraint.m_contactNormal1);
219 }
220 else if (offDiagSolverBodyIdB == solverBodyIdB)
221 {
222 btAssert(solverBodyIdB != -1);
223 const btSolverBody* solverBodyB = &solverBodyPool[solverBodyIdB];
224 const btScalar invMassB = solverBodyB->m_originalBody ? solverBodyB->m_originalBody->getInvMass() : 0.0;
225 offDiagA += computeDeltaVelocityInConstraintSpace(
226 offDiagConstraint.m_relpos2CrossNormal,
227 offDiagConstraint.m_contactNormal2,
228 invMassB, constraint.m_angularComponentB,
229 constraint.m_contactNormal2);
230 }
231 }
232
233 return offDiagA;
234}
235
236 void btMultiBodyMLCPConstraintSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
237{
238 createMLCPFastRigidBody(infoGlobal);
239 createMLCPFastMultiBody(infoGlobal);
240}
241
242 void btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody(const btContactSolverInfo& infoGlobal)
243{
244 int numContactRows = interleaveContactAndFriction1 ? 3 : 1;
245
246 int numConstraintRows = m_allConstraintPtrArray.size();
247
248 if (numConstraintRows == 0)
249 return;
250
251 int n = numConstraintRows;
252 {
253 BT_PROFILE("init b (rhs)");
254 m_b.resize(numConstraintRows);
255 m_bSplit.resize(numConstraintRows);
256 m_b.setZero();
257 m_bSplit.setZero();
258 for (int i = 0; i < numConstraintRows; i++)
259 {
260 btScalar jacDiag = m_allConstraintPtrArray[i]->m_jacDiagABInv;
261 if (!btFuzzyZero(jacDiag))
262 {
263 btScalar rhs = m_allConstraintPtrArray[i]->m_rhs;
264 btScalar rhsPenetration = m_allConstraintPtrArray[i]->m_rhsPenetration;
265 m_b[i] = rhs / jacDiag;
266 m_bSplit[i] = rhsPenetration / jacDiag;
267 }
268 }
269 }
270
271 // btScalar* w = 0;
272 // int nub = 0;
273
274 m_lo.resize(numConstraintRows);
275 m_hi.resize(numConstraintRows);
276
277 {
278 BT_PROFILE("init lo/ho");
279
280 for (int i = 0; i < numConstraintRows; i++)
281 {
282 if (0) //m_limitDependencies[i]>=0)
283 {
284 m_lo[i] = -BT_INFINITY;
285 m_hi[i] = BT_INFINITY;
286 }
287 else
288 {
289 m_lo[i] = m_allConstraintPtrArray[i]->m_lowerLimit;
290 m_hi[i] = m_allConstraintPtrArray[i]->m_upperLimit;
291 }
292 }
293 }
294
295 //
296 int m = m_allConstraintPtrArray.size();
297
298 int numBodies = m_tmpSolverBodyPool.size();
299 btAlignedObjectArray<int> bodyJointNodeArray;
300 {
301 BT_PROFILE("bodyJointNodeArray.resize");
302 bodyJointNodeArray.resize(numBodies, -1);
303 }
304 btAlignedObjectArray<btJointNode1> jointNodeArray;
305 {
306 BT_PROFILE("jointNodeArray.reserve");
307 jointNodeArray.reserve(2 * m_allConstraintPtrArray.size());
308 }
309
310 btMatrixXu& J3 = m_scratchJ3;
311 {
312 BT_PROFILE("J3.resize");
313 J3.resize(2 * m, 8);
314 }
315 btMatrixXu& JinvM3 = m_scratchJInvM3;
316 {
317 BT_PROFILE("JinvM3.resize/setZero");
318
319 JinvM3.resize(2 * m, 8);
320 JinvM3.setZero();
321 J3.setZero();
322 }
323 int cur = 0;
324 int rowOffset = 0;
325 btAlignedObjectArray<int>& ofs = m_scratchOfs;
326 {
327 BT_PROFILE("ofs resize");
328 ofs.resize(0);
329 ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
330 }
331 {
332 BT_PROFILE("Compute J and JinvM");
333 int c = 0;
334
335 int numRows = 0;
336
337 for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
338 {
339 ofs[c] = rowOffset;
340 int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
341 int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
342 btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
343 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
344
345 numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
346 if (orgBodyA)
347 {
348 {
349 int slotA = -1;
350 //find free jointNode slot for sbA
351 slotA = jointNodeArray.size();
352 jointNodeArray.expand(); //NonInitializing();
353 int prevSlot = bodyJointNodeArray[sbA];
354 bodyJointNodeArray[sbA] = slotA;
355 jointNodeArray[slotA].nextJointNodeIndex = prevSlot;
356 jointNodeArray[slotA].jointIndex = c;
357 jointNodeArray[slotA].constraintRowIndex = i;
358 jointNodeArray[slotA].otherBodyIndex = orgBodyB ? sbB : -1;
359 }
360 for (int row = 0; row < numRows; row++, cur++)
361 {
362 btVector3 normalInvMass = m_allConstraintPtrArray[i + row]->m_contactNormal1 * orgBodyA->getInvMass();
363 btVector3 relPosCrossNormalInvInertia = m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal * orgBodyA->getInvInertiaTensorWorld();
364
365 for (int r = 0; r < 3; r++)
366 {
367 J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal1[r]);
368 J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos1CrossNormal[r]);
369 JinvM3.setElem(cur, r, normalInvMass[r]);
370 JinvM3.setElem(cur, r + 4, relPosCrossNormalInvInertia[r]);
371 }
372 J3.setElem(cur, 3, 0);
373 JinvM3.setElem(cur, 3, 0);
374 J3.setElem(cur, 7, 0);
375 JinvM3.setElem(cur, 7, 0);
376 }
377 }
378 else
379 {
380 cur += numRows;
381 }
382 if (orgBodyB)
383 {
384 {
385 int slotB = -1;
386 //find free jointNode slot for sbA
387 slotB = jointNodeArray.size();
388 jointNodeArray.expand(); //NonInitializing();
389 int prevSlot = bodyJointNodeArray[sbB];
390 bodyJointNodeArray[sbB] = slotB;
391 jointNodeArray[slotB].nextJointNodeIndex = prevSlot;
392 jointNodeArray[slotB].jointIndex = c;
393 jointNodeArray[slotB].otherBodyIndex = orgBodyA ? sbA : -1;
394 jointNodeArray[slotB].constraintRowIndex = i;
395 }
396
397 for (int row = 0; row < numRows; row++, cur++)
398 {
399 btVector3 normalInvMassB = m_allConstraintPtrArray[i + row]->m_contactNormal2 * orgBodyB->getInvMass();
400 btVector3 relPosInvInertiaB = m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal * orgBodyB->getInvInertiaTensorWorld();
401
402 for (int r = 0; r < 3; r++)
403 {
404 J3.setElem(cur, r, m_allConstraintPtrArray[i + row]->m_contactNormal2[r]);
405 J3.setElem(cur, r + 4, m_allConstraintPtrArray[i + row]->m_relpos2CrossNormal[r]);
406 JinvM3.setElem(cur, r, normalInvMassB[r]);
407 JinvM3.setElem(cur, r + 4, relPosInvInertiaB[r]);
408 }
409 J3.setElem(cur, 3, 0);
410 JinvM3.setElem(cur, 3, 0);
411 J3.setElem(cur, 7, 0);
412 JinvM3.setElem(cur, 7, 0);
413 }
414 }
415 else
416 {
417 cur += numRows;
418 }
419 rowOffset += numRows;
420 }
421 }
422
423 //compute JinvM = J*invM.
424 const btScalar* JinvM = JinvM3.getBufferPointer();
425
426 const btScalar* Jptr = J3.getBufferPointer();
427 {
428 BT_PROFILE("m_A.resize");
429 m_A.resize(n, n);
430 }
431
432 {
433 BT_PROFILE("m_A.setZero");
434 m_A.setZero();
435 }
436 int c = 0;
437 {
438 int numRows = 0;
439 BT_PROFILE("Compute A");
440 for (int i = 0; i < m_allConstraintPtrArray.size(); i += numRows, c++)
441 {
442 int row__ = ofs[c];
443 int sbA = m_allConstraintPtrArray[i]->m_solverBodyIdA;
444 int sbB = m_allConstraintPtrArray[i]->m_solverBodyIdB;
445 // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
446 // btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
447
448 numRows = i < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[c].m_numConstraintRows : numContactRows;
449
450 const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
451
452 {
453 int startJointNodeA = bodyJointNodeArray[sbA];
454 while (startJointNodeA >= 0)
455 {
456 int j0 = jointNodeArray[startJointNodeA].jointIndex;
457 int cr0 = jointNodeArray[startJointNodeA].constraintRowIndex;
458 if (j0 < c)
459 {
460 int numRowsOther = cr0 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j0].m_numConstraintRows : numContactRows;
461 size_t ofsother = (m_allConstraintPtrArray[cr0]->m_solverBodyIdB == sbA) ? 8 * numRowsOther : 0;
462 //printf("%d joint i %d and j0: %d: ",count++,i,j0);
463 m_A.multiplyAdd2_p8r(JinvMrow,
464 Jptr + 2 * 8 * (size_t)ofs[j0] + ofsother, numRows, numRowsOther, row__, ofs[j0]);
465 }
466 startJointNodeA = jointNodeArray[startJointNodeA].nextJointNodeIndex;
467 }
468 }
469
470 {
471 int startJointNodeB = bodyJointNodeArray[sbB];
472 while (startJointNodeB >= 0)
473 {
474 int j1 = jointNodeArray[startJointNodeB].jointIndex;
475 int cj1 = jointNodeArray[startJointNodeB].constraintRowIndex;
476
477 if (j1 < c)
478 {
479 int numRowsOther = cj1 < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[j1].m_numConstraintRows : numContactRows;
480 size_t ofsother = (m_allConstraintPtrArray[cj1]->m_solverBodyIdB == sbB) ? 8 * numRowsOther : 0;
481 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)numRows,
482 Jptr + 2 * 8 * (size_t)ofs[j1] + ofsother, numRows, numRowsOther, row__, ofs[j1]);
483 }
484 startJointNodeB = jointNodeArray[startJointNodeB].nextJointNodeIndex;
485 }
486 }
487 }
488
489 {
490 BT_PROFILE("compute diagonal");
491 // compute diagonal blocks of m_A
492
493 int row__ = 0;
494 int numJointRows = m_allConstraintPtrArray.size();
495
496 int jj = 0;
497 for (; row__ < numJointRows;)
498 {
499 //int sbA = m_allConstraintPtrArray[row__]->m_solverBodyIdA;
500 int sbB = m_allConstraintPtrArray[row__]->m_solverBodyIdB;
501 // btRigidBody* orgBodyA = m_tmpSolverBodyPool[sbA].m_originalBody;
502 btRigidBody* orgBodyB = m_tmpSolverBodyPool[sbB].m_originalBody;
503
504 const unsigned int infom = row__ < m_tmpSolverNonContactConstraintPool.size() ? m_tmpConstraintSizesPool[jj].m_numConstraintRows : numContactRows;
505
506 const btScalar* JinvMrow = JinvM + 2 * 8 * (size_t)row__;
507 const btScalar* Jrow = Jptr + 2 * 8 * (size_t)row__;
508 m_A.multiply2_p8r(JinvMrow, Jrow, infom, infom, row__, row__);
509 if (orgBodyB)
510 {
511 m_A.multiplyAdd2_p8r(JinvMrow + 8 * (size_t)infom, Jrow + 8 * (size_t)infom, infom, infom, row__, row__);
512 }
513 row__ += infom;
514 jj++;
515 }
516 }
517 }
518
519 if (1)
520 {
521 // add cfm to the diagonal of m_A
522 for (int i = 0; i < m_A.rows(); ++i)
523 {
524 m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
525 }
526 }
527
529 {
530 BT_PROFILE("fill the upper triangle ");
531 m_A.copyLowerToUpperTriangle();
532 }
533
534 {
535 BT_PROFILE("resize/init x");
536 m_x.resize(numConstraintRows);
537 m_xSplit.resize(numConstraintRows);
538
539 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
540 {
541 for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
542 {
543 const btSolverConstraint& c = *m_allConstraintPtrArray[i];
544 m_x[i] = c.m_appliedImpulse;
545 m_xSplit[i] = c.m_appliedPushImpulse;
546 }
547 }
548 else
549 {
550 m_x.setZero();
551 m_xSplit.setZero();
552 }
553 }
554}
555
556 void btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody(const btContactSolverInfo& infoGlobal)
557{
558 const int multiBodyNumConstraints = m_multiBodyAllConstraintPtrArray.size();
559
560 if (multiBodyNumConstraints == 0)
561 return;
562
563 // 1. Compute b
564 {
565 BT_PROFILE("init b (rhs)");
566
567 m_multiBodyB.resize(multiBodyNumConstraints);
568 m_multiBodyB.setZero();
569
570 for (int i = 0; i < multiBodyNumConstraints; ++i)
571 {
572 const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
573 const btScalar jacDiag = constraint.m_jacDiagABInv;
574
575 if (!btFuzzyZero(jacDiag))
576 {
577 // Note that rhsPenetration is currently always zero because the split impulse hasn't been implemented for multibody yet.
578 const btScalar rhs = constraint.m_rhs;
579 m_multiBodyB[i] = rhs / jacDiag;
580 }
581 }
582 }
583
584 // 2. Compute lo and hi
585 {
586 BT_PROFILE("init lo/ho");
587
588 m_multiBodyLo.resize(multiBodyNumConstraints);
589 m_multiBodyHi.resize(multiBodyNumConstraints);
590
591 for (int i = 0; i < multiBodyNumConstraints; ++i)
592 {
593 const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
594 m_multiBodyLo[i] = constraint.m_lowerLimit;
595 m_multiBodyHi[i] = constraint.m_upperLimit;
596 }
597 }
598
599 // 3. Construct A matrix by using the impulse testing
600 {
601 BT_PROFILE("Compute A");
602
603 {
604 BT_PROFILE("m_A.resize");
605 m_multiBodyA.resize(multiBodyNumConstraints, multiBodyNumConstraints);
606 }
607
608 for (int i = 0; i < multiBodyNumConstraints; ++i)
609 {
610 // Compute the diagonal of A, which is A(i, i)
611 const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
612 const btScalar diagA = computeConstraintMatrixDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint);
613 m_multiBodyA.setElem(i, i, diagA);
614
615 // Computes the off-diagonals of A:
616 // a. The rest of i-th row of A, from A(i, i+1) to A(i, n)
617 // b. The rest of i-th column of A, from A(i+1, i) to A(n, i)
618 for (int j = i + 1; j < multiBodyNumConstraints; ++j)
619 {
620 const btMultiBodySolverConstraint& offDiagConstraint = *m_multiBodyAllConstraintPtrArray[j];
621 const btScalar offDiagA = computeConstraintMatrixOffDiagElementMultiBody(m_tmpSolverBodyPool, m_data, constraint, offDiagConstraint);
622
623 // Set the off-diagonal values of A. Note that A is symmetric.
624 m_multiBodyA.setElem(i, j, offDiagA);
625 m_multiBodyA.setElem(j, i, offDiagA);
626 }
627 }
628 }
629
630 // Add CFM to the diagonal of m_A
631 for (int i = 0; i < m_multiBodyA.rows(); ++i)
632 {
633 m_multiBodyA.setElem(i, i, m_multiBodyA(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
634 }
635
636 // 4. Initialize x
637 {
638 BT_PROFILE("resize/init x");
639
640 m_multiBodyX.resize(multiBodyNumConstraints);
641
642 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
643 {
644 for (int i = 0; i < multiBodyNumConstraints; ++i)
645 {
646 const btMultiBodySolverConstraint& constraint = *m_multiBodyAllConstraintPtrArray[i];
647 m_multiBodyX[i] = constraint.m_appliedImpulse;
648 }
649 }
650 else
651 {
652 m_multiBodyX.setZero();
653 }
654 }
655}
656
657 bool btMultiBodyMLCPConstraintSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
658{
659 bool result = true;
660
661 if (m_A.rows() != 0)
662 {
663 // If using split impulse, we solve 2 separate (M)LCPs
664 if (infoGlobal.m_splitImpulse)
665 {
666 const btMatrixXu Acopy = m_A;
667 const btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
668 // TODO(JS): Do we really need these copies when solveMLCP takes them as const?
669
670 result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
671 if (result)
672 result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo, m_hi, limitDependenciesCopy, infoGlobal.m_numIterations);
673 }
674 else
675 {
676 result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
677 }
678 }
679
680 if (!result)
681 return false;
682
683 if (m_multiBodyA.rows() != 0)
684 {
685 result = m_solver->solveMLCP(m_multiBodyA, m_multiBodyB, m_multiBodyX, m_multiBodyLo, m_multiBodyHi, m_multiBodyLimitDependencies, infoGlobal.m_numIterations);
686 }
687
688 return result;
689}
690
691 btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup(
692 btCollisionObject** bodies,
693 int numBodies,
694 btPersistentManifold** manifoldPtr,
695 int numManifolds,
696 btTypedConstraint** constraints,
697 int numConstraints,
698 const btContactSolverInfo& infoGlobal,
699 btIDebugDraw* debugDrawer)
700{
701 // 1. Setup for rigid-bodies
702 btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(
703 bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
704
705 // 2. Setup for multi-bodies
706 // a. Collect all different kinds of constraint as pointers into one array, m_allConstraintPtrArray
707 // b. Set the index array for frictional contact constraints, m_limitDependencies
708 {
709 BT_PROFILE("gather constraint data");
710
711 int dindex = 0;
712
713 const int numRigidBodyConstraints = m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size();
714 const int numMultiBodyConstraints = m_multiBodyNonContactConstraints.size() + m_multiBodyNormalContactConstraints.size() + m_multiBodyFrictionContactConstraints.size();
715
716 m_allConstraintPtrArray.resize(0);
717 m_multiBodyAllConstraintPtrArray.resize(0);
718
719 // i. Setup for rigid bodies
720
721 m_limitDependencies.resize(numRigidBodyConstraints);
722
723 for (int i = 0; i < m_tmpSolverNonContactConstraintPool.size(); ++i)
724 {
725 m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
726 m_limitDependencies[dindex++] = -1;
727 }
728
729 int firstContactConstraintOffset = dindex;
730
731 // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
732 if (interleaveContactAndFriction1)
733 {
734 for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
735 {
736 const int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
737
738 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
739 m_limitDependencies[dindex++] = -1;
740 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
741 int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
742 m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
743 if (numFrictionPerContact == 2)
744 {
745 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
746 m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
747 }
748 }
749 }
750 else
751 {
752 for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
753 {
754 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
755 m_limitDependencies[dindex++] = -1;
756 }
757 for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
758 {
759 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
760 m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
761 }
762 }
763
764 if (!m_allConstraintPtrArray.size())
765 {
766 m_A.resize(0, 0);
767 m_b.resize(0);
768 m_x.resize(0);
769 m_lo.resize(0);
770 m_hi.resize(0);
771 }
772
773 // ii. Setup for multibodies
774
775 dindex = 0;
776
777 m_multiBodyLimitDependencies.resize(numMultiBodyConstraints);
778
779 for (int i = 0; i < m_multiBodyNonContactConstraints.size(); ++i)
780 {
781 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNonContactConstraints[i]);
782 m_multiBodyLimitDependencies[dindex++] = -1;
783 }
784
785 firstContactConstraintOffset = dindex;
786
787 // The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
788 if (interleaveContactAndFriction1)
789 {
790 for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
791 {
792 const int numtiBodyNumFrictionPerContact = m_multiBodyNormalContactConstraints.size() == m_multiBodyFrictionContactConstraints.size() ? 1 : 2;
793
794 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
795 m_multiBodyLimitDependencies[dindex++] = -1;
796
797 btMultiBodySolverConstraint& frictionContactConstraint1 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact];
798 m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint1);
799
800 const int findex = (frictionContactConstraint1.m_frictionIndex * (1 + numtiBodyNumFrictionPerContact)) + firstContactConstraintOffset;
801
802 m_multiBodyLimitDependencies[dindex++] = findex;
803
804 if (numtiBodyNumFrictionPerContact == 2)
805 {
806 btMultiBodySolverConstraint& frictionContactConstraint2 = m_multiBodyFrictionContactConstraints[i * numtiBodyNumFrictionPerContact + 1];
807 m_multiBodyAllConstraintPtrArray.push_back(&frictionContactConstraint2);
808
809 m_multiBodyLimitDependencies[dindex++] = findex;
810 }
811 }
812 }
813 else
814 {
815 for (int i = 0; i < m_multiBodyNormalContactConstraints.size(); ++i)
816 {
817 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyNormalContactConstraints[i]);
818 m_multiBodyLimitDependencies[dindex++] = -1;
819 }
820 for (int i = 0; i < m_multiBodyFrictionContactConstraints.size(); ++i)
821 {
822 m_multiBodyAllConstraintPtrArray.push_back(&m_multiBodyFrictionContactConstraints[i]);
823 m_multiBodyLimitDependencies[dindex++] = m_multiBodyFrictionContactConstraints[i].m_frictionIndex + firstContactConstraintOffset;
824 }
825 }
826
827 if (!m_multiBodyAllConstraintPtrArray.size())
828 {
829 m_multiBodyA.resize(0, 0);
830 m_multiBodyB.resize(0);
831 m_multiBodyX.resize(0);
832 m_multiBodyLo.resize(0);
833 m_multiBodyHi.resize(0);
834 }
835 }
836
837 // Construct MLCP terms
838 {
839 BT_PROFILE("createMLCPFast");
840 createMLCPFast(infoGlobal);
841 }
842
843 return btScalar(0);
844}
845
846 btScalar btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
847{
848 bool result = true;
849 {
850 BT_PROFILE("solveMLCP");
851 result = solveMLCP(infoGlobal);
852 }
853
854 // Fallback to btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations if the solution isn't valid.
855 if (!result)
856 {
857 m_fallback++;
858 return btMultiBodyConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
859 }
860
861 {
862 BT_PROFILE("process MLCP results");
863
864 for (int i = 0; i < m_allConstraintPtrArray.size(); ++i)
865 {
866 const btSolverConstraint& c = *m_allConstraintPtrArray[i];
867
868 const btScalar deltaImpulse = m_x[i] - c.m_appliedImpulse;
869 c.m_appliedImpulse = m_x[i];
870
871 int sbA = c.m_solverBodyIdA;
872 int sbB = c.m_solverBodyIdB;
873
874 btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
875 btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
876
877 solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
878 solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
879
880 if (infoGlobal.m_splitImpulse)
881 {
882 const btScalar deltaPushImpulse = m_xSplit[i] - c.m_appliedPushImpulse;
883 solverBodyA.internalApplyPushImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaPushImpulse);
884 solverBodyB.internalApplyPushImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaPushImpulse);
885 c.m_appliedPushImpulse = m_xSplit[i];
886 }
887 }
888
889 for (int i = 0; i < m_multiBodyAllConstraintPtrArray.size(); ++i)
890 {
891 btMultiBodySolverConstraint& c = *m_multiBodyAllConstraintPtrArray[i];
892
893 const btScalar deltaImpulse = m_multiBodyX[i] - c.m_appliedImpulse;
894 c.m_appliedImpulse = m_multiBodyX[i];
895
896 btMultiBody* multiBodyA = c.m_multiBodyA;
897 if (multiBodyA)
898 {
899 const int ndofA = multiBodyA->getNumDofs() + 6;
900 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
901#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
902 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
903 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
904 multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
905#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
906 }
907 else
908 {
909 const int sbA = c.m_solverBodyIdA;
910 btSolverBody& solverBodyA = m_tmpSolverBodyPool[sbA];
911 solverBodyA.internalApplyImpulse(c.m_contactNormal1 * solverBodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
912 }
913
914 btMultiBody* multiBodyB = c.m_multiBodyB;
915 if (multiBodyB)
916 {
917 const int ndofB = multiBodyB->getNumDofs() + 6;
918 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
919#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
920 //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
921 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
922 multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
923#endif // DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
924 }
925 else
926 {
927 const int sbB = c.m_solverBodyIdB;
928 btSolverBody& solverBodyB = m_tmpSolverBodyPool[sbB];
929 solverBodyB.internalApplyImpulse(c.m_contactNormal2 * solverBodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
930 }
931 }
932 }
933
934 return btScalar(0);
935}
936
937btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface* solver)
938 : m_solver(solver), m_fallback(0)
939{
940 // Do nothing
941}
942
943btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver()
944{
945 // Do nothing
946}
947
948 void btMultiBodyMLCPConstraintSolver::setMLCPSolver(btMLCPSolverInterface* solver)
949{
950 m_solver = solver;
951}
952
953 int btMultiBodyMLCPConstraintSolver::getNumFallbacks() const
954{
955 return m_fallback;
956}
957
958 void btMultiBodyMLCPConstraintSolver::setNumFallbacks(int num)
959{
960 m_fallback = num;
961}
962
963 btConstraintSolverType btMultiBodyMLCPConstraintSolver::getSolverType() const
964{
965 return BT_MLCP_SOLVER;
966}
btConstraintSolverType
btConstraintSolverType
btConstraintSolver provides solver interface
Definition btConstraintSolver.h:33
BT_MLCP_SOLVER
@ BT_MLCP_SOLVER
Definition btConstraintSolver.h:35
SOLVER_USE_WARMSTARTING
@ SOLVER_USE_WARMSTARTING
Definition btContactSolverInfo.h:25
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition btDbvt.cpp:52
btMLCPSolverInterface.h
btMatrixXu
#define btMatrixXu
Definition btMatrixX.h:529
btMultiBodyConstraint.h
btMultiBodyLinkCollider.h
computeConstraintMatrixDiagElementMultiBody
static btScalar computeConstraintMatrixDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint)
Definition btMultiBodyMLCPConstraintSolver.cpp:66
computeConstraintMatrixOffDiagElementMultiBody
static btScalar computeConstraintMatrixOffDiagElementMultiBody(const btAlignedObjectArray< btSolverBody > &solverBodyPool, const btMultiBodyJacobianData &data, const btMultiBodySolverConstraint &constraint, const btMultiBodySolverConstraint &offDiagConstraint)
Definition btMultiBodyMLCPConstraintSolver.cpp:117
computeDeltaVelocityInConstraintSpace
static btScalar computeDeltaVelocityInConstraintSpace(const btVector3 &angularDeltaVelocity, const btVector3 &contactNormal, btScalar invMass, const btVector3 &angularJacobian, const btVector3 &linearJacobian)
Definition btMultiBodyMLCPConstraintSolver.cpp:36
interleaveContactAndFriction1
static bool interleaveContactAndFriction1
Definition btMultiBodyMLCPConstraintSolver.cpp:25
btMultiBodyMLCPConstraintSolver.h
btPersistentManifold.h
BT_PROFILE
#define BT_PROFILE(name)
Definition btQuickprof.h:198
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
BT_INFINITY
#define BT_INFINITY
Definition btScalar.h:405
btFuzzyZero
bool btFuzzyZero(btScalar x)
Definition btScalar.h:572
btAssert
#define btAssert(x)
Definition btScalar.h:153
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition btAlignedObjectArray.h:46
btAlignedObjectArray::resizeNoInitialize
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
Definition btAlignedObjectArray.h:194
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition btAlignedObjectArray.h:142
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition btAlignedObjectArray.h:203
btAlignedObjectArray::expand
T & expand(const T &fillValue=T())
Definition btAlignedObjectArray.h:242
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition btAlignedObjectArray.h:257
btAlignedObjectArray::reserve
void reserve(int _Count)
Definition btAlignedObjectArray.h:280
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition btCollisionObject.h:51
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition btIDebugDraw.h:27
btMLCPSolverInterface
original version written by Erwin Coumans, October 2013
Definition btMLCPSolverInterface.h:23
btMLCPSolverInterface::solveMLCP
virtual bool solveMLCP(const btMatrixXu &A, const btVectorXu &b, btVectorXu &x, const btVectorXu &lo, const btVectorXu &hi, const btAlignedObjectArray< int > &limitDependency, int numIterations, bool useSparsity=true)=0
btMultiBodyConstraintSolver::applyDeltaVee
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
Definition btMultiBodyConstraintSolver.cpp:229
btMultiBodyConstraintSolver::m_multiBodyNormalContactConstraints
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
Definition btMultiBodyConstraintSolver.h:34
btMultiBodyConstraintSolver::m_multiBodyFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
Definition btMultiBodyConstraintSolver.h:35
btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btMultiBodyConstraintSolver.cpp:203
btMultiBodyConstraintSolver::m_data
btMultiBodyJacobianData m_data
Definition btMultiBodyConstraintSolver.h:39
btMultiBodyConstraintSolver::m_multiBodyNonContactConstraints
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
Definition btMultiBodyConstraintSolver.h:32
btMultiBodyMLCPConstraintSolver::m_scratchJ3
btMatrixXu m_scratchJ3
Cache variable for constraint Jacobian matrix.
Definition btMultiBodyMLCPConstraintSolver.h:117
btMultiBodyMLCPConstraintSolver::m_multiBodyLimitDependencies
btAlignedObjectArray< int > m_multiBodyLimitDependencies
Indices of normal contact constraint associated with frictional contact constraint for multibodies.
Definition btMultiBodyMLCPConstraintSolver.h:95
btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlySetup
btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
Definition btMultiBodyMLCPConstraintSolver.cpp:691
btMultiBodyMLCPConstraintSolver::m_xSplit
btVectorXu m_xSplit
Split impulse cache vector corresponding to m_x.
Definition btMultiBodyMLCPConstraintSolver.h:57
btMultiBodyMLCPConstraintSolver::m_b
btVectorXu m_b
b vector in the MLCP formulation.
Definition btMultiBodyMLCPConstraintSolver.h:36
btMultiBodyMLCPConstraintSolver::m_multiBodyAllConstraintPtrArray
btAlignedObjectArray< btMultiBodySolverConstraint * > m_multiBodyAllConstraintPtrArray
Array of all the multibody constraints.
Definition btMultiBodyMLCPConstraintSolver.h:101
btMultiBodyMLCPConstraintSolver::m_solver
btMLCPSolverInterface * m_solver
MLCP solver.
Definition btMultiBodyMLCPConstraintSolver.h:104
btMultiBodyMLCPConstraintSolver::getNumFallbacks
int getNumFallbacks() const
Returns the number of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the ...
Definition btMultiBodyMLCPConstraintSolver.cpp:953
btMultiBodyMLCPConstraintSolver::setNumFallbacks
void setNumFallbacks(int num)
Sets the number of fallbacks. This function may be used to reset the number to zero.
Definition btMultiBodyMLCPConstraintSolver.cpp:958
btMultiBodyMLCPConstraintSolver::solveGroupCacheFriendlyIterations
btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btMultiBodyMLCPConstraintSolver.cpp:846
btMultiBodyMLCPConstraintSolver::m_lo
btVectorXu m_lo
Lower bound of constraint impulse, m_x.
Definition btMultiBodyMLCPConstraintSolver.h:42
btMultiBodyMLCPConstraintSolver::~btMultiBodyMLCPConstraintSolver
virtual ~btMultiBodyMLCPConstraintSolver()
Destructor.
Definition btMultiBodyMLCPConstraintSolver.cpp:943
btMultiBodyMLCPConstraintSolver::m_A
btMatrixXu m_A
A matrix in the MLCP formulation.
Definition btMultiBodyMLCPConstraintSolver.h:33
btMultiBodyMLCPConstraintSolver::btMultiBodyMLCPConstraintSolver
btMultiBodyMLCPConstraintSolver(btMLCPSolverInterface *solver)
Constructor.
Definition btMultiBodyMLCPConstraintSolver.cpp:937
btMultiBodyMLCPConstraintSolver::m_fallback
int m_fallback
Count of fallbacks of using btSequentialImpulseConstraintSolver, which happens when the MLCP solver f...
Definition btMultiBodyMLCPConstraintSolver.h:107
btMultiBodyMLCPConstraintSolver::m_multiBodyA
btMatrixXu m_multiBodyA
A matrix in the MLCP formulation.
Definition btMultiBodyMLCPConstraintSolver.h:65
btMultiBodyMLCPConstraintSolver::m_limitDependencies
btAlignedObjectArray< int > m_limitDependencies
Indices of normal contact constraint associated with frictional contact constraint for rigid bodies.
Definition btMultiBodyMLCPConstraintSolver.h:87
btMultiBodyMLCPConstraintSolver::m_bSplit
btVectorXu m_bSplit
Split impulse Cache vector corresponding to m_b.
Definition btMultiBodyMLCPConstraintSolver.h:54
btMultiBodyMLCPConstraintSolver::setMLCPSolver
void setMLCPSolver(btMLCPSolverInterface *solver)
Sets MLCP solver. Assumed it's not null.
Definition btMultiBodyMLCPConstraintSolver.cpp:948
btMultiBodyMLCPConstraintSolver::m_scratchOfs
btAlignedObjectArray< int > m_scratchOfs
Cache variable for offsets.
Definition btMultiBodyMLCPConstraintSolver.h:123
btMultiBodyMLCPConstraintSolver::createMLCPFastRigidBody
void createMLCPFastRigidBody(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms for constraints of two rigid bodies.
Definition btMultiBodyMLCPConstraintSolver.cpp:242
btMultiBodyMLCPConstraintSolver::m_hi
btVectorXu m_hi
Upper bound of constraint impulse, m_x.
Definition btMultiBodyMLCPConstraintSolver.h:45
btMultiBodyMLCPConstraintSolver::solveMLCP
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
Solves MLCP and returns the success.
Definition btMultiBodyMLCPConstraintSolver.cpp:657
btMultiBodyMLCPConstraintSolver::m_x
btVectorXu m_x
Constraint impulse, which is an output of MLCP solving.
Definition btMultiBodyMLCPConstraintSolver.h:39
btMultiBodyMLCPConstraintSolver::m_multiBodyLo
btVectorXu m_multiBodyLo
Lower bound of constraint impulse, m_x.
Definition btMultiBodyMLCPConstraintSolver.h:74
btMultiBodyMLCPConstraintSolver::createMLCPFast
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms, which are m_A, m_b, m_lo, and m_hi.
Definition btMultiBodyMLCPConstraintSolver.cpp:236
btMultiBodyMLCPConstraintSolver::m_allConstraintPtrArray
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
Array of all the rigid body constraints.
Definition btMultiBodyMLCPConstraintSolver.h:98
btMultiBodyMLCPConstraintSolver::m_scratchJInvM3
btMatrixXu m_scratchJInvM3
Cache variable for constraint Jacobian times inverse mass matrix.
Definition btMultiBodyMLCPConstraintSolver.h:120
btMultiBodyMLCPConstraintSolver::m_multiBodyX
btVectorXu m_multiBodyX
Constraint impulse, which is an output of MLCP solving.
Definition btMultiBodyMLCPConstraintSolver.h:71
btMultiBodyMLCPConstraintSolver::createMLCPFastMultiBody
void createMLCPFastMultiBody(const btContactSolverInfo &infoGlobal)
Constructs MLCP terms for constraints of two multi-bodies or one rigid body and one multibody.
Definition btMultiBodyMLCPConstraintSolver.cpp:556
btMultiBodyMLCPConstraintSolver::m_multiBodyHi
btVectorXu m_multiBodyHi
Upper bound of constraint impulse, m_x.
Definition btMultiBodyMLCPConstraintSolver.h:77
btMultiBodyMLCPConstraintSolver::getSolverType
virtual btConstraintSolverType getSolverType() const
Returns the constraint solver type.
Definition btMultiBodyMLCPConstraintSolver.cpp:963
btMultiBodyMLCPConstraintSolver::m_multiBodyB
btVectorXu m_multiBodyB
b vector in the MLCP formulation.
Definition btMultiBodyMLCPConstraintSolver.h:68
btMultiBody
Definition btMultiBody.h:51
btMultiBody::getNumDofs
int getNumDofs() const
Definition btMultiBody.h:167
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition btMultiBody.h:428
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition btPersistentManifold.h:65
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition btRigidBody.h:60
btRigidBody::getInvMass
btScalar getInvMass() const
Definition btRigidBody.h:263
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition btRigidBody.h:265
btSequentialImpulseConstraintSolver::m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
Definition btSequentialImpulseConstraintSolver.h:57
btSequentialImpulseConstraintSolver::m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
Definition btSequentialImpulseConstraintSolver.h:59
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btSequentialImpulseConstraintSolver.cpp:1730
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition btSequentialImpulseConstraintSolver.h:56
btSequentialImpulseConstraintSolver::m_tmpConstraintSizesPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
Definition btSequentialImpulseConstraintSolver.h:65
btSequentialImpulseConstraintSolver::m_tmpSolverNonContactConstraintPool
btConstraintArray m_tmpSolverNonContactConstraintPool
Definition btSequentialImpulseConstraintSolver.h:58
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition btTypedConstraint.h:76
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:229
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition btContactSolverInfo.h:44
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition btContactSolverInfo.h:56
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition btContactSolverInfo.h:62
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition btContactSolverInfo.h:52
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition btContactSolverInfo.h:42
btContactSolverInfo
Definition btContactSolverInfo.h:76
btJointNode1
Definition btMultiBodyMLCPConstraintSolver.cpp:28
btJointNode1::constraintRowIndex
int constraintRowIndex
Definition btMultiBodyMLCPConstraintSolver.cpp:32
btJointNode1::nextJointNodeIndex
int nextJointNodeIndex
Definition btMultiBodyMLCPConstraintSolver.cpp:31
btJointNode1::jointIndex
int jointIndex
Definition btMultiBodyMLCPConstraintSolver.cpp:29
btJointNode1::otherBodyIndex
int otherBodyIndex
Definition btMultiBodyMLCPConstraintSolver.cpp:30
btMultiBodyJacobianData
Definition btMultiBodyConstraint.h:44
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition btMultiBodyConstraint.h:46
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition btMultiBodyConstraint.h:45
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition btMultiBodySolverConstraint.h:30
btMultiBodySolverConstraint::m_deltaVelAindex
int m_deltaVelAindex
Definition btMultiBodySolverConstraint.h:37
btMultiBodySolverConstraint::m_deltaVelBindex
int m_deltaVelBindex
Definition btMultiBodySolverConstraint.h:39
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition btMultiBodySolverConstraint.h:42
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition btMultiBodySolverConstraint.h:69
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition btMultiBodySolverConstraint.h:55
btMultiBodySolverConstraint::m_frictionIndex
int m_frictionIndex
Definition btMultiBodySolverConstraint.h:67
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition btMultiBodySolverConstraint.h:54
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition btMultiBodySolverConstraint.h:45
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition btMultiBodySolverConstraint.h:43
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition btMultiBodySolverConstraint.h:74
btMultiBodySolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition btMultiBodySolverConstraint.h:58
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition btMultiBodySolverConstraint.h:47
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition btMultiBodySolverConstraint.h:44
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition btMultiBodySolverConstraint.h:59
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition btMultiBodySolverConstraint.h:40
btMultiBodySolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition btMultiBodySolverConstraint.h:51
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition btMultiBodySolverConstraint.h:70
btMultiBodySolverConstraint::m_jacAindex
int m_jacAindex
Definition btMultiBodySolverConstraint.h:38
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition btMultiBodySolverConstraint.h:73
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition btMultiBodySolverConstraint.h:48
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition btSolverBody.h:105
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition btSolverBody.h:120
btSolverBody::internalApplyPushImpulse
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
Definition btSolverBody.h:165
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition btSolverBody.h:243
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition btSolverBody.h:212
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition btSolverConstraint.h:31
btSolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition btSolverConstraint.h:62
btSolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition btSolverConstraint.h:38
btSolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition btSolverConstraint.h:41
btSolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition btSolverConstraint.h:44
btSolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition btSolverConstraint.h:40
btSolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition btSolverConstraint.h:43
btSolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition btSolverConstraint.h:35
btSolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition btSolverConstraint.h:63