//================================================================================
//-- Compute Impulse (Friction and restitution) ----------------------------------
//================================================================================
//
// -(1+e)(Vr • n)
// j = -------------------------------------------------------
// (1/Ma + 1/Mb) + (Ia' * (ra x n)^2) + (Ib' * (rb x n)^2)
//
// Where:
//
// j = Impulse
// e = Coefficient of Restitution
// Vr = Relative velocity (A.Velocity - B.Velocity)
// n = Collision normal
// Ma = Mass of A
// Mb = Mass of B
// Ia' = Inverse Moment of Inertia for A
// Ib' = Inverse Moment of Inertia for B
RigidBody2D a = contact.body[0];
RigidBody2D b = contact.body[1];
float ma = a.Mass_Inverse;
float mb = b.Mass_Inverse;
// (1/Ma + 1/Mb)
float m = ma + mb;
// Coefficient of restitution
float e = 0.1f;
// Calculate relative velocity
Vector2 velocity_Relative = a.Velocity - b.Velocity; // Velocity under gravity = (0, -9.81f)
Vector2 ra = contact.point[0] - a.Position;
Vector2 rb = contact.point[1] - b.Position;
float raXn = Math_Util.Cross(ra, contact.normal);
float rbXn = Math_Util.Cross(rb, contact.normal);
// (Ia' * (ra x n)^2)
float t0 = raXn * raXn * a.Inertia_Inverse;
// (Ib' * (rb x n)^2)
float t1 = rbXn * rbXn * b.Inertia_Inverse;
// (1/Ma + 1/Mb) + (Ia' * (ra x n)^2) + (Ib' * (rb x n)^2)
float denom = m + t0 + t1;
// Calculate scalar impulse
float j = -(1f + e) * Vector2.Dot(velocity_Relative, contact.normal);
j /= denom;
// Calculate vector impulse
Vector2 impulse = j * contact.normal;
// ===========================================
// -[FRICTION]--------------------------------
// ===========================================
// Coefficient of friction
//float f = 0f;
if (!a.HasInfiniteMass)
{
// Add impulse force at contact point for each body
a.ApplyImpulse(impulse, contact.point[0]);
if (a.Velocity.LengthSquared() < 0.001f)
{
a.Velocity = Vector2.Zero;
}
}
if (!b.HasInfiniteMass)
{
// Negate the force for the opposite body (Newton's Third Law)
b.ApplyImpulse(-impulse, contact.point[1]);
if (b.Velocity.LengthSquared() < 0.001f)
{
b.Velocity = Vector2.Zero;
}
}
public void Integrate(float duration)
{
// ==========================================================================================================
// --[Linear]------------------------------------------------------------------------------------------------
// ==========================================================================================================
// Update acceleration from accumulated forces
// F = M * A
// A = F / M
Vector2 acceleration = force_Accumulated * mass_Inverse;
// Update linear velocity from the acceleration
// V = V + A * dT
velocity_Linear += acceleration * duration;
// Update linear position
// P = P + V * dT
position += velocity_Linear * duration;
// ==========================================================================================================
// --[Angular]-----------------------------------------------------------------------------------------------
// ==========================================================================================================
// aA = T / I
float acceleration_Angular = torque_Accumulated * shape.Inertia_Inverse;
// aV = aV + aA * dT
velocity_Angular += acceleration_Angular * duration;
// R = R + aV * dT
rotation += velocity_Angular * duration;
// ==========================================================================================================
// Clear the accumulated forces
force_Accumulated = Vector2.Zero;
torque_Accumulated = 0f;
// Update collision shape if it exists
if (shape != null)
{
shape.Position = position;
shape.Rotation = rotation;
}
}
