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

Merge pull request #1182 from lunkhound/pr-fix-solver-simd

Browse Source 
SequentialImpulseConstraintSolver: fix bugs with SOLVER_SIMD flag
This commit is contained in:
erwincoumans
2017-06-20 12:19:15 -07:00
committed by GitHub
parent 99d2ab7871 e9ec5044d1
commit c48f5b1546
2 changed files with 152 additions and 233 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -269,36 +269,28 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBo
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
return m_resolveSingleConstraintRowGeneric(body1, body2, c);
#else
return resolveSingleConstraintRowGeneric(body1,body2,c);
#endif
}
// Project Gauss Seidel or the equivalent Sequential Impulse
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
return m_resolveSingleConstraintRowGeneric(body1, body2, c);
}
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
#else
return resolveSingleConstraintRowLowerLimit(body1,body2,c);
#endif
}
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
}
btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& c)
@@ -330,7 +322,7 @@ btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCach
return deltaImpulse;
}
btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
if (!c.m_rhsPenetration)
@@ -362,21 +354,30 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(bt
body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
return deltaImpulse;
#else
return resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
return gResolveSplitPenetrationImpulse_scalar_reference(body1,body2,c);
#endif
}
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
: m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
m_btSeed2(0)
{
m_btSeed2 = 0;
m_cachedSolverMode = 0;
setupSolverFunctions( false );
}
void btSequentialImpulseConstraintSolver::setupSolverFunctions( bool useSimd )
{
m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference;
m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference;
m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference;
if ( useSimd )
{
#ifdef USE_SIMD
m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2;
#endif //USE_SIMD
m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2;
#ifdef BT_ALLOW_SSE4
int cpuFeatures = btCpuFeatureUtility::getCpuFeatures();
@@ -386,7 +387,8 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(bt
m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3;
}
#endif//BT_ALLOW_SSE4
#endif //USE_SIMD
}
}
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
@@ -1262,6 +1264,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
BT_PROFILE("solveGroupCacheFriendlySetup");
(void)debugDrawer;
// if solver mode has changed,
if ( infoGlobal.m_solverMode != m_cachedSolverMode )
{
// update solver functions to use SIMD or non-SIMD
bool useSimd = !!( infoGlobal.m_solverMode & SOLVER_SIMD );
setupSolverFunctions( useSimd );
m_cachedSolverMode = infoGlobal.m_solverMode;
}
m_maxOverrideNumSolverIterations = 0;
#ifdef BT_ADDITIONAL_DEBUG
@@ -1658,145 +1668,6 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
}
}
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
///solve all joint constraints, using SIMD, if available
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
leastSquaresResidual += residual*residual;
}
}
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints using SIMD, if available
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
for (int c=0;c<numPoolConstraints;c++)
{
btScalar totalImpulse =0;
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if (applyFriction)
{
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
leastSquaresResidual += residual*residual;
}
}
}
}
} else
{
//non-SIMD version
///solve all joint constraints
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
@@ -1821,17 +1692,78 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (int j=0;j<numPoolConstraints;j++)
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
for (int c=0;c<numPoolConstraints;c++)
{
btScalar totalImpulse =0;
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if (applyFriction)
{
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (int j=0;j<numFrictionPoolConstraints;j++)
for (j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
@@ -1845,10 +1777,13 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
leastSquaresResidual += residual*residual;
}
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (int j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
@@ -1864,7 +1799,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
leastSquaresResidual += residual*residual;
}
}
}
}
return leastSquaresResidual;
}
@@ -1875,7 +1811,6 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
int iteration;
if (infoGlobal.m_splitImpulse)
{
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
@@ -1887,7 +1822,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
@@ -1900,31 +1835,6 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
}
}
}
else
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
btScalar leastSquaresResidual = 0.f;
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar residual = resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
#endif
break;
}
}
}
}
}
}

21
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -56,6 +56,9 @@ protected:
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
int m_cachedSolverMode; // used to check if SOLVER_SIMD flag has been changed
void setupSolverFunctions( bool useSimd );
btScalar m_leastSquaresResidual;
@@ -93,13 +96,15 @@ protected:
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
btSimdScalar resolveSplitPenetrationSIMD(
btSolverBody& bodyA,btSolverBody& bodyB,
const btSolverConstraint& contactConstraint);
btSimdScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
btScalar resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& bodyA,btSolverBody& bodyB,
const btSolverConstraint& contactConstraint);
btSimdScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
//internal method
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
@@ -109,6 +114,10 @@ protected:
btSimdScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btSimdScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint)
{
return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint );
}
protected: