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

205
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
@@ -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);
} }
@@ -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,7 +374,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
if (!(numConstraints + numManifolds)) if (!(numConstraints + numManifolds))
{ {
// printf("empty\n"); // printf("empty\n");
return 0.f; return 0.f;
} }
@@ -560,8 +560,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
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;
@@ -660,7 +660,7 @@ 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;
@@ -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;
} }
} }
} }
}
} }
@@ -860,8 +871,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
} }
///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;
@@ -902,8 +913,8 @@ 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;
@@ -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();
} }
} }