From b5495e789ddf74ebf2a66574bf55c59ec50e44eb Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 22 Jul 2018 11:30:16 +0200 Subject: [PATCH] Change constraint solver threshold-based termination condition on residual of velocity threshold, not on residual of impulse threshold. This avoids issues with systems with large mass ratios. Test: add this to BasicDemo/BasicExample.cpp in initPhysics m_dynamicsWorld->getSolverInfo().m_numIterations = 1000; m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-4; --- .../SharedMemory/PhysicsServerExample.cpp | 4 +- .../btSequentialImpulseConstraintSolver.cpp | 43 ++++++++++--------- .../btSequentialImpulseConstraintSolver.h | 16 +++---- .../btMultiBodyConstraintSolver.cpp | 6 ++- 4 files changed, 37 insertions(+), 32 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 5260d6356..376b143ba 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -2954,9 +2954,9 @@ btVector3 PhysicsServerExample::getRayTo(int x,int y) btVector3 hor; hor = rayForward.cross(vertical); - hor.normalize(); + hor.safeNormalize(); vertical = hor.cross(rayForward); - vertical.normalize(); + vertical.safeNormalize(); float tanfov = tanf(0.5f*fov); diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 1282ea96a..c03f9f861 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -43,7 +43,7 @@ int gNumSplitImpulseRecoveries = 0; //#define VERBOSE_RESIDUAL_PRINTF 1 ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. -static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); @@ -72,11 +72,11 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolver body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - return deltaImpulse; + return deltaImpulse*(1./c.m_jacDiagABInv); } -static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); @@ -97,7 +97,7 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSol body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - return deltaImpulse; + return deltaImpulse*(1./c.m_jacDiagABInv); } @@ -150,7 +150,7 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) #endif // Project Gauss Seidel or the equivalent Sequential Impulse -static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); @@ -177,12 +177,12 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); - return deltaImpulse; + return deltaImpulse.m_floats[0]/c.m_jacDiagABInv; } // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 -static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { #if defined (BT_ALLOW_SSE4) __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); @@ -202,7 +202,8 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); - return deltaImpulse; + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv); #else return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c); #endif @@ -210,7 +211,7 @@ static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& -static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); @@ -234,12 +235,12 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bod body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); - return deltaImpulse; + return deltaImpulse.m_floats[0]/c.m_jacDiagABInv; } // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 -static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) +static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) { #ifdef BT_ALLOW_SSE4 __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); @@ -257,7 +258,8 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBo body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128); body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128); body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128); - return deltaImpulse; + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv); #else return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c); #endif //BT_ALLOW_SSE4 @@ -268,30 +270,30 @@ static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBo -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { return m_resolveSingleConstraintRowGeneric(body1, body2, c); } // Project Gauss Seidel or the equivalent Sequential Impulse -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { return m_resolveSingleConstraintRowGeneric(body1, body2, c); } -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); } -btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { return m_resolveSingleConstraintRowLowerLimit(body1, body2, c); } -static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference( +static btScalar gResolveSplitPenetrationImpulse_scalar_reference( btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c) @@ -320,10 +322,10 @@ static btSimdScalar gResolveSplitPenetrationImpulse_scalar_reference( body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } - return deltaImpulse; + return deltaImpulse*(1./c.m_jacDiagABInv); } -static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) +static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -353,7 +355,8 @@ static btSimdScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& body1,btS body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); - return deltaImpulse; + btSimdScalar deltaImp = deltaImpulse; + return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else return gResolveSplitPenetrationImpulse_scalar_reference(body1,body2,c); #endif diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index c853aed77..b834c3dac 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -27,7 +27,7 @@ class btCollisionObject; #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" #include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" -typedef btSimdScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); +typedef btScalar(*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&); ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver @@ -100,12 +100,12 @@ protected: virtual void convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); - btSimdScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) + btScalar resolveSplitPenetrationSIMD(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) { return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); } - btSimdScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) + btScalar resolveSplitPenetrationImpulseCacheFriendly(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) { return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); } @@ -114,11 +114,11 @@ protected: int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep); void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep); - btSimdScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - 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) + btScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); + btScalar resolveSplitPenetrationImpulse(btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint) { return m_resolveSplitPenetrationImpulse( bodyA, bodyB, contactConstraint ); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 8c6cb6aa8..a08b851fb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -274,7 +274,8 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt { bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse); } - return deltaImpulse; + btScalar deltaVel =deltaImpulse/c.m_jacDiagABInv; + return deltaVel; } @@ -453,7 +454,8 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt bodyB->internalApplyImpulse(cB.m_contactNormal2*bodyB->internalGetInvMass(),cB.m_angularComponentB,deltaImpulseB); } - return deltaImpulseA+deltaImpulseB; + btScalar deltaVel =deltaImpulseA/cA.m_jacDiagABInv+deltaImpulseB/cB.m_jacDiagABInv; + return deltaVel; }