211 lines
7.9 KiB
C#
211 lines
7.9 KiB
C#
|
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<Vector2> 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<Line> GetLines(IList<Vector2> vertices)
|
||
|
|
{
|
||
|
|
List<Line> lines = new List<Line>(vertices.Count - 1);
|
||
|
|
GetLines(vertices, lines);
|
||
|
|
return lines;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static void GetLines(IList<Vector2> vertices, IList<Line> 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<Vector2> 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;
|
||
|
|
}
|
||
|
|
}
|