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 */ 15 ///original version written by Erwin Coumans, October 2013 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 60 ///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead 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 443 ///fill the upper triangle of the matrix, to make it symmetric 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 641
