Page MenuHome
Differential D1739 Diff 5899 extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

/* /*
Bullet Continuous Collision Detection and Physics Library Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty. This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software. In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely, including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions: subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution. 3. This notice may not be removed or altered from any source distribution.
*/ */
//#define COMPUTE_IMPULSE_DENOM 1 //#define COMPUTE_IMPULSE_DENOM 1
//#define BT_ADDITIONAL_DEBUG //#define BT_ADDITIONAL_DEBUG
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
#include "btSequentialImpulseConstraintSolver.h" #include "btSequentialImpulseConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "LinearMath/btIDebugDraw.h" #include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btCpuFeatureUtility.h"
//#include "btJacobianEntry.h" //#include "btJacobianEntry.h"
#include "LinearMath/btMinMax.h" #include "LinearMath/btMinMax.h"
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
#include <new> #include <new>
#include "LinearMath/btStackAlloc.h" #include "LinearMath/btStackAlloc.h"
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
//#include "btSolverBody.h" //#include "btSolverBody.h"
//#include "btSolverConstraint.h" //#include "btSolverConstraint.h"
#include "LinearMath/btAlignedObjectArray.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" #include "BulletDynamics/Dynamics/btRigidBody.h"
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
:m_btSeed2(0) ///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{ {
btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if (sum > c.m_upperLimit)
{
deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
{
c.m_appliedImpulse = sum;
} }
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver() body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
return deltaImpulse;
}
static btSimdScalar gResolveSingleConstraintRowLowerLimit_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{ {
btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedImpulse = sum;
} }
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
return deltaImpulse;
}
#ifdef USE_SIMD #ifdef USE_SIMD
#include <emmintrin.h> #include <emmintrin.h>
#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e)) #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
{ {
__m128 result = _mm_mul_ps( vec0, vec1); __m128 result = _mm_mul_ps( vec0, vec1);
return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) ); return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
} }
#endif//USE_SIMD
#if defined (BT_ALLOW_SSE4)
#include <intrin.h>
#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))
#if USE_SSE4_DOT
#define DOT_PRODUCT(a, b) SSE4_DP(a, b)
#else
#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)
// -(a*b) + c
#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)
// -(a*b) + c
#define FMNADD(a, b, c) _mm_nmacc_ps(a, b, c)
#endif
#else // USE_FMA
// c + 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))
#endif
#endif
// Project Gauss Seidel or the equivalent Sequential Impulse // Project Gauss Seidel or the equivalent Sequential Impulse
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) static btSimdScalar gResolveSingleConstraintRowGeneric_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{ {
#ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __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))); 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,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128)); __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 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))); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess; btSimdScalar resultLowerLess, resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 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) ); c.m_appliedImpulse = _mm_or_ps(_mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum));
__m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); __m128 upperMinApplied = _mm_sub_ps(upperLimit1, cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); 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) ); 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,body1.internalGetInvMass().mVec128); __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128, body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse; __m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
return deltaImpulse; return deltaImpulse;
#else
return resolveSingleConstraintRowGeneric(body1,body2,c);
#endif
} }
// Project Gauss Seidel or the equivalent Sequential Impulse
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
// const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse; // Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
if (sum < c.m_lowerLimit) static btSimdScalar gResolveSingleConstraintRowGeneric_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if (sum > c.m_upperLimit)
{
deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
{ {
c.m_appliedImpulse = sum; #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, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.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);
body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
return deltaImpulse;
#else
return gResolveSingleConstraintRowGeneric_sse2(body1,body2,c);
#endif
} }
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
return deltaImpulse;
}
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse2(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
{ {
#ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __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))); 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,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128, body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128)); __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128, body2.internalGetDeltaAngularVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); 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))); deltaImpulse = _mm_sub_ps(deltaImpulse, _mm_mul_ps(deltaVel2Dotn, _mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); btSimdScalar sum = _mm_add_ps(cpAppliedImp, deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess; btSimdScalar resultLowerLess, resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); resultLowerLess = _mm_cmplt_ps(sum, lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); resultUpperLess = _mm_cmplt_ps(sum, upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); __m128 lowMinApplied = _mm_sub_ps(lowerLimit1, cpAppliedImp);
deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); 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) ); 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,body1.internalGetInvMass().mVec128); __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128);
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128); __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128);
__m128 impulseMagnitude = deltaImpulse; __m128 impulseMagnitude = deltaImpulse;
body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentA, impulseMagnitude));
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentA.mVec128, impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128, _mm_mul_ps(linearComponentB, impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128, _mm_mul_ps(c.m_angularComponentB.mVec128, impulseMagnitude));
return deltaImpulse; return deltaImpulse;
}
// Enhanced version of gResolveSingleConstraintRowGeneric_sse2 with SSE4.1 and FMA3
static btSimdScalar gResolveSingleConstraintRowLowerLimit_sse4_1_fma3(btSolverBody& body1, btSolverBody& body2, 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, body1.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos1CrossNormal.mVec128, body1.internalGetDeltaAngularVelocity().mVec128));
const __m128 deltaVel2Dotn = _mm_add_ps(DOT_PRODUCT(c.m_contactNormal2.mVec128, body2.internalGetDeltaLinearVelocity().mVec128), DOT_PRODUCT(c.m_relpos2CrossNormal.mVec128, body2.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);
body1.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal1.mVec128, body1.internalGetInvMass().mVec128), deltaImpulse, body1.internalGetDeltaLinearVelocity().mVec128);
body1.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentA.mVec128, deltaImpulse, body1.internalGetDeltaAngularVelocity().mVec128);
body2.internalGetDeltaLinearVelocity().mVec128 = FMADD(_mm_mul_ps(c.m_contactNormal2.mVec128, body2.internalGetInvMass().mVec128), deltaImpulse, body2.internalGetDeltaLinearVelocity().mVec128);
body2.internalGetDeltaAngularVelocity().mVec128 = FMADD(c.m_angularComponentB.mVec128, deltaImpulse, body2.internalGetDeltaAngularVelocity().mVec128);
return deltaImpulse;
#else #else
return resolveSingleConstraintRowLowerLimit(body1,body2,c); return gResolveSingleConstraintRowLowerLimit_sse2(body1,body2,c);
#endif #endif //BT_ALLOW_SSE4
} }
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) #endif //USE_SIMD
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{ {
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; #ifdef USE_SIMD
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); return m_resolveSingleConstraintRowGeneric(body1, body2, c);
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); #else
return resolveSingleConstraintRowGeneric(body1,body2,c);
#endif
}
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; // Project Gauss Seidel or the equivalent Sequential Impulse
deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
if (sum < c.m_lowerLimit)
{ {
deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse; return gResolveSingleConstraintRowGeneric_scalar_reference(body1, body2, c);
c.m_appliedImpulse = c.m_lowerLimit;
} }
else
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{ {
c.m_appliedImpulse = sum; #ifdef USE_SIMD
return m_resolveSingleConstraintRowLowerLimit(body1, body2, c);
#else
return resolveSingleConstraintRowLowerLimit(body1,body2,c);
#endif
} }
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
return deltaImpulse;
btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
return gResolveSingleConstraintRowLowerLimit_scalar_reference(body1,body2,c);
} }
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1, btSolverBody& body1,
btSolverBody& body2, btSolverBody& body2,
const btSolverConstraint& c) const btSolverConstraint& c)
{ {
▲ Show 20 Lines • Show All 52 Lines • ▼ Show 20 Lines#ifdef USE_SIMD
body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
#else #else
resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
#endif #endif
} }
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
: m_resolveSingleConstraintRowGeneric(gResolveSingleConstraintRowGeneric_scalar_reference),
m_resolveSingleConstraintRowLowerLimit(gResolveSingleConstraintRowLowerLimit_scalar_reference),
m_btSeed2(0)
{
#ifdef USE_SIMD
m_resolveSingleConstraintRowGeneric = gResolveSingleConstraintRowGeneric_sse2;
m_resolveSingleConstraintRowLowerLimit=gResolveSingleConstraintRowLowerLimit_sse2;
#endif //USE_SIMD
#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
}
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
{
}
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;
}
#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
unsigned long btSequentialImpulseConstraintSolver::btRand2() unsigned long btSequentialImpulseConstraintSolver::btRand2()
{ {
m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff; m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
return m_btSeed2; return m_btSeed2;
} }
▲ Show 20 Lines • Show All 44 Lines • ▼ Show 20 Lines if (rb)
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_originalBody = rb;
solverBody->m_angularFactor = rb->getAngularFactor(); solverBody->m_angularFactor = rb->getAngularFactor();
solverBody->m_linearFactor = rb->getLinearFactor(); solverBody->m_linearFactor = rb->getLinearFactor();
solverBody->m_linearVelocity = rb->getLinearVelocity(); solverBody->m_linearVelocity = rb->getLinearVelocity();
solverBody->m_angularVelocity = rb->getAngularVelocity(); solverBody->m_angularVelocity = rb->getAngularVelocity();
solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep; solverBody->m_externalForceImpulse = rb->getTotalForce()*rb->getInvMass()*timeStep;
solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ; solverBody->m_externalTorqueImpulse = rb->getTotalTorque()*rb->getInvInertiaTensorWorld()*timeStep ;
} else } else
{ {
solverBody->m_worldTransform.setIdentity(); solverBody->m_worldTransform.setIdentity();
solverBody->internalSetInvMass(btVector3(0,0,0)); solverBody->internalSetInvMass(btVector3(0,0,0));
solverBody->m_originalBody = 0; solverBody->m_originalBody = 0;
solverBody->m_angularFactor.setValue(1,1,1); solverBody->m_angularFactor.setValue(1,1,1);
solverBody->m_linearFactor.setValue(1,1,1); solverBody->m_linearFactor.setValue(1,1,1);
solverBody->m_linearVelocity.setValue(0,0,0); solverBody->m_linearVelocity.setValue(0,0,0);
Show All 15 LinesbtScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
btScalar rest = restitution * -rel_vel; btScalar rest = restitution * -rel_vel;
return rest; 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)) if (colObj && colObj->hasAnisotropicFriction(frictionMode))
{ {
// transform to local coordinates // transform to local coordinates
btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
const btVector3& friction_scaling = colObj->getAnisotropicFriction(); const btVector3& friction_scaling = colObj->getAnisotropicFriction();
//apply anisotropic friction //apply anisotropic friction
loc_lateral *= friction_scaling; loc_lateral *= friction_scaling;
// ... and transform it back to global coordinates // ... and transform it back to global coordinates
frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; 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, 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, btScalar desiredVelocity, btScalar cfmSlip)
{ {
btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_solverBodyIdB = solverBodyIdB;
▲ Show 20 Lines • Show All 44 Lines • ▼ Show 20 Lines if (body1)
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = body1->getInvMass() + normalAxis.dot(vec); denom1 = body1->getInvMass() + normalAxis.dot(vec);
} }
btScalar denom = relaxation/(denom0+denom1); btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom; solverConstraint.m_jacDiagABInv = denom;
} }
{ {
btScalar rel_vel; btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse: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)); + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + solverConstraint.m_relpos2CrossNormal.dot(body1?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; btScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f; solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_cfm = cfmSlip; solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = 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, 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, btScalar desiredVelocity, btScalar cfmSlip)
{ {
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex; solverConstraint.m_frictionIndex = frictionIndex;
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
return solverConstraint; return solverConstraint;
} }
void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity, btScalar cfmSlip) btScalar desiredVelocity, btScalar cfmSlip)
{ {
btVector3 normalAxis(0,0,0); btVector3 normalAxis(0,0,0);
solverConstraint.m_contactNormal1 = normalAxis; solverConstraint.m_contactNormal1 = normalAxis;
solverConstraint.m_contactNormal2 = -normalAxis; solverConstraint.m_contactNormal2 = -normalAxis;
Show All 29 Lines solverConstraint.m_appliedPushImpulse = 0.f;
btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
btScalar sum = 0; btScalar sum = 0;
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
} }
{ {
btScalar rel_vel; btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity+solverBodyA.m_externalForceImpulse: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)); + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0)) btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity+solverBodyB.m_externalForceImpulse:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + solverConstraint.m_relpos2CrossNormal.dot(body1?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 velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip; solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction; solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction; solverConstraint.m_upperLimit = solverConstraint.m_friction;
} }
} }
btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(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, btScalar desiredVelocity, btScalar cfmSlip) btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(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, btScalar desiredVelocity, btScalar cfmSlip)
{ {
btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex; solverConstraint.m_frictionIndex = frictionIndex;
setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
return solverConstraint; return solverConstraint;
} }
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep) int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body,btScalar timeStep)
{ {
Show All 11 Lines if (body.getCompanionId() >= 0)
if (rb && (rb->getInvMass() || rb->isKinematicObject())) if (rb && (rb->getInvMass() || rb->isKinematicObject()))
{ {
solverBodyIdA = m_tmpSolverBodyPool.size(); solverBodyIdA = m_tmpSolverBodyPool.size();
btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
initSolverBody(&solverBody,&body,timeStep); initSolverBody(&solverBody,&body,timeStep);
body.setCompanionId(solverBodyIdA); body.setCompanionId(solverBodyIdA);
} else } else
{ {
if (m_fixedBodyId<0) if (m_fixedBodyId<0)
{ {
m_fixedBodyId = m_tmpSolverBodyPool.size(); m_fixedBodyId = m_tmpSolverBodyPool.size();
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
initSolverBody(&fixedBody,0,timeStep); initSolverBody(&fixedBody,0,timeStep);
} }
return m_fixedBodyId; 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; return solverBodyIdA;
} }
#include <stdio.h> #include <stdio.h>
void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
btScalar& relaxation, btScalar& relaxation,
const btVector3& rel_pos1, const btVector3& rel_pos2) const btVector3& rel_pos1, const btVector3& rel_pos2)
{ {
const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB(); // const btVector3& pos2 = cp.getPositionWorldOnB();
btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody* rb0 = bodyA->m_originalBody; btRigidBody* rb0 = bodyA->m_originalBody;
btRigidBody* rb1 = bodyB->m_originalBody; btRigidBody* rb1 = bodyB->m_originalBody;
// btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
// btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); //rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f; relaxation = 1.f;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
{ {
#ifdef COMPUTE_IMPULSE_DENOM #ifdef COMPUTE_IMPULSE_DENOM
btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else #else
btVector3 vec; btVector3 vec;
btScalar denom0 = 0.f; btScalar denom0 = 0.f;
btScalar denom1 = 0.f; btScalar denom1 = 0.f;
if (rb0) if (rb0)
{ {
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec); denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
} }
if (rb1) if (rb1)
{ {
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec); denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
} }
#endif //COMPUTE_IMPULSE_DENOM #endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1); btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom; solverConstraint.m_jacDiagABInv = denom;
} }
if (rb0) if (rb0)
{ {
solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB;
Show All 21 Lines#endif //COMPUTE_IMPULSE_DENOM
vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
// btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
btVector3 vel = vel1 - vel2; btVector3 vel = vel1 - vel2;
btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_friction = cp.m_combinedFriction;
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= btScalar(0.)) if (restitution <= btScalar(0.))
{ {
restitution = 0.f; restitution = 0.f;
}; };
} }
Show All 13 Lines // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
solverConstraint.m_appliedPushImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f;
{ {
btVector3 externalForceImpulseA = bodyA->m_originalBody ? bodyA->m_externalForceImpulse: btVector3(0,0,0); 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 externalTorqueImpulseA = bodyA->m_originalBody ? bodyA->m_externalTorqueImpulse: btVector3(0,0,0);
btVector3 externalForceImpulseB = bodyB->m_originalBody ? bodyB->m_externalForceImpulse: 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); btVector3 externalTorqueImpulseB = bodyB->m_originalBody ?bodyB->m_externalTorqueImpulse : btVector3(0,0,0);
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA) btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(bodyA->m_linearVelocity+externalForceImpulseA)
+ solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA); + solverConstraint.m_relpos1CrossNormal.dot(bodyA->m_angularVelocity+externalTorqueImpulseA);
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB) btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(bodyB->m_linearVelocity+externalForceImpulseB)
+ solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB); + solverConstraint.m_relpos2CrossNormal.dot(bodyB->m_angularVelocity+externalTorqueImpulseB);
btScalar rel_vel = vel1Dotn+vel2Dotn; btScalar rel_vel = vel1Dotn+vel2Dotn;
btScalar positionalError = 0.f; btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping; btScalar velocityError = restitution - rel_vel;// * damping;
btScalar erp = infoGlobal.m_erp2; btScalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{ {
erp = infoGlobal.m_erp; erp = infoGlobal.m_erp;
} }
if (penetration>0) if (penetration>0)
Show All 28 Lines
} }
void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
{ {
btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody* rb0 = bodyA->m_originalBody; btRigidBody* rb0 = bodyA->m_originalBody;
▲ Show 20 Lines • Show All 62 Lines • ▼ Show 20 Lines for (int j=0;j<manifold->getNumContacts();j++)
btManifoldPoint& cp = manifold->getContactPoint(j); btManifoldPoint& cp = manifold->getContactPoint(j);
if (cp.getDistance() <= manifold->getContactProcessingThreshold()) if (cp.getDistance() <= manifold->getContactProcessingThreshold())
{ {
btVector3 rel_pos1; btVector3 rel_pos1;
btVector3 rel_pos2; btVector3 rel_pos2;
btScalar relaxation; btScalar relaxation;
int frictionIndex = m_tmpSolverContactConstraintPool.size(); int frictionIndex = m_tmpSolverContactConstraintPool.size();
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
btRigidBody* rb0 = btRigidBody::upcast(colObj0); btRigidBody* rb0 = btRigidBody::upcast(colObj0);
btRigidBody* rb1 = btRigidBody::upcast(colObj1); btRigidBody* rb1 = btRigidBody::upcast(colObj1);
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_originalContactPoint = &cp; solverConstraint.m_originalContactPoint = &cp;
const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos1 = cp.getPositionWorldOnA();
const btVector3& pos2 = cp.getPositionWorldOnB(); const btVector3& pos2 = cp.getPositionWorldOnB();
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); btVector3 vel1;// = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); btVector3 vel2;// = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1); solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1,vel1);
solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 ); solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2,vel2 );
btVector3 vel = vel1 - vel2; btVector3 vel = vel1 - vel2;
btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
// const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos1 = cp.getPositionWorldOnA();
// const btVector3& pos2 = cp.getPositionWorldOnB(); // const btVector3& pos2 = cp.getPositionWorldOnB();
/////setup the friction constraints /////setup the friction constraints
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
Show All 24 Lines// const btVector3& pos2 = cp.getPositionWorldOnB();
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (axis0.length()>0.001) if (axis0.length()>0.001)
addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (axis1.length()>0.001) if (axis1.length()>0.001)
addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
} }
} }
///Bullet has several options to set the friction directions ///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically very frame ///By default, each contact has only a single friction direction that is recomputed automatically very frame
///based on the relative linear velocity. ///based on the relative linear velocity.
///If the relative velocity it zero, it will automatically compute a friction direction. ///If the relative velocity it zero, it will automatically compute a friction direction.
///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
/// ///
///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. ///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, ///The user can manually override the friction directions for certain contacts using a contact callback,
///and set the cp.m_lateralFrictionInitialized to true ///and set the cp.m_lateralFrictionInitialized to true
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///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 ///this will give a conveyor belt effect
/// ///
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
{ {
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
Show All 39 Lines// const btVector3& pos2 = cp.getPositionWorldOnB();
{ {
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
} }
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; int i;
Show All 23 Lines for (int i=0;i<numConstraints;i++)
btTypedConstraint* constraint = constraints[i]; btTypedConstraint* constraint = constraints[i];
if (constraint->isEnabled()) if (constraint->isEnabled())
{ {
if (!constraint->getRigidBodyA().isStaticOrKinematicObject()) if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
{ {
bool found=false; bool found=false;
for (int b=0;b<numBodies;b++) for (int b=0;b<numBodies;b++)
{ {
if (&constraint->getRigidBodyA()==bodies[b]) if (&constraint->getRigidBodyA()==bodies[b])
{ {
found = true; found = true;
break; break;
} }
} }
btAssert(found); btAssert(found);
} }
Show All 15 Lines for (int i=0;i<numConstraints;i++)
//make sure that dynamic bodies exist for all contact manifolds //make sure that dynamic bodies exist for all contact manifolds
for (int i=0;i<numManifolds;i++) for (int i=0;i<numManifolds;i++)
{ {
if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject()) if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
{ {
bool found=false; bool found=false;
for (int b=0;b<numBodies;b++) for (int b=0;b<numBodies;b++)
{ {
if (manifoldPtr[i]->getBody0()==bodies[b]) if (manifoldPtr[i]->getBody0()==bodies[b])
{ {
found = true; found = true;
break; break;
} }
} }
btAssert(found); btAssert(found);
} }
if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject()) if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
{ {
bool found=false; bool found=false;
for (int b=0;b<numBodies;b++) for (int b=0;b<numBodies;b++)
{ {
if (manifoldPtr[i]->getBody1()==bodies[b]) if (manifoldPtr[i]->getBody1()==bodies[b])
{ {
found = true; found = true;
break; break;
} }
} }
btAssert(found); btAssert(found);
} }
} }
#endif //BT_ADDITIONAL_DEBUG #endif //BT_ADDITIONAL_DEBUG
for (int i = 0; i < numBodies; i++) for (int i = 0; i < numBodies; i++)
{ {
bodies[i]->setCompanionId(-1); bodies[i]->setCompanionId(-1);
} }
m_tmpSolverBodyPool.reserve(numBodies+1); m_tmpSolverBodyPool.reserve(numBodies+1);
m_tmpSolverBodyPool.resize(0); m_tmpSolverBodyPool.resize(0);
//btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); //btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
//initSolverBody(&fixedBody,0); //initSolverBody(&fixedBody,0);
//convert all bodies //convert all bodies
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]); btRigidBody* body = btRigidBody::upcast(bodies[i]);
if (body && body->getInvMass()) if (body && body->getInvMass())
{ {
btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
btVector3 gyroForce (0,0,0); btVector3 gyroForce (0,0,0);
if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE) if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
{ {
gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); 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)
{
gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
solverBody.m_externalTorqueImpulse += gyroForce;
}
if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
{
gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
solverBody.m_externalTorqueImpulse += gyroForce;
}
} }
} }
if (1) if (1)
{ {
int j; int j;
for (j=0;j<numConstraints;j++) for (j=0;j<numConstraints;j++)
{ {
btTypedConstraint* constraint = constraints[j]; btTypedConstraint* constraint = constraints[j];
constraint->buildJacobian(); constraint->buildJacobian();
constraint->internalSetAppliedImpulse(0.0f); constraint->internalSetAppliedImpulse(0.0f);
} }
} }
//btRigidBody* rb0=0,*rb1=0; //btRigidBody* rb0=0,*rb1=0;
//if (1) //if (1)
{ {
{ {
int totalNumRows = 0; int totalNumRows = 0;
int i; int i;
m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
//calculate the total number of contraint rows //calculate the total number of contraint rows
for (i=0;i<numConstraints;i++) for (i=0;i<numConstraints;i++)
{ {
btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
btJointFeedback* fb = constraints[i]->getJointFeedback(); btJointFeedback* fb = constraints[i]->getJointFeedback();
if (fb) if (fb)
{ {
Show All 13 Lines //if (1)
{ {
info1.m_numConstraintRows = 0; info1.m_numConstraintRows = 0;
info1.nub = 0; info1.nub = 0;
} }
totalNumRows += info1.m_numConstraintRows; totalNumRows += info1.m_numConstraintRows;
} }
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
///setup the btSolverConstraints ///setup the btSolverConstraints
int currentRow = 0; int currentRow = 0;
for (i=0;i<numConstraints;i++) for (i=0;i<numConstraints;i++)
{ {
const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
if (info1.m_numConstraintRows) if (info1.m_numConstraintRows)
{ {
btAssert(currentRow<totalNumRows); btAssert(currentRow<totalNumRows);
btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
btTypedConstraint* constraint = constraints[i]; btTypedConstraint* constraint = constraints[i];
btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbA = constraint->getRigidBodyA();
btRigidBody& rbB = constraint->getRigidBodyB(); btRigidBody& rbB = constraint->getRigidBodyB();
▲ Show 20 Lines • Show All 91 Lines • ▼ Show 20 Lines //if (1)
sum += iMJlB.dot(solverConstraint.m_contactNormal2); sum += iMJlB.dot(solverConstraint.m_contactNormal2);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
btScalar fsum = btFabs(sum); btScalar fsum = btFabs(sum);
btAssert(fsum > SIMD_EPSILON); btAssert(fsum > SIMD_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f; solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
} }
{ {
btScalar rel_vel; btScalar rel_vel;
btVector3 externalForceImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalForceImpulse : 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 externalTorqueImpulseA = bodyAPtr->m_originalBody ? bodyAPtr->m_externalTorqueImpulse : btVector3(0,0,0);
btVector3 externalForceImpulseB = bodyBPtr->m_originalBody ? bodyBPtr->m_externalForceImpulse : 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 externalTorqueImpulseB = bodyBPtr->m_originalBody ?bodyBPtr->m_externalTorqueImpulse : btVector3(0,0,0);
btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA) btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()+externalForceImpulseA)
+ solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA); + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()+externalTorqueImpulseA);
btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB) btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()+externalForceImpulseB)
+ solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB); + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()+externalTorqueImpulseB);
rel_vel = vel1Dotn+vel2Dotn; rel_vel = vel1Dotn+vel2Dotn;
btScalar restitution = 0.f; btScalar restitution = 0.f;
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2 btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
btScalar velocityError = restitution - rel_vel * info2.m_damping; btScalar velocityError = restitution - rel_vel * info2.m_damping;
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
▲ Show 20 Lines • Show All 49 Lines • ▼ Show 20 Lines
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*/)
{ {
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) 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 tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2(j+1); int swapi = btRandInt2(j+1);
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp; m_orderNonContactConstraintPool[swapi] = tmp;
} }
//contact/friction constraints are not solved more than //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 tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2(j+1); int swapi = btRandInt2(j+1);
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp; m_orderTmpConstraintPool[swapi] = tmp;
} }
▲ Show 20 Lines • Show All 62 Lines • ▼ Show 20 Lines if (iteration< infoGlobal.m_numIterations)
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
} }
} }
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
{ {
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
if (totalImpulse>btScalar(0)) if (totalImpulse>btScalar(0))
{ {
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
} }
} }
} }
} }
} }
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{ {
//solve the friction constraints after all contact constraints, don't interleave them //solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j; int j;
for (j=0;j<numPoolConstraints;j++) for (j=0;j<numPoolConstraints;j++)
{ {
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
} }
///solve all friction constraints, using SIMD, if available ///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++) for (j=0;j<numFrictionPoolConstraints;j++)
{ {
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0)) if (totalImpulse>btScalar(0))
{ {
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
} }
} }
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j=0;j<numRollingFrictionPoolConstraints;j++) for (j=0;j<numRollingFrictionPoolConstraints;j++)
{ {
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0)) if (totalImpulse>btScalar(0))
{ {
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
} }
} }
} }
} }
} else } else
{ {
//non-SIMD version //non-SIMD version
///solve all joint constraints ///solve all joint constraints
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{ {
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
▲ Show 20 Lines • Show All 107 Lines • ▼ Show 20 LinesbtScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{ {
///this is a special step to resolve penetrations (just for contacts) ///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--) //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
{ {
solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
} }
} }
return 0.f; return 0.f;
} }
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
{ {
int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int i,j; int i,j;
Show All 25 Lines for (j=0;j<numPoolConstraints;j++)
btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
btJointFeedback* fb = constr->getJointFeedback(); btJointFeedback* fb = constr->getJointFeedback();
if (fb) if (fb)
{ {
fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; 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_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_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_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
} }
constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
{ {
constr->setEnabled(false); constr->setEnabled(false);
} }
} }
for ( i=0;i<m_tmpSolverBodyPool.size();i++) for ( i=0;i<m_tmpSolverBodyPool.size();i++)
{ {
btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
if (body) if (body)
{ {
if (infoGlobal.m_splitImpulse) if (infoGlobal.m_splitImpulse)
m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
else else
m_tmpSolverBodyPool[i].writebackVelocity(); m_tmpSolverBodyPool[i].writebackVelocity();
m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity( 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_externalForceImpulse);
m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity( m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(
m_tmpSolverBodyPool[i].m_angularVelocity+ m_tmpSolverBodyPool[i].m_angularVelocity+
m_tmpSolverBodyPool[i].m_externalTorqueImpulse); m_tmpSolverBodyPool[i].m_externalTorqueImpulse);
Show All 16 Lines
/// btSequentialImpulseConstraintSolver Sequentially applies impulses /// 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"); BT_PROFILE("solveGroup");
//you need to provide at least some bodies //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); solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
return 0.f; return 0.f;
} }
void btSequentialImpulseConstraintSolver::reset() void btSequentialImpulseConstraintSolver::reset()
{ {
m_btSeed2 = 0; m_btSeed2 = 0;
} }