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

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"
18#include "LinearMath/btMatrixX.h"
19#include "LinearMath/btQuickprof.h"
20#include "btSolveProjectedGaussSeidel.h"
21
22btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver)
23 : m_solver(solver),
24 m_fallback(0)
25{
26}
27
28btMLCPSolver::~btMLCPSolver()
29{
30}
31
32 bool gUseMatrixMultiply = false;
33 bool interleaveContactAndFriction = false;
34
35 btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
36{
37 btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodiesUnUsed, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
38
39 {
40 BT_PROFILE("gather constraint data");
41
42 int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
43
44 // int numBodies = m_tmpSolverBodyPool.size();
45 m_allConstraintPtrArray.resize(0);
46 m_limitDependencies.resize(m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.size());
47 btAssert(m_limitDependencies.size() == m_tmpSolverNonContactConstraintPool.size() + m_tmpSolverContactConstraintPool.size() + m_tmpSolverContactFrictionConstraintPool.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 {
53 m_allConstraintPtrArray.push_back(&m_tmpSolverNonContactConstraintPool[i]);
54 m_limitDependencies[dindex++] = -1;
55 }
56
58
59 int firstContactConstraintOffset = dindex;
60
61 if (interleaveContactAndFriction)
62 {
63 for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
64 {
65 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
66 m_limitDependencies[dindex++] = -1;
67 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact]);
68 int findex = (m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact].m_frictionIndex * (1 + numFrictionPerContact));
69 m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
70 if (numFrictionPerContact == 2)
71 {
72 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i * numFrictionPerContact + 1]);
73 m_limitDependencies[dindex++] = findex + firstContactConstraintOffset;
74 }
75 }
76 }
77 else
78 {
79 for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); i++)
80 {
81 m_allConstraintPtrArray.push_back(&m_tmpSolverContactConstraintPool[i]);
82 m_limitDependencies[dindex++] = -1;
83 }
84 for (int i = 0; i < m_tmpSolverContactFrictionConstraintPool.size(); i++)
85 {
86 m_allConstraintPtrArray.push_back(&m_tmpSolverContactFrictionConstraintPool[i]);
87 m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex + firstContactConstraintOffset;
88 }
89 }
90
91 if (!m_allConstraintPtrArray.size())
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");
105 createMLCP(infoGlobal);
106 }
107 else
108 {
109 BT_PROFILE("createMLCPFast");
110 createMLCPFast(infoGlobal);
111 }
112
113 return 0.f;
114}
115
116 bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
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
124 if (infoGlobal.m_splitImpulse)
125 {
126 btMatrixXu Acopy = m_A;
127 btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
128 // printf("solve first LCP\n");
129 result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
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 {
135 result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo, m_hi, m_limitDependencies, infoGlobal.m_numIterations);
136 }
137 return result;
138}
139
140 struct btJointNode
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
145 int constraintRowIndex;
146};
147
148 void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
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;
227 btAlignedObjectArray<int>& ofs = m_scratchOfs;
228 {
229 BT_PROFILE("ofs resize");
230 ofs.resize(0);
231 ofs.resizeNoInitialize(m_allConstraintPtrArray.size());
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
441 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
442 {
443 for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
444 {
445 const btSolverConstraint& c = *m_allConstraintPtrArray[i];
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
458 void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
459{
460 int numBodies = this->m_tmpSolverBodyPool.size();
461 int numConstraintRows = m_allConstraintPtrArray.size();
462
463 m_b.resize(numConstraintRows);
464 if (infoGlobal.m_splitImpulse)
465 m_bSplit.resize(numConstraintRows);
466
467 m_bSplit.setZero();
468 m_b.setZero();
469
470 for (int i = 0; i < numConstraintRows; i++)
471 {
472 if (m_allConstraintPtrArray[i]->m_jacDiagABInv)
473 {
474 m_b[i] = m_allConstraintPtrArray[i]->m_rhs / m_allConstraintPtrArray[i]->m_jacDiagABInv;
475 if (infoGlobal.m_splitImpulse)
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 //Minv.printMatrix("Minv=");
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 //J.printMatrix("J");
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);
557 if (infoGlobal.m_splitImpulse)
558 m_xSplit.resize(numConstraintRows);
559 // m_x.setZero();
560
561 for (int i = 0; i < m_allConstraintPtrArray.size(); i++)
562 {
563 const btSolverConstraint& c = *m_allConstraintPtrArray[i];
564 m_x[i] = c.m_appliedImpulse;
565 if (infoGlobal.m_splitImpulse)
566 m_xSplit[i] = c.m_appliedPushImpulse;
567 }
568}
569
570 btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
571{
572 bool result = true;
573 {
574 BT_PROFILE("solveMLCP");
575 // printf("m_A(%d,%d)\n", m_A.rows(),m_A.cols());
576 result = solveMLCP(infoGlobal);
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 {
586 btSolverConstraint& c = *m_allConstraintPtrArray[i];
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
602 if (infoGlobal.m_splitImpulse)
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++;
616 btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
617 }
618
619 return 0.f;
620}
SOLVER_USE_WARMSTARTING
@ SOLVER_USE_WARMSTARTING
Definition btContactSolverInfo.h:25
interleaveContactAndFriction
bool interleaveContactAndFriction
Definition btMLCPSolver.cpp:33
gUseMatrixMultiply
bool gUseMatrixMultiply
Definition btMLCPSolver.cpp:32
btMLCPSolver.h
btMatrixX.h
btMatrixXu
#define btMatrixXu
Definition btMatrixX.h:529
setElem
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition btMatrixX.h:514
btQuickprof.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
btSolveProjectedGaussSeidel.h
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::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
btMLCPSolver::m_scratchTmp
btMatrixXu m_scratchTmp
Definition btMLCPSolver.h:53
btMLCPSolver::m_b
btVectorXu m_b
Definition btMLCPSolver.h:28
btMLCPSolver::m_scratchJTranspose
btMatrixXu m_scratchJTranspose
Definition btMLCPSolver.h:52
btMLCPSolver::~btMLCPSolver
virtual ~btMLCPSolver()
Definition btMLCPSolver.cpp:28
btMLCPSolver::m_scratchMInv
btMatrixXu m_scratchMInv
Definition btMLCPSolver.h:50
btMLCPSolver::m_limitDependencies
btAlignedObjectArray< int > m_limitDependencies
Definition btMLCPSolver.h:39
btMLCPSolver::m_scratchJ
btMatrixXu m_scratchJ
Definition btMLCPSolver.h:51
btMLCPSolver::m_x
btVectorXu m_x
Definition btMLCPSolver.h:29
btMLCPSolver::createMLCP
virtual void createMLCP(const btContactSolverInfo &infoGlobal)
Definition btMLCPSolver.cpp:458
btMLCPSolver::m_scratchJ3
btMatrixXu m_scratchJ3
The following scratch variables are not stateful – contents are cleared prior to each use.
Definition btMLCPSolver.h:47
btMLCPSolver::m_xSplit
btVectorXu m_xSplit
Definition btMLCPSolver.h:35
btMLCPSolver::solveMLCP
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
Definition btMLCPSolver.cpp:116
btMLCPSolver::m_bSplit
btVectorXu m_bSplit
when using 'split impulse' we solve two separate (M)LCPs
Definition btMLCPSolver.h:34
btMLCPSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btMLCPSolver.cpp:35
btMLCPSolver::m_hi
btVectorXu m_hi
Definition btMLCPSolver.h:31
btMLCPSolver::m_A
btMatrixXu m_A
Definition btMLCPSolver.h:27
btMLCPSolver::m_scratchOfs
btAlignedObjectArray< int > m_scratchOfs
Definition btMLCPSolver.h:49
btMLCPSolver::m_scratchJInvM3
btMatrixXu m_scratchJInvM3
Definition btMLCPSolver.h:48
btMLCPSolver::createMLCPFast
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
Definition btMLCPSolver.cpp:148
btMLCPSolver::solveGroupCacheFriendlyIterations
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btMLCPSolver.cpp:570
btMLCPSolver::m_fallback
int m_fallback
Definition btMLCPSolver.h:42
btMLCPSolver::btMLCPSolver
btMLCPSolver(btMLCPSolverInterface *solver)
original version written by Erwin Coumans, October 2013
Definition btMLCPSolver.cpp:22
btMLCPSolver::m_allConstraintPtrArray
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
Definition btMLCPSolver.h:40
btMLCPSolver::m_solver
btMLCPSolverInterface * m_solver
Definition btMLCPSolver.h:41
btMLCPSolver::m_lo
btVectorXu m_lo
Definition btMLCPSolver.h:30
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::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition btSequentialImpulseConstraintSolver.cpp:1403
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
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
btJointNode
Definition btMLCPSolver.cpp:141
btJointNode::nextJointNodeIndex
int nextJointNodeIndex
Definition btMLCPSolver.cpp:144
btJointNode::constraintRowIndex
int constraintRowIndex
Definition btMLCPSolver.cpp:145
btJointNode::jointIndex
int jointIndex
Definition btMLCPSolver.cpp:142
btJointNode::otherBodyIndex
int otherBodyIndex
Definition btMLCPSolver.cpp:143
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition btSolverBody.h:105
btSolverBody::m_invMass
btVector3 m_invMass
Definition btSolverBody.h:112
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