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

Bullet Collision Detection & Physics Library

btMLCPSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
16
17 #include "btMLCPSolver.h"
18 #include "LinearMath/btMatrixX.h"
19 #include "LinearMath/btQuickprof.h"
20 #include "btSolveProjectedGaussSeidel.h"
21
22 btMLCPSolver::btMLCPSolver(btMLCPSolverInterface* solver)
23 : m_solver(solver),
24 m_fallback(0)
25 {
26 }
27
28 btMLCPSolver::~btMLCPSolver()
29 {
30 }
31
32 bool gUseMatrixMultiply = false;
33 bool interleaveContactAndFriction = false;
34
35 btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodiesUnUsed, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
36 {
37 btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodiesUnUsed, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
38
39 {
40 BT_PROFILE("gather constraint data");
41
42 int numFrictionPerContact = m_tmpSolverContactConstraintPool.size() == m_tmpSolverContactFrictionConstraintPool.size() ? 1 : 2;
43
44 // 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
536 {
537 {
538 BT_PROFILE("J*Minv");
539 tmp = J * Minv;
540 }
541 {
542 BT_PROFILE("J*tmp");
543 m_A = tmp * J_transpose;
544 }
545 }
546
547 if (1)
548 {
549 // add cfm to the diagonal of m_A
550 for (int i = 0; i < m_A.rows(); ++i)
551 {
552 m_A.setElem(i, i, m_A(i, i) + infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
553 }
554 }
555
556 m_x.resize(numConstraintRows);
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 }
btMLCPSolver::m_scratchJ3
btMatrixXu m_scratchJ3
The following scratch variables are not stateful – contents are cleared prior to each use.
Definition: btMLCPSolver.h:47
btSequentialImpulseConstraintSolver::m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactFrictionConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:59
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:76
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:50
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
BT_INFINITY
#define BT_INFINITY
Definition: btScalar.h:405
btMLCPSolver::solveMLCP
virtual bool solveMLCP(const btContactSolverInfo &infoGlobal)
Definition: btMLCPSolver.cpp:116
btSolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btSolverConstraint.h:43
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btMLCPSolver::m_scratchJ
btMatrixXu m_scratchJ
Definition: btMLCPSolver.h:51
btContactSolverInfo
Definition: btContactSolverInfo.h:73
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btSolverConstraint.h:31
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:1726
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
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
gUseMatrixMultiply
bool gUseMatrixMultiply
Definition: btMLCPSolver.cpp:32
btSolveProjectedGaussSeidel.h
btSolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btSolverConstraint.h:38
SOLVER_USE_WARMSTARTING
@ SOLVER_USE_WARMSTARTING
Definition: btContactSolverInfo.h:25
btMLCPSolver::m_solver
btMLCPSolverInterface * m_solver
Definition: btMLCPSolver.h:41
btMatrixX.h
btMLCPSolver::m_scratchOfs
btAlignedObjectArray< int > m_scratchOfs
Definition: btMLCPSolver.h:49
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
btJointNode::constraintRowIndex
int constraintRowIndex
Definition: btMLCPSolver.cpp:145
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:243
btMLCPSolver::createMLCP
virtual void createMLCP(const btContactSolverInfo &infoGlobal)
Definition: btMLCPSolver.cpp:458
btSolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btSolverConstraint.h:41
btJointNode::nextJointNodeIndex
int nextJointNodeIndex
Definition: btMLCPSolver.cpp:144
btSolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btSolverConstraint.h:35
btSolverBody::m_invMass
btVector3 m_invMass
Definition: btSolverBody.h:112
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btMLCPSolver::btMLCPSolver
btMLCPSolver(btMLCPSolverInterface *solver)
original version written by Erwin Coumans, October 2013
Definition: btMLCPSolver.cpp:22
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btMLCPSolver::m_x
btVectorXu m_x
Definition: btMLCPSolver.h:29
btMLCPSolverInterface
original version written by Erwin Coumans, October 2013
Definition: btMLCPSolverInterface.h:23
btMLCPSolver.h
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btSolverBody::internalApplyPushImpulse
void internalApplyPushImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, btScalar impulseMagnitude)
Definition: btSolverBody.h:165
btJointNode::otherBodyIndex
int otherBodyIndex
Definition: btMLCPSolver.cpp:143
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btMLCPSolver::m_bSplit
btVectorXu m_bSplit
when using 'split impulse' we solve two separate (M)LCPs
Definition: btMLCPSolver.h:34
btMatrixXu
#define btMatrixXu
Definition: btMatrixX.h:530
btMLCPSolver::m_b
btVectorXu m_b
Definition: btMLCPSolver.h:28
btMLCPSolver::m_limitDependencies
btAlignedObjectArray< int > m_limitDependencies
Definition: btMLCPSolver.h:39
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:54
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition: btContactSolverInfo.h:50
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:60
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:65
btJointNode
Definition: btMLCPSolver.cpp:141
btMLCPSolver::m_allConstraintPtrArray
btAlignedObjectArray< btSolverConstraint * > m_allConstraintPtrArray
Definition: btMLCPSolver.h:40
btFuzzyZero
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:572
btAlignedObjectArray< int >
btSolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btSolverConstraint.h:62
btSequentialImpulseConstraintSolver::m_tmpSolverContactConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:57
btMLCPSolver::m_lo
btVectorXu m_lo
Definition: btMLCPSolver.h:30
btAlignedObjectArray::expand
T & expand(const T &fillValue=T())
Definition: btAlignedObjectArray.h:242
btSequentialImpulseConstraintSolver::m_tmpConstraintSizesPool
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
Definition: btSequentialImpulseConstraintSolver.h:65
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:56
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:1399
btAlignedObjectArray::reserve
void reserve(int _Count)
Definition: btAlignedObjectArray.h:280
btSolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btSolverConstraint.h:63
btQuickprof.h
btMLCPSolver::m_A
btMatrixXu m_A
Definition: btMLCPSolver.h:27
btMLCPSolver::~btMLCPSolver
virtual ~btMLCPSolver()
Definition: btMLCPSolver.cpp:28
btSequentialImpulseConstraintSolver::m_tmpSolverNonContactConstraintPool
btConstraintArray m_tmpSolverNonContactConstraintPool
Definition: btSequentialImpulseConstraintSolver.h:58
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btMLCPSolver::createMLCPFast
virtual void createMLCPFast(const btContactSolverInfo &infoGlobal)
Definition: btMLCPSolver.cpp:148
btMLCPSolver::m_scratchJInvM3
btMatrixXu m_scratchJInvM3
Definition: btMLCPSolver.h:48
btJointNode::jointIndex
int jointIndex
Definition: btMLCPSolver.cpp:142
btMLCPSolver::m_scratchTmp
btMatrixXu m_scratchTmp
Definition: btMLCPSolver.h:53
interleaveContactAndFriction
bool interleaveContactAndFriction
Definition: btMLCPSolver.cpp:33
btMLCPSolver::m_xSplit
btVectorXu m_xSplit
Definition: btMLCPSolver.h:35
btAlignedObjectArray::resizeNoInitialize
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
Definition: btAlignedObjectArray.h:194
setElem
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:515
btMLCPSolver::m_fallback
int m_fallback
Definition: btMLCPSolver.h:42
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btSolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btSolverConstraint.h:44
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
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_scratchJTranspose
btMatrixXu m_scratchJTranspose
Definition: btMLCPSolver.h:52
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btMLCPSolver::m_scratchMInv
btMatrixXu m_scratchMInv
Definition: btMLCPSolver.h:50
btSolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btSolverConstraint.h:40
btMLCPSolver::m_hi
btVectorXu m_hi
Definition: btMLCPSolver.h:31