Real-Time Physics Simulation Forum

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;

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

		// Calculate the tangent vector. To Do: Research relative velocity 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, normal, t1);
		//}
		//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;
	
		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);
		}	
	}
}