Syntriax.Engine/Engine.Physics2D/CollisionResolver2D.cs

45 lines
1.6 KiB
C#

using Syntriax.Engine.Core;
using Syntriax.Engine.Physics2D.Abstract;
namespace Syntriax.Engine.Physics2D;
public class CollisionResolver2D : ICollisionResolver2D
{
public void Resolve(CollisionDetectionInformation collisionInformation)
{
Vector2D displacementVector = collisionInformation.Normal * collisionInformation.Penetration;
ICollider2D left = collisionInformation.Left;
ICollider2D right = collisionInformation.Right;
bool isLeftStatic = left.RigidBody2D?.IsStatic ?? true;
bool isRightStatic = right.RigidBody2D?.IsStatic ?? true;
if (isLeftStatic && isRightStatic)
return;
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;
}
left.Recalculate();
right.Recalculate();
left.OnCollisionResolved?.Invoke(collisionInformation.Left, collisionInformation);
right.OnCollisionResolved?.Invoke(right, collisionInformation);
}
}