Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Expose various advanced friction options to the developer, and use a higher-quality friction model by default, to match ODE quickstep constraint solver.

Browse Source 
Thanks to Martijn Reuvers for bringing this up, and reproduction case.
See issue here: http://code.google.com/p/bullet/issues/detail?id=177
This commit is contained in:
erwin.coumans
2009-01-20 01:21:48 +00:00
parent 987b5cbfb1
commit 57fb21879b
2 changed files with 149 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -22,9 +22,12 @@ enum btSolverMode
SOLVER_FRICTION_SEPARATE = 2, SOLVER_FRICTION_SEPARATE = 2,
SOLVER_USE_WARMSTARTING = 4, SOLVER_USE_WARMSTARTING = 4,
SOLVER_USE_FRICTION_WARMSTARTING = 8, SOLVER_USE_FRICTION_WARMSTARTING = 8,
SOLVER_CACHE_FRIENDLY = 16, SOLVER_USE_1_FRICTION_DIRECTION = 16,
SOLVER_SIMD = 32,//enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
SOLVER_CUDA = 64 //will be open sourced during Game Developers Conference 2009. Much faster. 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 struct btContactSolverInfoData

275
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -49,8 +49,8 @@ btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) #define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 ) static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
{ {
__m128 result = _mm_mul_ps( vec0, 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 ) ) ); return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
} }
#endif//USE_SIMD #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; 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 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 deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*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; 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 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 deltaVel2Dotn = -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
@@ -180,8 +180,8 @@ SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstra
unsigned long btSequentialImpulseConstraintSolver::btRand2() unsigned long btSequentialImpulseConstraintSolver::btRand2()
{ {
m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
return m_btSeed2; return m_btSeed2;
} }
@@ -189,29 +189,29 @@ unsigned long btSequentialImpulseConstraintSolver::btRand2()
//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
int btSequentialImpulseConstraintSolver::btRandInt2 (int n) int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
{ {
// seems good; xor-fold and modulus // seems good; xor-fold and modulus
const unsigned long un = static_cast<unsigned long>(n); const unsigned long un = static_cast<unsigned long>(n);
unsigned long r = btRand2(); unsigned long r = btRand2();
// note: probably more aggressive than it needs to be -- might be // note: probably more aggressive than it needs to be -- might be
// able to get away without one or two of the innermost branches. // able to get away without one or two of the innermost branches.
if (un <= 0x00010000UL) { if (un <= 0x00010000UL) {
r ^= (r >> 16); r ^= (r >> 16);
if (un <= 0x00000100UL) { if (un <= 0x00000100UL) {
r ^= (r >> 8); r ^= (r >> 8);
if (un <= 0x00000010UL) { if (un <= 0x00000010UL) {
r ^= (r >> 4); r ^= (r >> 4);
if (un <= 0x00000004UL) { if (un <= 0x00000004UL) {
r ^= (r >> 2); r ^= (r >> 2);
if (un <= 0x00000002UL) { if (un <= 0x00000002UL) {
r ^= (r >> 1); 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) 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* body0=btRigidBody::upcast(colObj0);
btRigidBody* body1=btRigidBody::upcast(colObj1); btRigidBody* body1=btRigidBody::upcast(colObj1);
@@ -269,7 +269,7 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
solverConstraint.m_originalContactPoint = 0; solverConstraint.m_originalContactPoint = 0;
solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedImpulse = 0.f;
// solverConstraint.m_appliedPushImpulse = 0.f; // solverConstraint.m_appliedPushImpulse = 0.f;
solverConstraint.m_penetration = 0.f; solverConstraint.m_penetration = 0.f;
{ {
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
@@ -307,11 +307,11 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
#ifdef _USE_JACOBIAN #ifdef _USE_JACOBIAN
solverConstraint.m_jac = btJacobianEntry ( solverConstraint.m_jac = btJacobianEntry (
rel_pos1,rel_pos2,solverConstraint.m_contactNormal, rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
body0->getInvInertiaDiagLocal(), body0->getInvInertiaDiagLocal(),
body0->getInvMass(), body0->getInvMass(),
body1->getInvInertiaDiagLocal(), body1->getInvInertiaDiagLocal(),
body1->getInvMass()); body1->getInvMass());
#endif //_USE_JACOBIAN #endif //_USE_JACOBIAN
@@ -374,10 +374,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (!(numConstraints + numManifolds)) if (!(numConstraints + numManifolds))
{ {
// printf("empty\n"); // printf("empty\n");
return 0.f; return 0.f;
} }
if (1) if (1)
{ {
int j; int j;
@@ -396,9 +396,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
//if (1) //if (1)
{ {
{ {
int totalNumRows = 0; int totalNumRows = 0;
//calculate the total number of contraint rows //calculate the total number of contraint rows
for (int i=0;i<numConstraints;i++) for (int i=0;i<numConstraints;i++)
{ {
@@ -411,8 +411,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btTypedConstraint::btConstraintInfo1 info1; btTypedConstraint::btConstraintInfo1 info1;
info1.m_numConstraintRows = 0; info1.m_numConstraintRows = 0;
///setup the btSolverConstraints ///setup the btSolverConstraints
int currentRow = 0; int currentRow = 0;
@@ -425,15 +425,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
btTypedConstraint* constraint = constraints[i]; btTypedConstraint* constraint = constraints[i];
btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbA = constraint->getRigidBodyA();
btRigidBody& rbB = constraint->getRigidBodyB(); btRigidBody& rbB = constraint->getRigidBodyB();
int solverBodyIdA = getOrInitSolverBody(rbA); int solverBodyIdA = getOrInitSolverBody(rbA);
int solverBodyIdB = getOrInitSolverBody(rbB); int solverBodyIdB = getOrInitSolverBody(rbB);
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
@@ -453,7 +453,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f); bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
bodyBPtr->m_deltaLinearVelocity.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); bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
btTypedConstraint::btConstraintInfo2 info2; btTypedConstraint::btConstraintInfo2 info2;
@@ -489,7 +489,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal); sum += iMJlB.dot(solverConstraint.m_contactNormal);
@@ -515,13 +515,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedImpulse = 0.f;
} }
} }
} }
} }
} }
{ {
int i; int i;
btPersistentManifold* manifold = 0; btPersistentManifold* manifold = 0;
@@ -533,7 +533,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
manifold = manifoldPtr[i]; manifold = manifoldPtr[i];
colObj0 = (btCollisionObject*)manifold->getBody0(); colObj0 = (btCollisionObject*)manifold->getBody0();
colObj1 = (btCollisionObject*)manifold->getBody1(); colObj1 = (btCollisionObject*)manifold->getBody1();
int solverBodyIdA=-1; int solverBodyIdA=-1;
int solverBodyIdB=-1; int solverBodyIdB=-1;
@@ -549,21 +549,21 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
for (int j=0;j<manifold->getNumContacts();j++) for (int j=0;j<manifold->getNumContacts();j++)
{ {
btManifoldPoint& cp = manifold->getContactPoint(j); btManifoldPoint& cp = manifold->getContactPoint(j);
///this is a bad test and results in jitter -> always solve for those zero-distanc contacts! ///this is a bad test and results in jitter -> always solve for those zero-distanc contacts!
///-> if (cp.getDistance() <= btScalar(0.)) ///-> if (cp.getDistance() <= btScalar(0.))
//if (cp.getDistance() <= manifold->getContactBreakingThreshold()) //if (cp.getDistance() <= manifold->getContactBreakingThreshold())
{ {
const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB(); const btVector3& pos2 = cp.getPositionWorldOnB();
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
relaxation = 1.f; relaxation = 1.f;
btScalar rel_vel; btScalar rel_vel;
btVector3 vel; btVector3 vel;
@@ -604,7 +604,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
} }
#endif //COMPUTE_IMPULSE_DENOM #endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1); btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom; solverConstraint.m_jacDiagABInv = denom;
} }
@@ -616,11 +616,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
vel = vel1 - vel2; vel = vel1 - vel2;
rel_vel = cp.m_normalWorldOnB.dot(vel); rel_vel = cp.m_normalWorldOnB.dot(vel);
solverConstraint.m_penetration = cp.getDistance()+infoGlobal.m_linearSlop; solverConstraint.m_penetration = cp.getDistance()+infoGlobal.m_linearSlop;
//solverConstraint.m_penetration = cp.getDistance(); //solverConstraint.m_penetration = cp.getDistance();
@@ -628,12 +628,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_friction = cp.m_combinedFriction;
if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
{ {
solverConstraint.m_restitution = 0.f; solverConstraint.m_restitution = 0.f;
@@ -660,8 +660,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedImpulse = 0.f;
} }
// solverConstraint.m_appliedPushImpulse = 0.f; // solverConstraint.m_appliedPushImpulse = 0.f;
{ {
btScalar rel_vel; btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
@@ -698,73 +698,84 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (1) if (1)
{ {
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
if (!cp.m_lateralFrictionInitialized) if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !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)
{ {
cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) 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_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
cp.m_lateralFrictionDir2.normalize();//?? addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir2,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; 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 } 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]; btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) frictionConstraint1.m_appliedImpulse = 0.f;
{ if (!(infoGlobal.m_solverMode & SOLVER_USE_1_FRICTION_DIRECTION))
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
{ {
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
frictionConstraint2.m_appliedImpulse = 0.f; frictionConstraint2.m_appliedImpulse = 0.f;
} }
} }
} }
}
} }
@@ -773,11 +784,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
} }
} }
} }
btContactSolverInfo info = infoGlobal; btContactSolverInfo info = infoGlobal;
int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
@@ -857,15 +868,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
{ {
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
} }
///solve all friction constraints, using SIMD, if available ///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++) for (j=0;j<numFrictionPoolConstraints;j++)
{ {
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0)) if (totalImpulse>btScalar(0))
{ {
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 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); resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
} }
///solve all friction constraints ///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++) for (j=0;j<numFrictionPoolConstraints;j++)
{ {
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0)) if (totalImpulse>btScalar(0))
{ {
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); 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<numPoolConstraints;j++) for (j=0;j<numPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
btAssert(pt); btAssert(pt);
@@ -969,9 +980,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
} else } else
{ {
for ( i=0;i<m_tmpSolverBodyPool.size();i++) for ( i=0;i<m_tmpSolverBodyPool.size();i++)
{ {
m_tmpSolverBodyPool[i].writebackVelocity(); m_tmpSolverBodyPool[i].writebackVelocity();
} }
} }