Contact Resolution (wrong impulse equations?)

Started by
3 comments, last by Irlan Robson 9 years, 3 months ago

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);
}

Advertisement

You want to pass vRa and vRb into your ApplyForce() function and not vContactPoint. Also make sure that prbLeft->m_vPos is the location of the center of mass and not the position of the rigid body. The way artists author models they are usually different. Don't assume that the position and center of mass are coincident.

I also recommend to rename ApplyForce() to ApplyImpulse().

You want to pass vRa and vRb into your ApplyForce() function and not vContactPoint. Also make sure that prbLeft->m_vPos is the location of the center of mass and not the position of the rigid body. The way artists author models they are usually different. Don't assume that the position and center of mass are coincident.

I also recommend to rename ApplyForce() to ApplyImpulse().

The first one didn't solved the problem but thanks for pointing it out.

I've renamed to ApplyImpulse (since it's a instantaneous change in velocity without a time scale).

I still don't have correct impulses.

I'm using GJK + EPA and is giving me the correct contact normals in the direction I want (checked a milion times), so that's shouldn't be a problem.

You want to pass vRa and vRb into your ApplyForce() function and not vContactPoint. Also make sure that prbLeft->m_vPos is the location of the center of mass and not the position of the rigid body. The way artists author models they are usually different. Don't assume that the position and center of mass are coincident.

I also recommend to rename ApplyForce() to ApplyImpulse().

Also, this is how I'm integrating the rigid bodies:


if ( m_ui32CollFlags & SIF_STATIC_OBJECT ) { return; }

	//Linear dynamics.
	m_vLinVel += _vForce * (m_fInvMass * _fTimeScale);
	m_vPos += m_vLinVel * _fTimeScale;

	//Angular dynamics.
	m_vAngVel += (m_mWorldInvInertia * _vTorque) * _fTimeScale;
	
	CQuaternion qDtOrient = CQuaternion(ML_ZERO, m_vAngVel.x, m_vAngVel.y, m_vAngVel.z) * m_qOrient * ML_HALF;
	m_qOrient += qDtOrient * _fTimeScale;

	m_qOrient.Normalize();
	m_qOrient.ToRotationMatrix(m_mOrient);
	CMatrix4x4 mInvRot = m_mOrient;
	mInvRot.Transpose();
	m_mWorldInvInertia = m_mOrient * m_mLocalInvInertia * mInvRot;
	
	m_mTransform = m_mOrient;
	reinterpret_cast<CVector3&>(m_mTransform._41) = m_vPos;


...and the new contact resolution:


const CVector3& vContactPoint = _rbcContact.cpContactPoint.vPointOnB;
	const CVector3& vContactNormal = _rbcContact.cpContactPoint.vNormalB;

	//Normal impulse (A to B).
	float fInvMass0 = _rbcContact.prbLeft->InvMass();
	float fInvMass1 = _rbcContact.prbRight->InvMass();
	float fRestitution = ML_HALF;

	CVector3 vR0 = vContactPoint - _rbcContact.prbLeft->m_vPos;
	CVector3 vR1 = vContactPoint - _rbcContact.prbRight->m_vPos;

	//The total velocity of the rigid bodies (linear + angular) at the contact point.
	CVector3 vTotalVel0 = _rbcContact.prbLeft->m_vLinVel + _rbcContact.prbLeft->m_vAngVel.Cross(vR0);
	CVector3 vTotalVel1 = _rbcContact.prbRight->m_vLinVel + _rbcContact.prbRight->m_vAngVel.Cross(vR1);

	//Total relative velocity of the rigid bodies (linear + angular).
	CVector3 vTotalRelVel = vTotalVel0 - vTotalVel1;
	float fRelVelDotNormal = -(ML_ONE + fRestitution) * vTotalRelVel.Dot(vContactNormal);
	
	//Mass.
	float fRotNormalMass0 = ( _rbcContact.prbLeft->m_mWorldInvInertia * vR0.Cross(vContactNormal).Cross(vR0)).Dot(vContactNormal);
	float fRotNormalMass1 = (_rbcContact.prbRight->m_mWorldInvInertia * vR1.Cross(vContactNormal).Cross(vR1)).Dot(vContactNormal);
	float fNormalMass = fInvMass0 + fInvMass1 + fRotNormalMass0 + fRotNormalMass1;
	
	/* (use later)
	Erin Catto (GDC 2006) approach (use later).
	float fTimeScale = 0.03333f;
	float fBiasFactor = 0.1f;
	float fSlop = 0.1f;
	float fTotalBias = (fBiasFactor / fTimeScale) * CMathLib::Max(ML_ZERO, _rbcContact.cpContactPoint.fDistance - fSlop);
	float fNormalForceLen = ( fRelVelDotNormal + fTotalBias ) / (fInvMassA + fInvMassB + fNormalDenominator);
	//fNormalForceLen = CMathLib::Max(fNormalForceLen, ML_ZERO);
	*/
	
	float fNormalForceLen = fRelVelDotNormal / fNormalMass;
	CVector3 vNormalForce = vContactNormal * fNormalForceLen;

	if (!(_rbcContact.prbLeft->m_ui32CollFlags & CShapeInstance::SIF_STATIC_OBJECT)) {
		_rbcContact.prbLeft->m_vLinVel += vNormalForce * fInvMass0;
		_rbcContact.prbLeft->m_vAngVel += _rbcContact.prbLeft->m_mWorldInvInertia * vR0.Cross(vNormalForce);
	}

	if (!(_rbcContact.prbRight->m_ui32CollFlags & CShapeInstance::SIF_STATIC_OBJECT)) {
		_rbcContact.prbRight->m_vLinVel -= vNormalForce * fInvMass1;
		_rbcContact.prbRight->m_vAngVel -= _rbcContact.prbRight->m_mWorldInvInertia * vR1.Cross(vNormalForce);
	}

Solved.

The problem was with the mathematics module. I was computing a quaternion incorrectly.

Instead of passing CQuaternion(ML_ZERO, m_vAngVel.x, m_vAngVel.y, m_vAngVel.z) what was needed to do was CQuaternion(m_vAngVel.x, m_vAngVel.y, m_vAngVel.z, ML_ZERO) because the last argument was the w-value of a quarternion in my implementation.

Thank you, Dirk Gregorius.

This topic is closed to new replies.

Advertisement