asdfsadfafasfdasf

Game/Game1.cs

@@ -155,7 +155,6 @@ public class Game1 : Game
         if (Keyboard.GetState().IsKeyDown(Keys.E))
             gameManager.Camera.Rotation -= gameTime.ElapsedGameTime.Nanoseconds * 0.000025f;
 
-        Console.WriteLine(gameObjectBall.Transform.Position);
         // if (Keyboard.GetState().IsKeyDown(Keys.N))
         //     seconds = 39.12f;
         // if (Keyboard.GetState().IsKeyDown(Keys.Space))
@@ -174,7 +173,7 @@ public class Game1 : Game
 
         while (physicsTimer + 0.01f < gameTime.TotalGameTime.TotalMilliseconds * .001f)//seconds)
         {
-            Console.WriteLine($"Physics Timer: {physicsTimer}");
+            // Console.WriteLine($"Physics Timer: {physicsTimer}");
             physicsTimer += 0.01f;
             engine.Step(.01f);
         }

Game/Physics2D/Abstract/ICollider2D.cs

@@ -13,7 +13,7 @@ public interface ICollider2D : IBehaviour, IAssignableTransform
 
     IRigidBody2D? RigidBody2D { get; }
 
-    IReadOnlyList<Vector2> Vertices { get; }
+    IList<Vector2> Vertices { get; }
 
     bool CheckCollision(Vector2 point);
     void Recalculate();

Game/Physics2D/Collider2DAABBBehaviour.cs

@@ -15,8 +15,7 @@ public class Collider2DAABBBehaviour : BehaviourOverride, ICollider2D
     public AABB AABBWorld { get; private set; } = null!;
 
     private IRigidBody2D? _rigidBody2D = null;
-    private List<Vector2> vertices = new List<Vector2>(4);
+    private List<Vector2> _vertices = new List<Vector2>(4);
-    private IReadOnlyList<Vector2>? _verticesReadOnly = null;
 
     public IRigidBody2D? RigidBody2D
     {
@@ -35,7 +34,7 @@ public class Collider2DAABBBehaviour : BehaviourOverride, ICollider2D
     ITransform IAssignableTransform.Transform => Transform;
     public bool Assign(ITransform transform) => GameObject.Assign(transform);
 
-    public IReadOnlyList<Vector2> Vertices { get { if (_verticesReadOnly is null) _verticesReadOnly = vertices.AsReadOnly(); return _verticesReadOnly; } }
+    public IList<Vector2> Vertices => _vertices;
 
     public bool CheckCollision(Vector2 point)
     {
@@ -49,13 +48,12 @@ public class Collider2DAABBBehaviour : BehaviourOverride, ICollider2D
             AABBLocal.UpperBoundary.Scale(Transform.Scale) + Transform.Position
         );
 
-        vertices.Clear();
+        Vertices.Clear();
-        vertices.Add(AABBWorld.LowerBoundary);
+        Vertices.Add(AABBWorld.LowerBoundary);
-        vertices.Add(new Vector2(AABBWorld.LowerBoundary.X, AABBWorld.UpperBoundary.Y));
+        Vertices.Add(new Vector2(AABBWorld.LowerBoundary.X, AABBWorld.UpperBoundary.Y));
-        vertices.Add(AABBWorld.UpperBoundary);
+        Vertices.Add(AABBWorld.UpperBoundary);
-        vertices.Add(new Vector2(AABBWorld.UpperBoundary.X, AABBWorld.LowerBoundary.Y));
+        Vertices.Add(new Vector2(AABBWorld.UpperBoundary.X, AABBWorld.LowerBoundary.Y));
     }
-
     public Collider2DAABBBehaviour(Vector2 lowerBoundary, Vector2 upperBoundary)
     {
         AABBLocal = new AABB(lowerBoundary, upperBoundary);

Game/Physics2D/PhysicsEngine2D.cs

@@ -38,6 +38,7 @@ public class PhysicsEngine2D : IPhysicsEngine2D
     }
 
     // List<IGameObject> gameObjects = new List<IGameObject>();
+    private List<Line> lines = new List<Line>(32);
 
     public void Step(float deltaTime)
     {
@@ -66,15 +67,55 @@ public class PhysicsEngine2D : IPhysicsEngine2D
             //     }
 
             for (int i = colliders.Count - 1; i >= 0; i--)
-                CheckCollisions(colliders[i], colliders, (c1, c2) =>
+                CheckCollisions(colliders[i], colliders, (c1, c2, v) =>
                 {
-                    if (c1.RigidBody2D is IRigidBody2D c1RigidBody) { c1RigidBody.Velocity = -c1RigidBody.Velocity; StepRigidBody(c1RigidBody, intervalDeltaTime); c1.Recalculate(); }
+                    NewMethod(c1, c2, v, intervalDeltaTime);
-                    if (c2.RigidBody2D is IRigidBody2D c2RigidBody) { c2RigidBody.Velocity = -c2RigidBody.Velocity; StepRigidBody(c2RigidBody, intervalDeltaTime); c2.Recalculate(); }
+                    NewMethod(c2, c1, v, intervalDeltaTime);
+
+                    // if (c2.RigidBody2D is IRigidBody2D)
+                    // {
+                    //     c2.RigidBody2D.Velocity = -c2.RigidBody2D.Velocity;
+                    //     StepRigidBody(c2.RigidBody2D, intervalDeltaTime);
+                    // }
+
+                    c1.Recalculate();
+                    c2.Recalculate();
                 });
         }
     }
 
-    private void CheckCollisions(ICollider2D collider2D, List<ICollider2D> collider2Ds, Action<ICollider2D, ICollider2D> OnCollisionDetectedAction)
+    private void NewMethod(ICollider2D c1, ICollider2D c2, Vector2 vertex, float intervalDeltaTime)
+    {
+        if (c1.RigidBody2D is not IRigidBody2D)
+            return;
+
+        Line vertexTrajectory = new Line(vertex, vertex - c1.RigidBody2D.Velocity);
+        if (vertexTrajectory.LengthSquared <= 0.001f)
+            return;
+
+        c2.Vertices.ToLines(lines);
+
+        Vector2 normal = Vector2.UnitY;
+        float t = 0f;
+
+        foreach (var line in lines)
+        {
+            if (!vertexTrajectory.Intersects(line, out Vector2? intersectionPoint) || intersectionPoint is not null)
+                continue;
+
+            t = vertexTrajectory.GetT(vertex);
+            Vector2 lineDirection = line.Direction;
+            normal = new(lineDirection.Y, lineDirection.X);
+            break;
+        }
+
+        StepRigidBody(c1.RigidBody2D, -t);
+
+        c1.RigidBody2D.Velocity = Vector2.Reflect(c1.RigidBody2D.Velocity, normal);
+        StepRigidBody(c1.RigidBody2D, intervalDeltaTime - t);
+    }
+
+    private void CheckCollisions(ICollider2D collider2D, List<ICollider2D> collider2Ds, Action<ICollider2D, ICollider2D, Vector2> OnCollisionDetectedAction)
     {
         for (int i = 0; i < collider2Ds.Count; i++)
         {
@@ -82,9 +123,15 @@ public class PhysicsEngine2D : IPhysicsEngine2D
             if (collider2DItem == collider2D)
                 continue;
 
-            for (int y = 0; y < collider2DItem.Vertices.Count; y++)
+            for (int y = 0; y < collider2D.Vertices.Count; y++)
-                if (collider2D.CheckCollision(collider2DItem.Vertices[y]))
+            {
-                    OnCollisionDetectedAction?.Invoke(collider2D, collider2DItem);
+                Vector2 vertex = collider2D.Vertices[y];
+                if (collider2D.CheckCollision(vertex))
+                {
+                    OnCollisionDetectedAction?.Invoke(collider2D, collider2DItem, vertex);
+                    y--; // Recheck current vertex
+                }
+            }
         }
     }
 

Game/Physics2D/PhysicsMath.cs

@@ -6,7 +6,13 @@ using Microsoft.Xna.Framework;
 
 namespace Syntriax.Engine.Physics2D;
 
-public record Line(Vector2 From, Vector2 To);
+public record Line(Vector2 From, Vector2 To)
+{
+    public Vector2 Direction => Vector2.Normalize(To - From);
+    public float Length => (From - To).Length();
+    public float LengthSquared => (From - To).LengthSquared();
+}
+
 public record LineEquation(float Slope, float OffsetY);
 public record Triangle(Vector2 A, Vector2 B, Vector2 C);
 public record Circle(Vector2 Position, float Radius);
@@ -90,14 +96,14 @@ public static class PhysicsMath
         return new Triangle(p1, p2, p3);
     }
 
-    public static List<Line> ToLines(IList<Vector2> vertices)
+    public static IList<Line> ToLines(this IList<Vector2> vertices)
     {
         List<Line> lines = new List<Line>(vertices.Count - 1);
         ToLines(vertices, lines);
         return lines;
     }
 
-    public static void ToLines(IList<Vector2> vertices, IList<Line> lines)
+    public static void ToLines(this IList<Vector2> vertices, IList<Line> lines)
     {
         lines.Clear();
         for (int i = 0; i < vertices.Count - 1; i++)
@@ -123,14 +129,7 @@ public static class PhysicsMath
     }
 
     public static bool LaysOn(this Vector2 point, Line line)
-    {
+        => line.Resolve(point.X) == point;
-        LineEquation lineEquation = line.ToLineEquation();
-
-        // y = mx + b
-        float y = lineEquation.Slope * point.X + lineEquation.OffsetY;
-
-        return y == point.Y;
-    }
 
     public static LineEquation ToLineEquation(this Line line)
     {
@@ -178,6 +177,32 @@ public static class PhysicsMath
         => ((l1.From.X - l0.From.X) * (l0.To.Y - l0.From.Y) - (l1.From.Y - l0.From.Y) * (l0.To.X - l0.From.X)) /
             ((l1.To.Y - l1.From.Y) * (l0.To.X - l0.From.X) - (l1.To.X - l1.From.X) * (l0.To.Y - l0.From.Y));
 
+    public static float GetT(this Line line, Vector2 point)
+    {
+        if (!point.LaysOn(line))
+            throw new Exception("Point does not lay on Line");
+
+        float fromX = MathF.Abs(line.From.X);
+        float toX = MathF.Abs(line.To.X);
+        float pointX = MathF.Abs(point.X);
+
+        float min = MathF.Min(fromX, toX);
+        float max = MathF.Max(fromX, toX) - min;
+
+        pointX -= min;
+
+        return pointX / max;
+    }
+
+    public static Vector2 Resolve(this Line line, float x)
+    {
+        LineEquation lineEquation = line.ToLineEquation();
+
+        // y = mx + b
+        float y = lineEquation.Slope * x + lineEquation.OffsetY;
+        return new Vector2(x, y);
+    }
+
     public static Vector2 IntersectionPoint(this Line l1, Line l2)
         => Vector2.Lerp(l1.From, l1.To, IntersectionParameterT(l1, l2));