From e9ec5044d176eb63d9643504a481d829d573f021 Mon Sep 17 00:00:00 2001 From: Lunkhound Date: Sat, 10 Jun 2017 04:21:49 -0700 Subject: [PATCH] SequentialImpulseConstraintSolver: fix bugs with SOLVER_SIMD flag --- .../btSequentialImpulseConstraintSolver.cpp | 364 +++++++----------- .../btSequentialImpulseConstraintSolver.h | 21 +- 2 files changed, 152 insertions(+), 233 deletions(-) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index fb9d9335f..b0d57a3e8 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -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,32 +354,42 @@ 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) - { +btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() +{ + 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_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2; + m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2; + m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2; #ifdef BT_ALLOW_SSE4 - int cpuFeatures = btCpuFeatureUtility::getCpuFeatures(); - if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1)) - { - m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3; - m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; - } + int cpuFeatures = btCpuFeatureUtility::getCpuFeatures(); + if ((cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_FMA3) && (cpuFeatures & btCpuFeatureUtility::CPU_FEATURE_SSE4_1)) + { + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse4_1_fma3; + 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;jisEnabled()) - { - 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;cbtScalar(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;jbtScalar(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;jbtScalar(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;jsolveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); } } + ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (int j=0;jbtScalar(0)) + for (int c=0;cbtScalar(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;jbtScalar(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; + } + } } - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - for (int j=0;jbtScalar(0)) + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (int j=0;jrollingFrictionConstraint.m_friction) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + 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; - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - leastSquaresResidual += residual*residual; + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + 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-1)) - { -#ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration); -#endif - break; - } - } - } - } + } } } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 2c4315c72..16c7eb74c 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -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: