45 lines
1.6 KiB
C#
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);
|
||
|
|
}
|
||
|
|
}
|