When I disable warm starting, the simulation is stable for the simple cases, except when stacking boxes.
Dirk, I have used the btPlaneSpace approach you recommended when the relative velocity in the normal direction is zero.
Code: Select all
void AMG3DRigidBodyCollisionResponder::computeMasses(const AMG3DScalar& dt)
{
if(m_bSkipCollision)
return;
if(dt<=0.0f)
return;
const AMG3DScalar k_allowedPenetration = 0.01f;
AMG3DScalar k_biasFactor = (AMG3DPhysicsWorld::g_AMG3D_bPositionCorrection) ? 0.2f : 0.0f;
for(int i=0; i<m_cdContactData.iNumContacts; ++i) {
AMG3DVector4 normal = m_cdContactData.contacts[i].vContactNormal;
AMG3DVector4 rap = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody1->m_vPosition;
AMG3DVector4 rbp = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody2->m_vPosition;
AMG3DVector4 rapcrossn = rap.cross(normal);
AMG3DVector4 rbpcrossn = rbp.cross(normal);
// Compute the normal mass
AMG3DScalar Kn = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
Kn += ( (m_pRigidBody1->m_matInvIWorld*rapcrossn).cross(rap) + (m_pRigidBody2->m_matInvIWorld*rbpcrossn).cross(rbp) ).dot(normal);
m_cdContactData.contacts[i].massNormal = 1.0f/Kn;
AMG3DVector4 vap = m_pRigidBody1->m_vVelocity + m_pRigidBody1->m_vAngularVelocity.cross(rap);
AMG3DVector4 vbp = m_pRigidBody2->m_vVelocity + m_pRigidBody2->m_vAngularVelocity.cross(rbp);
// Relative velocity
AMG3DVector4 vab = vap - vbp;
// The tangent vector depends on relative velocity and therefore needs to be computed every time the velocity is updated
// Calculate the tangent vector
AMG3DVector4 vabn = normal*(vab.dot(normal));
AMG3DVector4 vabt = vab - vabn;
AMG3DVector4 t1, t2;
if(vabt.magnitude()>0) {
vabt.normalize();
t1 = vabt;
AMG3DVector4Cross(&t2, t1, normal);
}
else {
normal.buildOrthoBasis(&t1, &t2);
}
//AMG3DVector4 rapcrosst1 = rap.cross(t1);
//AMG3DVector4 rbpcrosst1 = rbp.cross(t1);
//// Compute the tangent mass in the first direction
//AMG3DScalar Kt1 = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
//Kt1 += ( (m_pRigidBody1->m_matInvIWorld*rapcrosst1).cross(rap) + (m_pRigidBody2->m_matInvIWorld*rbpcrosst1).cross(rbp) ).dot(t1);
//m_cdContactData.contacts[i].massTangent1 = 1.0f/Kt1;
//AMG3DVector4 rapcrosst2 = rap.cross(t2);
//AMG3DVector4 rbpcrosst2 = rbp.cross(t2);
//// Compute the tangent mass in the second direction
//AMG3DScalar Kt2 = m_pRigidBody1->m_sInvMass + m_pRigidBody2->m_sInvMass;
//Kt2 += ( (m_pRigidBody1->m_matInvIWorld*rapcrosst2).cross(rap) + (m_pRigidBody2->m_matInvIWorld*rbpcrosst2).cross(rbp) ).dot(t2);
//m_cdContactData.contacts[i].massTangent2 = 1.0f/Kt2;
// Compute the bias (prevents sinking, useful for stacking)
m_cdContactData.contacts[i].bias = k_biasFactor / dt * AMG3D_MATH_MAX(0.0f, m_cdContactData.contacts[i].sPenetrationDepth - k_allowedPenetration);
if(AMG3DPhysicsWorld::g_AMG3D_bWarmStarting) {
AMG3DVector4 P = m_cdContactData.contacts[i].Pn*normal;/* + m_cdContactData.contacts[i].Pt1*t1 + m_cdContactData.contacts[i].Pt2*t2;*/
m_pRigidBody1->m_vVelocity += P*m_pRigidBody1->m_sInvMass;
m_pRigidBody1->m_vAngularVelocity += m_pRigidBody1->m_matInvIWorld*rap.cross(P);
m_pRigidBody2->m_vVelocity -= P*m_pRigidBody2->m_sInvMass;
m_pRigidBody2->m_vAngularVelocity -= m_pRigidBody2->m_matInvIWorld*rbp.cross(P);
}
}
}
}