Engine-Pong/Game/Physics2D/PhysicsEngine2D.cs

147 lines
4.6 KiB
C#
Raw Normal View History

2023-11-30 17:52:09 +03:00
using System;
using System.Collections.Generic;
2023-12-05 17:39:38 +03:00
2023-11-30 17:52:09 +03:00
using Microsoft.Xna.Framework;
2023-12-05 17:39:38 +03:00
2023-11-30 17:52:09 +03:00
using Syntriax.Engine.Core.Abstract;
using Syntriax.Engine.Physics2D.Abstract;
2023-12-07 10:55:49 +03:00
using Syntriax.Engine.Physics2D.Primitives;
2023-11-30 17:52:09 +03:00
namespace Syntriax.Engine.Physics2D;
public class PhysicsEngine2D : IPhysicsEngine2D
{
2023-12-06 13:25:16 +03:00
private List<IRigidBody2D> rigidBodies = new List<IRigidBody2D>(32);
private List<ICollider2D> colliders = new List<ICollider2D>(64);
2023-11-30 17:52:09 +03:00
private int _iterationCount = 1;
public int IterationCount { get => _iterationCount; set => _iterationCount = value < 1 ? 1 : value; }
public void AddRigidBody(IRigidBody2D rigidBody)
{
if (rigidBodies.Contains(rigidBody))
return;
rigidBodies.Add(rigidBody);
foreach (var collider2D in rigidBody.BehaviourController.GetBehaviours<ICollider2D>())
colliders.Add(collider2D);
rigidBody.BehaviourController.OnBehaviourAdded += OnBehaviourAdded;
rigidBody.BehaviourController.OnBehaviourRemoved += OnBehaviourRemoved;
}
public void RemoveRigidBody(IRigidBody2D rigidBody)
{
rigidBodies.Remove(rigidBody);
}
2023-12-06 15:56:46 +03:00
private List<Line> lines = new List<Line>(32);
2023-12-06 14:14:55 +03:00
2023-11-30 17:52:09 +03:00
public void Step(float deltaTime)
{
float intervalDeltaTime = deltaTime / IterationCount;
2023-12-05 17:55:38 +03:00
for (int iterationIndex = 0; iterationIndex < IterationCount; iterationIndex++)
2023-11-30 17:52:09 +03:00
{
foreach (var rigidBody in rigidBodies)
2023-12-04 23:55:43 +03:00
StepRigidBody(rigidBody, intervalDeltaTime);
2023-11-30 17:52:09 +03:00
2023-12-01 17:42:07 +03:00
foreach (var collider in colliders)
2023-12-05 17:39:38 +03:00
collider.Recalculate();
2023-12-01 17:42:07 +03:00
2023-12-06 13:25:16 +03:00
for (int i = colliders.Count - 1; i >= 0; i--)
2023-12-06 16:52:48 +03:00
CheckCollisions(colliders[i], colliders, (collider1, collider2, collisionVertex) =>
2023-12-06 17:39:21 +03:00
ResolveCollision(collider1, collider2, collisionVertex, intervalDeltaTime)
);
2023-12-06 13:25:16 +03:00
}
}
2023-12-06 16:52:48 +03:00
private void ResolveCollision(ICollider2D c1, ICollider2D c2, Vector2 vertex, float intervalDeltaTime)
2023-12-06 15:56:46 +03:00
{
if (c1.RigidBody2D is not IRigidBody2D)
return;
2023-12-06 16:51:27 +03:00
Line vertexTrajectory = new Line(vertex, vertex - c1.RigidBody2D.Velocity * intervalDeltaTime);
2023-12-06 15:56:46 +03:00
if (vertexTrajectory.LengthSquared <= 0.001f)
return;
c2.Vertices.ToLines(lines);
Vector2 normal = Vector2.UnitY;
2023-12-06 17:36:56 +03:00
float t = 1f;
bool hasIntersectedWithLines = false;
2023-12-06 15:56:46 +03:00
foreach (var line in lines)
{
2023-12-06 16:51:27 +03:00
if (!vertexTrajectory.Intersects(line, out Vector2? intersectionPoint))
2023-12-06 15:56:46 +03:00
continue;
2023-12-06 16:51:27 +03:00
if (intersectionPoint is not Vector2 ip)
continue;
2023-12-06 17:36:56 +03:00
float tTemp = vertexTrajectory.GetT(ip);
if (tTemp > t) // React to the earliest collision value
continue;
t = tTemp;
2023-12-06 15:56:46 +03:00
Vector2 lineDirection = line.Direction;
normal = new(lineDirection.Y, lineDirection.X);
2023-12-06 17:36:56 +03:00
hasIntersectedWithLines = true;
2023-12-06 15:56:46 +03:00
}
2023-12-06 17:36:56 +03:00
if (!hasIntersectedWithLines)
return;
float rewindInterval = t * intervalDeltaTime;
StepRigidBody(c1.RigidBody2D, -rewindInterval);
2023-12-06 15:56:46 +03:00
c1.RigidBody2D.Velocity = Vector2.Reflect(c1.RigidBody2D.Velocity, normal);
2023-12-06 17:36:56 +03:00
StepRigidBody(c1.RigidBody2D, rewindInterval);
2023-12-06 15:56:46 +03:00
}
private void CheckCollisions(ICollider2D collider2D, List<ICollider2D> collider2Ds, Action<ICollider2D, ICollider2D, Vector2> OnCollisionDetectedAction)
2023-12-06 13:25:16 +03:00
{
2023-12-06 14:14:55 +03:00
for (int i = 0; i < collider2Ds.Count; i++)
2023-12-06 13:25:16 +03:00
{
ICollider2D collider2DItem = collider2Ds[i];
if (collider2DItem == collider2D)
continue;
2023-12-05 17:55:38 +03:00
2023-12-06 15:56:46 +03:00
for (int y = 0; y < collider2D.Vertices.Count; y++)
{
Vector2 vertex = collider2D.Vertices[y];
2023-12-06 16:51:27 +03:00
if (collider2DItem.CheckCollision(vertex))
2023-12-06 15:56:46 +03:00
OnCollisionDetectedAction?.Invoke(collider2D, collider2DItem, vertex);
}
2023-11-30 17:52:09 +03:00
}
}
2023-12-06 13:25:16 +03:00
private void StepRigidBody(IRigidBody2D rigidBody, float intervalDeltaTime)
2023-12-04 23:55:43 +03:00
{
Vector2 nextPosition = rigidBody.Transform.Position;
nextPosition += rigidBody.Velocity * intervalDeltaTime;
rigidBody.Transform.Position = nextPosition;
}
2023-11-30 17:52:09 +03:00
private void OnBehaviourAdded(IBehaviourController controller, IBehaviour behaviour)
{
if (behaviour is not ICollider2D collider2D)
return;
colliders.Add(collider2D);
}
private void OnBehaviourRemoved(IBehaviourController controller, IBehaviour behaviour)
{
if (behaviour is not ICollider2D collider2D)
return;
colliders.Remove(collider2D);
}
}