That's the flow of my physics engine:
Detect collisions (contact information already debugged);
Update penetrations (only one iteration);
Update velocities (only one iteration for a while);
Apply forces to the rigid bodies (integrate);
I'm not using any kind of biasing to resolve the problem with the rigid bodies impulses yet, but I'm facing with the strange behaviour of the objects after the penetration resolution.
Flipping the sign of the contact normal and the A to B -> B to A equations didn't correct either. That's not my first time with physics engine development (the last one I did was quite stable) so I accept any kind of opinion even from advanced users.
Summarizing the problem: some objects react normally to the relative velocity along the normal but another only are solved by the penetration change.
They actually don't penetrate inside the other but the velocity reaction is discarded; this is why it's strange because if the normal sign was wrong the objects would get stuck inside the other of even penetrating, but that doesn't happen at all!
The rotational motion was discarded also to facilitate the debugging but I still don't have correct contact resolution.
That's the impulse equation:
if (_rbcContact.prbLeft->m_ui32CollFlags & CShapeInstance::SIF_STATIC_OBJECT
&& _rbcContact.prbRight->m_ui32CollFlags & CShapeInstance::SIF_STATIC_OBJECT) {
return;
}
const CVector3& vContactPoint = _rbcContact.cpContactPoint.vPointOnB;
const CVector3& vContactNormal = _rbcContact.cpContactPoint.vNormalB;
//Normal impulse (A to B). Penetration it's correct and is positive always (already checked milion times).
float fInvMassA = _rbcContact.prbLeft->InvMass();
float fInvMassB = _rbcContact.prbRight->InvMass();
float fRestitution = ML_HALF;
CVector3 vRa = vContactPoint - _rbcContact.prbLeft->m_vPos;
CVector3 vRb = vContactPoint - _rbcContact.prbRight->m_vPos;
//The total velocity of the rigid bodies (linear + angular) at the contact point.
CVector3 vTotalVelA = _rbcContact.prbLeft->m_vLinVel + _rbcContact.prbLeft->m_vAngVel.Cross(vRa);
CVector3 vTotalVelB = _rbcContact.prbRight->m_vLinVel + _rbcContact.prbRight->m_vAngVel.Cross(vRb);
//Total relative velocity of the rigid bodies (linear + angular).
CVector3 vTotalRelVel = vTotalVelA - vTotalVelB;
float fRelVelDotNormal = -(ML_ONE + fRestitution) * vTotalRelVel.Dot(vContactNormal);
float fNormalDenominator = (
(_rbcContact.prbLeft->m_mWorldInvInertia * vRa.Cross(vContactNormal)).Cross(vRa) +
(_rbcContact.prbRight->m_mWorldInvInertia * vRb.Cross(vContactNormal)).Cross(vRb)
).Dot(vContactNormal);
float fNormalMass = fInvMassA + fInvMassB + fNormalDenominator;
float fNormalForceLen = fRelVelDotNormal / fNormalMass;
CVector3 vNormalForce = vContactNormal * fNormalForceLen;
if (!(_rbcContact.prbLeft->m_ui32CollFlags & CShapeInstance::SIF_STATIC_OBJECT)) {
_rbcContact.prbLeft->ApplyForce(vNormalForce, vContactPoint);
}
if (!(_rbcContact.prbRight->m_ui32CollFlags & CShapeInstance::SIF_STATIC_OBJECT)) {
_rbcContact.prbRight->ApplyForce(-vNormalForce, vContactPoint);
}
void CRigidBody::ApplyForce(const CVector3& _vForce, const CVector3& _vAt) {
m_vLinVel += _vForce * m_fInvMass;
//m_vAngVel += m_mWorldInvInertia * _vAt.Cross(_vForce);
}