Engine-Pong/Game/Physics2D/PhysicsMath.cs

119 lines
4.3 KiB
C#
Raw Normal View History

2023-12-05 17:04:26 +03:00
using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
2024-01-23 12:16:58 +03:00
using Syntriax.Engine.Core;
2023-12-07 10:55:49 +03:00
using Syntriax.Engine.Physics2D.Primitives;
2023-12-05 17:04:26 +03:00
2023-12-07 10:55:49 +03:00
namespace Syntriax.Engine.Physics2D;
2023-12-05 17:04:26 +03:00
public static class PhysicsMath
{
2024-01-23 12:16:58 +03:00
public static Vector2D Scale(this Vector2D original, Vector2D scale)
=> new Vector2D(original.X * scale.X, original.Y * scale.Y);
2023-12-06 13:50:39 +03:00
2024-01-23 12:16:58 +03:00
public static Triangle ToSuperTriangle(this IList<Vector2D> vertices)
2023-12-05 17:04:26 +03:00
{
2023-12-06 13:25:16 +03:00
float minX = float.MaxValue, minY = float.MaxValue;
float maxX = float.MinValue, maxY = float.MinValue;
2023-12-05 17:04:26 +03:00
2024-01-23 12:16:58 +03:00
foreach (Vector2D point in vertices)
2023-12-05 17:04:26 +03:00
{
2024-01-22 18:46:51 +03:00
minX = MathF.Min(minX, point.X);
minY = MathF.Min(minY, point.Y);
maxX = MathF.Max(maxX, point.X);
maxY = MathF.Max(maxY, point.Y);
2023-12-05 17:04:26 +03:00
}
2023-12-06 13:25:16 +03:00
float dx = maxX - minX;
float dy = maxY - minY;
2024-01-22 18:46:51 +03:00
float deltaMax = MathF.Max(dx, dy);
2023-12-06 13:25:16 +03:00
float midX = (minX + maxX) / 2;
float midY = (minY + maxY) / 2;
2023-12-05 17:04:26 +03:00
2024-01-23 12:16:58 +03:00
Vector2D p1 = new Vector2D((float)midX - 20f * (float)deltaMax, (float)midY - (float)deltaMax);
Vector2D p2 = new Vector2D((float)midX, (float)midY + 20 * (float)deltaMax);
Vector2D p3 = new Vector2D((float)midX + 20 * (float)deltaMax, (float)midY - (float)deltaMax);
2023-12-05 17:04:26 +03:00
return new Triangle(p1, p2, p3);
}
2024-01-23 12:16:58 +03:00
public static IList<Line> ToLines(this IList<Vector2D> vertices)
2023-12-05 17:04:26 +03:00
{
List<Line> lines = new List<Line>(vertices.Count - 1);
2023-12-06 11:24:49 +03:00
ToLines(vertices, lines);
2023-12-05 17:04:26 +03:00
return lines;
}
2024-01-23 12:16:58 +03:00
public static void ToLines(this IList<Vector2D> vertices, IList<Line> lines)
2023-12-05 17:04:26 +03:00
{
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]));
}
2024-01-23 12:16:58 +03:00
public static bool LaysOn(this Vector2D point, Line line)
2023-12-07 10:55:49 +03:00
=> line.Resolve(point.X).ApproximatelyEquals(point);
2023-12-06 11:24:49 +03:00
2023-12-06 13:25:16 +03:00
// Given three collinear points p, q, r, the function checks if
// point q lies on line segment 'pr'
2024-01-23 12:16:58 +03:00
public static bool OnSegment(Vector2D p, Vector2D q, Vector2D r)
2023-12-06 13:25:16 +03:00
{
2024-01-22 18:46:51 +03:00
if (q.X <= MathF.Max(p.X, r.X) && q.X >= MathF.Min(p.X, r.X) &&
q.Y <= MathF.Max(p.Y, r.Y) && q.Y >= MathF.Min(p.Y, r.Y))
2023-12-06 13:25:16 +03:00
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
2024-01-23 12:16:58 +03:00
public static int Orientation(Vector2D p, Vector2D q, Vector2D r)
2023-12-06 13:25:16 +03:00
{
// 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
}
2024-01-23 12:16:58 +03:00
public static float IntersectionParameterT(Vector2D p0, Vector2D p1, Vector2D q0, Vector2D q1)
2023-12-06 13:25:16 +03:00
=> ((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));
2023-12-06 15:56:46 +03:00
2023-12-07 10:55:49 +03:00
public static bool ApproximatelyEquals(this float a, float b)
2023-12-07 11:14:18 +03:00
=> ApproximatelyEquals(a, b, float.Epsilon);
public static bool ApproximatelyEquals(this Vector2 a, Vector2 b)
=> ApproximatelyEquals(a, b, float.Epsilon);
public static bool ApproximatelyEquals(this Vector2 a, Vector2 b, float epsilon)
=> ApproximatelyEquals(a.X, b.X, epsilon) && ApproximatelyEquals(a.Y, b.Y, epsilon);
2024-01-23 12:16:58 +03:00
public static bool ApproximatelyEquals(this Vector2D a, Vector2D b)
=> ApproximatelyEquals(a, b, float.Epsilon);
public static bool ApproximatelyEquals(this Vector2D a, Vector2D b, float epsilon)
=> ApproximatelyEquals(a.X, b.X, epsilon) && ApproximatelyEquals(a.Y, b.Y, epsilon);
2023-12-07 11:14:18 +03:00
public static bool ApproximatelyEquals(this float a, float b, float epsilon)
2023-12-06 16:51:27 +03:00
{
if (a == b)
return true;
const float floatNormal = (1 << 23) * float.Epsilon;
2024-01-22 18:46:51 +03:00
float absA = MathF.Abs(a);
float absB = MathF.Abs(b);
float diff = MathF.Abs(a - b);
2023-12-06 16:51:27 +03:00
if (a == 0.0f || b == 0.0f || diff < floatNormal)
return diff < (epsilon * floatNormal);
2024-01-22 18:46:51 +03:00
return diff / MathF.Min(absA + absB, float.MaxValue) < epsilon;
2023-12-06 16:51:27 +03:00
}
2023-12-05 17:04:26 +03:00
}