1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 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 16 #include "btNNCGConstraintSolver.h" 17 18 19 20 21 22 23 btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) 24 { 25 btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer); 26 27 m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); 28 m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); 29 m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size()); 30 m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size()); 31 32 m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size()); 33 m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size()); 34 m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size()); 35 m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size()); 36 37 return val; 38 } 39 40 btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) 41 { 42 43 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); 44 int numConstraintPool = m_tmpSolverContactConstraintPool.size(); 45 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); 46 47 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) 48 { 49 if (1) // uncomment this for a bit less random ((iteration & 7) == 0) 50 { 51 52 for (int j=0; j<numNonContactPool; ++j) { 53 int tmp = m_orderNonContactConstraintPool[j]; 54 int swapi = btRandInt2(j+1); 55 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; 56 m_orderNonContactConstraintPool[swapi] = tmp; 57 } 58 59 //contact/friction constraints are not solved more than 60 if (iteration< infoGlobal.m_numIterations) 61 { 62 for (int j=0; j<numConstraintPool; ++j) { 63 int tmp = m_orderTmpConstraintPool[j]; 64 int swapi = btRandInt2(j+1); 65 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; 66 m_orderTmpConstraintPool[swapi] = tmp; 67 } 68 69 for (int j=0; j<numFrictionPool; ++j) { 70 int tmp = m_orderFrictionConstraintPool[j]; 71 int swapi = btRandInt2(j+1); 72 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; 73 m_orderFrictionConstraintPool[swapi] = tmp; 74 } 75 } 76 } 77 } 78 79 80 btScalar deltaflengthsqr = 0; 81 82 if (infoGlobal.m_solverMode & SOLVER_SIMD) 83 { 84 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 85 { 86 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; 87 if (iteration < constraint.m_overrideNumSolverIterations) 88 { 89 btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); 90 m_deltafNC[j] = deltaf; 91 deltaflengthsqr += deltaf * deltaf; 92 } 93 } 94 } else 95 { 96 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 97 { 98 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; 99 if (iteration < constraint.m_overrideNumSolverIterations) 100 { 101 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); 102 m_deltafNC[j] = deltaf; 103 deltaflengthsqr += deltaf * deltaf; 104 } 105 } 106 } 107 108 109 if (m_onlyForNoneContact) 110 { 111 if (iteration==0) 112 { 113 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j]; 114 } else { 115 // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation 116 btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; 117 if (beta>1) 118 { 119 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0; 120 } else 121 { 122 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 123 { 124 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; 125 if (iteration < constraint.m_overrideNumSolverIterations) 126 { 127 btScalar additionaldeltaimpulse = beta * m_pNC[j]; 128 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; 129 m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j]; 130 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; 131 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; 132 const btSolverConstraint& c = constraint; 133 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); 134 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); 135 } 136 } 137 } 138 } 139 m_deltafLengthSqrPrev = deltaflengthsqr; 140 } 141 142 143 144 if (infoGlobal.m_solverMode & SOLVER_SIMD) 145 { 146 147 if (iteration< infoGlobal.m_numIterations) 148 { 149 for (int j=0;j<numConstraints;j++) 150 { 151 if (constraints[j]->isEnabled()) 152 { 153 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); 154 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 155 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; 156 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; 157 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); 158 } 159 } 160 161 ///solve all contact constraints using SIMD, if available 162 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) 163 { 164 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 165 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; 166 167 for (int c=0;c<numPoolConstraints;c++) 168 { 169 btScalar totalImpulse =0; 170 171 { 172 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; 173 btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 174 m_deltafC[c] = deltaf; 175 deltaflengthsqr += deltaf*deltaf; 176 totalImpulse = solveManifold.m_appliedImpulse; 177 } 178 bool applyFriction = true; 179 if (applyFriction) 180 { 181 { 182 183 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]]; 184 185 if (totalImpulse>btScalar(0)) 186 { 187 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 188 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 189 btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 190 m_deltafCF[c*multiplier] = deltaf; 191 deltaflengthsqr += deltaf*deltaf; 192 } else { 193 m_deltafCF[c*multiplier] = 0; 194 } 195 } 196 197 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) 198 { 199 200 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; 201 202 if (totalImpulse>btScalar(0)) 203 { 204 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 205 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 206 btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 207 m_deltafCF[c*multiplier+1] = deltaf; 208 deltaflengthsqr += deltaf*deltaf; 209 } else { 210 m_deltafCF[c*multiplier+1] = 0; 211 } 212 } 213 } 214 } 215 216 } 217 else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS 218 { 219 //solve the friction constraints after all contact constraints, don't interleave them 220 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 221 int j; 222 223 for (j=0;j<numPoolConstraints;j++) 224 { 225 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 226 //resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 227 btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 228 m_deltafC[j] = deltaf; 229 deltaflengthsqr += deltaf*deltaf; 230 } 231 232 233 234 ///solve all friction constraints, using SIMD, if available 235 236 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 237 for (j=0;j<numFrictionPoolConstraints;j++) 238 { 239 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 240 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 241 242 if (totalImpulse>btScalar(0)) 243 { 244 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 245 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 246 247 //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 248 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 249 m_deltafCF[j] = deltaf; 250 deltaflengthsqr += deltaf*deltaf; 251 } else { 252 m_deltafCF[j] = 0; 253 } 254 } 255 256 257 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); 258 for (j=0;j<numRollingFrictionPoolConstraints;j++) 259 { 260 261 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; 262 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; 263 if (totalImpulse>btScalar(0)) 264 { 265 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; 266 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) 267 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; 268 269 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; 270 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; 271 272 btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); 273 m_deltafCRF[j] = deltaf; 274 deltaflengthsqr += deltaf*deltaf; 275 } else { 276 m_deltafCRF[j] = 0; 277 } 278 } 279 280 281 } 282 } 283 284 285 286 } else 287 { 288 289 if (iteration< infoGlobal.m_numIterations) 290 { 291 for (int j=0;j<numConstraints;j++) 292 { 293 if (constraints[j]->isEnabled()) 294 { 295 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep); 296 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); 297 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; 298 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; 299 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); 300 } 301 } 302 ///solve all contact constraints 303 int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); 304 for (int j=0;j<numPoolConstraints;j++) 305 { 306 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 307 btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 308 m_deltafC[j] = deltaf; 309 deltaflengthsqr += deltaf*deltaf; 310 } 311 ///solve all friction constraints 312 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); 313 for (int j=0;j<numFrictionPoolConstraints;j++) 314 { 315 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 316 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; 317 318 if (totalImpulse>btScalar(0)) 319 { 320 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 321 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; 322 323 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); 324 m_deltafCF[j] = deltaf; 325 deltaflengthsqr += deltaf*deltaf; 326 } else { 327 m_deltafCF[j] = 0; 328 } 329 } 330 331 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); 332 for (int j=0;j<numRollingFrictionPoolConstraints;j++) 333 { 334 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; 335 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; 336 if (totalImpulse>btScalar(0)) 337 { 338 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; 339 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) 340 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; 341 342 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; 343 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; 344 345 btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); 346 m_deltafCRF[j] = deltaf; 347 deltaflengthsqr += deltaf*deltaf; 348 } else { 349 m_deltafCRF[j] = 0; 350 } 351 } 352 } 353 } 354 355 356 357 358 if (!m_onlyForNoneContact) 359 { 360 if (iteration==0) 361 { 362 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j]; 363 for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j]; 364 for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j]; 365 if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) 366 { 367 for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j]; 368 } 369 } else 370 { 371 // deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation 372 btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2; 373 if (beta>1) { 374 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0; 375 for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0; 376 for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0; 377 if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) { 378 for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0; 379 } 380 } else { 381 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) 382 { 383 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; 384 if (iteration < constraint.m_overrideNumSolverIterations) { 385 btScalar additionaldeltaimpulse = beta * m_pNC[j]; 386 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; 387 m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j]; 388 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; 389 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; 390 const btSolverConstraint& c = constraint; 391 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); 392 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); 393 } 394 } 395 for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) 396 { 397 btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; 398 if (iteration< infoGlobal.m_numIterations) { 399 btScalar additionaldeltaimpulse = beta * m_pC[j]; 400 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; 401 m_pC[j] = beta * m_pC[j] + m_deltafC[j]; 402 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; 403 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; 404 const btSolverConstraint& c = constraint; 405 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); 406 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); 407 } 408 } 409 for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) 410 { 411 btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; 412 if (iteration< infoGlobal.m_numIterations) { 413 btScalar additionaldeltaimpulse = beta * m_pCF[j]; 414 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; 415 m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j]; 416 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; 417 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; 418 const btSolverConstraint& c = constraint; 419 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); 420 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); 421 } 422 } 423 if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) { 424 for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) 425 { 426 btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; 427 if (iteration< infoGlobal.m_numIterations) { 428 btScalar additionaldeltaimpulse = beta * m_pCRF[j]; 429 constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse; 430 m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j]; 431 btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; 432 btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; 433 const btSolverConstraint& c = constraint; 434 body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse); 435 body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse); 436 } 437 } 438 } 439 } 440 } 441 m_deltafLengthSqrPrev = deltaflengthsqr; 442 } 443 444 return deltaflengthsqr; 445 } 446 447 btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) 448 { 449 m_pNC.resizeNoInitialize(0); 450 m_pC.resizeNoInitialize(0); 451 m_pCF.resizeNoInitialize(0); 452 m_pCRF.resizeNoInitialize(0); 453 454 m_deltafNC.resizeNoInitialize(0); 455 m_deltafC.resizeNoInitialize(0); 456 m_deltafCF.resizeNoInitialize(0); 457 m_deltafCRF.resizeNoInitialize(0); 458 459 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); 460 } 461 462 463 464
