From 57fb21879b4c218779f44f0332ca6a997c52a060 Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Tue, 20 Jan 2009 01:21:48 +0000 Subject: [PATCH] Expose various advanced friction options to the developer, and use a higher-quality friction model by default, to match ODE quickstep constraint solver. Thanks to Martijn Reuvers for bringing this up, and reproduction case. See issue here: http://code.google.com/p/bullet/issues/detail?id=177 --- .../ConstraintSolver/btContactSolverInfo.h | 9 +- .../btSequentialImpulseConstraintSolver.cpp | 275 +++++++++--------- 2 files changed, 149 insertions(+), 135 deletions(-) diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 2e255fd8d..2138434e0 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -22,9 +22,12 @@ enum btSolverMode SOLVER_FRICTION_SEPARATE = 2, SOLVER_USE_WARMSTARTING = 4, SOLVER_USE_FRICTION_WARMSTARTING = 8, - SOLVER_CACHE_FRIENDLY = 16, - SOLVER_SIMD = 32,//enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version - SOLVER_CUDA = 64 //will be open sourced during Game Developers Conference 2009. Much faster. + SOLVER_USE_1_FRICTION_DIRECTION = 16, + SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, + SOLVER_ENABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, + SOLVER_CACHE_FRIENDLY = 128, + SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version + SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster. }; struct btContactSolverInfoData diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 3a75fe744..468a1eed9 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -49,8 +49,8 @@ btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() #define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) { - __m128 result = _mm_mul_ps( vec0, vec1); - return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) ); + __m128 result = _mm_mul_ps( vec0, vec1); + return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) ); } #endif//USE_SIMD @@ -94,7 +94,7 @@ SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstra btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); - + const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -157,7 +157,7 @@ SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstra btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity); const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity); - + deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; @@ -180,8 +180,8 @@ SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstra unsigned long btSequentialImpulseConstraintSolver::btRand2() { - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; - return m_btSeed2; + m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + return m_btSeed2; } @@ -189,29 +189,29 @@ unsigned long btSequentialImpulseConstraintSolver::btRand2() //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) int btSequentialImpulseConstraintSolver::btRandInt2 (int n) { - // seems good; xor-fold and modulus - const unsigned long un = static_cast(n); - unsigned long r = btRand2(); + // seems good; xor-fold and modulus + const unsigned long un = static_cast(n); + unsigned long r = btRand2(); - // note: probably more aggressive than it needs to be -- might be - // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) { - r ^= (r >> 16); - if (un <= 0x00000100UL) { - r ^= (r >> 8); - if (un <= 0x00000010UL) { - r ^= (r >> 4); - if (un <= 0x00000004UL) { - r ^= (r >> 2); - if (un <= 0x00000002UL) { - r ^= (r >> 1); - } - } - } - } - } + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000UL) { + r ^= (r >> 16); + if (un <= 0x00000100UL) { + r ^= (r >> 8); + if (un <= 0x00000010UL) { + r ^= (r >> 4); + if (un <= 0x00000004UL) { + r ^= (r >> 2); + if (un <= 0x00000002UL) { + r ^= (r >> 1); + } + } + } + } + } - return (int) (r % un); + return (int) (r % un); } @@ -252,7 +252,7 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation) { - + btRigidBody* body0=btRigidBody::upcast(colObj0); btRigidBody* body1=btRigidBody::upcast(colObj1); @@ -269,7 +269,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c solverConstraint.m_originalContactPoint = 0; solverConstraint.m_appliedImpulse = 0.f; -// solverConstraint.m_appliedPushImpulse = 0.f; + // solverConstraint.m_appliedPushImpulse = 0.f; solverConstraint.m_penetration = 0.f; { btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); @@ -307,11 +307,11 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c #ifdef _USE_JACOBIAN solverConstraint.m_jac = btJacobianEntry ( - rel_pos1,rel_pos2,solverConstraint.m_contactNormal, - body0->getInvInertiaDiagLocal(), - body0->getInvMass(), - body1->getInvInertiaDiagLocal(), - body1->getInvMass()); + rel_pos1,rel_pos2,solverConstraint.m_contactNormal, + body0->getInvInertiaDiagLocal(), + body0->getInvMass(), + body1->getInvInertiaDiagLocal(), + body1->getInvMass()); #endif //_USE_JACOBIAN @@ -374,10 +374,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol if (!(numConstraints + numManifolds)) { -// printf("empty\n"); + // printf("empty\n"); return 0.f; } - + if (1) { int j; @@ -396,9 +396,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol //if (1) { { - + int totalNumRows = 0; - + //calculate the total number of contraint rows for (int i=0;igetRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); int solverBodyIdA = getOrInitSolverBody(rbA); int solverBodyIdB = getOrInitSolverBody(rbB); - + btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; @@ -453,7 +453,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f); bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); - + btTypedConstraint::btConstraintInfo2 info2; @@ -489,7 +489,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; - + btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJlB.dot(solverConstraint.m_contactNormal); @@ -515,13 +515,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_appliedImpulse = 0.f; - + } } } } } - + { int i; btPersistentManifold* manifold = 0; @@ -533,7 +533,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol manifold = manifoldPtr[i]; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - + int solverBodyIdA=-1; int solverBodyIdB=-1; @@ -549,21 +549,21 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol for (int j=0;jgetNumContacts();j++) { - + btManifoldPoint& cp = manifold->getContactPoint(j); - + ///this is a bad test and results in jitter -> always solve for those zero-distanc contacts! ///-> if (cp.getDistance() <= btScalar(0.)) //if (cp.getDistance() <= manifold->getContactBreakingThreshold()) { - + const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + - relaxation = 1.f; btScalar rel_vel; btVector3 vel; @@ -604,7 +604,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); } #endif //COMPUTE_IMPULSE_DENOM - + btScalar denom = relaxation/(denom0+denom1); solverConstraint.m_jacDiagABInv = denom; } @@ -616,11 +616,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - + vel = vel1 - vel2; - + rel_vel = cp.m_normalWorldOnB.dot(vel); - + solverConstraint.m_penetration = cp.getDistance()+infoGlobal.m_linearSlop; //solverConstraint.m_penetration = cp.getDistance(); @@ -628,12 +628,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol - + solverConstraint.m_friction = cp.m_combinedFriction; - + if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) { solverConstraint.m_restitution = 0.f; @@ -660,8 +660,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol solverConstraint.m_appliedImpulse = 0.f; } -// solverConstraint.m_appliedPushImpulse = 0.f; - + // solverConstraint.m_appliedPushImpulse = 0.f; + { btScalar rel_vel; btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) @@ -698,73 +698,84 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol if (1) { - solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - if (!cp.m_lateralFrictionInitialized) - { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if (lat_rel_vel > SIMD_EPSILON)//0.0f) + solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) { - cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if ((infoGlobal.m_solverMode & SOLVER_ENABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if(!(infoGlobal.m_solverMode & SOLVER_USE_1_FRICTION_DIRECTION)) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize();//?? + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } cp.m_lateralFrictionInitialized = true; + } else + { + //re-calculate friction direction every frame, todo: check if this is really needed + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if (!(infoGlobal.m_solverMode & SOLVER_USE_1_FRICTION_DIRECTION)) + { + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } + cp.m_lateralFrictionInitialized = true; + } + + } else + { + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if (!(infoGlobal.m_solverMode & SOLVER_USE_1_FRICTION_DIRECTION)) + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } + + if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) + { + { + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); + } else + { + frictionConstraint1.m_appliedImpulse = 0.f; + } + } + + if (!(infoGlobal.m_solverMode & SOLVER_USE_1_FRICTION_DIRECTION)) + { + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + if (rb0) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + if (rb1) + m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); + } else + { + frictionConstraint2.m_appliedImpulse = 0.f; + } } } else - { - //re-calculate friction direction every frame, todo: check if this is really needed - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - { - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionInitialized = true; - } - } - - } else - { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - } - - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - { { btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); - if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse); - } else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); - if (rb1) - m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse); - } else + frictionConstraint1.m_appliedImpulse = 0.f; + if (!(infoGlobal.m_solverMode & SOLVER_USE_1_FRICTION_DIRECTION)) { + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; frictionConstraint2.m_appliedImpulse = 0.f; } } } - } } @@ -773,11 +784,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } } - + btContactSolverInfo info = infoGlobal; - - + + int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); @@ -857,15 +868,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - + } ///solve all friction constraints, using SIMD, if available - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;jbtScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); @@ -902,12 +913,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } ///solve all friction constraints - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;jbtScalar(0)) { solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); @@ -917,7 +928,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( } } } - + } @@ -946,7 +957,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod for (j=0;j