Big work-in-progress refactoring of the constraint solver:
1) Add fast branchless SIMD support for constraint solver (Windows only until we get other contributions). See resolveSingleConstraintRowGenericSIMD in Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp resolveSingleConstraintRowGenericSIMD can be used for all constraints, including contact, point 2 point, hinge, generic etc. 2) During this refactoring, all constraints support the obsolete 'solveConstraintObsolete' while we add 'getInfo1' and 'getInfo2' support. This interface is almost identical interface to Open Dynamics Engine, to make it easier to port Dantzig LCP solver. 3) Some minor refactoring to reduce huge constructor overhead in math classes.
This commit is contained in:
erwin.coumans
@@ -37,29 +37,73 @@ subject to the following restrictions:
|
||||
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
|
||||
:m_btSeed2(0)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
|
||||
{
|
||||
}
|
||||
|
||||
// Project Gauss Seidel or the equivalent Sequential Impulse
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
#ifdef USE_SIMD
|
||||
#include <emmintrin.h>
|
||||
#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
|
||||
static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
|
||||
{
|
||||
float deltaImpulse;
|
||||
deltaImpulse = c.m_rhs-c.m_appliedImpulse*c.m_cfm;
|
||||
btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
|
||||
btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
|
||||
btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
|
||||
__m128 result = _mm_mul_ps( vec0, vec1);
|
||||
return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
|
||||
}
|
||||
#endif//USE_SIMD
|
||||
|
||||
// Project Gauss Seidel or the equivalent Sequential Impulse
|
||||
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
#ifdef USE_SIMD
|
||||
__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 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(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
|
||||
__m128 deltaVel2Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body2.m_deltaLinearVelocity.mVec128) ,_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128));
|
||||
__m128 delta_rel_vel = _mm_sub_ps(deltaVel1Dotn,deltaVel2Dotn);
|
||||
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
|
||||
deltaImpulse = _mm_add_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_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
|
||||
__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_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
|
||||
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body2.m_invMass));
|
||||
__m128 impulseMagnitude = deltaImpulse;
|
||||
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
|
||||
body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
|
||||
body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
|
||||
body2.m_deltaAngularVelocity.mVec128 = _mm_sub_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
|
||||
#else
|
||||
resolveSingleConstraintRowGeneric(body1,body2,c);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Project Gauss Seidel or the equivalent Sequential Impulse
|
||||
SIMD_FORCE_INLINE void 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_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
|
||||
const btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
|
||||
const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
|
||||
deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
|
||||
deltaImpulse += deltaVel2Dotn*c.m_jacDiagABInv;
|
||||
|
||||
btScalar sum = c.m_appliedImpulse + deltaImpulse;
|
||||
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;
|
||||
@@ -69,8 +113,71 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRow(btSolverBod
|
||||
{
|
||||
c.m_appliedImpulse = sum;
|
||||
}
|
||||
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
|
||||
body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse);
|
||||
if (body1.m_invMass)
|
||||
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
|
||||
if (body2.m_invMass)
|
||||
body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse);
|
||||
}
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
#ifdef USE_SIMD
|
||||
|
||||
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
|
||||
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
|
||||
__m128 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(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
|
||||
__m128 deltaVel2Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body2.m_deltaLinearVelocity.mVec128) ,_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128));
|
||||
__m128 delta_rel_vel = _mm_sub_ps(deltaVel1Dotn,deltaVel2Dotn);
|
||||
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
|
||||
deltaImpulse = _mm_add_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);
|
||||
__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_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
|
||||
__m128 linearComponentB = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body2.m_invMass));
|
||||
//body1.applyImpulse(c.m_contactNormal*body1.m_invMass.m_vec128,c.m_angularComponentA,deltaImpulse);
|
||||
//body2.applyImpulse(c.m_contactNormal*body2.m_invMass.m_vec128,c.m_angularComponentB,-deltaImpulse);
|
||||
__m128 impulseMagnitude = deltaImpulse;
|
||||
body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
|
||||
body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
|
||||
body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
|
||||
body2.m_deltaAngularVelocity.mVec128 = _mm_sub_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
|
||||
|
||||
#else
|
||||
resolveSingleConstraintRowLowerLimit(body1,body2,c);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Project Gauss Seidel or the equivalent Sequential Impulse
|
||||
SIMD_FORCE_INLINE void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
|
||||
{
|
||||
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
|
||||
const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.m_deltaLinearVelocity) + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
|
||||
const btScalar deltaVel2Dotn = c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
|
||||
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
|
||||
{
|
||||
c.m_appliedImpulse = sum;
|
||||
}
|
||||
if (body1.m_invMass)
|
||||
body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
|
||||
if (body2.m_invMass)
|
||||
body2.applyImpulse(c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,-deltaImpulse);
|
||||
}
|
||||
|
||||
|
||||
@@ -113,39 +220,24 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
|
||||
{
|
||||
btRigidBody* rb = btRigidBody::upcast(collisionObject);
|
||||
btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
|
||||
|
||||
solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
|
||||
solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
|
||||
|
||||
if (rb)
|
||||
{
|
||||
solverBody->m_angularVelocity = rb->getAngularVelocity() ;
|
||||
solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
|
||||
solverBody->m_friction = collisionObject->getFriction();
|
||||
solverBody->m_invMass = rb->getInvMass();
|
||||
solverBody->m_linearVelocity = rb->getLinearVelocity();
|
||||
solverBody->m_originalBody = rb;
|
||||
solverBody->m_angularFactor = rb->getAngularFactor();
|
||||
} else
|
||||
{
|
||||
solverBody->m_angularVelocity.setValue(0,0,0);
|
||||
solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
|
||||
solverBody->m_friction = collisionObject->getFriction();
|
||||
solverBody->m_invMass = 0.f;
|
||||
solverBody->m_linearVelocity.setValue(0,0,0);
|
||||
solverBody->m_originalBody = 0;
|
||||
solverBody->m_angularFactor = 1.f;
|
||||
}
|
||||
solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
|
||||
solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
|
||||
}
|
||||
|
||||
|
||||
@@ -157,149 +249,19 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel,
|
||||
return rest;
|
||||
}
|
||||
|
||||
//SIMD_FORCE_INLINE
|
||||
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
|
||||
btSolverBody& body1,
|
||||
btSolverBody& body2,
|
||||
const btSolverConstraint& contactConstraint,
|
||||
const btContactSolverInfo& solverInfo)
|
||||
{
|
||||
(void)solverInfo;
|
||||
|
||||
if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
|
||||
{
|
||||
|
||||
gNumSplitImpulseRecoveries++;
|
||||
btScalar normalImpulse;
|
||||
|
||||
// Optimized version of projected relative velocity, use precomputed cross products with normal
|
||||
// body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
|
||||
// body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
|
||||
// btVector3 vel = vel1 - vel2;
|
||||
// btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel);
|
||||
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
|
||||
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
|
||||
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
|
||||
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
|
||||
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
|
||||
|
||||
btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
|
||||
// btScalar positionalError = contactConstraint.m_penetration;
|
||||
|
||||
btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
|
||||
normalImpulse = penetrationImpulse+velocityImpulse;
|
||||
|
||||
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
|
||||
btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
|
||||
|
||||
body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
|
||||
|
||||
body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//SIMD_FORCE_INLINE
|
||||
void btSequentialImpulseConstraintSolver::resolveSingleFrictionCacheFriendly(
|
||||
btSolverBody& body1,
|
||||
btSolverBody& body2,
|
||||
const btSolverConstraint& contactConstraint,
|
||||
const btContactSolverInfo& solverInfo,
|
||||
btScalar appliedNormalImpulse)
|
||||
{
|
||||
(void)solverInfo;
|
||||
|
||||
const btScalar combinedFriction = contactConstraint.m_friction;
|
||||
|
||||
const btScalar limit = appliedNormalImpulse * combinedFriction;
|
||||
|
||||
if (appliedNormalImpulse>btScalar(0.))
|
||||
//friction
|
||||
{
|
||||
|
||||
btScalar j1;
|
||||
{
|
||||
|
||||
#ifndef _USE_JACOBIAN
|
||||
btScalar rel_vel;
|
||||
const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
|
||||
const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
|
||||
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
#else
|
||||
btScalar rel_vel = contactConstraint.m_jac.getRelativeVelocity(body1.m_linearVelocity1+body1.m_deltaLinearVelocity,body1.m_angularVelocity1+body1.m_deltaAngularVelocity,
|
||||
body2.m_linearVelocity1+body2.m_deltaLinearVelocity,body2.m_angularVelocity1+body2.m_deltaAngularVelocity);
|
||||
#endif //_USE_JACOBIAN
|
||||
|
||||
// calculate j that moves us to zero relative velocity
|
||||
j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
|
||||
#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
|
||||
#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
|
||||
btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
|
||||
contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
|
||||
|
||||
if (limit < contactConstraint.m_appliedImpulse)
|
||||
{
|
||||
contactConstraint.m_appliedImpulse = limit;
|
||||
} else
|
||||
{
|
||||
if (contactConstraint.m_appliedImpulse < -limit)
|
||||
contactConstraint.m_appliedImpulse = -limit;
|
||||
}
|
||||
j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
|
||||
#else
|
||||
if (limit < j1)
|
||||
{
|
||||
j1 = limit;
|
||||
} else
|
||||
{
|
||||
if (j1 < -limit)
|
||||
j1 = -limit;
|
||||
}
|
||||
|
||||
#endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
|
||||
|
||||
//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
|
||||
//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
body1.applyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
|
||||
|
||||
body2.applyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
|
||||
btRigidBody* body0=btRigidBody::upcast(colObj0);
|
||||
btRigidBody* body1=btRigidBody::upcast(colObj1);
|
||||
|
||||
btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
|
||||
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
|
||||
memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
|
||||
solverConstraint.m_contactNormal = normalAxis;
|
||||
|
||||
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
||||
@@ -310,8 +272,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
solverConstraint.m_originalContactPoint = 0;
|
||||
|
||||
solverConstraint.m_appliedImpulse = btScalar(0.);
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
// solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
solverConstraint.m_penetration = 0.f;
|
||||
{
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
|
||||
@@ -355,10 +317,57 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
|
||||
body1->getInvInertiaDiagLocal(),
|
||||
body1->getInvMass());
|
||||
#endif //_USE_JACOBIAN
|
||||
|
||||
|
||||
{
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
|
||||
btScalar vel2Dotn = solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
|
||||
+ solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
|
||||
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
positionalError = 0;//-solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
|
||||
solverConstraint.m_restitution=0.f;
|
||||
|
||||
btSimdScalar velocityError = solverConstraint.m_restitution - rel_vel;
|
||||
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
|
||||
solverConstraint.m_rhs = velocityImpulse;
|
||||
solverConstraint.m_cfm = 0.f;
|
||||
solverConstraint.m_lowerLimit = 0;
|
||||
solverConstraint.m_upperLimit = 1e10f;
|
||||
}
|
||||
|
||||
return solverConstraint;
|
||||
}
|
||||
|
||||
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
|
||||
{
|
||||
int solverBodyIdA = -1;
|
||||
|
||||
if (body.getCompanionId() >= 0)
|
||||
{
|
||||
//body has already been converted
|
||||
solverBodyIdA = body.getCompanionId();
|
||||
} else
|
||||
{
|
||||
btRigidBody* rb = btRigidBody::upcast(&body);
|
||||
if (rb && rb->getInvMass())
|
||||
{
|
||||
solverBodyIdA = m_tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,&body);
|
||||
body.setCompanionId(solverBodyIdA);
|
||||
} else
|
||||
{
|
||||
return 0;//assume first one is a fixed solver body
|
||||
}
|
||||
}
|
||||
return solverBodyIdA;
|
||||
}
|
||||
#include <stdio.h>
|
||||
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
|
||||
{
|
||||
@@ -372,16 +381,149 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
// printf("empty\n");
|
||||
return 0.f;
|
||||
}
|
||||
btPersistentManifold* manifold = 0;
|
||||
btCollisionObject* colObj0=0,*colObj1=0;
|
||||
|
||||
btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&fixedBody,0);
|
||||
|
||||
//btRigidBody* rb0=0,*rb1=0;
|
||||
|
||||
//if (1)
|
||||
{
|
||||
{
|
||||
|
||||
int totalNumRows = 0;
|
||||
|
||||
//calculate the total number of contraint rows
|
||||
for (int i=0;i<numConstraints;i++)
|
||||
{
|
||||
|
||||
btTypedConstraint::btConstraintInfo1 info1;
|
||||
constraints[i]->getInfo1(&info1);
|
||||
totalNumRows += info1.m_numConstraintRows;
|
||||
}
|
||||
m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
|
||||
|
||||
btTypedConstraint::btConstraintInfo1 info1;
|
||||
info1.m_numConstraintRows = 0;
|
||||
|
||||
if (m_tmpSolverNonContactConstraintPool.size()>3)
|
||||
{
|
||||
printf("dsadas\n");
|
||||
}
|
||||
///setup the btSolverConstraints
|
||||
int currentRow = 0;
|
||||
|
||||
for (int i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
|
||||
{
|
||||
constraints[i]->getInfo1(&info1);
|
||||
if (info1.m_numConstraintRows)
|
||||
{
|
||||
btAssert(currentRow<totalNumRows);
|
||||
|
||||
btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
|
||||
btTypedConstraint* constraint = constraints[i];
|
||||
|
||||
|
||||
|
||||
btRigidBody& rbA = constraint->getRigidBodyA();
|
||||
btRigidBody& rbB = constraint->getRigidBodyB();
|
||||
|
||||
int solverBodyIdA = getOrInitSolverBody(rbA);
|
||||
int solverBodyIdB = getOrInitSolverBody(rbB);
|
||||
|
||||
btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
|
||||
btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
|
||||
|
||||
for (int j=0;j<info1.m_numConstraintRows;j++)
|
||||
{
|
||||
memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint));
|
||||
currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
|
||||
currentConstraintRow[j].m_upperLimit = FLT_MAX;
|
||||
currentConstraintRow[j].m_appliedImpulse = 0.f;
|
||||
currentConstraintRow[j].m_penetration = 0.f;
|
||||
currentConstraintRow[j].m_appliedPushImpulse = 0.f;
|
||||
currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
|
||||
currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
|
||||
}
|
||||
|
||||
bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
|
||||
bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
|
||||
bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
|
||||
|
||||
|
||||
|
||||
btTypedConstraint::btConstraintInfo2 info2;
|
||||
info2.fps = 1.f/infoGlobal.m_timeStep;
|
||||
info2.erp = infoGlobal.m_erp;
|
||||
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
|
||||
info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
|
||||
info2.m_J2linearAxis = 0;
|
||||
info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
|
||||
info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
|
||||
info2.m_constraintError = ¤tConstraintRow->m_rhs;
|
||||
info2.cfm = ¤tConstraintRow->m_cfm;
|
||||
info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
|
||||
info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
|
||||
constraints[i]->getInfo2(&info2);
|
||||
|
||||
///finalize the constraint setup
|
||||
for (int j=0;j<info1.m_numConstraintRows;j++)
|
||||
{
|
||||
btSolverConstraint& solverConstraint = currentConstraintRow[j];
|
||||
|
||||
{
|
||||
const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
|
||||
solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1;
|
||||
}
|
||||
{
|
||||
const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
|
||||
solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2;
|
||||
}
|
||||
|
||||
{
|
||||
btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
|
||||
btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
|
||||
btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
|
||||
btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
|
||||
|
||||
btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
|
||||
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
|
||||
sum += iMJlB.dot(solverConstraint.m_contactNormal);
|
||||
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
|
||||
|
||||
solverConstraint.m_jacDiagABInv = 1./sum;
|
||||
}
|
||||
|
||||
|
||||
///fix rhs
|
||||
///todo: add force/torque accelerators
|
||||
{
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
|
||||
btScalar vel2Dotn = solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
|
||||
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
|
||||
btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
|
||||
solverConstraint.m_restitution = 0.f;
|
||||
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int i;
|
||||
btPersistentManifold* manifold = 0;
|
||||
btCollisionObject* colObj0=0,*colObj1=0;
|
||||
|
||||
|
||||
for (i=0;i<numManifolds;i++)
|
||||
{
|
||||
@@ -394,49 +536,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
|
||||
if (manifold->getNumContacts())
|
||||
{
|
||||
|
||||
|
||||
|
||||
if (colObj0->getIslandTag() >= 0)
|
||||
{
|
||||
if (colObj0->getCompanionId() >= 0)
|
||||
{
|
||||
//body has already been converted
|
||||
solverBodyIdA = colObj0->getCompanionId();
|
||||
} else
|
||||
{
|
||||
solverBodyIdA = m_tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,colObj0);
|
||||
colObj0->setCompanionId(solverBodyIdA);
|
||||
}
|
||||
} else
|
||||
{
|
||||
//create a static body
|
||||
solverBodyIdA = m_tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,colObj0);
|
||||
}
|
||||
|
||||
if (colObj1->getIslandTag() >= 0)
|
||||
{
|
||||
if (colObj1->getCompanionId() >= 0)
|
||||
{
|
||||
solverBodyIdB = colObj1->getCompanionId();
|
||||
} else
|
||||
{
|
||||
solverBodyIdB = m_tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,colObj1);
|
||||
colObj1->setCompanionId(solverBodyIdB);
|
||||
}
|
||||
} else
|
||||
{
|
||||
//create a static body
|
||||
solverBodyIdB = m_tmpSolverBodyPool.size();
|
||||
btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
|
||||
initSolverBody(&solverBody,colObj1);
|
||||
}
|
||||
solverBodyIdA = getOrInitSolverBody(*colObj0);
|
||||
solverBodyIdB = getOrInitSolverBody(*colObj1);
|
||||
}
|
||||
|
||||
btVector3 rel_pos1;
|
||||
@@ -463,10 +564,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
btScalar rel_vel;
|
||||
btVector3 vel;
|
||||
|
||||
int frictionIndex = m_tmpSolverConstraintPool.size();
|
||||
int frictionIndex = m_tmpSolverContactConstraintPool.size();
|
||||
|
||||
{
|
||||
btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
|
||||
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
|
||||
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
||||
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
||||
|
||||
@@ -541,7 +642,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
///warm starting (or zero if disabled)
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
@@ -555,7 +656,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
}
|
||||
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
// solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
|
||||
{
|
||||
btScalar rel_vel;
|
||||
@@ -568,14 +669,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
positionalError = -solverConstraint.m_penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
|
||||
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
|
||||
|
||||
btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
|
||||
solverConstraint.m_rhs = (penetrationImpulse+velocityImpulse);
|
||||
btScalar velocityError = solverConstraint.m_restitution - rel_vel;// * damping;
|
||||
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
|
||||
btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
|
||||
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
|
||||
solverConstraint.m_cfm = 0.f;
|
||||
solverConstraint.m_lowerLimit = 0;
|
||||
solverConstraint.m_upperLimit = 1e30f;
|
||||
solverConstraint.m_upperLimit = 1e10f;
|
||||
}
|
||||
|
||||
|
||||
@@ -594,7 +694,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
|
||||
if (1)
|
||||
{
|
||||
solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
|
||||
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
if (!cp.m_lateralFrictionInitialized)
|
||||
{
|
||||
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
|
||||
@@ -615,9 +715,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
//re-calculate friction direction every frame, todo: check if this is really needed
|
||||
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
|
||||
{
|
||||
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
||||
cp.m_lateralFrictionInitialized = true;
|
||||
}
|
||||
}
|
||||
@@ -632,7 +732,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
|
||||
{
|
||||
{
|
||||
btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
|
||||
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
|
||||
@@ -646,7 +746,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
}
|
||||
}
|
||||
{
|
||||
btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[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;
|
||||
@@ -672,6 +772,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
|
||||
btContactSolverInfo info = infoGlobal;
|
||||
|
||||
if (1)
|
||||
{
|
||||
int j;
|
||||
for (j=0;j<numConstraints;j++)
|
||||
@@ -681,8 +782,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
}
|
||||
}
|
||||
|
||||
int numConstraintPool = m_tmpSolverConstraintPool.size();
|
||||
int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
|
||||
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
|
||||
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
|
||||
///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
|
||||
m_orderTmpConstraintPool.resize(numConstraintPool);
|
||||
@@ -706,8 +807,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
|
||||
{
|
||||
BT_PROFILE("solveGroupCacheFriendlyIterations");
|
||||
int numConstraintPool = m_tmpSolverConstraintPool.size();
|
||||
int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
|
||||
|
||||
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
|
||||
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
|
||||
//should traverse the contacts random order...
|
||||
int iteration;
|
||||
@@ -735,82 +837,96 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
|
||||
}
|
||||
}
|
||||
|
||||
for (j=0;j<numConstraints;j++)
|
||||
if (infoGlobal.m_solverMode & SOLVER_SIMD)
|
||||
{
|
||||
btTypedConstraint* constraint = constraints[j];
|
||||
///todo: use solver bodies, so we don't need to copy from/to btRigidBody
|
||||
|
||||
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
|
||||
///solve all joint constraints, using SIMD, if available
|
||||
for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
{
|
||||
m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
|
||||
}
|
||||
if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
|
||||
{
|
||||
m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
|
||||
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
|
||||
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
|
||||
}
|
||||
|
||||
constraint->solveConstraint(infoGlobal.m_timeStep);
|
||||
|
||||
if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
|
||||
}
|
||||
if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
|
||||
{
|
||||
m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
|
||||
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
|
||||
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
|
||||
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
|
||||
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
|
||||
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverConstraintPool.size();
|
||||
///solve all contact constraints using SIMD, if available
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
resolveSingleConstraintRow(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
|
||||
///solve all friction constraints, using SIMD, if available
|
||||
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
|
||||
for (j=0;j<numFrictionPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
|
||||
btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
|
||||
m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;
|
||||
resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
|
||||
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
|
||||
totalImpulse);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
|
||||
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
|
||||
{
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverConstraintPool.size();
|
||||
int j;
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
|
||||
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
|
||||
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
|
||||
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
|
||||
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
|
||||
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
|
||||
///solve all joint constraints
|
||||
for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
||||
{
|
||||
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
|
||||
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
|
||||
}
|
||||
|
||||
for (j=0;j<numConstraints;j++)
|
||||
{
|
||||
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
|
||||
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
|
||||
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
|
||||
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
|
||||
|
||||
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
|
||||
}
|
||||
|
||||
///solve all contact constraints
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
///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;
|
||||
|
||||
if (totalImpulse>btScalar(0))
|
||||
{
|
||||
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
|
||||
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
|
||||
|
||||
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
|
||||
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
@@ -830,23 +946,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
|
||||
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
|
||||
int numPoolConstraints = m_tmpSolverConstraintPool.size();
|
||||
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
||||
int j;
|
||||
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
|
||||
btAssert(pt);
|
||||
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
|
||||
{
|
||||
pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
|
||||
}
|
||||
|
||||
//do a callback here?
|
||||
|
||||
}
|
||||
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
@@ -865,8 +981,9 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod
|
||||
|
||||
|
||||
m_tmpSolverBodyPool.resize(0);
|
||||
m_tmpSolverConstraintPool.resize(0);
|
||||
m_tmpSolverFrictionConstraintPool.resize(0);
|
||||
m_tmpSolverContactConstraintPool.resize(0);
|
||||
m_tmpSolverNonContactConstraintPool.resize(0);
|
||||
m_tmpSolverContactFrictionConstraintPool.resize(0);
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user