Original Post
I have figured out the exact contact points and the velocity for my rigid bodies but something is still not correct with my impulse calculation as something as simple as two interpenetrating circles do not resolve penetration correctly based on impulses. I calculate the impulse as follows: I have tried zeroing out the velocity vector if it is too small due to floating point inaccuracies but that doesn't help. The only force acting here is gravity and that still allows one shape to intersect another. The shape positions are updated according to the body positions in the rigid body integrate method: This is where the force due to gravity pulls one shape inside the other very slowly as if the shape were made of quicksand. I've tried everything I can think of and I've simply no idea how to stop this from occuring. Collision detection works perfectly based on shape position alone, so something about this impulse process just isn't correct. I've combed through a lot of oliii's code for ideas but I still can't find any obvious errors in my code based on his. Can anyone help here, this is driving me crazy! Thank you.
//================================================================================
//-- 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;
}
}
