116 lines
4.0 KiB
C#
116 lines
4.0 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
|
|
using Microsoft.Xna.Framework;
|
|
|
|
using Syntriax.Engine.Physics2D.Primitives;
|
|
|
|
namespace Syntriax.Engine.Physics2D;
|
|
|
|
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 Triangle ToSuperTriangle(this IList<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((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 IList<Line> ToLines(this IList<Vector2> vertices)
|
|
{
|
|
List<Line> lines = new List<Line>(vertices.Count - 1);
|
|
ToLines(vertices, lines);
|
|
return lines;
|
|
}
|
|
|
|
public static void ToLines(this 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 LaysOn(this Vector2 point, Line line)
|
|
=> line.Resolve(point.X).ApproximatelyEquals(point);
|
|
|
|
|
|
// 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 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;
|
|
|
|
const float floatNormal = (1 << 23) * float.Epsilon;
|
|
float absA = Math.Abs(a);
|
|
float absB = Math.Abs(b);
|
|
float diff = Math.Abs(a - b);
|
|
|
|
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 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);
|
|
}
|