77 lines
3.1 KiB
C#
77 lines
3.1 KiB
C#
using Engine.Core;
|
|
|
|
namespace Engine.Physics2D;
|
|
|
|
public class CollisionResolver2D : ICollisionResolver2D
|
|
{
|
|
public void Resolve(CollisionDetectionInformation collisionInformation)
|
|
{
|
|
ICollider2D left = collisionInformation.Detector;
|
|
ICollider2D right = collisionInformation.Detected;
|
|
|
|
bool isLeftStatic = left.RigidBody2D?.IsStatic ?? true;
|
|
bool isRightStatic = right.RigidBody2D?.IsStatic ?? true;
|
|
|
|
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)
|
|
left.Transform.Position -= displacementVector;
|
|
else
|
|
{
|
|
float leftMass = left.RigidBody2D?.Mass ?? float.Epsilon;
|
|
float rightMass = right.RigidBody2D?.Mass ?? float.Epsilon;
|
|
float sumMass = leftMass + rightMass;
|
|
|
|
float leftMomentumPercentage = leftMass / sumMass;
|
|
float rightMomentumPercentage = rightMass / sumMass;
|
|
|
|
right.Transform.Position += leftMomentumPercentage * displacementVector;
|
|
left.Transform.Position -= rightMomentumPercentage * displacementVector;
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
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;
|
|
}
|
|
}
|