AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1798 from erwincoumans/master

Browse Source 
Change constraint solver threshold-based termination condition on res…
This commit is contained in:
erwincoumans 2018-07-22 12:28:32 +02:00 committed by GitHub
commit defd961f60
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 37 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -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);

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

@ -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

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

@ -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 );
}

6
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@ -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;
}