mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-17 06:51:05 +00:00
+ make compound versus soft body work (soft body uses interpolated transform)
+ fixed issue with persistent manifold, warmstarting values were not initialized properly + don't clear manifold in sphere-sphere collision (need warmstarting) + added support for 'split impulse', decouple positional error correction from velocity correction This avoids adding momentum due to penetration correction, it can be tuned using following variables: solverInfo.m_splitImpulse = true/false (disable/enable) solverInfo.m_splitImpulsePenetrationThreshold (below this value, baumgarte/mixed velocity/penetration is used (cheaper, looks more plausible) solverInfo.m_linearSlop (less jitter, when small amound of penetration is allowed)
This commit is contained in:
parent
ea86559480
commit
561066af75
@ -40,6 +40,7 @@ subject to the following restrictions:
|
||||
|
||||
#ifdef SHOW_NUM_DEEP_PENETRATIONS
|
||||
extern int gNumDeepPenetrationChecks;
|
||||
extern int gNumSplitImpulseRecoveries;
|
||||
extern int gNumGjkChecks;
|
||||
#endif //
|
||||
|
||||
@ -236,6 +237,14 @@ void GimpactConcaveDemo::renderme()
|
||||
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
||||
yStart += yIncr;
|
||||
|
||||
glRasterPos3f(xOffset,yStart,0);
|
||||
sprintf(buf,"gNumSplitImpulseRecoveries= %d",gNumSplitImpulseRecoveries);
|
||||
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
||||
yStart += yIncr;
|
||||
|
||||
|
||||
|
||||
|
||||
glRasterPos3f(xOffset,yStart,0);
|
||||
sprintf(buf,"gNumGjkChecks= %d",gNumGjkChecks);
|
||||
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
||||
|
@ -41,6 +41,7 @@ btCollisionShape* gShapePtr[maxNumObjects];//1 rigidbody has 1 shape (no re-use
|
||||
|
||||
#ifdef SHOW_NUM_DEEP_PENETRATIONS
|
||||
extern int gNumDeepPenetrationChecks;
|
||||
extern int gNumSplitImpulseRecoveries;
|
||||
extern int gNumGjkChecks;
|
||||
extern int gNumAlignedAllocs;
|
||||
extern int gNumAlignedFree;
|
||||
@ -1073,7 +1074,11 @@ void DemoApplication::renderme()
|
||||
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
||||
yStart += yIncr;
|
||||
|
||||
|
||||
glRasterPos3f(xOffset,yStart,0);
|
||||
sprintf(buf,"gNumSplitImpulseRecoveries= %d",gNumSplitImpulseRecoveries);
|
||||
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
||||
yStart += yIncr;
|
||||
|
||||
glRasterPos3f(xOffset,yStart,0);
|
||||
sprintf(buf,"gNumAlignedAllocs = %d",gNumAlignedAllocs);
|
||||
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
|
||||
|
File diff suppressed because it is too large
Load Diff
@ -19,6 +19,7 @@
|
||||
#pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
|
||||
#pragma warning(disable:4996) //Turn off warnings about deprecated C routines
|
||||
#pragma warning(disable:4786) // Disable the "debug name too long" warning
|
||||
#pragma warning(disable:4244) // Disable the "possible loss of data" warning
|
||||
|
||||
#include <dae/daeWin32Platform.h>
|
||||
#elif defined( __GCC__ )
|
||||
|
@ -77,17 +77,22 @@ void btCompoundCollisionAlgorithm::processCollision (btCollisionObject* body0,bt
|
||||
|
||||
//backup
|
||||
btTransform orgTrans = colObj->getWorldTransform();
|
||||
btTransform orgInterpolationTrans = colObj->getInterpolationWorldTransform();
|
||||
|
||||
btCollisionShape* orgShape = colObj->getCollisionShape();
|
||||
|
||||
const btTransform& childTrans = compoundShape->getChildTransform(i);
|
||||
//btTransform newChildWorldTrans = orgTrans*childTrans ;
|
||||
colObj->setWorldTransform( orgTrans*childTrans );
|
||||
btTransform newChildWorldTrans = orgTrans*childTrans ;
|
||||
colObj->setWorldTransform( newChildWorldTrans);
|
||||
colObj->setInterpolationWorldTransform(newChildWorldTrans);
|
||||
|
||||
//the contactpoint is still projected back using the original inverted worldtrans
|
||||
colObj->setCollisionShape( childShape );
|
||||
m_childCollisionAlgorithms[i]->processCollision(colObj,otherObj,dispatchInfo,resultOut);
|
||||
//revert back
|
||||
colObj->setCollisionShape( orgShape);
|
||||
colObj->setWorldTransform( orgTrans );
|
||||
colObj->setInterpolationWorldTransform(orgInterpolationTrans);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -56,7 +56,7 @@ void btSphereSphereCollisionAlgorithm::processCollision (btCollisionObject* col0
|
||||
btScalar radius0 = sphere0->getRadius();
|
||||
btScalar radius1 = sphere1->getRadius();
|
||||
|
||||
m_manifoldPtr->clearManifold();
|
||||
//m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
|
||||
|
||||
///iff distance positive, don't generate a new contact
|
||||
if ( len > (radius0+radius1))
|
||||
|
@ -183,7 +183,8 @@ void btPersistentManifold::AddManifoldPoint(const btManifoldPoint& newPoint)
|
||||
|
||||
|
||||
}
|
||||
replaceContactPoint(newPoint,insertIndex);
|
||||
btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
|
||||
m_pointCache[insertIndex] = newPoint;
|
||||
}
|
||||
|
||||
btScalar btPersistentManifold::getContactBreakingThreshold() const
|
||||
|
@ -138,7 +138,7 @@ public:
|
||||
#define MAINTAIN_PERSISTENCY 1
|
||||
#ifdef MAINTAIN_PERSISTENCY
|
||||
int lifeTime = m_pointCache[insertIndex].getLifeTime();
|
||||
btScalar appliedImpulse = 0.f;//m_pointCache[insertIndex].m_appliedImpulse;
|
||||
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
|
||||
btAssert(lifeTime>=0);
|
||||
void* cache = m_pointCache[insertIndex].m_userPersistentData;
|
||||
|
||||
|
@ -26,7 +26,12 @@ struct btContactSolverInfoData
|
||||
int m_numIterations;
|
||||
btScalar m_maxErrorReduction;
|
||||
btScalar m_sor;
|
||||
btScalar m_erp;
|
||||
btScalar m_erp;//used as Baumgarte factor
|
||||
bool m_splitImpulse;
|
||||
btScalar m_splitImpulsePenetrationThreshold;
|
||||
btScalar m_linearSlop;
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
@ -41,11 +46,12 @@ struct btContactSolverInfo : public btContactSolverInfoData
|
||||
m_restitution = btScalar(0.);
|
||||
m_maxErrorReduction = btScalar(20.);
|
||||
m_numIterations = 10;
|
||||
m_erp = btScalar(0.4);
|
||||
m_erp = btScalar(0.2);
|
||||
m_sor = btScalar(1.3);
|
||||
m_splitImpulse = true;
|
||||
m_splitImpulsePenetrationThreshold = 1.f;
|
||||
m_linearSlop = 0.0002f;
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //CONTACT_SOLVER_INFO
|
||||
|
@ -150,9 +150,11 @@ void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject
|
||||
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);
|
||||
}
|
||||
|
||||
btScalar penetrationResolveFactor = btScalar(0.9);
|
||||
int gNumSplitImpulseRecoveries = 0;
|
||||
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
|
||||
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
|
||||
@ -162,8 +164,65 @@ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
|
||||
}
|
||||
|
||||
|
||||
void resolveSplitPenetrationImpulseCacheFriendly(
|
||||
btSolverBody& body1,
|
||||
btSolverBody& body2,
|
||||
const btSolverConstraint& contactConstraint,
|
||||
const btContactSolverInfo& solverInfo);
|
||||
|
||||
//SIMD_FORCE_INLINE
|
||||
void 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;
|
||||
// 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);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
//velocity + friction
|
||||
@ -185,55 +244,52 @@ btScalar resolveSingleCollisionCombinedCacheFriendly(
|
||||
(void)solverInfo;
|
||||
|
||||
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);
|
||||
bool useSplitImpulse = false;
|
||||
|
||||
btScalar rel_vel;
|
||||
btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
|
||||
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
|
||||
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
|
||||
|
||||
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_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
|
||||
btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
|
||||
contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
|
||||
|
||||
normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
if (body1.m_invMass)
|
||||
{
|
||||
|
||||
|
||||
// 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_linearVelocity)
|
||||
+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
|
||||
btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
|
||||
+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
|
||||
|
||||
rel_vel = vel1Dotn-vel2Dotn;
|
||||
|
||||
btScalar positionalError = 0.f;
|
||||
if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration<solverInfo.m_splitImpulsePenetrationThreshold))
|
||||
{
|
||||
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_appliedImpulse;
|
||||
btScalar sum = oldNormalImpulse + normalImpulse;
|
||||
contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
|
||||
|
||||
normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
|
||||
|
||||
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
|
||||
contactConstraint.m_angularComponentA,normalImpulse);
|
||||
}
|
||||
if (body2.m_invMass)
|
||||
{
|
||||
contactConstraint.m_angularComponentA,normalImpulse);
|
||||
|
||||
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
|
||||
contactConstraint.m_angularComponentB,-normalImpulse);
|
||||
contactConstraint.m_angularComponentB,-normalImpulse);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
@ -311,14 +367,9 @@ btScalar resolveSingleFrictionCacheFriendly(
|
||||
|
||||
}
|
||||
|
||||
if (body1.m_invMass)
|
||||
{
|
||||
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
|
||||
}
|
||||
if (body2.m_invMass)
|
||||
{
|
||||
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
|
||||
}
|
||||
body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
|
||||
|
||||
body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
|
||||
|
||||
}
|
||||
return 0.f;
|
||||
@ -360,7 +411,6 @@ btScalar resolveSingleFrictionCacheFriendly(
|
||||
const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
|
||||
|
||||
|
||||
//if (contactConstraint.m_appliedVelocityImpulse > 0.f)
|
||||
if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
|
||||
{
|
||||
lat_rel_vel = btSqrt(lat_rel_vel);
|
||||
@ -370,7 +420,7 @@ btScalar resolveSingleFrictionCacheFriendly(
|
||||
btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
|
||||
btScalar friction_impulse = lat_rel_vel /
|
||||
(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
|
||||
btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction;
|
||||
btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
|
||||
|
||||
GEN_set_min(friction_impulse, normal_impulse);
|
||||
GEN_set_max(friction_impulse, -normal_impulse);
|
||||
@ -406,7 +456,7 @@ void btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3&
|
||||
solverConstraint.m_originalContactPoint = 0;
|
||||
|
||||
solverConstraint.m_appliedImpulse = btScalar(0.);
|
||||
solverConstraint.m_appliedVelocityImpulse = 0.f;
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
solverConstraint.m_penetration = 0.f;
|
||||
{
|
||||
btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
|
||||
@ -669,15 +719,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
|
||||
rel_vel = cp.m_normalWorldOnB.dot(vel);
|
||||
|
||||
solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,0.f);
|
||||
//solverConstraint.m_penetration = cp.getDistance();
|
||||
|
||||
solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
|
||||
solverConstraint.m_friction = cp.m_combinedFriction;
|
||||
solverConstraint.m_restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
|
||||
if (solverConstraint.m_restitution <= btScalar(0.))
|
||||
{
|
||||
solverConstraint.m_restitution = 0.f;
|
||||
};
|
||||
|
||||
|
||||
btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
|
||||
solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep);
|
||||
|
||||
@ -685,24 +736,25 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
|
||||
{
|
||||
solverConstraint.m_penetration = btScalar(0.);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
///warm starting (or zero if disabled)
|
||||
if (m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = cp.m_appliedImpulse;
|
||||
if (rb0)
|
||||
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,cp.m_appliedImpulse);
|
||||
if (rb1)
|
||||
m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-cp.m_appliedImpulse);
|
||||
} else
|
||||
{
|
||||
solverConstraint.m_appliedImpulse = 0.f;
|
||||
}
|
||||
solverConstraint.m_appliedVelocityImpulse = 0.f;
|
||||
|
||||
|
||||
|
||||
|
||||
solverConstraint.m_appliedPushImpulse = 0.f;
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
|
||||
btScalar lat_rel_vel = frictionDir1.length2();
|
||||
@ -844,35 +896,41 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
|
||||
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,
|
||||
m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse);
|
||||
totalImpulse);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverConstraintPool.size();
|
||||
int j;
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
{
|
||||
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
|
||||
btAssert(pt);
|
||||
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
|
||||
//do a callback here?
|
||||
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
|
||||
{
|
||||
{
|
||||
int numPoolConstraints = m_tmpSolverConstraintPool.size();
|
||||
int j;
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
|
||||
|
||||
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
|
||||
m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return 0.f;
|
||||
|
||||
return 0.f;
|
||||
}
|
||||
|
||||
|
||||
@ -883,12 +941,32 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
|
||||
solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
|
||||
|
||||
|
||||
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
|
||||
int numPoolConstraints = m_tmpSolverConstraintPool.size();
|
||||
int j;
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
m_tmpSolverBodyPool[i].writebackVelocity();
|
||||
|
||||
const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
|
||||
btAssert(pt);
|
||||
pt->m_appliedImpulse = solveManifold.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();
|
||||
}
|
||||
}
|
||||
|
||||
// printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
|
||||
|
||||
@ -1312,3 +1390,4 @@ void btSequentialImpulseConstraintSolver::reset()
|
||||
m_btSeed2 = 0;
|
||||
}
|
||||
|
||||
|
||||
|
@ -21,6 +21,7 @@ class btRigidBody;
|
||||
#include "LinearMath/btMatrix3x3.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btAlignedAllocator.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
|
||||
///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
|
||||
@ -36,6 +37,10 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
btVector3 m_linearVelocity;
|
||||
btVector3 m_centerOfMassPosition;
|
||||
|
||||
btVector3 m_pushVelocity;
|
||||
btVector3 m_turnVelocity;
|
||||
|
||||
|
||||
SIMD_FORCE_INLINE void getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
|
||||
{
|
||||
velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
|
||||
@ -44,9 +49,22 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
|
||||
SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
{
|
||||
m_linearVelocity += linearComponent*impulseMagnitude;
|
||||
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
if (m_invMass)
|
||||
{
|
||||
m_linearVelocity += linearComponent*impulseMagnitude;
|
||||
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
{
|
||||
if (m_invMass)
|
||||
{
|
||||
m_pushVelocity += linearComponent*impulseMagnitude;
|
||||
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
@ -54,6 +72,24 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_linearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_angularVelocity);
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void writebackVelocity(btScalar timeStep)
|
||||
{
|
||||
if (m_invMass)
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_linearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_angularVelocity);
|
||||
|
||||
//correct the position/orientation based on push/turn recovery
|
||||
btTransform newTransform;
|
||||
btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
|
||||
m_originalBody->setWorldTransform(newTransform);
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
@ -74,3 +110,4 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverBody
|
||||
|
||||
#endif //BT_SOLVER_BODY_H
|
||||
|
||||
|
||||
|
@ -36,7 +36,9 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
|
||||
btVector3 m_angularComponentA;
|
||||
|
||||
btVector3 m_angularComponentB;
|
||||
mutable btScalar m_appliedVelocityImpulse;
|
||||
|
||||
mutable btScalar m_appliedPushImpulse;
|
||||
|
||||
mutable btScalar m_appliedImpulse;
|
||||
int m_solverBodyIdA;
|
||||
int m_solverBodyIdB;
|
||||
@ -69,3 +71,4 @@ ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint
|
||||
#endif //BT_SOLVER_CONSTRAINT_H
|
||||
|
||||
|
||||
|
||||
|
@ -40,7 +40,7 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura
|
||||
m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
|
||||
|
||||
mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16);
|
||||
m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc;
|
||||
m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc;
|
||||
m_swappedSoftRigidConcaveCreateFunc->m_swapped=true;
|
||||
#endif
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user