diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp | 1755 |
1 files changed, 820 insertions, 935 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 63174a6ec0..def3227b43 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -21,7 +21,6 @@ subject to the following restrictions: #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btCpuFeatureUtility.h" @@ -34,9 +33,9 @@ subject to the following restrictions: //#include "btSolverBody.h" //#include "btSolverConstraint.h" #include "LinearMath/btAlignedObjectArray.h" -#include <string.h> //for memset +#include <string.h> //for memset -int gNumSplitImpulseRecoveries = 0; +int gNumSplitImpulseRecoveries = 0; #include "BulletDynamics/Dynamics/btRigidBody.h" @@ -45,13 +44,13 @@ int gNumSplitImpulseRecoveries = 0; ///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check. static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity()); const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity()); // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) @@ -69,21 +68,20 @@ static btScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody c.m_appliedImpulse = sum; } - bodyA.internalApplyImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - bodyB.internalApplyImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - return deltaImpulse*(1./c.m_jacDiagABInv); + return deltaImpulse * (1. / c.m_jacDiagABInv); } - static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { - btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm; + btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm; const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetDeltaAngularVelocity()); const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetDeltaAngularVelocity()); - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; if (sum < c.m_lowerLimit) { @@ -94,58 +92,55 @@ static btScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverB { c.m_appliedImpulse = sum; } - bodyA.internalApplyImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); - bodyB.internalApplyImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + bodyA.internalApplyImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); - return deltaImpulse*(1./c.m_jacDiagABInv); + return deltaImpulse * (1. / c.m_jacDiagABInv); } - - #ifdef USE_SIMD #include <emmintrin.h> - -#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) -static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) +#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e)) +static inline __m128 btSimdDot3(__m128 vec0, __m128 vec1) { - __m128 result = _mm_mul_ps( vec0, vec1); - return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); + __m128 result = _mm_mul_ps(vec0, vec1); + return _mm_add_ps(btVecSplat(result, 0), _mm_add_ps(btVecSplat(result, 1), btVecSplat(result, 2))); } -#if defined (BT_ALLOW_SSE4) +#if defined(BT_ALLOW_SSE4) #include <intrin.h> -#define USE_FMA 1 -#define USE_FMA3_INSTEAD_FMA4 1 -#define USE_SSE4_DOT 1 +#define USE_FMA 1 +#define USE_FMA3_INSTEAD_FMA4 1 +#define USE_SSE4_DOT 1 -#define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f) -#define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f)) +#define SSE4_DP(a, b) _mm_dp_ps(a, b, 0x7f) +#define SSE4_DP_FP(a, b) _mm_cvtss_f32(_mm_dp_ps(a, b, 0x7f)) #if USE_SSE4_DOT -#define DOT_PRODUCT(a, b) SSE4_DP(a, b) +#define DOT_PRODUCT(a, b) SSE4_DP(a, b) #else -#define DOT_PRODUCT(a, b) btSimdDot3(a, b) +#define DOT_PRODUCT(a, b) btSimdDot3(a, b) #endif #if USE_FMA #if USE_FMA3_INSTEAD_FMA4 // a*b + c -#define FMADD(a, b, c) _mm_fmadd_ps(a, b, c) +#define FMADD(a, b, c) _mm_fmadd_ps(a, b, c) // -(a*b) + c -#define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c) -#else // USE_FMA3 +#define FMNADD(a, b, c) _mm_fnmadd_ps(a, b, c) +#else // USE_FMA3 // a*b + c -#define FMADD(a, b, c) _mm_macc_ps(a, b, c) +#define FMADD(a, b, c) _mm_macc_ps(a, b, c) // -(a*b) + c -#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c) +#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c) #endif -#else // USE_FMA +#else // USE_FMA // c + a*b -#define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b)) +#define FMADD(a, b, c) _mm_add_ps(c, _mm_mul_ps(a, b)) // c - a*b -#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b)) +#define FMNADD(a, b, c) _mm_sub_ps(c, _mm_mul_ps(a, b)) #endif #endif @@ -153,8 +148,8 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm))); __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); @@ -170,52 +165,49 @@ static btScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& bodyA, btS __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp); deltaImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied)); c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1)); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); - return deltaImpulse.m_floats[0]/c.m_jacDiagABInv; + return deltaImpulse.m_floats[0] / c.m_jacDiagABInv; } - // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 static btScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { -#if defined (BT_ALLOW_SSE4) - __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); - __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); - const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); - const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit); - const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); - const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); - deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); - deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); - tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum - const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit); - const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp); - deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower); - c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower); - bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); +#if defined(BT_ALLOW_SSE4) + __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); + __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm); + const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); + const __m128 upperLimit = _mm_set_ps1(c.m_upperLimit); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); + deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); + deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); + tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); // sum + const __m128 maskLower = _mm_cmpgt_ps(tmp, lowerLimit); + const __m128 maskUpper = _mm_cmpgt_ps(upperLimit, tmp); + deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), _mm_blendv_ps(_mm_sub_ps(upperLimit, c.m_appliedImpulse), deltaImpulse, maskUpper), maskLower); + c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, _mm_blendv_ps(upperLimit, tmp, maskUpper), maskLower); + bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128); - bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); + bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128); btSimdScalar deltaImp = deltaImpulse; - return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv); + return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else - return gResolveSingleConstraintRowGeneric_sse2(bodyA,bodyB,c); + return gResolveSingleConstraintRowGeneric_sse2(bodyA, bodyB, c); #endif } - - static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); btSimdScalar deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse), _mm_set1_ps(c.m_cfm))); __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); @@ -228,104 +220,98 @@ static btScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& bodyA, __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; bodyA.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); bodyA.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyA.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); bodyB.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); bodyB.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(bodyB.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); - return deltaImpulse.m_floats[0]/c.m_jacDiagABInv; + return deltaImpulse.m_floats[0] / c.m_jacDiagABInv; } - // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3 static btScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { #ifdef BT_ALLOW_SSE4 - __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); - __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm); - const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); - const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); - const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); - deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); - deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); - tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); - const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit); - deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask); - c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask); - bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); + __m128 tmp = _mm_set_ps1(c.m_jacDiagABInv); + __m128 deltaImpulse = _mm_set_ps1(c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm); + const __m128 lowerLimit = _mm_set_ps1(c.m_lowerLimit); + const __m128 deltaVel1Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal1.mVec128, bodyA.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetDeltaAngularVelocity().mVec128)); + const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, bodyB.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetDeltaAngularVelocity().mVec128)); + deltaImpulse = FMNADD(deltaVel1Dotn, tmp, deltaImpulse); + deltaImpulse = FMNADD(deltaVel2Dotn, tmp, deltaImpulse); + tmp = _mm_add_ps(c.m_appliedImpulse, deltaImpulse); + const __m128 mask = _mm_cmpgt_ps(tmp, lowerLimit); + deltaImpulse = _mm_blendv_ps(_mm_sub_ps(lowerLimit, c.m_appliedImpulse), deltaImpulse, mask); + c.m_appliedImpulse = _mm_blendv_ps(lowerLimit, tmp, mask); + bodyA.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128), deltaImpulse, bodyA.internalGetDeltaLinearVelocity().mVec128); bodyA.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, bodyA.internalGetDeltaAngularVelocity().mVec128); - bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); + bodyB.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128), deltaImpulse, bodyB.internalGetDeltaLinearVelocity().mVec128); bodyB.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, bodyB.internalGetDeltaAngularVelocity().mVec128); btSimdScalar deltaImp = deltaImpulse; - return deltaImp.m_floats[0]*(1./c.m_jacDiagABInv); + return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else - return gResolveSingleConstraintRowLowerLimit_sse2(bodyA,bodyB,c); -#endif //BT_ALLOW_SSE4 + return gResolveSingleConstraintRowLowerLimit_sse2(bodyA, bodyB, c); +#endif //BT_ALLOW_SSE4 } +#endif //USE_SIMD -#endif //USE_SIMD - - - -btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c); } // Project Gauss Seidel or the equivalent Sequential Impulse -btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { return m_resolveSingleConstraintRowGeneric(bodyA, bodyB, c); } -btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c); } - -btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) +btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { return m_resolveSingleConstraintRowLowerLimit(bodyA, bodyB, c); } - static btScalar gResolveSplitPenetrationImpulse_scalar_reference( - btSolverBody& bodyA, - btSolverBody& bodyB, - const btSolverConstraint& c) + btSolverBody& bodyA, + btSolverBody& bodyB, + const btSolverConstraint& c) { btScalar deltaImpulse = 0.f; - if (c.m_rhsPenetration) - { - gNumSplitImpulseRecoveries++; - deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity()); - const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity()); - - deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; - deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; - const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse; - if (sum < c.m_lowerLimit) - { - deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse; - c.m_appliedPushImpulse = c.m_lowerLimit; - } - else - { - c.m_appliedPushImpulse = sum; - } - bodyA.internalApplyPushImpulse(c.m_contactNormal1*bodyA.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - bodyB.internalApplyPushImpulse(c.m_contactNormal2*bodyB.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); - } - return deltaImpulse*(1./c.m_jacDiagABInv); + if (c.m_rhsPenetration) + { + gNumSplitImpulseRecoveries++; + deltaImpulse = c.m_rhsPenetration - btScalar(c.m_appliedPushImpulse) * c.m_cfm; + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(bodyA.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(bodyA.internalGetTurnVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(bodyB.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(bodyB.internalGetTurnVelocity()); + + deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv; + deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv; + const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse; + if (sum < c.m_lowerLimit) + { + deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse; + c.m_appliedPushImpulse = c.m_lowerLimit; + } + else + { + c.m_appliedPushImpulse = sum; + } + bodyA.internalApplyPushImpulse(c.m_contactNormal1 * bodyA.internalGetInvMass(), c.m_angularComponentA, deltaImpulse); + bodyB.internalApplyPushImpulse(c.m_contactNormal2 * bodyB.internalGetInvMass(), c.m_angularComponentB, deltaImpulse); + } + return deltaImpulse * (1. / c.m_jacDiagABInv); } -static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& c) +static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA, btSolverBody& bodyB, const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -334,113 +320,109 @@ static btScalar gResolveSplitPenetrationImpulse_sse2(btSolverBody& bodyA,btSolve gNumSplitImpulseRecoveries++; __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse); - __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); - __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); - __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,bodyA.internalGetTurnVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,bodyB.internalGetTurnVelocity().mVec128)); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); - btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); - btSimdScalar resultLowerLess,resultUpperLess; - resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); - resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); - __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); - deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,bodyA.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,bodyB.internalGetInvMass().mVec128); + __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); + __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); + __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse), _mm_set1_ps(c.m_cfm))); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, bodyA.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, bodyA.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, bodyB.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, bodyB.internalGetTurnVelocity().mVec128)); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel1Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv))); + btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse); + btSimdScalar resultLowerLess, resultUpperLess; + resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1); + resultUpperLess = _mm_cmplt_ps(sum, upperLimit1); + __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp); + deltaImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse)); + c.m_appliedPushImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum)); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, bodyA.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, bodyB.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; - bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); - bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); - bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); + bodyA.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyA.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude)); + bodyA.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyA.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude)); + bodyB.internalGetPushVelocity().mVec128 = _mm_add_ps(bodyB.internalGetPushVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude)); + bodyB.internalGetTurnVelocity().mVec128 = _mm_add_ps(bodyB.internalGetTurnVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude)); btSimdScalar deltaImp = deltaImpulse; return deltaImp.m_floats[0] * (1. / c.m_jacDiagABInv); #else - return gResolveSplitPenetrationImpulse_scalar_reference(bodyA,bodyB,c); + return gResolveSplitPenetrationImpulse_scalar_reference(bodyA, bodyB, c); #endif } - btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() { - m_btSeed2 = 0; - m_cachedSolverMode = 0; - setupSolverFunctions( false ); + m_btSeed2 = 0; + m_cachedSolverMode = 0; + setupSolverFunctions(false); } -void btSequentialImpulseConstraintSolver::setupSolverFunctions( bool useSimd ) +void btSequentialImpulseConstraintSolver::setupSolverFunctions(bool useSimd) { - m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference; - m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference; - m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference; + m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_scalar_reference; + m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_scalar_reference; + m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_scalar_reference; - if ( useSimd ) - { + if (useSimd) + { #ifdef USE_SIMD - m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2; - m_resolveSingleConstraintRowLowerLimit = gResolveSingleConstraintRowLowerLimit_sse2; - m_resolveSplitPenetrationImpulse = gResolveSplitPenetrationImpulse_sse2; + 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; - } -#endif//BT_ALLOW_SSE4 -#endif //USE_SIMD - } + 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() - { - } - - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric() - { - return gResolveSingleConstraintRowGeneric_scalar_reference; - } +btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() +{ +} - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit() - { - return gResolveSingleConstraintRowLowerLimit_scalar_reference; - } +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric() +{ + return gResolveSingleConstraintRowGeneric_scalar_reference; +} +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit() +{ + return gResolveSingleConstraintRowLowerLimit_scalar_reference; +} #ifdef USE_SIMD - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() - { - return gResolveSingleConstraintRowGeneric_sse2; - } - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit() - { - return gResolveSingleConstraintRowLowerLimit_sse2; - } +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverGeneric() +{ + return gResolveSingleConstraintRowGeneric_sse2; +} +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE2ConstraintRowSolverLowerLimit() +{ + return gResolveSingleConstraintRowLowerLimit_sse2; +} #ifdef BT_ALLOW_SSE4 - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric() - { - return gResolveSingleConstraintRowGeneric_sse4_1_fma3; - } - btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit() - { - return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; - } -#endif //BT_ALLOW_SSE4 -#endif //USE_SIMD +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverGeneric() +{ + return gResolveSingleConstraintRowGeneric_sse4_1_fma3; +} +btSingleConstraintRowSolver btSequentialImpulseConstraintSolver::getSSE4_1ConstraintRowSolverLowerLimit() +{ + return gResolveSingleConstraintRowLowerLimit_sse4_1_fma3; +} +#endif //BT_ALLOW_SSE4 +#endif //USE_SIMD unsigned long btSequentialImpulseConstraintSolver::btRand2() { - m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; + m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff; return m_btSeed2; } - - //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 const unsigned long un = static_cast<unsigned long>(n); @@ -448,15 +430,20 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) // note: probably more aggressive than it needs to be -- might be // able to get away without one or two of the innermost branches. - if (un <= 0x00010000UL) { + if (un <= 0x00010000UL) + { r ^= (r >> 16); - if (un <= 0x00000100UL) { + if (un <= 0x00000100UL) + { r ^= (r >> 8); - if (un <= 0x00000010UL) { + if (un <= 0x00000010UL) + { r ^= (r >> 4); - if (un <= 0x00000004UL) { + if (un <= 0x00000004UL) + { r ^= (r >> 2); - if (un <= 0x00000002UL) { + if (un <= 0x00000002UL) + { r ^= (r >> 1); } } @@ -464,70 +451,56 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) } } - return (int) (r % un); + return (int)(r % un); } - - -void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) +void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep) { + btRigidBody* rb = collisionObject ? btRigidBody::upcast(collisionObject) : 0; - btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; - - solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f); - solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + solverBody->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f); + solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f); if (rb) { solverBody->m_worldTransform = rb->getWorldTransform(); - solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor()); + solverBody->internalSetInvMass(btVector3(rb->getInvMass(), rb->getInvMass(), rb->getInvMass()) * rb->getLinearFactor()); solverBody->m_originalBody = rb; solverBody->m_angularFactor = rb->getAngularFactor(); solverBody->m_linearFactor = rb->getLinearFactor(); solverBody->m_linearVelocity = rb->getLinearVelocity(); solverBody->m_angularVelocity = rb->getAngularVelocity(); - solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep; - solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ; - - } else + solverBody->m_externalForceImpulse = rb->getTotalForce() * rb->getInvMass() * timeStep; + solverBody->m_externalTorqueImpulse = rb->getTotalTorque() * rb->getInvInertiaTensorWorld() * timeStep; + } + else { solverBody->m_worldTransform.setIdentity(); - solverBody->internalSetInvMass(btVector3(0,0,0)); + solverBody->internalSetInvMass(btVector3(0, 0, 0)); solverBody->m_originalBody = 0; - solverBody->m_angularFactor.setValue(1,1,1); - solverBody->m_linearFactor.setValue(1,1,1); - solverBody->m_linearVelocity.setValue(0,0,0); - solverBody->m_angularVelocity.setValue(0,0,0); - solverBody->m_externalForceImpulse.setValue(0,0,0); - solverBody->m_externalTorqueImpulse.setValue(0,0,0); + solverBody->m_angularFactor.setValue(1, 1, 1); + solverBody->m_linearFactor.setValue(1, 1, 1); + solverBody->m_linearVelocity.setValue(0, 0, 0); + solverBody->m_angularVelocity.setValue(0, 0, 0); + solverBody->m_externalForceImpulse.setValue(0, 0, 0); + solverBody->m_externalTorqueImpulse.setValue(0, 0, 0); } - - } - - - - - btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold) { //printf("rel_vel =%f\n", rel_vel); - if (btFabs(rel_vel)<velocityThreshold) + if (btFabs(rel_vel) < velocityThreshold) return 0.; btScalar rest = restitution * -rel_vel; return rest; } - - -void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) +void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionObject* colObj, btVector3& frictionDirection, int frictionMode) { - - if (colObj && colObj->hasAnisotropicFriction(frictionMode)) { // transform to local coordinates @@ -538,16 +511,10 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb // ... and transform it back to global coordinates frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; } - } - - - -void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { - - btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; @@ -568,12 +535,13 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_contactNormal1 = normalAxis; btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor(); - }else + solverConstraint.m_angularComponentA = body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor(); + } + else { solverConstraint.m_contactNormal1.setZero(); solverConstraint.m_relpos1CrossNormal.setZero(); - solverConstraint.m_angularComponentA .setZero(); + solverConstraint.m_angularComponentA.setZero(); } if (bodyA) @@ -581,8 +549,9 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_contactNormal2 = -normalAxis; btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld()*ftorqueAxis1*bodyA->getAngularFactor(); - } else + solverConstraint.m_angularComponentB = bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor(); + } + else { solverConstraint.m_contactNormal2.setZero(); solverConstraint.m_relpos2CrossNormal.setZero(); @@ -595,32 +564,28 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr btScalar denom1 = 0.f; if (body0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); denom0 = body0->getInvMass() + normalAxis.dot(vec); } if (bodyA) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); denom1 = bodyA->getInvMass() + normalAxis.dot(vec); } - btScalar denom = relaxation/(denom0+denom1); + btScalar denom = relaxation / (denom0 + denom1); solverConstraint.m_jacDiagABInv = denom; } { - - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(bodyA?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0)); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; -// btScalar positionalError = 0.f; + // btScalar positionalError = 0.f; - btScalar velocityError = desiredVelocity - rel_vel; + btScalar velocityError = desiredVelocity - rel_vel; btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; btScalar penetrationImpulse = btScalar(0); @@ -628,8 +593,8 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR) { btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis); - btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep; - penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; + btScalar positionalError = -distance * infoGlobal.m_frictionERP / infoGlobal.m_timeStep; + penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; } solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; @@ -637,11 +602,10 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - } } -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; @@ -650,15 +614,13 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c return solverConstraint; } - -void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2, - btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, - btScalar desiredVelocity, btScalar cfmSlip) +void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, + btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity, btScalar cfmSlip) { - btVector3 normalAxis(0,0,0); - + btVector3 normalAxis(0, 0, 0); solverConstraint.m_contactNormal1 = normalAxis; solverConstraint.m_contactNormal2 = -normalAxis; @@ -671,8 +633,8 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_friction = combinedTorsionalFriction; - solverConstraint.m_originalContactPoint = 0; + solverConstraint.m_friction = combinedTorsionalFriction; + solverConstraint.m_originalContactPoint = 0; solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; @@ -680,138 +642,125 @@ void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSo { btVector3 ftorqueAxis1 = -normalAxis1; solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld() * ftorqueAxis1 * body0->getAngularFactor() : btVector3(0, 0, 0); } { btVector3 ftorqueAxis1 = normalAxis1; solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld()*ftorqueAxis1*bodyA->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentB = bodyA ? bodyA->getInvInertiaTensorWorld() * ftorqueAxis1 * bodyA->getAngularFactor() : btVector3(0, 0, 0); } - { - btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); - btVector3 iMJaB = bodyA?bodyA->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + btVector3 iMJaA = body0 ? body0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0); + btVector3 iMJaB = bodyA ? bodyA->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0); btScalar sum = 0; sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; + solverConstraint.m_jacDiagABInv = btScalar(1.) / sum; } { - - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(bodyA?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0 ? solverBodyA.m_linearVelocity + solverBodyA.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : btVector3(0, 0, 0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyA ? solverBodyB.m_linearVelocity + solverBodyB.m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(bodyA ? solverBodyB.m_angularVelocity : btVector3(0, 0, 0)); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; -// btScalar positionalError = 0.f; + // btScalar positionalError = 0.f; - btSimdScalar velocityError = desiredVelocity - rel_vel; - btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); + btSimdScalar velocityError = desiredVelocity - rel_vel; + btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction; - } } - - - - - - - -btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2, - colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } - -int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep) +int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body, btScalar timeStep) { #if BT_THREADSAFE - int solverBodyId = -1; - bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL; - if ( isRigidBodyType && !body.isStaticOrKinematicObject() ) - { - // dynamic body - // Dynamic bodies can only be in one island, so it's safe to write to the companionId - solverBodyId = body.getCompanionId(); - if ( solverBodyId < 0 ) - { - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &solverBody, &body, timeStep ); - body.setCompanionId( solverBodyId ); - } - } - else if (isRigidBodyType && body.isKinematicObject()) - { - // - // NOTE: must test for kinematic before static because some kinematic objects also - // identify as "static" - // - // Kinematic bodies can be in multiple islands at once, so it is a - // race condition to write to them, so we use an alternate method - // to record the solverBodyId - int uniqueId = body.getWorldArrayIndex(); - const int INVALID_SOLVER_BODY_ID = -1; - if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size()) - { - m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); - } - solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ]; - // if no table entry yet, - if ( solverBodyId == INVALID_SOLVER_BODY_ID ) - { - // create a table entry for this body - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &solverBody, &body, timeStep ); - m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId; - } - } - else - { - bool isMultiBodyType = (body.getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK); - // Incorrectly set collision object flags can degrade performance in various ways. + int solverBodyId = -1; + bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; + if (isRigidBodyType && !body.isStaticOrKinematicObject()) + { + // dynamic body + // Dynamic bodies can only be in one island, so it's safe to write to the companionId + solverBodyId = body.getCompanionId(); + if (solverBodyId < 0) + { + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + body.setCompanionId(solverBodyId); + } + } + else if (isRigidBodyType && body.isKinematicObject()) + { + // + // NOTE: must test for kinematic before static because some kinematic objects also + // identify as "static" + // + // Kinematic bodies can be in multiple islands at once, so it is a + // race condition to write to them, so we use an alternate method + // to record the solverBodyId + int uniqueId = body.getWorldArrayIndex(); + const int INVALID_SOLVER_BODY_ID = -1; + if (uniqueId >= m_kinematicBodyUniqueIdToSolverBodyTable.size()) + { + m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); + } + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId]; + // if no table entry yet, + if (solverBodyId == INVALID_SOLVER_BODY_ID) + { + // create a table entry for this body + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId; + } + } + else + { + bool isMultiBodyType = (body.getInternalType() & btCollisionObject::CO_FEATHERSTONE_LINK); + // Incorrectly set collision object flags can degrade performance in various ways. if (!isMultiBodyType) { - btAssert( body.isStaticOrKinematicObject() ); + btAssert(body.isStaticOrKinematicObject()); } - //it could be a multibody link collider - // all fixed bodies (inf mass) get mapped to a single solver id - if ( m_fixedBodyId < 0 ) - { - m_fixedBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &fixedBody, 0, timeStep ); - } - solverBodyId = m_fixedBodyId; - } - btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() ); + //it could be a multibody link collider + // all fixed bodies (inf mass) get mapped to a single solver id + if (m_fixedBodyId < 0) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody, 0, timeStep); + } + solverBodyId = m_fixedBodyId; + } + btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size()); return solverBodyId; -#else // BT_THREADSAFE +#else // BT_THREADSAFE - int solverBodyIdA = -1; + int solverBodyIdA = -1; if (body.getCompanionId() >= 0) { //body has already been converted solverBodyIdA = body.getCompanionId(); - btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); - } else + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); + } + else { btRigidBody* rb = btRigidBody::upcast(&body); //convert both active and kinematic objects (for their velocity) @@ -819,233 +768,216 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& { solverBodyIdA = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,&body,timeStep); + initSolverBody(&solverBody, &body, timeStep); body.setCompanionId(solverBodyIdA); - } else + } + else { - - if (m_fixedBodyId<0) + if (m_fixedBodyId < 0) { m_fixedBodyId = m_tmpSolverBodyPool.size(); btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody(&fixedBody,0,timeStep); + initSolverBody(&fixedBody, 0, timeStep); } return m_fixedBodyId; -// return 0;//assume first one is a fixed solver body + // return 0;//assume first one is a fixed solver body } } return solverBodyIdA; -#endif // BT_THREADSAFE - +#endif // BT_THREADSAFE } #include <stdio.h> - void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2) { + // const btVector3& pos1 = cp.getPositionWorldOnA(); + // const btVector3& pos2 = cp.getPositionWorldOnB(); - // const btVector3& pos1 = cp.getPositionWorldOnA(); - // const btVector3& pos2 = cp.getPositionWorldOnB(); - - btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; - btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - - btRigidBody* rb0 = bodyA->m_originalBody; - btRigidBody* rb1 = bodyB->m_originalBody; - -// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); -// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); - //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); - - relaxation = infoGlobal.m_sor; - btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep; - - //cfm = 1 / ( dt * kp + kd ) - //erp = dt * kp / ( dt * kp + kd ) - - btScalar cfm = infoGlobal.m_globalCfm; - btScalar erp = infoGlobal.m_erp2; - - if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)) - { - if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) - cfm = cp.m_contactCFM; - if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP) - erp = cp.m_contactERP; - } else - { - if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) - { - btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 ); - if (denom < SIMD_EPSILON) - { - denom = SIMD_EPSILON; - } - cfm = btScalar(1) / denom; - erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; - } - } - - cfm *= invTimeStep; - - - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); - - { -#ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (rb0) - { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } - if (rb1) - { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); - } -#endif //COMPUTE_IMPULSE_DENOM - - btScalar denom = relaxation/(denom0+denom1+cfm); - solverConstraint.m_jacDiagABInv = denom; - } - - if (rb0) - { - solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = torqueAxis0; - } else - { - solverConstraint.m_contactNormal1.setZero(); - solverConstraint.m_relpos1CrossNormal.setZero(); - } - if (rb1) - { - solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; - }else - { - solverConstraint.m_contactNormal2.setZero(); - solverConstraint.m_relpos2CrossNormal.setZero(); - } - - btScalar restitution = 0.f; - btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; - - { - btVector3 vel1,vel2; + btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + btRigidBody* rb0 = bodyA->m_originalBody; + btRigidBody* rb1 = bodyB->m_originalBody; - // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - btVector3 vel = vel1 - vel2; - btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); + // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); + // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); + relaxation = infoGlobal.m_sor; + btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep; + //cfm = 1 / ( dt * kp + kd ) + //erp = dt * kp / ( dt * kp + kd ) - solverConstraint.m_friction = cp.m_combinedFriction; + btScalar cfm = infoGlobal.m_globalCfm; + btScalar erp = infoGlobal.m_erp2; + if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)) + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) + cfm = cp.m_contactCFM; + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP) + erp = cp.m_contactERP; + } + else + { + if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING) + { + btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1); + if (denom < SIMD_EPSILON) + { + denom = SIMD_EPSILON; + } + cfm = btScalar(1) / denom; + erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom; + } + } - restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); - if (restitution <= btScalar(0.)) - { - restitution = 0.f; - }; - } + cfm *= invTimeStep; + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0); + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0); - ///warm starting (or zero if disabled) - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; - if (rb0) - bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); - if (rb1) - bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); - } else - { - solverConstraint.m_appliedImpulse = 0.f; - } + { +#ifdef COMPUTE_IMPULSE_DENOM + btScalar denom0 = rb0->computeImpulseDenominator(pos1, cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2, cp.m_normalWorldOnB); +#else + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (rb0) + { + vec = (solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } + if (rb1) + { + vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); + } +#endif //COMPUTE_IMPULSE_DENOM - solverConstraint.m_appliedPushImpulse = 0.f; + btScalar denom = relaxation / (denom0 + denom1 + cfm); + solverConstraint.m_jacDiagABInv = denom; + } - { + if (rb0) + { + solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + } + else + { + solverConstraint.m_contactNormal1.setZero(); + solverConstraint.m_relpos1CrossNormal.setZero(); + } + if (rb1) + { + solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + } + else + { + solverConstraint.m_contactNormal2.setZero(); + solverConstraint.m_relpos2CrossNormal.setZero(); + } - btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0); - btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0); - btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: btVector3(0,0,0); - btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0); + btScalar restitution = 0.f; + btScalar penetration = cp.getDistance() + infoGlobal.m_linearSlop; + { + btVector3 vel1, vel2; - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) - + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) - + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB); - btScalar rel_vel = vel1Dotn+vel2Dotn; + vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0, 0, 0); + vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0, 0, 0); - btScalar positionalError = 0.f; - btScalar velocityError = restitution - rel_vel;// * damping; + // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + btVector3 vel = vel1 - vel2; + btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); + solverConstraint.m_friction = cp.m_combinedFriction; - - if (penetration>0) - { - positionalError = 0; + restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold); + if (restitution <= btScalar(0.)) + { + restitution = 0.f; + }; + } - velocityError -= penetration *invTimeStep; - } else - { - positionalError = -penetration * erp*invTimeStep; + ///warm starting (or zero if disabled) + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; + if (rb0) + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse); + if (rb1) + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() , -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse); + } + else + { + solverConstraint.m_appliedImpulse = 0.f; + } - } + solverConstraint.m_appliedPushImpulse = 0.f; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + { + btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse : btVector3(0, 0, 0); + btVector3 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse : btVector3(0, 0, 0); + btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse : btVector3(0, 0, 0); + btVector3 externalTorqueImpulseB = bodyB->m_originalBody ? bodyB->m_externalTorqueImpulse : btVector3(0, 0, 0); - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - //combine position and velocity into rhs - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;//-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhsPenetration = 0.f; + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity + externalTorqueImpulseA); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity + externalTorqueImpulseB); + btScalar rel_vel = vel1Dotn + vel2Dotn; - } else - { - //split position and velocity into rhs and m_rhsPenetration - solverConstraint.m_rhs = velocityImpulse; - solverConstraint.m_rhsPenetration = penetrationImpulse; - } - solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; - solverConstraint.m_lowerLimit = 0; - solverConstraint.m_upperLimit = 1e10f; - } + btScalar positionalError = 0.f; + btScalar velocityError = restitution - rel_vel; // * damping; + if (penetration > 0) + { + positionalError = 0; + velocityError -= penetration * invTimeStep; + } + else + { + positionalError = -penetration * erp * invTimeStep; + } + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + //combine position and velocity into rhs + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; //-solverConstraint.m_contactNormal1.dot(bodyA->m_externalForce*bodyA->m_invMass-bodyB->m_externalForce/bodyB->m_invMass)*solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhsPenetration = 0.f; + } + else + { + //split position and velocity into rhs and m_rhsPenetration + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_rhsPenetration = penetrationImpulse; + } + solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } } - - -void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, - int solverBodyIdA, int solverBodyIdB, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse(btSolverConstraint& solverConstraint, + int solverBodyIdA, int solverBodyIdB, + btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { - btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; @@ -1058,10 +990,11 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver { frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; if (rb0) - bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1 * rb0->getInvMass() , frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse); if (rb1) - bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); - } else + bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2 * rb1->getInvMass() , -frictionConstraint1.m_angularComponentB, -(btScalar)frictionConstraint1.m_appliedImpulse); + } + else { frictionConstraint1.m_appliedImpulse = 0.f; } @@ -1069,50 +1002,45 @@ void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolver if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex + 1]; if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; if (rb0) - bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1 * rb0->getInvMass(), frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse); if (rb1) - bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); - } else + bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2 * rb1->getInvMass(), -frictionConstraint2.m_angularComponentB, -(btScalar)frictionConstraint2.m_appliedImpulse); + } + else { frictionConstraint2.m_appliedImpulse = 0.f; } } } - - - -void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal) { - btCollisionObject* colObj0=0,*colObj1=0; + btCollisionObject *colObj0 = 0, *colObj1 = 0; colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); - int solverBodyIdA = getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep); - int solverBodyIdB = getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep); + int solverBodyIdA = getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep); -// btRigidBody* bodyA = btRigidBody::upcast(colObj0); -// btRigidBody* bodyB = btRigidBody::upcast(colObj1); + // btRigidBody* bodyA = btRigidBody::upcast(colObj0); + // btRigidBody* bodyB = btRigidBody::upcast(colObj1); btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; - - ///avoid collision response between two static objects if (!solverBodyA || (solverBodyA->m_invMass.fuzzyZero() && (!solverBodyB || solverBodyB->m_invMass.fuzzyZero()))) return; - int rollingFriction=1; - for (int j=0;j<manifold->getNumContacts();j++) + int rollingFriction = 1; + for (int j = 0; j < manifold->getNumContacts(); j++) { - btManifoldPoint& cp = manifold->getContactPoint(j); if (cp.getDistance() <= manifold->getContactProcessingThreshold()) @@ -1121,7 +1049,6 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 rel_pos2; btScalar relaxation; - int frictionIndex = m_tmpSolverContactConstraintPool.size(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); solverConstraint.m_solverBodyIdA = solverBodyIdA; @@ -1137,43 +1064,38 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m btVector3 vel1; btVector3 vel2; - - solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1); - solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 ); - btVector3 vel = vel1 - vel2; + solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1); + solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2); + + btVector3 vel = vel1 - vel2; btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); - - - /////setup the friction constraints solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + if ((cp.m_combinedRollingFriction > 0.f) && (rollingFriction > 0)) { - { - addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - btVector3 axis0,axis1; - btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + addTorsionalFrictionConstraint(cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, cp.m_combinedSpinningFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + btVector3 axis0, axis1; + btPlaneSpace1(cp.m_normalWorldOnB, axis0, axis1); axis0.normalize(); axis1.normalize(); - - applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (axis0.length()>0.001) - addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp, - cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - if (axis1.length()>0.001) - addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp, - cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + applyAnisotropicFriction(colObj0, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1, axis0, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1, axis1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (axis0.length() > 0.001) + addTorsionalFrictionConstraint(axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + if (axis1.length() > 0.001) + addTorsionalFrictionConstraint(axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, + cp.m_combinedRollingFriction, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); } } @@ -1188,106 +1110,97 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. /// ///The user can manually override the friction directions for certain contacts using a contact callback, - ///and set the cp.m_lateralFrictionInitialized to true + ///and use contactPoint.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// - - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) + + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) { cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { - cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal); + cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); - if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); - cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); + cp.m_lateralFrictionDir2.normalize(); //?? + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); } - - } else + } + else { - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); } - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { - cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; + cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } - - } else + } + else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); + addFrictionConstraint(cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); - + addFrictionConstraint(cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); } - setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); - - - - + setFrictionConstraintImpulse(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } } } -void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { int i; btPersistentManifold* manifold = 0; -// btCollisionObject* colObj0=0,*colObj1=0; - + // btCollisionObject* colObj0=0,*colObj1=0; - for (i=0;i<numManifolds;i++) + for (i = 0; i < numManifolds; i++) { manifold = manifoldPtr[i]; - convertContact(manifold,infoGlobal); + convertContact(manifold, infoGlobal); } } - void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* currentConstraintRow, - btTypedConstraint* constraint, - const btTypedConstraint::btConstraintInfo1& info1, - int solverBodyIdA, - int solverBodyIdB, - const btContactSolverInfo& infoGlobal - ) + btTypedConstraint* constraint, + const btTypedConstraint::btConstraintInfo1& info1, + int solverBodyIdA, + int solverBodyIdB, + const btContactSolverInfo& infoGlobal) { const btRigidBody& rbA = constraint->getRigidBodyA(); const btRigidBody& rbB = constraint->getRigidBodyB(); - const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; - const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + const btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + const btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; - if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations) m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - for (int j=0;j<info1.m_numConstraintRows;j++) + for (int j = 0; j < info1.m_numConstraintRows; j++) { - memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); + memset(¤tConstraintRow[j], 0, sizeof(btSolverConstraint)); currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY; currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; currentConstraintRow[j].m_appliedImpulse = 0.f; @@ -1297,15 +1210,15 @@ void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* curre currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; } - // these vectors are already cleared in initSolverBody, no need to redundantly clear again - btAssert(bodyAPtr->getDeltaLinearVelocity().isZero()); - btAssert(bodyAPtr->getDeltaAngularVelocity().isZero()); - btAssert(bodyAPtr->getPushVelocity().isZero()); - btAssert(bodyAPtr->getTurnVelocity().isZero()); - btAssert(bodyBPtr->getDeltaLinearVelocity().isZero()); - btAssert(bodyBPtr->getDeltaAngularVelocity().isZero()); - btAssert(bodyBPtr->getPushVelocity().isZero()); - btAssert(bodyBPtr->getTurnVelocity().isZero()); + // these vectors are already cleared in initSolverBody, no need to redundantly clear again + btAssert(bodyAPtr->getDeltaLinearVelocity().isZero()); + btAssert(bodyAPtr->getDeltaAngularVelocity().isZero()); + btAssert(bodyAPtr->getPushVelocity().isZero()); + btAssert(bodyAPtr->getTurnVelocity().isZero()); + btAssert(bodyBPtr->getDeltaLinearVelocity().isZero()); + btAssert(bodyBPtr->getDeltaAngularVelocity().isZero()); + btAssert(bodyBPtr->getPushVelocity().isZero()); + btAssert(bodyBPtr->getTurnVelocity().isZero()); //bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); //bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); //bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); @@ -1315,17 +1228,16 @@ void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* curre //bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); //bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); - btTypedConstraint::btConstraintInfo2 info2; - info2.fps = 1.f/infoGlobal.m_timeStep; + info2.fps = 1.f / infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1; info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; - info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this + info2.rowskip = sizeof(btSolverConstraint) / sizeof(btScalar); //check this ///the size of btSolverConstraint needs be a multiple of btScalar - btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); + btAssert(info2.rowskip * sizeof(btScalar) == sizeof(btSolverConstraint)); info2.m_constraintError = ¤tConstraintRow->m_rhs; currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; info2.m_damping = infoGlobal.m_damping; @@ -1336,16 +1248,16 @@ void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* curre constraint->getInfo2(&info2); ///finalize the constraint setup - for (int j=0;j<info1.m_numConstraintRows;j++) + for (int j = 0; j < info1.m_numConstraintRows; j++) { btSolverConstraint& solverConstraint = currentConstraintRow[j]; - if (solverConstraint.m_upperLimit>=constraint->getBreakingImpulseThreshold()) + if (solverConstraint.m_upperLimit >= constraint->getBreakingImpulseThreshold()) { solverConstraint.m_upperLimit = constraint->getBreakingImpulseThreshold(); } - if (solverConstraint.m_lowerLimit<=-constraint->getBreakingImpulseThreshold()) + if (solverConstraint.m_lowerLimit <= -constraint->getBreakingImpulseThreshold()) { solverConstraint.m_lowerLimit = -constraint->getBreakingImpulseThreshold(); } @@ -1354,18 +1266,18 @@ void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* curre { const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal; - solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor(); + solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld() * ftorqueAxis1 * constraint->getRigidBodyA().getAngularFactor(); } { const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal; - solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor(); + solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld() * ftorqueAxis2 * constraint->getRigidBodyB().getAngularFactor(); } { - btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass(); - btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; - btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal? - btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; + btVector3 iMJlA = solverConstraint.m_contactNormal1 * rbA.getInvMass(); + btVector3 iMJaA = rbA.getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal; + btVector3 iMJlB = solverConstraint.m_contactNormal2 * rbB.getInvMass(); //sign of normal? + btVector3 iMJaB = rbB.getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal; btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); @@ -1373,41 +1285,38 @@ void btSequentialImpulseConstraintSolver::convertJoint(btSolverConstraint* curre sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); btScalar fsum = btFabs(sum); btAssert(fsum > SIMD_EPSILON); - btScalar sorRelaxation = 1.f;//todo: get from globalInfo? - solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f; + btScalar sorRelaxation = 1.f; //todo: get from globalInfo? + solverConstraint.m_jacDiagABInv = fsum > SIMD_EPSILON ? sorRelaxation / sum : 0.f; } { btScalar rel_vel; - btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0,0,0); - btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0); + btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : btVector3(0, 0, 0); + btVector3 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0, 0, 0); - btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0,0,0); - btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0); + btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : btVector3(0, 0, 0); + btVector3 externalTorqueImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalTorqueImpulse : btVector3(0, 0, 0); - btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) - + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity() + externalForceImpulseA) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity() + externalTorqueImpulseA); - btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) - + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity() + externalForceImpulseB) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity() + externalTorqueImpulseB); - rel_vel = vel1Dotn+vel2Dotn; + rel_vel = vel1Dotn + vel2Dotn; btScalar restitution = 0.f; - btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 - btScalar velocityError = restitution - rel_vel * info2.m_damping; - btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; - btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; - solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; + btScalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2 + btScalar velocityError = restitution - rel_vel * info2.m_damping; + btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv; + btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv; + solverConstraint.m_rhs = penetrationImpulse + velocityImpulse; solverConstraint.m_appliedImpulse = 0.f; } } } - -void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal) +void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) { - BT_PROFILE("convertJoints"); - for (int j=0;j<numConstraints;j++) + BT_PROFILE("convertJoints"); + for (int j = 0; j < numConstraints; j++) { btTypedConstraint* constraint = constraints[j]; constraint->buildJacobian(); @@ -1418,7 +1327,7 @@ void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** cons m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); //calculate the total number of contraint rows - for (int i=0;i<numConstraints;i++) + for (int i = 0; i < numConstraints; i++) { btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; btJointFeedback* fb = constraints[i]->getJointFeedback(); @@ -1433,7 +1342,8 @@ void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** cons if (constraints[i]->isEnabled()) { constraints[i]->getInfo1(&info1); - } else + } + else { info1.m_numConstraintRows = 0; info1.nub = 0; @@ -1442,110 +1352,105 @@ void btSequentialImpulseConstraintSolver::convertJoints(btTypedConstraint** cons } m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); - ///setup the btSolverConstraints int currentRow = 0; - for (int i=0;i<numConstraints;i++) + for (int i = 0; i < numConstraints; i++) { const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; if (info1.m_numConstraintRows) { - btAssert(currentRow<totalNumRows); + btAssert(currentRow < totalNumRows); btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; btTypedConstraint* constraint = constraints[i]; btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); - int solverBodyIdA = getOrInitSolverBody(rbA,infoGlobal.m_timeStep); - int solverBodyIdB = getOrInitSolverBody(rbB,infoGlobal.m_timeStep); + int solverBodyIdA = getOrInitSolverBody(rbA, infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBody(rbB, infoGlobal.m_timeStep); - convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); - } - currentRow+=info1.m_numConstraintRows; + convertJoint(currentConstraintRow, constraint, info1, solverBodyIdA, solverBodyIdB, infoGlobal); + } + currentRow += info1.m_numConstraintRows; } } - void btSequentialImpulseConstraintSolver::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { - BT_PROFILE("convertBodies"); + BT_PROFILE("convertBodies"); for (int i = 0; i < numBodies; i++) { bodies[i]->setCompanionId(-1); } #if BT_THREADSAFE - m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 ); -#endif // BT_THREADSAFE + m_kinematicBodyUniqueIdToSolverBodyTable.resize(0); +#endif // BT_THREADSAFE - m_tmpSolverBodyPool.reserve(numBodies+1); + m_tmpSolverBodyPool.reserve(numBodies + 1); m_tmpSolverBodyPool.resize(0); //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - //initSolverBody(&fixedBody,0); + //initSolverBody(&fixedBody,0); - for (int i=0;i<numBodies;i++) + for (int i = 0; i < numBodies; i++) { - int bodyId = getOrInitSolverBody(*bodies[i],infoGlobal.m_timeStep); + int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); btRigidBody* body = btRigidBody::upcast(bodies[i]); if (body && body->getInvMass()) { btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - btVector3 gyroForce (0,0,0); - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) + btVector3 gyroForce(0, 0, 0); + if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) { gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); - solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep; } - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) + if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) { gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); solverBody.m_externalTorqueImpulse += gyroForce; } - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) + if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) { gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); solverBody.m_externalTorqueImpulse += gyroForce; - } } } } - -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { m_fixedBodyId = -1; 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; - } + // 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 - //make sure that dynamic bodies exist for all (enabled) constraints - for (int i=0;i<numConstraints;i++) + //make sure that dynamic bodies exist for all (enabled) constraints + for (int i = 0; i < numConstraints; i++) { btTypedConstraint* constraint = constraints[i]; if (constraint->isEnabled()) { if (!constraint->getRigidBodyA().isStaticOrKinematicObject()) { - bool found=false; - for (int b=0;b<numBodies;b++) + bool found = false; + for (int b = 0; b < numBodies; b++) { - - if (&constraint->getRigidBodyA()==bodies[b]) + if (&constraint->getRigidBodyA() == bodies[b]) { found = true; break; @@ -1555,10 +1460,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } if (!constraint->getRigidBodyB().isStaticOrKinematicObject()) { - bool found=false; - for (int b=0;b<numBodies;b++) + bool found = false; + for (int b = 0; b < numBodies; b++) { - if (&constraint->getRigidBodyB()==bodies[b]) + if (&constraint->getRigidBodyB() == bodies[b]) { found = true; break; @@ -1568,50 +1473,46 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } } - //make sure that dynamic bodies exist for all contact manifolds - for (int i=0;i<numManifolds;i++) - { - if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject()) - { - bool found=false; - for (int b=0;b<numBodies;b++) - { - - if (manifoldPtr[i]->getBody0()==bodies[b]) - { - found = true; - break; - } - } - btAssert(found); - } - if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject()) - { - bool found=false; - for (int b=0;b<numBodies;b++) - { - if (manifoldPtr[i]->getBody1()==bodies[b]) - { - found = true; - break; - } - } - btAssert(found); - } - } -#endif //BT_ADDITIONAL_DEBUG - + //make sure that dynamic bodies exist for all contact manifolds + for (int i = 0; i < numManifolds; i++) + { + if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject()) + { + bool found = false; + for (int b = 0; b < numBodies; b++) + { + if (manifoldPtr[i]->getBody0() == bodies[b]) + { + found = true; + break; + } + } + btAssert(found); + } + if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject()) + { + bool found = false; + for (int b = 0; b < numBodies; b++) + { + if (manifoldPtr[i]->getBody1() == bodies[b]) + { + found = true; + break; + } + } + btAssert(found); + } + } +#endif //BT_ADDITIONAL_DEBUG //convert all bodies - convertBodies(bodies, numBodies, infoGlobal); - - convertJoints(constraints, numConstraints, infoGlobal); + convertBodies(bodies, numBodies, infoGlobal); - convertContacts(manifoldPtr,numManifolds,infoGlobal); + convertJoints(constraints, numConstraints, infoGlobal); + convertContacts(manifoldPtr, numManifolds, infoGlobal); -// btContactSolverInfo info = infoGlobal; - + // btContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); @@ -1620,35 +1521,33 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2); + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2); else m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); { int i; - for (i=0;i<numNonContactPool;i++) + for (i = 0; i < numNonContactPool; i++) { m_orderNonContactConstraintPool[i] = i; } - for (i=0;i<numConstraintPool;i++) + for (i = 0; i < numConstraintPool; i++) { m_orderTmpConstraintPool[i] = i; } - for (i=0;i<numFrictionPool;i++) + for (i = 0; i < numFrictionPool; i++) { m_orderFrictionConstraintPool[i] = i; } } return 0.f; - } - -btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/) +btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */, int /*numBodies*/, btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* /*debugDrawer*/) { - BT_PROFILE("solveSingleIteration"); + BT_PROFILE("solveSingleIteration"); btScalar leastSquaresResidual = 0.f; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); @@ -1657,29 +1556,31 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { - if (1) // uncomment this for a bit less random ((iteration & 7) == 0) + if (1) // uncomment this for a bit less random ((iteration & 7) == 0) { - - for (int j=0; j<numNonContactPool; ++j) { + for (int j = 0; j < numNonContactPool; ++j) + { int tmp = m_orderNonContactConstraintPool[j]; - int swapi = btRandInt2(j+1); + int swapi = btRandInt2(j + 1); m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; m_orderNonContactConstraintPool[swapi] = tmp; } //contact/friction constraints are not solved more than - if (iteration< infoGlobal.m_numIterations) + if (iteration < infoGlobal.m_numIterations) { - for (int j=0; j<numConstraintPool; ++j) { + for (int j = 0; j < numConstraintPool; ++j) + { int tmp = m_orderTmpConstraintPool[j]; - int swapi = btRandInt2(j+1); + int swapi = btRandInt2(j + 1); m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; m_orderTmpConstraintPool[swapi] = tmp; } - for (int j=0; j<numFrictionPool; ++j) { + for (int j = 0; j < numFrictionPool; ++j) + { int tmp = m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2(j+1); + int swapi = btRandInt2(j + 1); m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; m_orderFrictionConstraintPool[swapi] = tmp; } @@ -1687,253 +1588,240 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration } } - ///solve all joint constraints - for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) + ///solve all joint constraints + for (int j = 0; j < m_tmpSolverNonContactConstraintPool.size(); j++) + { + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; - if (iteration < constraint.m_overrideNumSolverIterations) - { - btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint); - leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); - } + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA], m_tmpSolverBodyPool[constraint.m_solverBodyIdB], constraint); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } + } - if (iteration< infoGlobal.m_numIterations) + if (iteration < infoGlobal.m_numIterations) + { + for (int j = 0; j < numConstraints; j++) { - for (int j=0;j<numConstraints;j++) + if (constraints[j]->isEnabled()) { - 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); - } + 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 + 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; - ///solve all contact constraints - if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) + for (int c = 0; c < numPoolConstraints; c++) { - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1; + btScalar totalImpulse = 0; - 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 = btMax(leastSquaresResidual, residual * residual); + totalImpulse = solveManifold.m_appliedImpulse; + } + bool applyFriction = true; + if (applyFriction) + { { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]]; - btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]]; - totalImpulse = solveManifold.m_appliedImpulse; - } - bool applyFriction = true; - if (applyFriction) - { + if (totalImpulse > btScalar(0)) { + solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - 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 = btMax(leastSquaresResidual, residual*residual); - } + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } + } - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) - { - - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; + 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; + 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 = btMax(leastSquaresResidual, residual*residual); - } + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(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 = btMax(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 = btMax(leastSquaresResidual, residual * residual); + } + ///solve all friction constraints - ///solve all friction constraints + 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; - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) + if (totalImpulse > btScalar(0)) { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; - 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 = btMax(leastSquaresResidual, residual*residual); - } + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(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)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; + if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - 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)) - { - 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 = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); - leastSquaresResidual = btMax(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 = btMax(leastSquaresResidual, residual * residual); + } } + } return leastSquaresResidual; } - -void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); int iteration; if (infoGlobal.m_splitImpulse) { { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { - btScalar leastSquaresResidual =0.f; + btScalar leastSquaresResidual = 0.f; { int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int j; - for (j=0;j<numPoolConstraints;j++) + for (j = 0; j < numPoolConstraints; j++) { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); - leastSquaresResidual = btMax(leastSquaresResidual, residual*residual); + btScalar residual = resolveSplitPenetrationImpulse(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual = btMax(leastSquaresResidual, residual * residual); } } - if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1)) + if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration); + printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration); #endif break; } } - } + } } } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlyIterations"); { ///this is a special step to resolve penetrations (just for contacts) - solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + for (int iteration = 0; iteration < maxIterations; iteration++) //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { - m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); + m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1))) + if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration); + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif break; } } - } return 0.f; } void btSequentialImpulseConstraintSolver::writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - for (int j=iBegin; j<iEnd; j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; - btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint; - btAssert(pt); - pt->m_appliedImpulse = solveManifold.m_appliedImpulse; + for (int j = iBegin; j < iEnd; j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j]; + btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint; + btAssert(pt); + pt->m_appliedImpulse = solveManifold.m_appliedImpulse; // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - // printf("pt->m_appliedImpulseLateral1 = %f\n", f); - pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; - } - //do a callback here? + // printf("pt->m_appliedImpulseLateral1 = %f\n", f); + pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex + 1].m_appliedImpulse; } + //do a callback here? + } } void btSequentialImpulseConstraintSolver::writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - for (int j=iBegin; j<iEnd; j++) + for (int j = iBegin; j < iEnd; j++) { const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; btJointFeedback* fb = constr->getJointFeedback(); if (fb) { - fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; - fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ - + fb->m_appliedForceBodyA += solverConstr.m_contactNormal1 * solverConstr.m_appliedImpulse * constr->getRigidBodyA().getLinearFactor() / infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += solverConstr.m_contactNormal2 * solverConstr.m_appliedImpulse * constr->getRigidBodyB().getLinearFactor() / infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * constr->getRigidBodyA().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal * constr->getRigidBodyB().getAngularFactor() * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep; /*RGM ???? */ } constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); - if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + if (btFabs(solverConstr.m_appliedImpulse) >= constr->getBreakingImpulseThreshold()) { constr->setEnabled(false); } } } - void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - for (int i=iBegin; i<iEnd; i++) + for (int i = iBegin; i < iEnd; i++) { btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; if (body) @@ -1944,11 +1832,11 @@ void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, m_tmpSolverBodyPool[i].writebackVelocity(); m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( - m_tmpSolverBodyPool[i].m_linearVelocity+ + m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_externalForceImpulse); m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( - m_tmpSolverBodyPool[i].m_angularVelocity+ + m_tmpSolverBodyPool[i].m_angularVelocity + m_tmpSolverBodyPool[i].m_externalTorqueImpulse); if (infoGlobal.m_splitImpulse) @@ -1959,17 +1847,17 @@ void btSequentialImpulseConstraintSolver::writeBackBodies(int iBegin, int iEnd, } } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveGroupCacheFriendlyFinish"); if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) { - writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal); + writeBackContacts(0, m_tmpSolverContactConstraintPool.size(), infoGlobal); } - writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal); - writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal); + writeBackJoints(0, m_tmpSolverNonContactConstraintPool.size(), infoGlobal); + writeBackBodies(0, m_tmpSolverBodyPool.size(), infoGlobal); m_tmpSolverContactConstraintPool.resizeNoInitialize(0); m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); @@ -1980,25 +1868,22 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo return 0.f; } - - /// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btDispatcher* /*dispatcher*/) +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btDispatcher* /*dispatcher*/) { - BT_PROFILE("solveGroup"); //you need to provide at least some bodies - solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); + solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer); + solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); return 0.f; } -void btSequentialImpulseConstraintSolver::reset() +void btSequentialImpulseConstraintSolver::reset() { m_btSeed2 = 0; } |