diff --git a/Engine.Physics2D/CollisionResolver2D.cs b/Engine.Physics2D/CollisionResolver2D.cs index 90341be..426c893 100644 --- a/Engine.Physics2D/CollisionResolver2D.cs +++ b/Engine.Physics2D/CollisionResolver2D.cs @@ -6,8 +6,6 @@ public class CollisionResolver2D : ICollisionResolver2D { public void Resolve(CollisionDetectionInformation collisionInformation) { - Vector2D displacementVector = collisionInformation.Normal * collisionInformation.Penetration; - ICollider2D left = collisionInformation.Detector; ICollider2D right = collisionInformation.Detected; @@ -17,6 +15,20 @@ public class CollisionResolver2D : ICollisionResolver2D if (isLeftStatic && isRightStatic) return; + Displace(collisionInformation, left, right, isLeftStatic, isRightStatic); + Bounce(collisionInformation, left, right, isLeftStatic, isRightStatic); + + left.Recalculate(); + right.Recalculate(); + + left.Resolve(collisionInformation); + right.Resolve(collisionInformation); + } + + private static void Displace(CollisionDetectionInformation collisionInformation, ICollider2D left, ICollider2D right, bool isLeftStatic, bool isRightStatic) + { + Vector2D displacementVector = collisionInformation.Normal * collisionInformation.Penetration; + if (isLeftStatic) right.Transform.Position += displacementVector; else if (isRightStatic) @@ -33,11 +45,32 @@ public class CollisionResolver2D : ICollisionResolver2D right.Transform.Position += leftMomentumPercentage * displacementVector; left.Transform.Position -= rightMomentumPercentage * displacementVector; } + } - left.Recalculate(); - right.Recalculate(); + private static void Bounce(CollisionDetectionInformation collisionInformation, ICollider2D left, ICollider2D right, bool isLeftStatic, bool isRightStatic) + { + Vector2D leftVelocity = left.RigidBody2D?.Velocity ?? Vector2D.Zero; + Vector2D rightVelocity = right.RigidBody2D?.Velocity ?? Vector2D.Zero; - left.Resolve(collisionInformation); - right.Resolve(collisionInformation); + Vector2D relativeVelocity = leftVelocity - rightVelocity; + float velocityAlongNormal = Vector2D.Dot(relativeVelocity, collisionInformation.Normal); + + if (velocityAlongNormal > 0) + { + collisionInformation = collisionInformation.Reverse(); + velocityAlongNormal = -velocityAlongNormal; + } + + float e = (left.RigidBody2D?.Material.Restitution ?? 0f).Add(right.RigidBody2D?.Material.Restitution ?? 0f).Divide(2f); + + float leftMassEffective = isLeftStatic ? float.PositiveInfinity : left.RigidBody2D?.Mass ?? float.Epsilon; + float rightMassEffective = isRightStatic ? float.PositiveInfinity : right.RigidBody2D?.Mass ?? float.Epsilon; + float impulse = -(1f + e) * velocityAlongNormal / ((1f / leftMassEffective) + (1f / rightMassEffective)); + + if (!isLeftStatic) + left.RigidBody2D?.Velocity += impulse / leftMassEffective * collisionInformation.Normal; + + if (!isRightStatic) + right.RigidBody2D?.Velocity -= impulse / rightMassEffective * collisionInformation.Normal; } }