void CollisionHandler::CollisionItem::ApplyCollision( RigidBody& i_oRigidBody1, RigidBody& i_oRigidBody2, double i_rMu, double /*i_rFrameTime*/ )
{
// i_oRigidBody2 collides with i_oRigidBody1.
// Total elastic force:
double rTotalElasticConst;
{
double fK1 = i_oRigidBody1.ElasticConstant();
double fK2 = i_oRigidBody2.ElasticConstant();
if ( *(int*)&fK1 != 0 && *(int*)&fK2 != 0 ) // the two bodies' behavior is the same as two spring in series
{
rTotalElasticConst = fK1 * fK2 / (fK1 + fK2);
}
else if ( *(int*)&fK1 == 0 ) // only the second body produce elastic force
{
rTotalElasticConst = fK2;
}
else // only the first body produce elastic force or neither
{
rTotalElasticConst = fK1;
}
}
PCEVector3D vElasticForce = m_vImpactNormal * ( rTotalElasticConst * m_rDeformation );
// Fluid Friction:
PCEVector3D vTotalFluidFriction = ( i_oRigidBody1.Velocity() * i_oRigidBody1.Muv() ) + ( i_oRigidBody2.Velocity() * i_oRigidBody2.Muv() );
PCEVector3D vNormalFluidFriction = DotProduct(vTotalFluidFriction, m_vImpactNormal) * m_vImpactNormal;
PCEVector3D vTangFluidFriction = vTotalFluidFriction - vNormalFluidFriction;
// Force Normal Component: elastic force + fluid friction normal
double rNormalForceModule = DotProduct( vElasticForce + vNormalFluidFriction, m_vImpactNormal );
if(rNormalForceModule < 0)
{
rNormalForceModule = 0;
}
PCEVector3D vNormForce = m_vImpactNormal * rNormalForceModule;
// Force Tangent Component: dry friction and fluid friction tangent
PCEVector3D vTangVelocity = m_vImpactVelocity - (DotProduct(m_vImpactVelocity,m_vImpactNormal) * m_vImpactNormal);
double rFrictionForceModule = rNormalForceModule * i_rMu;
PCEVector3D vTangForce = ( vTangVelocity * rFrictionForceModule ) + vTangFluidFriction;
/*
double modVtang = vTangVelocity.Module();
if(modVtang > 9.81f * i_rFrameTime)
{
vTangForce /= modVtang;
}
else
{
vTangForce /= ( 9.8f * i_rFrameTime );
}
*/
vNormForce += vTangForce; // now vNormForce is the total force
i_oRigidBody1.ApplyForce(vNormForce, m_vImpactPoint);
vNormForce[0] = -vNormForce[0];
vNormForce[1] = -vNormForce[1];
vNormForce[2] = -vNormForce[2];
i_oRigidBody2.ApplyForce(vNormForce, m_vImpactPoint);
}
