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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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