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