From d40183db65da1f135306ac4c7353300b8e466d0a Mon Sep 17 00:00:00 2001
From: Syntriax <syntriax@gmail.com>
Date: Wed, 24 Jan 2024 12:29:11 +0300
Subject: [PATCH] feat: float & Vector2D.ApproximatelyEquals

---
 Engine.Core/Extensions/FloatExtensions.cs    | 22 ++++++++++++++++++++
 Engine.Core/Extensions/Vector2DExtensions.cs |  3 +++
 Engine.Core/Vector2D.cs                      |  3 +++
 3 files changed, 28 insertions(+)
 create mode 100644 Engine.Core/Extensions/FloatExtensions.cs

diff --git a/Engine.Core/Extensions/FloatExtensions.cs b/Engine.Core/Extensions/FloatExtensions.cs
new file mode 100644
index 0000000..019bc39
--- /dev/null
+++ b/Engine.Core/Extensions/FloatExtensions.cs
@@ -0,0 +1,22 @@
+namespace Syntriax.Engine.Core;
+
+public static class FloatExtensions
+{
+    public static bool ApproximatelyEquals(this float a, float b)
+        => ApproximatelyEquals(a, b, float.Epsilon);
+    public static bool ApproximatelyEquals(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;
+    }
+}
diff --git a/Engine.Core/Extensions/Vector2DExtensions.cs b/Engine.Core/Extensions/Vector2DExtensions.cs
index 30d37d6..f2a4c41 100644
--- a/Engine.Core/Extensions/Vector2DExtensions.cs
+++ b/Engine.Core/Extensions/Vector2DExtensions.cs
@@ -27,4 +27,7 @@ public static class Vector2DExtensions
     public static float Cross(this Vector2D left, Vector2D right) => Vector2D.Cross(left, right);
     public static float AngleBetween(this Vector2D left, Vector2D right) => Vector2D.Angle(left, right);
     public static float Dot(this Vector2D left, Vector2D right) => Vector2D.Dot(left, right);
+
+
+    public static bool ApproximatelyEquals(this Vector2D left, Vector2D right, float epsilon = float.Epsilon) => Vector2D.ApproximatelyEquals(left, right, epsilon);
 }
diff --git a/Engine.Core/Vector2D.cs b/Engine.Core/Vector2D.cs
index cc3f9e8..336fd4e 100644
--- a/Engine.Core/Vector2D.cs
+++ b/Engine.Core/Vector2D.cs
@@ -49,5 +49,8 @@ public record Vector2D(float X, float Y)
     public static float Angle(Vector2D left, Vector2D right) => MathF.Acos(Dot(left, right) / (Length(left) * Length(right)));
     public static float Dot(Vector2D left, Vector2D right) => left.X * right.X + left.Y * right.Y;
 
+    public static bool ApproximatelyEquals(Vector2D left, Vector2D right, float epsilon = float.Epsilon)
+        => left.X.ApproximatelyEquals(right.X, epsilon) && left.Y.ApproximatelyEquals(right.Y, epsilon);
+
     public override string ToString() => $"{nameof(Vector2D)}({X}, {Y})";
 }