diff --git a/Game/Game1.cs b/Game/Game1.cs index bfcb25d..0a07534 100644 --- a/Game/Game1.cs +++ b/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); } diff --git a/Game/Physics2D/Abstract/ICollider2D.cs b/Game/Physics2D/Abstract/ICollider2D.cs index 1152632..64eba9b 100644 --- a/Game/Physics2D/Abstract/ICollider2D.cs +++ b/Game/Physics2D/Abstract/ICollider2D.cs @@ -13,7 +13,7 @@ public interface ICollider2D : IBehaviour, IAssignableTransform IRigidBody2D? RigidBody2D { get; } - IReadOnlyList Vertices { get; } + IList Vertices { get; } bool CheckCollision(Vector2 point); void Recalculate(); diff --git a/Game/Physics2D/Collider2DAABBBehaviour.cs b/Game/Physics2D/Collider2DAABBBehaviour.cs index 6e917d7..d1b8fce 100644 --- a/Game/Physics2D/Collider2DAABBBehaviour.cs +++ b/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 vertices = new List(4); - private IReadOnlyList? _verticesReadOnly = null; + private List _vertices = new List(4); 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 Vertices { get { if (_verticesReadOnly is null) _verticesReadOnly = vertices.AsReadOnly(); return _verticesReadOnly; } } + public IList 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.Add(AABBWorld.LowerBoundary); - vertices.Add(new Vector2(AABBWorld.LowerBoundary.X, AABBWorld.UpperBoundary.Y)); - vertices.Add(AABBWorld.UpperBoundary); - vertices.Add(new Vector2(AABBWorld.UpperBoundary.X, AABBWorld.LowerBoundary.Y)); + Vertices.Clear(); + Vertices.Add(AABBWorld.LowerBoundary); + Vertices.Add(new Vector2(AABBWorld.LowerBoundary.X, AABBWorld.UpperBoundary.Y)); + Vertices.Add(AABBWorld.UpperBoundary); + Vertices.Add(new Vector2(AABBWorld.UpperBoundary.X, AABBWorld.LowerBoundary.Y)); } - public Collider2DAABBBehaviour(Vector2 lowerBoundary, Vector2 upperBoundary) { AABBLocal = new AABB(lowerBoundary, upperBoundary); diff --git a/Game/Physics2D/PhysicsEngine2D.cs b/Game/Physics2D/PhysicsEngine2D.cs index 4caeb41..7803293 100644 --- a/Game/Physics2D/PhysicsEngine2D.cs +++ b/Game/Physics2D/PhysicsEngine2D.cs @@ -38,6 +38,7 @@ public class PhysicsEngine2D : IPhysicsEngine2D } // List gameObjects = new List(); + private List lines = new List(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(); } - if (c2.RigidBody2D is IRigidBody2D c2RigidBody) { c2RigidBody.Velocity = -c2RigidBody.Velocity; StepRigidBody(c2RigidBody, intervalDeltaTime); c2.Recalculate(); } + NewMethod(c1, c2, v, intervalDeltaTime); + 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 collider2Ds, Action 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 collider2Ds, Action 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++) - if (collider2D.CheckCollision(collider2DItem.Vertices[y])) - OnCollisionDetectedAction?.Invoke(collider2D, collider2DItem); + for (int y = 0; y < collider2D.Vertices.Count; y++) + { + Vector2 vertex = collider2D.Vertices[y]; + if (collider2D.CheckCollision(vertex)) + { + OnCollisionDetectedAction?.Invoke(collider2D, collider2DItem, vertex); + y--; // Recheck current vertex + } + } } } diff --git a/Game/Physics2D/PhysicsMath.cs b/Game/Physics2D/PhysicsMath.cs index 273e503..052cdb1 100644 --- a/Game/Physics2D/PhysicsMath.cs +++ b/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 ToLines(IList vertices) + public static IList ToLines(this IList vertices) { List lines = new List(vertices.Count - 1); ToLines(vertices, lines); return lines; } - public static void ToLines(IList vertices, IList lines) + public static void ToLines(this IList vertices, IList 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) - { - LineEquation lineEquation = line.ToLineEquation(); - - // y = mx + b - float y = lineEquation.Slope * point.X + lineEquation.OffsetY; - - return y == point.Y; - } + => line.Resolve(point.X) == point; 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));