Engine-Pong/Game/Physics2D/Collider2DBehaviour.cs

354 lines
12 KiB
C#

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;
using Syntriax.Engine.Physics2D.Abstract;
namespace Syntriax.Engine.Physics2D;
public class Collider2DBehaviour(IList<Vector2> vertices) : BehaviourOverride, ICollider2D
{
private List<Triangle> triangles = new List<Triangle>(32);
private readonly List<Vector2> _vertices = new List<Vector2>(32);
private IRigidBody2D? _rigidBody2D = null;
public Action<IAssignableTransform>? OnTransformAssigned { get => GameObject.OnTransformAssigned; set => GameObject.OnTransformAssigned = value; }
public Action<ICollider2D, ICollider2D>? OnCollision { get; set; } = null;
private IList<Vector2> verticesOriginal { get; } = vertices;
public Vector2 OffsetPosition { get; set; } = Vector2.Zero;
public Vector2 OffsetScale { get; set; } = Vector2.One;
public float OffsetRotation { get; set; } = 0f;
ITransform IAssignableTransform.Transform => Transform;
public IReadOnlyList<Vector2> Vertices => _vertices;
public IRigidBody2D? RigidBody2D
{
get
{
if (_rigidBody2D is null)
BehaviourController.TryGetBehaviour(out _rigidBody2D);
return _rigidBody2D;
}
}
public bool Assign(ITransform transform) => GameObject.Assign(transform);
public bool CheckCollision(Vector2 point, ICollider2D otherCollider, out CollisionInformation collisionInformation)
{
collisionInformation = new CollisionInformation(Vector2.Zero, Vector2.Zero);
foreach (var triangle in triangles)
{
if (!isInside(point, triangle))
continue;
OnCollision?.Invoke(this, otherCollider);
Edge main = new() { A = otherCollider.Transform.Position, B = point };
foreach (var edge in GetEdges(triangle))
{
if (!DoIntersect(main, edge))
continue;
Vector2 contactPoint = ClosestPointOnEdge(point, edge);
Vector2 normal = contactPoint - point;
normal.Normalize();
collisionInformation = new CollisionInformation(normal, contactPoint);
GameObject gameObject = Game1.gameManager.InstantiateGameObject<GameObject>();
gameObject.BehaviourController.AddBehaviour<DisplayableSpriteBehaviour>().Assign(Game1.spriteBox);
gameObject.Transform.Position = point;
gameObject.Transform.Scale = new Vector2(1f, .01f) * 100f;
gameObject.Transform.Rotation = (float)Math.Atan2(normal.Y, normal.X);
break;
}
return true;
}
return false;
}
public void RecalculateVertices()
{
triangles.Clear();
_vertices.Clear();
foreach (var vertex in verticesOriginal)
{
Vector2 scaledPosition = new Vector2(vertex.X * Transform.Scale.X * OffsetScale.X, vertex.Y * Transform.Scale.Y * OffsetScale.Y);
_vertices.Add(scaledPosition + Transform.Position);
}
Triangle superTriangle = GetSuperTriangle(_vertices);
triangles.Add(superTriangle);
List<Triangle> badTriangles = new(32);
List<Edge> polygon = new(32);
foreach (var vertex in _vertices)
{
badTriangles.Clear();
polygon.Clear();
foreach (var triangle in triangles)
{
Circle circle = 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 triangle in badTriangles)
triangles.Remove(triangle);
foreach (var edge in polygon)
{
triangles.Add(new()
{
A = edge.A,
B = edge.B,
C = vertex
});
}
}
for (int i = triangles.Count - 1; i >= 0; i--)
{
Triangle triangle = triangles[i];
if (
triangle.A == superTriangle.A || triangle.A == superTriangle.B || triangle.A == superTriangle.C ||
triangle.B == superTriangle.A || triangle.B == superTriangle.B || triangle.B == superTriangle.C ||
triangle.C == superTriangle.A || triangle.C == superTriangle.B || triangle.C == superTriangle.C
)
triangles.RemoveAt(i);
}
// for (int i = 0; i < triangles.Count; i++)
// {
// Triangle triangle = triangles[i];
// triangle.A += Transform.Position;
// triangle.B += Transform.Position;
// triangle.C += Transform.Position;
// triangles[i] = triangle;
// }
}
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
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 = edge.A.X + t * edgeVector.X;
float closestY = edge.A.Y + t * edgeVector.Y;
return new Vector2(closestX, closestY);
}
private bool DoesEdgeExistInTriangles(Edge edge, List<Triangle> 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<Edge> 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<Vector2> vertices)
{
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(midX - 20 * deltaMax, midY - deltaMax);
Vector2 p2 = new Vector2(midX, midY + 20 * deltaMax);
Vector2 p3 = new Vector2(midX + 20 * deltaMax, midY - 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;
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);
// Check if the slopes are not parallel
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;
result.Center = new Vector2(x, 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.
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
}
// 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
}
}