diff --git a/Game/Physics2D/Abstract/ICollider2D.cs b/Game/Physics2D/Abstract/ICollider2D.cs index f0717ee..131cac3 100644 --- a/Game/Physics2D/Abstract/ICollider2D.cs +++ b/Game/Physics2D/Abstract/ICollider2D.cs @@ -19,7 +19,7 @@ public interface ICollider2D : IBehaviour, IAssignableTransform IReadOnlyList Vertices { get; } - bool CheckCollision(Vector2 point, ICollider2D otherCollider, out CollisionInformation collisionInformation); + bool CheckCollision(Vector2 point, ICollider2D otherCollider, out CollisionInformation? collisionInformation); void RecalculateVertices(); } diff --git a/Game/Physics2D/Collider2DBehaviour.cs b/Game/Physics2D/Collider2DBehaviour.cs index b71af97..33a2c91 100644 --- a/Game/Physics2D/Collider2DBehaviour.cs +++ b/Game/Physics2D/Collider2DBehaviour.cs @@ -2,8 +2,11 @@ using System; using System.Collections.Generic; using System.Collections.ObjectModel; using System.Diagnostics.CodeAnalysis; + using Microsoft.Xna.Framework; + using Pong; + using Syntriax.Engine.Core; using Syntriax.Engine.Core.Abstract; using Syntriax.Engine.Graphics.TwoDimensional; @@ -11,6 +14,7 @@ using Syntriax.Engine.Physics2D.Abstract; namespace Syntriax.Engine.Physics2D; + public class Collider2DBehaviour(IList vertices) : BehaviourOverride, ICollider2D { private List triangles = new List(32); @@ -49,19 +53,19 @@ public class Collider2DBehaviour(IList vertices) : BehaviourOverride, I foreach (var triangle in triangles) { - if (!isInside(point, triangle)) + if (!PhysicsMath.IsInTriangle(point, triangle)) continue; OnCollision?.Invoke(this, otherCollider); - Edge main = new() { A = otherCollider.Transform.Position, B = point }; + Line main = new(otherCollider.Transform.Position, point); - foreach (var edge in GetEdges(triangle)) + foreach (var line in PhysicsMath.GetLines([triangle.A, triangle.B, triangle.C])) { - if (!DoIntersect(main, edge)) + if (!PhysicsMath.DoIntersect(main, line)) continue; - Vector2 contactPoint = ClosestPointOnEdge(point, edge); + Vector2 contactPoint = PhysicsMath.ClosestPointOnLine(point, line); Vector2 normal = contactPoint - point; if (normal.LengthSquared() < 0.001f) @@ -89,11 +93,11 @@ public class Collider2DBehaviour(IList vertices) : BehaviourOverride, I _vertices.Add(scaledPosition + Transform.Position); } - Triangle superTriangle = GetSuperTriangle(_vertices); + Triangle superTriangle = PhysicsMath.GetSuperTriangle(_vertices); triangles.Add(superTriangle); List badTriangles = new(32); - List polygon = new(32); + List polygon = new(32); foreach (var vertex in _vertices) { @@ -102,30 +106,21 @@ public class Collider2DBehaviour(IList vertices) : BehaviourOverride, I foreach (var triangle in triangles) { - Circle circle = GetCircumCircle(triangle); + Circle circle = PhysicsMath.GetCircumCircle(triangle); if (Vector2.DistanceSquared(circle.Center, vertex) <= circle.Radius * circle.Radius) badTriangles.Add(triangle); } foreach (var triangle in badTriangles) - foreach (var edge in GetEdges(triangle)) - { - if (DoesEdgeExistInTriangles(edge, badTriangles)) - polygon.Add(edge); - } + foreach (var line in PhysicsMath.GetLines([triangle.A, triangle.B, triangle.C])) + if (PhysicsMath.DoesLineExistInVertices(line, [triangle.A, triangle.B, triangle.C])) + polygon.Add(line); foreach (var triangle in badTriangles) triangles.Remove(triangle); - foreach (var edge in polygon) - { - triangles.Add(new() - { - A = edge.A, - B = edge.B, - C = vertex - }); - } + foreach (var line in polygon) + triangles.Add(new(line.From, line.To, vertex)); } for (int i = triangles.Count - 1; i >= 0; i--) @@ -150,15 +145,15 @@ public class Collider2DBehaviour(IList vertices) : BehaviourOverride, I for (int i = 0; i < triangles.Count; i++) { Triangle triangle = triangles[i]; - foreach (var edge in GetEdges(triangle)) + foreach (var line in PhysicsMath.GetLines([triangle.A, triangle.B, triangle.C])) { GameObject gameObject = Game1.gameManager.InstantiateGameObject(); DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour(); displayableSpriteBehaviour.Color = Color.Aqua; displayableSpriteBehaviour.Origin = new(0.5f, 1f); displayableSpriteBehaviour.Assign(Game1.spriteBox); - gameObject.Transform.Position = edge.A; - Vector2 vector2 = edge.B - edge.A; + gameObject.Transform.Position = line.From; + Vector2 vector2 = line.To - line.From; gameObject.Transform.Scale = new Vector2(2f, .0f) + Vector2.UnitY * vector2.Length(); gameObject.Transform.Rotation = (float)Math.Atan2(vector2.X, vector2.Y); gameObjects.Add(gameObject); @@ -167,7 +162,7 @@ public class Collider2DBehaviour(IList vertices) : BehaviourOverride, I displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour(); displayableSpriteBehaviour.Color = Color.Crimson; displayableSpriteBehaviour.Assign(Game1.spriteBox); - gameObject.Transform.Position = edge.B; + gameObject.Transform.Position = line.To; gameObject.Transform.Scale = new Vector2(4f, 4f); gameObjects.Add(gameObject); } @@ -187,200 +182,4 @@ public class Collider2DBehaviour(IList vertices) : BehaviourOverride, I private List gameObjects = new List(32); - private Vector2 ClosestPointOnEdge(Vector2 point, Edge edge) - { - // Convert edge points to vectors - var edgeVector = new Vector2(edge.B.X - edge.A.X, edge.B.Y - edge.A.Y); - var pointVector = new Vector2(point.X - edge.A.X, point.Y - edge.A.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); - - // 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 = edge.A.X + t * edgeVector.X; - double closestY = edge.A.Y + t * edgeVector.Y; - - return new Vector2((float)closestX, (float)closestY); - } - - private bool DoesEdgeExistInTriangles(Edge edge, List triangles) - { - foreach (var triangle in triangles) - foreach (var edgeOther in GetEdges(triangle)) - if (edge.A == edgeOther.A && edge.B == edgeOther.B) - return true; - else if (edge.A == edgeOther.B && edge.B == edgeOther.A) - return true; - return false; - } - - private List GetEdges(Triangle triangle) - => [ - new() { A = triangle.A, B = triangle.B }, - new() { A = triangle.B, B = triangle.C }, - new() { A = triangle.C, B = triangle.A } - ]; - - private Triangle GetSuperTriangle(IReadOnlyList vertices) - { - double minX = double.MaxValue, minY = double.MaxValue; - double maxX = double.MinValue, maxY = double.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); - } - - double dx = maxX - minX; - double dy = maxY - minY; - double deltaMax = Math.Max(dx, dy); - double midX = (minX + maxX) / 2; - double 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() { A = p1, B = p2, C = p3 }; - } - - private struct Triangle { public Vector2 A, B, C; } - - private double GetArea(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)) / 2.0); - } - - /* A function to check whether point P(x, y) lies - inside the triangle formed by A(x1, y1), - B(x2, y2) and C(x3, y3) */ - private bool isInside(Vector2 point, Triangle triangle) - { - double A = GetArea(triangle); - /* Calculate area of triangle ABC */ - // double A = area(x1, y1, x2, y2, x3, y3); - - double A1 = GetArea(new() { A = point, B = triangle.B, C = 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() { A = triangle.A, B = point, C = triangle.C }); - // double A2 = area(x1, y1, x, y, x3, y3); - - /* Calculate area of triangle PAB */ - double A3 = GetArea(new() { A = triangle.A, B = triangle.B, C = 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; - } - - private struct Edge - { - public Vector2 A; - public Vector2 B; - } - - private struct Circle - { - public double Radius; - public Vector2 Center; - } - - private Circle GetCircumCircle(Triangle triangle) - { - Circle result = new(); - - 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); - - // Check if the slopes are not parallel - if (Math.Abs(slopeAB - slopeBC) > double.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; - - result.Center = new Vector2((float)x, (float)y); - result.Radius = Vector2.Distance(result.Center, triangle.A); - } - else - { - // If slopes are parallel, use the midpoints of the sides as the circumcenter - result.Center = (midAB + midBC) / 2; - result.Radius = Vector2.Distance(result.Center, triangle.A); - } - - return result; - } - - // Given three collinear points p, q, r, the function checks if - // point q lies on line segment 'pr' - private 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 - private 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 - } - - // The main function that returns true if line segment 'edge1.Aedge1.B' - // and 'edge2.Aedge2.B' intersect. - private bool DoIntersect(Edge edge1, Edge edge2) - { - // Find the four orientations needed for general and - // special cases - int o1 = Orientation(edge1.A, edge1.B, edge2.A); - int o2 = Orientation(edge1.A, edge1.B, edge2.B); - int o3 = Orientation(edge2.A, edge2.B, edge1.A); - int o4 = Orientation(edge2.A, edge2.B, edge1.B); - - // General case - if (o1 != o2 && o3 != o4) - return true; - - // Special Cases - // edge1.A, edge1.B and edge2.A are collinear and edge2.A lies on segment edge1.Aedge1.B - if (o1 == 0 && OnSegment(edge1.A, edge2.A, edge1.B)) return true; - - // edge1.A, edge1.B and edge2.B are collinear and edge2.B lies on segment edge1.Aedge1.B - if (o2 == 0 && OnSegment(edge1.A, edge2.B, edge1.B)) return true; - - // edge2.A, edge2.B and edge1.A are collinear and edge1.A lies on segment edge2.Aedge2.B - if (o3 == 0 && OnSegment(edge2.A, edge1.A, edge2.B)) return true; - - // edge2.A, edge2.B and edge1.B are collinear and edge1.B lies on segment edge2.Aedge2.B - if (o4 == 0 && OnSegment(edge2.A, edge1.B, edge2.B)) return true; - - return false; // Doesn't fall in any of the above cases - } } diff --git a/Game/Physics2D/PhysicsEngine2D.cs b/Game/Physics2D/PhysicsEngine2D.cs index 61646fb..4303d8f 100644 --- a/Game/Physics2D/PhysicsEngine2D.cs +++ b/Game/Physics2D/PhysicsEngine2D.cs @@ -51,71 +51,6 @@ public class PhysicsEngine2D : IPhysicsEngine2D foreach (var collider in colliders) collider.RecalculateVertices(); - for (int colliderIX = 0; colliderIX < colliders.Count; colliderIX++) - { - ICollider2D colliderX = colliders[colliderIX]; - for (int colliderIY = colliderIX + 1; colliderIY < colliders.Count; colliderIY++) - for (int verticesIndex = 0; verticesIndex < colliderX.Vertices.Count; verticesIndex++) - { - ICollider2D colliderY = colliders[colliderIY]; - bool v = colliderY.CheckCollision(colliderX.Vertices[verticesIndex], colliderX, out var _); - if (!colliderY.CheckCollision(colliderX.Vertices[verticesIndex], colliderX, out var collisionInformation)) - continue; - - if (colliderX.BehaviourController.TryGetBehaviour(out IRigidBody2D? rigidX)) - { - Vector2 xVertex = colliderX.Vertices[verticesIndex]; - Vector2 edgeDirection = new Vector2(collisionInformation.Normal.Y, collisionInformation.Normal.X); - Vector2 p1 = xVertex - rigidX.Velocity * intervalDeltaTime; - float t = IntersectionParameterT(collisionInformation.ContactPosition - edgeDirection * 100f, collisionInformation.ContactPosition + edgeDirection * 100f, xVertex, p1); - Vector2 vertexNewPosition = Vector2.Lerp(xVertex, p1, t); - rigidX.Velocity = Vector2.Reflect(rigidX.Velocity, collisionInformation.Normal); - rigidX.Transform.Position -= xVertex - vertexNewPosition; - - // { - // GameObject gameObject = Game1.gameManager.InstantiateGameObject(); - // DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour(); - // displayableSpriteBehaviour.Assign(Game1.spriteBox); - // gameObject.Transform.Position = collisionInformation.ContactPosition; - // gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f; - // gameObject.Transform.Rotation = (float)Math.Atan2(collisionInformation.Normal.Y, collisionInformation.Normal.X); - // } - // { - // GameObject gameObject = Game1.gameManager.InstantiateGameObject(); - // DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour(); - // displayableSpriteBehaviour.Color = Color.Aqua; - // displayableSpriteBehaviour.Assign(Game1.spriteBox); - // gameObject.Transform.Position = collisionInformation.ContactPosition; - // gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f; - // gameObject.Transform.Rotation = (float)Math.Atan2(rigidX.Velocity.Y, rigidX.Velocity.X); - // } - // { - // GameObject gameObject = Game1.gameManager.InstantiateGameObject(); - // DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour(); - // displayableSpriteBehaviour.Color = Color.Pink; - // displayableSpriteBehaviour.Assign(Game1.spriteBox); - // gameObject.Transform.Position = collisionInformation.ContactPosition; - // gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f; - // gameObject.Transform.Rotation = (float)Math.Atan2(edgeDirection.Y, edgeDirection.X); - // } - // { - // GameObject gameObject = Game1.gameManager.InstantiateGameObject(); - // DisplayableSpriteBehaviour displayableSpriteBehaviour = gameObject.BehaviourController.AddBehaviour(); - // displayableSpriteBehaviour.Color = Color.DarkGoldenrod; - // displayableSpriteBehaviour.Assign(Game1.spriteBox); - // gameObject.Transform.Position = collisionInformation.ContactPosition; - // gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f; - // gameObject.Transform.Rotation = (float)Math.Atan2((xVertex - p1).Y, (xVertex - p1).X); - // } - - StepRigidBody(rigidX, intervalDeltaTime * (1f - t)); - colliders[colliderIX].RecalculateVertices(); - verticesIndex--; - } - Console.WriteLine($"/////////////////////////////////////////////"); - // Console.WriteLine($"Collision"); - } - } } } @@ -127,15 +62,6 @@ public class PhysicsEngine2D : IPhysicsEngine2D rigidBody.Transform.Position = nextPosition; } - private float IntersectionParameterT(Vector2 p0, Vector2 p1, Vector2 q0, Vector2 q1) - { - // Solve the linear interpolation equation for 't' - float t = ((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)); - - return t; - } - private void OnBehaviourAdded(IBehaviourController controller, IBehaviour behaviour) { if (behaviour is not ICollider2D collider2D) diff --git a/Game/Physics2D/PhysicsMath.cs b/Game/Physics2D/PhysicsMath.cs new file mode 100644 index 0000000..9986d33 --- /dev/null +++ b/Game/Physics2D/PhysicsMath.cs @@ -0,0 +1,210 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics.CodeAnalysis; + +using Microsoft.Xna.Framework; + +namespace Syntriax.Engine.Physics2D; + +public record Line(Vector2 From, Vector2 To); +public record Triangle(Vector2 A, Vector2 B, Vector2 C); +public record Circle(Vector2 Center, double Radius); + +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(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 GetIntersectionPoint(Line l1, Line l2) + => Vector2.Lerp(l1.From, l1.To, IntersectionParameterT(l1, l2)); + + public static Vector2 ClosestPointOnLine(Vector2 point, Line line) + { + // 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 + double 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; + + return new Vector2((float)closestX, (float)closestY); + } + + public static double GetArea(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 IsInTriangle(Vector2 point, Triangle triangle) + { + 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 DoIntersect(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 DoIntersect(Line l1, Line l2, [NotNullWhen(returnValue: true)] out Vector2? point) + { + point = null; + + bool result = DoIntersect(l1, l2); + + if (result) + point = GetIntersectionPoint(l1, l2); + + return result; + } + + public static Circle GetCircumCircle(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); + + Vector2 center; + if (Math.Abs(slopeAB - slopeBC) > double.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; + center = new Vector2((float)x, (float)y); + } + else + center = (midAB + midBC) * .5f; + + return new(center, Vector2.Distance(center, triangle.A)); + } + + public static Triangle GetSuperTriangle(IList vertices) + { + double minX = double.MaxValue, minY = double.MaxValue; + double maxX = double.MinValue, maxY = double.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); + } + + double dx = maxX - minX; + double dy = maxY - minY; + double deltaMax = Math.Max(dx, dy); + double midX = (minX + maxX) / 2; + double 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 static List GetLines(IList vertices) + { + List lines = new List(vertices.Count - 1); + GetLines(vertices, lines); + return lines; + } + + public static void GetLines(IList vertices, 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])); + } + + public static bool DoesLineExistInVertices(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; + } +}