2023-11-30 17:52:09 +03:00
|
|
|
using System;
|
|
|
|
using System.Collections.Generic;
|
|
|
|
using Microsoft.Xna.Framework;
|
|
|
|
using Syntriax.Engine.Core.Abstract;
|
|
|
|
using Syntriax.Engine.Physics2D.Abstract;
|
|
|
|
|
|
|
|
namespace Syntriax.Engine.Physics2D;
|
|
|
|
|
|
|
|
public class PhysicsEngine2D : IPhysicsEngine2D
|
|
|
|
{
|
|
|
|
private IList<IRigidBody2D> rigidBodies = new List<IRigidBody2D>(32);
|
|
|
|
private IList<ICollider2D> colliders = new List<ICollider2D>(64);
|
|
|
|
|
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
|
|
|
public void Step(float deltaTime)
|
|
|
|
{
|
|
|
|
float intervalDeltaTime = deltaTime / IterationCount;
|
|
|
|
|
|
|
|
for (int i = 0; i < IterationCount; i++)
|
|
|
|
{
|
|
|
|
foreach (var rigidBody in rigidBodies)
|
|
|
|
{
|
|
|
|
Vector2 nextPosition = rigidBody.Transform.Position;
|
|
|
|
nextPosition += rigidBody.Velocity * intervalDeltaTime;
|
|
|
|
|
2023-12-01 17:42:07 +03:00
|
|
|
rigidBody.Transform.Position = nextPosition;
|
2023-11-30 17:52:09 +03:00
|
|
|
}
|
|
|
|
|
2023-12-01 17:42:07 +03:00
|
|
|
|
|
|
|
foreach (var collider in colliders)
|
|
|
|
collider.RecalculateVertices();
|
|
|
|
|
2023-11-30 17:52:09 +03:00
|
|
|
for (int colliderIX = 0; colliderIX < colliders.Count; colliderIX++)
|
|
|
|
{
|
|
|
|
ICollider2D colliderX = colliders[colliderIX];
|
|
|
|
for (int colliderIY = colliderIX + 1; colliderIY < colliders.Count; colliderIY++)
|
2023-12-04 17:48:22 +03:00
|
|
|
for (int i1 = 0; i1 < colliderX.Vertices.Count; i1++)
|
2023-12-01 17:42:07 +03:00
|
|
|
{
|
2023-12-04 17:48:22 +03:00
|
|
|
Vector2 vertex = colliderX.Vertices[i1];
|
|
|
|
if (!colliders[colliderIY].CheckCollision(vertex, colliderX, out var collisionInformation))
|
2023-12-01 17:42:07 +03:00
|
|
|
continue;
|
|
|
|
|
2023-12-04 17:48:22 +03:00
|
|
|
Console.WriteLine($"normal = {collisionInformation.Normal}");
|
|
|
|
// // if (colliders[colliderIX].BehaviourController.TryGetBehaviour(out IRigidBody2D? rigidX))
|
|
|
|
// // rigidX.Velocity = Vector2.Reflect(rigidX.Velocity, collisionInformation.Normal);
|
2023-12-04 13:35:31 +03:00
|
|
|
if (colliders[colliderIX].BehaviourController.TryGetBehaviour(out IRigidBody2D? rigidX))
|
2023-12-04 17:48:22 +03:00
|
|
|
{
|
|
|
|
Console.WriteLine($"rigidX.Velocity = {rigidX.Velocity}");
|
|
|
|
rigidX.Velocity = Vector2.Reflect(rigidX.Velocity, collisionInformation.Normal);
|
|
|
|
|
|
|
|
Vector2 vector2 = -new Vector2(collisionInformation.Normal.X * rigidX.Velocity.X, collisionInformation.Normal.Y * rigidX.Velocity.Y);
|
|
|
|
Console.WriteLine($"Displacement = {vector2}");
|
|
|
|
rigidX.Transform.Position += vector2 * intervalDeltaTime * 2;
|
|
|
|
Console.WriteLine($"rigidX.Velocity = {rigidX.Velocity}");
|
|
|
|
}
|
2023-12-04 13:35:31 +03:00
|
|
|
if (colliders[colliderIY].BehaviourController.TryGetBehaviour(out IRigidBody2D? rigidY))
|
2023-12-04 17:48:22 +03:00
|
|
|
{
|
|
|
|
Console.WriteLine($"rigidY.Velocity = {rigidY.Velocity}");
|
|
|
|
rigidY.Velocity = Vector2.Reflect(rigidY.Velocity, collisionInformation.Normal);
|
|
|
|
Vector2 vector2 = -new Vector2(collisionInformation.Normal.X * rigidY.Velocity.X, collisionInformation.Normal.Y * rigidY.Velocity.Y);
|
|
|
|
Console.WriteLine($"Displacement = {vector2}");
|
|
|
|
rigidY.Transform.Position += vector2 * intervalDeltaTime * 2;
|
|
|
|
Console.WriteLine($"rigidY.Velocity = {rigidY.Velocity}");
|
|
|
|
}
|
|
|
|
Console.WriteLine($"/////////////////////////////////////////////");
|
|
|
|
colliders[colliderIY].RecalculateVertices();
|
|
|
|
colliders[colliderIX].RecalculateVertices();
|
2023-12-04 13:35:31 +03:00
|
|
|
|
2023-12-04 13:14:23 +03:00
|
|
|
// Console.WriteLine($"Collision");
|
2023-12-01 17:42:07 +03:00
|
|
|
}
|
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);
|
|
|
|
}
|
|
|
|
}
|