diff --git a/Game/Game1.cs b/Game/Game1.cs index ab4b321..dab7b50 100644 --- a/Game/Game1.cs +++ b/Game/Game1.cs @@ -90,22 +90,26 @@ public class Game1 : Game goPlayAreaTop.Transform.Position = new Vector2(0f, 288f + 20f); goPlayAreaTop.Transform.Scale = new Vector2(10240f, 40f); // goPlayAreaTop.BehaviourController.AddBehaviour().Assign(spriteBox); + goPlayAreaTop.BehaviourController.AddBehaviour().AABBLocal = new AABB(-Vector2.One, Vector2.One); engine.AddRigidBody(goPlayAreaTop.BehaviourController.AddBehaviour()); IGameObject goPlayAreaBottom = gameManager.InstantiateGameObject(); goPlayAreaBottom.Transform.Position = new Vector2(0f, -(288f + 20f)); goPlayAreaBottom.Transform.Scale = new Vector2(10240f, 40f); // goPlayAreaBottom.BehaviourController.AddBehaviour().Assign(spriteBox); + goPlayAreaBottom.BehaviourController.AddBehaviour().AABBLocal = new AABB(-Vector2.One, Vector2.One); engine.AddRigidBody(goPlayAreaBottom.BehaviourController.AddBehaviour()); IGameObject goPlayAreaRight = gameManager.InstantiateGameObject(); goPlayAreaRight.Transform.Position = new Vector2(512f + 20f, 0f); goPlayAreaRight.Transform.Scale = new Vector2(40f, 5760f); // goPlayAreaRight.BehaviourController.AddBehaviour().Assign(spriteBox); + goPlayAreaRight.BehaviourController.AddBehaviour().AABBLocal = new AABB(-Vector2.One, Vector2.One); engine.AddRigidBody(goPlayAreaRight.BehaviourController.AddBehaviour()); IGameObject goPlayAreaLeft = gameManager.InstantiateGameObject(); goPlayAreaLeft.Transform.Position = new Vector2(-(512f + 20f), 0f); goPlayAreaLeft.Transform.Scale = new Vector2(40f, 5760f); // goPlayAreaLeft.BehaviourController.AddBehaviour().Assign(spriteBox); + goPlayAreaLeft.BehaviourController.AddBehaviour().AABBLocal = new AABB(-Vector2.One, Vector2.One); engine.AddRigidBody(goPlayAreaLeft.BehaviourController.AddBehaviour()); // IGameObject goPlayAreaCenter = gameManager.InstantiateGameObject(); diff --git a/Game/Physics2D/Abstract/ICollider2D.cs b/Game/Physics2D/Abstract/ICollider2D.cs index 6edb610..1152632 100644 --- a/Game/Physics2D/Abstract/ICollider2D.cs +++ b/Game/Physics2D/Abstract/ICollider2D.cs @@ -1,6 +1,5 @@ using System; using System.Collections.Generic; -using System.Diagnostics.CodeAnalysis; using Microsoft.Xna.Framework; @@ -10,10 +9,12 @@ namespace Syntriax.Engine.Physics2D.Abstract; public interface ICollider2D : IBehaviour, IAssignableTransform { - IRigidBody2D? RigidBody2D { get; } - Action? OnCollisionPreResolve { get; set; } - bool CheckCollision(Vector2 point, ICollider2D otherCollider); + IRigidBody2D? RigidBody2D { get; } + + IReadOnlyList Vertices { get; } + + bool CheckCollision(Vector2 point); void Recalculate(); } diff --git a/Game/Physics2D/Collider2DAABBBehaviour.cs b/Game/Physics2D/Collider2DAABBBehaviour.cs index bd1f73c..c432ba8 100644 --- a/Game/Physics2D/Collider2DAABBBehaviour.cs +++ b/Game/Physics2D/Collider2DAABBBehaviour.cs @@ -9,17 +9,14 @@ using Syntriax.Engine.Physics2D.Abstract; namespace Syntriax.Engine.Physics2D; -public class Collider2DAABBBehaviour(Vector2 lowerBound, Vector2 upperBound) : BehaviourOverride, ICollider2D +public class Collider2DAABBBehaviour : BehaviourOverride, ICollider2D { - public Vector2 LowerBound { get; } = lowerBound; - public Vector2 UpperBound { get; } = upperBound; - - - private Vector2 worldLowerBound = lowerBound; - private Vector2 worldUpperBound = upperBound; - + public AABB AABBLocal { get; set; } = null!; + public AABB AABBWorld { get; private set; } = null!; private IRigidBody2D? _rigidBody2D = null; + private List vertices = new List(4); + private IReadOnlyList? _verticesReadOnly = null; public IRigidBody2D? RigidBody2D { @@ -38,18 +35,36 @@ public class Collider2DAABBBehaviour(Vector2 lowerBound, Vector2 upperBound) : B ITransform IAssignableTransform.Transform => Transform; public bool Assign(ITransform transform) => GameObject.Assign(transform); - public bool CheckCollision(Vector2 point, ICollider2D otherCollider) - { - if (point.X >= worldLowerBound.X && point.X <= worldUpperBound.X && - point.Y >= worldLowerBound.Y && point.Y <= worldUpperBound.Y) - return true; + public IReadOnlyList Vertices { get { if (_verticesReadOnly is null) _verticesReadOnly = vertices.AsReadOnly(); return _verticesReadOnly; } } - return false; + public bool CheckCollision(Vector2 point) + { + return AABBWorld.Inside(point); } public void Recalculate() { - worldLowerBound = LowerBound + Transform.Position; - worldUpperBound = UpperBound + Transform.Position; + AABBWorld = new AABB( + AABBLocal.LowerBoundary + Transform.Position, + AABBLocal.UpperBoundary + Transform.Position + ); + + vertices.Clear(); + vertices.Add(AABBWorld.LowerBoundary); + vertices.Add(new(AABBWorld.LowerBoundary.X, AABBWorld.UpperBoundary.Y)); + vertices.Add(AABBWorld.UpperBoundary); + vertices.Add(new(AABBWorld.LowerBoundary.Y, AABBWorld.UpperBoundary.X)); + } + + public Collider2DAABBBehaviour(Vector2 lowerBoundary, Vector2 upperBoundary) + { + AABBLocal = new AABB(lowerBoundary, upperBoundary); + AABBWorld = new AABB(lowerBoundary, upperBoundary); + } + + public Collider2DAABBBehaviour() + { + AABBLocal = new(Vector2.Zero, Vector2.Zero); + AABBWorld = new(Vector2.Zero, Vector2.Zero); } } diff --git a/Game/Physics2D/PhysicsEngine2D.cs b/Game/Physics2D/PhysicsEngine2D.cs index 59b1d93..ddcb322 100644 --- a/Game/Physics2D/PhysicsEngine2D.cs +++ b/Game/Physics2D/PhysicsEngine2D.cs @@ -14,8 +14,8 @@ namespace Syntriax.Engine.Physics2D; public class PhysicsEngine2D : IPhysicsEngine2D { - private IList rigidBodies = new List(32); - private IList colliders = new List(64); + private List rigidBodies = new List(32); + private List colliders = new List(64); private int _iterationCount = 1; @@ -54,19 +54,30 @@ public class PhysicsEngine2D : IPhysicsEngine2D foreach (var collider in colliders) collider.Recalculate(); - for (int ix = colliders.Count - 1; ix >= 0; ix--) - { - ICollider2D colliderX = colliders[ix]; - for (int iy = colliders.Count - 1; iy >= ix + 1; iy--) + for (int i = colliders.Count - 1; i >= 0; i--) + CheckCollisions(colliders[i], colliders, (c1, c2) => { - ICollider2D colliderY = colliders[iy]; - - } - } + if (c1.RigidBody2D is IRigidBody2D c1RigidBody) c1RigidBody.Velocity = -c1RigidBody.Velocity; + if (c2.RigidBody2D is IRigidBody2D c2RigidBody) c2RigidBody.Velocity = -c2RigidBody.Velocity; + }); } } - private static void StepRigidBody(IRigidBody2D rigidBody, float intervalDeltaTime) + private void CheckCollisions(ICollider2D collider2D, List collider2Ds, Action OnCollisionDetectedAction) + { + for (int i = collider2Ds.Count - 1; i >= 0; i--) + { + ICollider2D collider2DItem = collider2Ds[i]; + if (collider2DItem == collider2D) + continue; + + foreach (var vertex in collider2DItem.Vertices) + if (collider2D.CheckCollision(vertex)) + OnCollisionDetectedAction?.Invoke(collider2D, collider2DItem); + } + } + + private void StepRigidBody(IRigidBody2D rigidBody, float intervalDeltaTime) { Vector2 nextPosition = rigidBody.Transform.Position; nextPosition += rigidBody.Velocity * intervalDeltaTime; diff --git a/Game/Physics2D/PhysicsMath.cs b/Game/Physics2D/PhysicsMath.cs index 54cf2cc..3f645f3 100644 --- a/Game/Physics2D/PhysicsMath.cs +++ b/Game/Physics2D/PhysicsMath.cs @@ -9,21 +9,11 @@ namespace Syntriax.Engine.Physics2D; public record Line(Vector2 From, Vector2 To); public record LineEquation(float Slope, float OffsetY); public record Triangle(Vector2 A, Vector2 B, Vector2 C); -public record Circle(Vector2 Center, double Radius); +public record Circle(Vector2 Position, float Radius); +public record AABB(Vector2 LowerBoundary, Vector2 UpperBoundary); public static class PhysicsMath { - public static float IntersectionParameterT(Vector2 p0, Vector2 p1, Vector2 q0, Vector2 q1) - => ((q0.X - p0.X) * (p1.Y - p0.Y) - (q0.Y - p0.Y) * (p1.X - p0.X)) / - ((q1.Y - q0.Y) * (p1.X - p0.X) - (q1.X - q0.X) * (p1.Y - p0.Y)); - - public static float IntersectionParameterT(this Line l1, Line l2) - => ((l2.From.X - l1.From.X) * (l1.To.Y - l1.From.Y) - (l2.From.Y - l1.From.Y) * (l1.To.X - l1.From.X)) / - ((l2.To.Y - l2.From.Y) * (l1.To.X - l1.From.X) - (l2.To.X - l2.From.X) * (l1.To.Y - l1.From.Y)); - - public static Vector2 IntersectionPoint(this Line l1, Line l2) - => Vector2.Lerp(l1.From, l1.To, IntersectionParameterT(l1, l2)); - public static Vector2 ClosestPointTo(this Line line, Vector2 point) { // Convert edge points to vectors @@ -31,118 +21,38 @@ public static class PhysicsMath var pointVector = new Vector2(point.X - line.From.X, point.Y - line.From.Y); // Calculate the projection of pointVector onto edgeVector - double t = (pointVector.X * edgeVector.X + pointVector.Y * edgeVector.Y) / (edgeVector.X * edgeVector.X + edgeVector.Y * edgeVector.Y); + float t = (pointVector.X * edgeVector.X + pointVector.Y * edgeVector.Y) / (edgeVector.X * edgeVector.X + edgeVector.Y * edgeVector.Y); // Clamp t to the range [0, 1] to ensure the closest point is on the edge t = Math.Max(0, Math.Min(1, t)); // Calculate the closest point on the edge - double closestX = line.From.X + t * edgeVector.X; - double closestY = line.From.Y + t * edgeVector.Y; + float closestX = line.From.X + t * edgeVector.X; + float closestY = line.From.Y + t * edgeVector.Y; return new Vector2((float)closestX, (float)closestY); } - public static double GetArea(this Triangle triangle) + public static float GetArea(this Triangle triangle) { return Math.Abs((triangle.A.X * (triangle.B.Y - triangle.C.Y) + triangle.B.X * (triangle.C.Y - triangle.A.Y) + triangle.C.X * (triangle.A.Y - triangle.B.Y)) * .5f); } - public static bool IsPointInside(this Triangle triangle, Vector2 point) - { - double A = GetArea(triangle); - /* Calculate area of triangle ABC */ - // double A = area(x1, y1, x2, y2, x3, y3); - - double A1 = GetArea(new Triangle(point, triangle.B, triangle.C)); - /* Calculate area of triangle PBC */ - // double A1 = area(x, y, x2, y2, x3, y3); - - /* Calculate area of triangle PAC */ - double A2 = GetArea(new Triangle(triangle.A, point, triangle.C)); - // double A2 = area(x1, y1, x, y, x3, y3); - - /* Calculate area of triangle PAB */ - double A3 = GetArea(new Triangle(triangle.A, triangle.B, point)); - // double A3 = area(x1, y1, x2, y2, x, y); - - /* Check if sum of A1, A2 and A3 is same as A */ - return A >= A1 + A2 + A3; - } - - // Given three collinear points p, q, r, the function checks if - // point q lies on line segment 'pr' - public static bool OnSegment(Vector2 p, Vector2 q, Vector2 r) - { - if (q.X <= Math.Max(p.X, r.X) && q.X >= Math.Min(p.X, r.X) && - q.Y <= Math.Max(p.Y, r.Y) && q.Y >= Math.Min(p.Y, r.Y)) - return true; - - return false; - } - - // To find orientation of ordered triplet (p, q, r). - // The function returns following values - // 0 --> p, q and r are collinear - // 1 --> Clockwise - // 2 --> Counterclockwise - public static int Orientation(Vector2 p, Vector2 q, Vector2 r) - { - // See https://www.geeksforgeeks.org/orientation-3-ordered-points/ - // for details of below formula. - double val = (q.Y - p.Y) * (r.X - q.X) - - (q.X - p.X) * (r.Y - q.Y); - - if (val == 0) return 0; // collinear - - return (val > 0) ? 1 : 2; // clock or counterclock wise - } - - public static bool Intersects(this Line l1, Line l2) - { - int o1 = Orientation(l1.From, l1.To, l2.From); - int o2 = Orientation(l1.From, l1.To, l2.To); - int o3 = Orientation(l2.From, l2.To, l1.From); - int o4 = Orientation(l2.From, l2.To, l1.To); - - if (o1 != o2 && o3 != o4) - return true; - - if (o1 == 0 && OnSegment(l1.From, l2.From, l1.To)) return true; - if (o2 == 0 && OnSegment(l1.From, l2.To, l1.To)) return true; - if (o3 == 0 && OnSegment(l2.From, l1.From, l2.To)) return true; - if (o4 == 0 && OnSegment(l2.From, l1.To, l2.To)) return true; - - return false; - } - - public static bool Intersects(this Line l1, Line l2, [NotNullWhen(returnValue: true)] out Vector2? point) - { - point = null; - - bool result = Intersects(l1, l2); - - if (result) - point = IntersectionPoint(l1, l2); - - return result; - } - public static Circle ToCircumCircle(this Triangle triangle) { Vector2 midAB = (triangle.A + triangle.B) / 2; Vector2 midBC = (triangle.B + triangle.C) / 2; - double slopeAB = (triangle.B.Y - triangle.A.Y) / (triangle.B.X - triangle.A.X); - double slopeBC = (triangle.C.Y - triangle.B.Y) / (triangle.C.X - triangle.B.X); + float slopeAB = (triangle.B.Y - triangle.A.Y) / (triangle.B.X - triangle.A.X); + float slopeBC = (triangle.C.Y - triangle.B.Y) / (triangle.C.X - triangle.B.X); Vector2 center; - if (Math.Abs(slopeAB - slopeBC) > double.Epsilon) + if (Math.Abs(slopeAB - slopeBC) > float.Epsilon) { - double x = (slopeAB * slopeBC * (triangle.A.Y - triangle.C.Y) + slopeBC * (triangle.A.X + triangle.B.X) - slopeAB * (triangle.B.X + triangle.C.X)) / (2 * (slopeBC - slopeAB)); - double y = -(x - (triangle.A.X + triangle.B.X) / 2) / slopeAB + (triangle.A.Y + triangle.B.Y) / 2; + float x = (slopeAB * slopeBC * (triangle.A.Y - triangle.C.Y) + slopeBC * (triangle.A.X + triangle.B.X) - slopeAB * (triangle.B.X + triangle.C.X)) / (2 * (slopeBC - slopeAB)); + float y = -(x - (triangle.A.X + triangle.B.X) / 2) / slopeAB + (triangle.A.Y + triangle.B.Y) / 2; center = new Vector2((float)x, (float)y); } else @@ -153,8 +63,8 @@ public static class PhysicsMath public static Triangle ToSuperTriangle(IList vertices) { - double minX = double.MaxValue, minY = double.MaxValue; - double maxX = double.MinValue, maxY = double.MinValue; + float minX = float.MaxValue, minY = float.MaxValue; + float maxX = float.MinValue, maxY = float.MinValue; foreach (Vector2 point in vertices) { @@ -164,11 +74,11 @@ public static class PhysicsMath maxY = Math.Max(maxY, point.Y); } - double dx = maxX - minX; - double dy = maxY - minY; - double deltaMax = Math.Max(dx, dy); - double midX = (minX + maxX) / 2; - double midY = (minY + maxY) / 2; + float dx = maxX - minX; + float dy = maxY - minY; + float deltaMax = Math.Max(dx, dy); + float midX = (minX + maxX) / 2; + float midY = (minY + maxY) / 2; Vector2 p1 = new Vector2((float)midX - 20f * (float)deltaMax, (float)midY - (float)deltaMax); Vector2 p2 = new Vector2((float)midX, (float)midY + 20 * (float)deltaMax); @@ -228,4 +138,101 @@ public static class PhysicsMath return new LineEquation(slope, yOffset); } + + // Given three collinear points p, q, r, the function checks if + // point q lies on line segment 'pr' + public static bool OnSegment(Vector2 p, Vector2 q, Vector2 r) + { + if (q.X <= Math.Max(p.X, r.X) && q.X >= Math.Min(p.X, r.X) && + q.Y <= Math.Max(p.Y, r.Y) && q.Y >= Math.Min(p.Y, r.Y)) + return true; + + return false; + } + + // To find orientation of ordered triplet (p, q, r). + // The function returns following values + // 0 --> p, q and r are collinear + // 1 --> Clockwise + // 2 --> Counterclockwise + public static int Orientation(Vector2 p, Vector2 q, Vector2 r) + { + // See https://www.geeksforgeeks.org/orientation-3-ordered-points/ + // for details of below formula. + float val = (q.Y - p.Y) * (r.X - q.X) - + (q.X - p.X) * (r.Y - q.Y); + + if (val == 0) return 0; // collinear + + return (val > 0) ? 1 : 2; // clock or counterclock wise + } + + public static float IntersectionParameterT(Vector2 p0, Vector2 p1, Vector2 q0, Vector2 q1) + => ((q0.X - p0.X) * (p1.Y - p0.Y) - (q0.Y - p0.Y) * (p1.X - p0.X)) / + ((q1.Y - q0.Y) * (p1.X - p0.X) - (q1.X - q0.X) * (p1.Y - p0.Y)); + + public static float IntersectionParameterT(this Line l0, Line l1) + => ((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 Vector2 IntersectionPoint(this Line l1, Line l2) + => Vector2.Lerp(l1.From, l1.To, IntersectionParameterT(l1, l2)); + + public static bool Intersects(this Line l1, Line l2) + { + int o1 = Orientation(l1.From, l1.To, l2.From); + int o2 = Orientation(l1.From, l1.To, l2.To); + int o3 = Orientation(l2.From, l2.To, l1.From); + int o4 = Orientation(l2.From, l2.To, l1.To); + + if (o1 != o2 && o3 != o4) + return true; + + if (o1 == 0 && OnSegment(l1.From, l2.From, l1.To)) return true; + if (o2 == 0 && OnSegment(l1.From, l2.To, l1.To)) return true; + if (o3 == 0 && OnSegment(l2.From, l1.From, l2.To)) return true; + if (o4 == 0 && OnSegment(l2.From, l1.To, l2.To)) return true; + + return false; + } + + public static bool Intersects(this Line l1, Line l2, [NotNullWhen(returnValue: true)] out Vector2? point) + { + point = null; + + bool result = Intersects(l1, l2); + + if (result) + point = IntersectionPoint(l1, l2); + + return result; + } + + public static bool Intersects(this Circle circle, Circle circleOther) + { + float distanceSquared = (circle.Position - circleOther.Position).LengthSquared(); + float radiusSumSquared = circle.Radius * circle.Radius + circleOther.Radius * circleOther.Radius; + + return distanceSquared < radiusSumSquared; + } + + public static bool Inside(this Triangle triangle, Vector2 point) + { + float originalTriangleArea = GetArea(triangle); + + float pointTriangleArea1 = GetArea(new Triangle(point, triangle.B, triangle.C)); + float pointTriangleArea2 = GetArea(new Triangle(triangle.A, point, triangle.C)); + float pointTriangleArea3 = GetArea(new Triangle(triangle.A, triangle.B, point)); + + float pointTriangleAreasSum = pointTriangleArea1 + pointTriangleArea2 + pointTriangleArea3; + + return originalTriangleArea >= pointTriangleAreasSum; + } + + public static bool Inside(this AABB aabb, Vector2 point) + => point.X >= aabb.LowerBoundary.X && point.X <= aabb.UpperBoundary.X && + point.Y >= aabb.LowerBoundary.Y && point.Y <= aabb.UpperBoundary.Y; + + public static bool Inside(this Circle circle, Vector2 point) + => (circle.Position - point).LengthSquared() <= circle.Radius * circle.Radius; }