1005 lines
39 KiB
C++
1005 lines
39 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
|
|
|
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.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
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.
|
|
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.
|
|
*/
|
|
|
|
//#define COMPUTE_IMPULSE_DENOM 1
|
|
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
|
|
|
|
#include "btSequentialImpulseConstraintSolver.h"
|
|
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
|
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
|
#include "btContactConstraint.h"
|
|
#include "btSolve2LinearConstraint.h"
|
|
#include "btContactSolverInfo.h"
|
|
#include "LinearMath/btIDebugDraw.h"
|
|
#include "btJacobianEntry.h"
|
|
#include "LinearMath/btMinMax.h"
|
|
#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
|
|
#include <new>
|
|
#include "LinearMath/btStackAlloc.h"
|
|
#include "LinearMath/btQuickprof.h"
|
|
#include "btSolverBody.h"
|
|
#include "btSolverConstraint.h"
|
|
#include "LinearMath/btAlignedObjectArray.h"
|
|
#include <string.h> //for memset
|
|
|
|
btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
|
|
:m_btSeed2(0)
|
|
{
|
|
|
|
}
|
|
|
|
btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
|
|
{
|
|
}
|
|
|
|
#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 )
|
|
{
|
|
__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;
|
|
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;
|
|
}
|
|
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);
|
|
}
|
|
|
|
|
|
|
|
unsigned long btSequentialImpulseConstraintSolver::btRand2()
|
|
{
|
|
m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
|
|
return m_btSeed2;
|
|
}
|
|
|
|
|
|
|
|
//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
|
|
int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
|
|
{
|
|
// seems good; xor-fold and modulus
|
|
const unsigned long un = static_cast<unsigned long>(n);
|
|
unsigned long r = btRand2();
|
|
|
|
// note: probably more aggressive than it needs to be -- might be
|
|
// able to get away without one or two of the innermost branches.
|
|
if (un <= 0x00010000UL) {
|
|
r ^= (r >> 16);
|
|
if (un <= 0x00000100UL) {
|
|
r ^= (r >> 8);
|
|
if (un <= 0x00000010UL) {
|
|
r ^= (r >> 4);
|
|
if (un <= 0x00000004UL) {
|
|
r ^= (r >> 2);
|
|
if (un <= 0x00000002UL) {
|
|
r ^= (r >> 1);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return (int) (r % un);
|
|
}
|
|
|
|
|
|
|
|
void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* 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_invMass = rb->getInvMass();
|
|
solverBody->m_originalBody = rb;
|
|
solverBody->m_angularFactor = rb->getAngularFactor();
|
|
} else
|
|
{
|
|
solverBody->m_invMass = 0.f;
|
|
solverBody->m_originalBody = 0;
|
|
solverBody->m_angularFactor = 1.f;
|
|
}
|
|
}
|
|
|
|
|
|
int gNumSplitImpulseRecoveries = 0;
|
|
|
|
btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
|
|
{
|
|
btScalar rest = restitution * -rel_vel;
|
|
return rest;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
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_tmpSolverContactFrictionConstraintPool.expand();
|
|
memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
|
|
solverConstraint.m_contactNormal = normalAxis;
|
|
|
|
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
|
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
|
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
|
|
solverConstraint.m_frictionIndex = frictionIndex;
|
|
|
|
solverConstraint.m_friction = cp.m_combinedFriction;
|
|
solverConstraint.m_originalContactPoint = 0;
|
|
|
|
solverConstraint.m_appliedImpulse = 0.f;
|
|
// solverConstraint.m_appliedPushImpulse = 0.f;
|
|
solverConstraint.m_penetration = 0.f;
|
|
{
|
|
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
|
|
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
|
|
solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
|
|
}
|
|
{
|
|
btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
|
|
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
|
|
solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
|
|
}
|
|
|
|
#ifdef COMPUTE_IMPULSE_DENOM
|
|
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
|
|
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
|
|
#else
|
|
btVector3 vec;
|
|
btScalar denom0 = 0.f;
|
|
btScalar denom1 = 0.f;
|
|
if (body0)
|
|
{
|
|
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
|
|
denom0 = body0->getInvMass() + normalAxis.dot(vec);
|
|
}
|
|
if (body1)
|
|
{
|
|
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
|
|
denom1 = body1->getInvMass() + normalAxis.dot(vec);
|
|
}
|
|
|
|
|
|
#endif //COMPUTE_IMPULSE_DENOM
|
|
btScalar denom = relaxation/(denom0+denom1);
|
|
solverConstraint.m_jacDiagABInv = denom;
|
|
|
|
#ifdef _USE_JACOBIAN
|
|
solverConstraint.m_jac = btJacobianEntry (
|
|
rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
|
|
body0->getInvInertiaDiagLocal(),
|
|
body0->getInvMass(),
|
|
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)
|
|
{
|
|
BT_PROFILE("solveGroupCacheFriendlySetup");
|
|
(void)stackAlloc;
|
|
(void)debugDrawer;
|
|
|
|
|
|
if (!(numConstraints + numManifolds))
|
|
{
|
|
// printf("empty\n");
|
|
return 0.f;
|
|
}
|
|
|
|
if (1)
|
|
{
|
|
int j;
|
|
for (j=0;j<numConstraints;j++)
|
|
{
|
|
btTypedConstraint* constraint = constraints[j];
|
|
constraint->buildJacobian();
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
|
|
///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 = btScalar(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++)
|
|
{
|
|
manifold = manifoldPtr[i];
|
|
colObj0 = (btCollisionObject*)manifold->getBody0();
|
|
colObj1 = (btCollisionObject*)manifold->getBody1();
|
|
|
|
int solverBodyIdA=-1;
|
|
int solverBodyIdB=-1;
|
|
|
|
if (manifold->getNumContacts())
|
|
{
|
|
solverBodyIdA = getOrInitSolverBody(*colObj0);
|
|
solverBodyIdB = getOrInitSolverBody(*colObj1);
|
|
}
|
|
|
|
btVector3 rel_pos1;
|
|
btVector3 rel_pos2;
|
|
btScalar relaxation;
|
|
|
|
for (int j=0;j<manifold->getNumContacts();j++)
|
|
{
|
|
|
|
btManifoldPoint& cp = manifold->getContactPoint(j);
|
|
|
|
///this is a bad test and results in jitter -> always solve for those zero-distanc contacts!
|
|
///-> if (cp.getDistance() <= btScalar(0.))
|
|
//if (cp.getDistance() <= manifold->getContactBreakingThreshold())
|
|
{
|
|
|
|
const btVector3& pos1 = cp.getPositionWorldOnA();
|
|
const btVector3& pos2 = cp.getPositionWorldOnB();
|
|
|
|
rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
|
|
rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
|
|
|
|
|
|
relaxation = 1.f;
|
|
btScalar rel_vel;
|
|
btVector3 vel;
|
|
|
|
int frictionIndex = m_tmpSolverContactConstraintPool.size();
|
|
|
|
{
|
|
btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
|
|
btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
|
btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
|
|
|
solverConstraint.m_solverBodyIdA = solverBodyIdA;
|
|
solverConstraint.m_solverBodyIdB = solverBodyIdB;
|
|
solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
|
|
|
|
solverConstraint.m_originalContactPoint = &cp;
|
|
|
|
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
|
|
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
|
|
btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
|
|
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
|
|
{
|
|
#ifdef COMPUTE_IMPULSE_DENOM
|
|
btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
|
|
btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
|
|
#else
|
|
btVector3 vec;
|
|
btScalar denom0 = 0.f;
|
|
btScalar denom1 = 0.f;
|
|
if (rb0)
|
|
{
|
|
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
|
|
denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
|
|
}
|
|
if (rb1)
|
|
{
|
|
vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
|
|
denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
|
|
}
|
|
#endif //COMPUTE_IMPULSE_DENOM
|
|
|
|
btScalar denom = relaxation/(denom0+denom1);
|
|
solverConstraint.m_jacDiagABInv = denom;
|
|
}
|
|
|
|
solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
|
|
solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
|
|
solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
|
|
|
|
|
|
btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
|
|
btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
|
|
|
|
vel = vel1 - vel2;
|
|
|
|
rel_vel = cp.m_normalWorldOnB.dot(vel);
|
|
|
|
solverConstraint.m_penetration = cp.getDistance()+infoGlobal.m_linearSlop;
|
|
//solverConstraint.m_penetration = cp.getDistance();
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
solverConstraint.m_friction = cp.m_combinedFriction;
|
|
|
|
|
|
if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
|
|
{
|
|
solverConstraint.m_restitution = 0.f;
|
|
} else
|
|
{
|
|
solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
|
if (solverConstraint.m_restitution <= btScalar(0.))
|
|
{
|
|
solverConstraint.m_restitution = 0.f;
|
|
};
|
|
}
|
|
|
|
|
|
///warm starting (or zero if disabled)
|
|
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
|
{
|
|
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
|
|
if (rb0)
|
|
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
|
|
if (rb1)
|
|
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
|
|
} else
|
|
{
|
|
solverConstraint.m_appliedImpulse = 0.f;
|
|
}
|
|
|
|
// solverConstraint.m_appliedPushImpulse = 0.f;
|
|
|
|
{
|
|
btScalar rel_vel;
|
|
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
|
|
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
|
|
btScalar vel2Dotn = solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
|
|
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
|
|
|
|
rel_vel = vel1Dotn-vel2Dotn;
|
|
|
|
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;
|
|
solverConstraint.m_cfm = 0.f;
|
|
solverConstraint.m_lowerLimit = 0;
|
|
solverConstraint.m_upperLimit = 1e10f;
|
|
}
|
|
|
|
|
|
#ifdef _USE_JACOBIAN
|
|
solverConstraint.m_jac = btJacobianEntry (
|
|
rel_pos1,rel_pos2,cp.m_normalWorldOnB,
|
|
rb0->getInvInertiaDiagLocal(),
|
|
rb0->getInvMass(),
|
|
rb1->getInvInertiaDiagLocal(),
|
|
rb1->getInvMass());
|
|
#endif //_USE_JACOBIAN
|
|
|
|
/////setup the friction constraints
|
|
|
|
|
|
|
|
if (1)
|
|
{
|
|
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
|
|
if (!cp.m_lateralFrictionInitialized)
|
|
{
|
|
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
|
|
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
|
|
if (lat_rel_vel > SIMD_EPSILON)//0.0f)
|
|
{
|
|
cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
|
|
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
|
if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
|
|
{
|
|
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
|
|
cp.m_lateralFrictionDir2.normalize();//??
|
|
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
|
|
cp.m_lateralFrictionInitialized = true;
|
|
}
|
|
} else
|
|
{
|
|
//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);
|
|
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;
|
|
}
|
|
}
|
|
|
|
} else
|
|
{
|
|
addFrictionConstraint(cp.m_lateralFrictionDir1,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);
|
|
}
|
|
|
|
if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
|
|
{
|
|
{
|
|
btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
|
|
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
|
{
|
|
frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
|
|
if (rb0)
|
|
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
|
|
if (rb1)
|
|
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
|
|
} else
|
|
{
|
|
frictionConstraint1.m_appliedImpulse = 0.f;
|
|
}
|
|
}
|
|
{
|
|
btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
|
|
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
|
{
|
|
frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
|
|
if (rb0)
|
|
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
|
|
if (rb1)
|
|
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
|
|
} else
|
|
{
|
|
frictionConstraint2.m_appliedImpulse = 0.f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
btContactSolverInfo info = infoGlobal;
|
|
|
|
|
|
|
|
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);
|
|
m_orderFrictionConstraintPool.resize(numFrictionPool);
|
|
{
|
|
int i;
|
|
for (i=0;i<numConstraintPool;i++)
|
|
{
|
|
m_orderTmpConstraintPool[i] = i;
|
|
}
|
|
for (i=0;i<numFrictionPool;i++)
|
|
{
|
|
m_orderFrictionConstraintPool[i] = i;
|
|
}
|
|
}
|
|
|
|
return 0.f;
|
|
|
|
}
|
|
|
|
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_tmpSolverContactConstraintPool.size();
|
|
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
|
|
|
|
//should traverse the contacts random order...
|
|
int iteration;
|
|
{
|
|
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
|
|
{
|
|
|
|
int j;
|
|
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
|
|
{
|
|
if ((iteration & 7) == 0) {
|
|
for (j=0; j<numConstraintPool; ++j) {
|
|
int tmp = m_orderTmpConstraintPool[j];
|
|
int swapi = btRandInt2(j+1);
|
|
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
|
|
m_orderTmpConstraintPool[swapi] = tmp;
|
|
}
|
|
|
|
for (j=0; j<numFrictionPool; ++j) {
|
|
int tmp = m_orderFrictionConstraintPool[j];
|
|
int swapi = btRandInt2(j+1);
|
|
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
|
|
m_orderFrictionConstraintPool[swapi] = tmp;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (infoGlobal.m_solverMode & SOLVER_SIMD)
|
|
{
|
|
///solve all joint constraints, using SIMD, if available
|
|
for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
|
|
{
|
|
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
|
|
resolveSingleConstraintRowGenericSIMD(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 using SIMD, if available
|
|
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
|
for (j=0;j<numPoolConstraints;j++)
|
|
{
|
|
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
|
|
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
|
|
}
|
|
///solve all friction constraints, using SIMD, if available
|
|
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;
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
|
|
/// btSequentialImpulseConstraintSolver Sequentially applies impulses
|
|
btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
|
|
{
|
|
BT_PROFILE("solveGroup");
|
|
//we only implement SOLVER_CACHE_FRIENDLY now
|
|
//you need to provide at least some bodies
|
|
btAssert(bodies);
|
|
btAssert(numBodies);
|
|
|
|
int i;
|
|
|
|
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
|
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
|
|
|
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
|
|
int j;
|
|
|
|
for (j=0;j<numPoolConstraints;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_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)
|
|
{
|
|
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
|
|
{
|
|
m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
|
|
}
|
|
} else
|
|
{
|
|
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
|
|
{
|
|
m_tmpSolverBodyPool[i].writebackVelocity();
|
|
}
|
|
}
|
|
|
|
|
|
m_tmpSolverBodyPool.resize(0);
|
|
m_tmpSolverContactConstraintPool.resize(0);
|
|
m_tmpSolverNonContactConstraintPool.resize(0);
|
|
m_tmpSolverContactFrictionConstraintPool.resize(0);
|
|
|
|
return 0.f;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void btSequentialImpulseConstraintSolver::reset()
|
|
{
|
|
m_btSeed2 = 0;
|
|
}
|
|
|
|
|