From 6183d2225f9f4ecc3d0f4ce9831769e52e352598 Mon Sep 17 00:00:00 2001 From: Syntriax Date: Thu, 7 Dec 2023 10:55:49 +0300 Subject: [PATCH] Refactor --- Game/Physics2D/Collider2DAABBBehaviour.cs | 3 +- Game/Physics2D/PhysicsEngine2D.cs | 1 + Game/Physics2D/PhysicsMath.cs | 197 ++-------------------- Game/Physics2D/Primitives/AABB.cs | 10 ++ Game/Physics2D/Primitives/Circle.cs | 16 ++ Game/Physics2D/Primitives/Line.cs | 121 +++++++++++++ Game/Physics2D/Primitives/LineEquation.cs | 6 + Game/Physics2D/Primitives/Shape.cs | 56 ++++++ Game/Physics2D/Primitives/Triangle.cs | 51 ++++++ 9 files changed, 276 insertions(+), 185 deletions(-) create mode 100644 Game/Physics2D/Primitives/AABB.cs create mode 100644 Game/Physics2D/Primitives/Circle.cs create mode 100644 Game/Physics2D/Primitives/Line.cs create mode 100644 Game/Physics2D/Primitives/LineEquation.cs create mode 100644 Game/Physics2D/Primitives/Shape.cs create mode 100644 Game/Physics2D/Primitives/Triangle.cs diff --git a/Game/Physics2D/Collider2DAABBBehaviour.cs b/Game/Physics2D/Collider2DAABBBehaviour.cs index d1b8fce..ccb59d2 100644 --- a/Game/Physics2D/Collider2DAABBBehaviour.cs +++ b/Game/Physics2D/Collider2DAABBBehaviour.cs @@ -6,6 +6,7 @@ using Microsoft.Xna.Framework; using Syntriax.Engine.Core; using Syntriax.Engine.Core.Abstract; using Syntriax.Engine.Physics2D.Abstract; +using Syntriax.Engine.Physics2D.Primitives; namespace Syntriax.Engine.Physics2D; @@ -38,7 +39,7 @@ public class Collider2DAABBBehaviour : BehaviourOverride, ICollider2D public bool CheckCollision(Vector2 point) { - return AABBWorld.Inside(point); + return AABBWorld.Overlaps(point); } public void Recalculate() diff --git a/Game/Physics2D/PhysicsEngine2D.cs b/Game/Physics2D/PhysicsEngine2D.cs index 49f9fbb..029686f 100644 --- a/Game/Physics2D/PhysicsEngine2D.cs +++ b/Game/Physics2D/PhysicsEngine2D.cs @@ -5,6 +5,7 @@ using Microsoft.Xna.Framework; using Syntriax.Engine.Core.Abstract; using Syntriax.Engine.Physics2D.Abstract; +using Syntriax.Engine.Physics2D.Primitives; namespace Syntriax.Engine.Physics2D; diff --git a/Game/Physics2D/PhysicsMath.cs b/Game/Physics2D/PhysicsMath.cs index 474ab0c..3874330 100644 --- a/Game/Physics2D/PhysicsMath.cs +++ b/Game/Physics2D/PhysicsMath.cs @@ -1,76 +1,18 @@ using System; using System.Collections.Generic; -using System.Diagnostics.CodeAnalysis; using Microsoft.Xna.Framework; +using Syntriax.Engine.Physics2D.Primitives; + namespace Syntriax.Engine.Physics2D; -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); -public record AABB(Vector2 LowerBoundary, Vector2 UpperBoundary); - public static class PhysicsMath { public static Vector2 Scale(this Vector2 original, Vector2 scale) => new Vector2(original.X * scale.X, original.Y * scale.Y); - public static Vector2 ClosestPointTo(this Line line, Vector2 point) - { - // Convert edge points to vectors - var edgeVector = new Vector2(line.To.X - line.From.X, line.To.Y - line.From.Y); - var pointVector = new Vector2(point.X - line.From.X, point.Y - line.From.Y); - - // Calculate the projection of pointVector onto edgeVector - 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 - 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 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 Circle ToCircumCircle(this Triangle triangle) - { - Vector2 midAB = (triangle.A + triangle.B) / 2; - Vector2 midBC = (triangle.B + triangle.C) / 2; - - 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) > float.Epsilon) - { - 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 - center = (midAB + midBC) * .5f; - - return new(center, Vector2.Distance(center, triangle.A)); - } - - public static Triangle ToSuperTriangle(IList vertices) + public static Triangle ToSuperTriangle(this IList vertices) { float minX = float.MaxValue, minY = float.MaxValue; float maxX = float.MinValue, maxY = float.MinValue; @@ -111,35 +53,9 @@ public static class PhysicsMath lines.Add(new(vertices[^1], vertices[0])); } - public static bool ExistIn(Line lineToCheck, List vertices) - { - for (int i = 0; i < vertices.Count - 1; i++) - { - Vector2 vertexCurrent = vertices[i]; - Vector2 vertexNext = vertices[i]; - if (lineToCheck.From == vertexCurrent && lineToCheck.To == vertexNext) return true; - if (lineToCheck.From == vertexNext && lineToCheck.To == vertexCurrent) return true; - } - - Vector2 vertexFirst = vertices[0]; - Vector2 vertexLast = vertices[^1]; - if (lineToCheck.From == vertexFirst && lineToCheck.To == vertexLast) return true; - if (lineToCheck.From == vertexLast && lineToCheck.To == vertexFirst) return true; - return false; - } - public static bool LaysOn(this Vector2 point, Line line) - => ApproximatelyEqualEpsilon(line.Resolve(point.X), point, float.Epsilon); + => line.Resolve(point.X).ApproximatelyEquals(point); - public static LineEquation ToLineEquation(this Line line) - { - Vector2 slopeVector = line.To - line.From; - float slope = slopeVector.Y / slopeVector.X; - - float yOffset = line.From.Y - (slope * line.From.X); - - return new LineEquation(slope, yOffset); - } // Given three collinear points p, q, r, the function checks if // point q lies on line segment 'pr' @@ -173,98 +89,10 @@ public static class PhysicsMath => ((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 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)); - - 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; - - public static bool ApproximatelyEqualEpsilon(float a, float b, float epsilon) + public static bool ApproximatelyEquals(this float a, float b) + => ApproximatelyEqualsEpsilon(a, b, float.Epsilon); + public static bool ApproximatelyEqualsEpsilon(this float a, float b, float epsilon) { if (a == b) return true; @@ -277,10 +105,11 @@ public static class PhysicsMath if (a == 0.0f || b == 0.0f || diff < floatNormal) return diff < (epsilon * floatNormal); - return diff / Math.Min((absA + absB), float.MaxValue) < epsilon; - } - public static bool ApproximatelyEqualEpsilon(Vector2 a, Vector2 b, float epsilon) - { - return ApproximatelyEqualEpsilon(a.X, b.X, epsilon) && ApproximatelyEqualEpsilon(a.Y, b.Y, epsilon); + return diff / Math.Min(absA + absB, float.MaxValue) < epsilon; } + + public static bool ApproximatelyEquals(this Vector2 a, Vector2 b) + => ApproximatelyEqualEpsilon(a, b, float.Epsilon); + public static bool ApproximatelyEqualEpsilon(this Vector2 a, Vector2 b, float epsilon) + => ApproximatelyEqualsEpsilon(a.X, b.X, epsilon) && ApproximatelyEqualsEpsilon(a.Y, b.Y, epsilon); } diff --git a/Game/Physics2D/Primitives/AABB.cs b/Game/Physics2D/Primitives/AABB.cs new file mode 100644 index 0000000..94a4a65 --- /dev/null +++ b/Game/Physics2D/Primitives/AABB.cs @@ -0,0 +1,10 @@ +using Microsoft.Xna.Framework; + +namespace Syntriax.Engine.Physics2D.Primitives; + +public record AABB(Vector2 LowerBoundary, Vector2 UpperBoundary) +{ + public bool Overlaps(Vector2 point) + => point.X >= LowerBoundary.X && point.X <= UpperBoundary.X && + point.Y >= LowerBoundary.Y && point.Y <= UpperBoundary.Y; +} diff --git a/Game/Physics2D/Primitives/Circle.cs b/Game/Physics2D/Primitives/Circle.cs new file mode 100644 index 0000000..734119e --- /dev/null +++ b/Game/Physics2D/Primitives/Circle.cs @@ -0,0 +1,16 @@ +using Microsoft.Xna.Framework; + +namespace Syntriax.Engine.Physics2D.Primitives; + +public record Circle(Vector2 Position, float Radius) +{ + public bool Intersects(Circle circleOther) + { + float distanceSquared = (Position - circleOther.Position).LengthSquared(); + float radiusSumSquared = Radius * Radius + circleOther.Radius * circleOther.Radius; + + return distanceSquared < radiusSumSquared; + } + + public bool Overlaps(Vector2 point) => (Position - point).LengthSquared() <= Radius * Radius; +} diff --git a/Game/Physics2D/Primitives/Line.cs b/Game/Physics2D/Primitives/Line.cs new file mode 100644 index 0000000..483c9ad --- /dev/null +++ b/Game/Physics2D/Primitives/Line.cs @@ -0,0 +1,121 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics.CodeAnalysis; + +using Microsoft.Xna.Framework; + +namespace Syntriax.Engine.Physics2D.Primitives; + +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 LineEquation LineEquation + { + get + { + Vector2 slopeVector = To - From; + float slope = slopeVector.Y / slopeVector.X; + + float yOffset = From.Y - (slope * From.X); + + return new LineEquation(slope, yOffset); + } + } + + public bool Intersects(Vector2 point) + => Resolve(point.X).ApproximatelyEquals(point); + + public float GetT(Vector2 point) + { + float fromX = MathF.Abs(From.X); + float toX = MathF.Abs(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 bool Exist(List vertices) + { + for (int i = 0; i < vertices.Count - 1; i++) + { + Vector2 vertexCurrent = vertices[i]; + Vector2 vertexNext = vertices[i]; + if (From == vertexCurrent && To == vertexNext) return true; + if (From == vertexNext && To == vertexCurrent) return true; + } + + Vector2 vertexFirst = vertices[0]; + Vector2 vertexLast = vertices[^1]; + if (From == vertexFirst && To == vertexLast) return true; + if (From == vertexLast && To == vertexFirst) return true; + return false; + } + + public float IntersectionParameterT(Line other) + => ((other.From.X - From.X) * (To.Y - From.Y) - (other.From.Y - From.Y) * (To.X - From.X)) / + ((other.To.Y - other.From.Y) * (To.X - From.X) - (other.To.X - other.From.X) * (To.Y - From.Y)); + + + public Vector2 Resolve(float x) + => new Vector2(x, LineEquation.Resolve(x)); + + public Vector2 ClosestPointTo(Vector2 point) + { + // Convert edge points to vectors + var edgeVector = new Vector2(To.X - From.X, To.Y - From.Y); + var pointVector = new Vector2(point.X - From.X, point.Y - From.Y); + + // Calculate the projection of pointVector onto edgeVector + 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 + float closestX = From.X + t * edgeVector.X; + float closestY = From.Y + t * edgeVector.Y; + + return new Vector2((float)closestX, (float)closestY); + } + + public Vector2 IntersectionPoint(Line other) + => Vector2.Lerp(From, To, IntersectionParameterT(other)); + + public bool Intersects(Line other) + { + int o1 = PhysicsMath.Orientation(From, To, other.From); + int o2 = PhysicsMath.Orientation(From, To, other.To); + int o3 = PhysicsMath.Orientation(other.From, other.To, From); + int o4 = PhysicsMath.Orientation(other.From, other.To, To); + + if (o1 != o2 && o3 != o4) + return true; + + if (o1 == 0 && PhysicsMath.OnSegment(From, other.From, To)) return true; + if (o2 == 0 && PhysicsMath.OnSegment(From, other.To, To)) return true; + if (o3 == 0 && PhysicsMath.OnSegment(other.From, From, other.To)) return true; + if (o4 == 0 && PhysicsMath.OnSegment(other.From, To, other.To)) return true; + + return false; + } + + public bool Intersects(Line other, [NotNullWhen(returnValue: true)] out Vector2? point) + { + point = null; + + bool result = Intersects(other); + + if (result) + point = IntersectionPoint(other); + + return result; + } +} diff --git a/Game/Physics2D/Primitives/LineEquation.cs b/Game/Physics2D/Primitives/LineEquation.cs new file mode 100644 index 0000000..a85596c --- /dev/null +++ b/Game/Physics2D/Primitives/LineEquation.cs @@ -0,0 +1,6 @@ +namespace Syntriax.Engine.Physics2D.Primitives; + +public record LineEquation(float Slope, float OffsetY) +{ + public float Resolve(float x) => Slope * x + OffsetY; // y = mx + b +} diff --git a/Game/Physics2D/Primitives/Shape.cs b/Game/Physics2D/Primitives/Shape.cs new file mode 100644 index 0000000..8f827ce --- /dev/null +++ b/Game/Physics2D/Primitives/Shape.cs @@ -0,0 +1,56 @@ +using System; +using System.Collections.Generic; + +using Microsoft.Xna.Framework; + +namespace Syntriax.Engine.Physics2D.Primitives; + +public record Shape(IList Vertices) +{ + public Triangle SuperTriangle + { + get + { + float minX = float.MaxValue, minY = float.MaxValue; + float maxX = float.MinValue, maxY = float.MinValue; + + foreach (Vector2 point in Vertices) + { + minX = Math.Min(minX, point.X); + minY = Math.Min(minY, point.Y); + maxX = Math.Max(maxX, point.X); + maxY = Math.Max(maxY, point.Y); + } + + 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); + Vector2 p3 = new Vector2((float)midX + 20 * (float)deltaMax, (float)midY - (float)deltaMax); + + return new Triangle(p1, p2, p3); + } + } + + public IList Lines + { + get + { + List lines = new List(Vertices.Count - 1); + GetLinesNonAlloc(lines); + return lines; + } + } + + public void GetLinesNonAlloc(IList lines) + { + lines.Clear(); + for (int i = 0; i < Vertices.Count - 1; i++) + lines.Add(new(Vertices[i], Vertices[i + 1])); + lines.Add(new(Vertices[^1], Vertices[0])); + } +} diff --git a/Game/Physics2D/Primitives/Triangle.cs b/Game/Physics2D/Primitives/Triangle.cs new file mode 100644 index 0000000..9d610ab --- /dev/null +++ b/Game/Physics2D/Primitives/Triangle.cs @@ -0,0 +1,51 @@ +using System; + +using Microsoft.Xna.Framework; + +namespace Syntriax.Engine.Physics2D.Primitives; + +public record Triangle(Vector2 A, Vector2 B, Vector2 C) +{ + public float Area => Math.Abs(( + A.X * (B.Y - C.Y) + + B.X * (C.Y - A.Y) + + C.X * (A.Y - B.Y) + ) * .5f); + + public Circle CircumCircle + { + get + { + Vector2 midAB = (A + B) / 2; + Vector2 midBC = (B + C) / 2; + + float slopeAB = (B.Y - A.Y) / (B.X - A.X); + float slopeBC = (C.Y - B.Y) / (C.X - B.X); + + Vector2 center; + if (Math.Abs(slopeAB - slopeBC) > float.Epsilon) + { + float x = (slopeAB * slopeBC * (A.Y - C.Y) + slopeBC * (A.X + B.X) - slopeAB * (B.X + C.X)) / (2 * (slopeBC - slopeAB)); + float y = -(x - (A.X + B.X) / 2) / slopeAB + (A.Y + B.Y) / 2; + center = new Vector2((float)x, (float)y); + } + else + center = (midAB + midBC) * .5f; + + return new(center, Vector2.Distance(center, A)); + } + } + + public bool Overlaps(Vector2 point) + { + float originalTriangleArea = Area; + + float pointTriangleArea1 = new Triangle(point, B, C).Area; + float pointTriangleArea2 = new Triangle(A, point, C).Area; + float pointTriangleArea3 = new Triangle(A, B, point).Area; + + float pointTriangleAreasSum = pointTriangleArea1 + pointTriangleArea2 + pointTriangleArea3; + + return originalTriangleArea >= pointTriangleAreasSum; + } +}