From 40e4be6d364b83541daadcb617377e415cea06cf Mon Sep 17 00:00:00 2001
From: cosmonaut <evan@moonside.games>
Date: Wed, 4 May 2022 11:03:02 -0700
Subject: [PATCH] fix Matrix3x2 rotation

---
 src/Math/Fixed/Fix64.cs     |  6 ++++++
 src/Math/Fixed/Matrix3x2.cs | 30 ++++++++++++++----------------
 2 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/Math/Fixed/Fix64.cs b/src/Math/Fixed/Fix64.cs
index a14105c..a0e9292 100644
--- a/src/Math/Fixed/Fix64.cs
+++ b/src/Math/Fixed/Fix64.cs
@@ -104,6 +104,12 @@ namespace MoonWorks.Math.Fixed
 					   : integralPart + One;
 		}
 
+		//Formula taken from https://docs.microsoft.com/en-us/dotnet/api/system.math.ieeeremainder?view=net-6.0
+		public static Fix64 IEEERemainder(Fix64 dividend, Fix64 divisor)
+		{
+			return dividend - (divisor * Round(dividend / divisor));
+		}
+
 		public static Fix64 Min(Fix64 x, Fix64 y)
 		{
 			return (x < y) ? x : y;
diff --git a/src/Math/Fixed/Matrix3x2.cs b/src/Math/Fixed/Matrix3x2.cs
index 2ff7db9..f234ce3 100644
--- a/src/Math/Fixed/Matrix3x2.cs
+++ b/src/Math/Fixed/Matrix3x2.cs
@@ -51,6 +51,8 @@ namespace MoonWorks.Math.Fixed
 			0, 0
 		);
 
+		private static readonly Fix64 RotationEpsilon = (Fix64.One / new Fix64(1000)) * (Fix64.Pi / new Fix64(180));
+
 		/// <summary>
 		/// Returns the multiplicative identity matrix.
 		/// </summary>
@@ -343,31 +345,29 @@ namespace MoonWorks.Math.Fixed
 		{
 			Matrix3x2 result;
 
-			radians = radians % Fix64.PiTimes2;
+			radians = Fix64.IEEERemainder(radians, Fix64.PiTimes2);
 
 			Fix64 c, s;
 
-            Fix64 epsilon = (Fix64) 0.001f * (Fix64.Pi / new Fix64(180));
-
-			if (radians > -epsilon && radians < epsilon)
+			if (radians > -RotationEpsilon && radians < RotationEpsilon)
 			{
 				// Exact case for zero rotation.
 				c = Fix64.One;
 				s = Fix64.Zero;
 			}
-			else if (radians > Fix64.PiOver2 - epsilon && radians < Fix64.PiOver2 + epsilon)
+			else if (radians > Fix64.PiOver2 - RotationEpsilon && radians < Fix64.PiOver2 + RotationEpsilon)
 			{
 				// Exact case for 90 degree rotation.
 				c = Fix64.Zero;
 				s = Fix64.One;
 			}
-			else if (radians < -Fix64.Pi + epsilon || radians > Fix64.Pi - epsilon)
+			else if (radians < -Fix64.Pi + RotationEpsilon || radians > Fix64.Pi - RotationEpsilon)
 			{
 				// Exact case for 180 degree rotation.
 				c = -Fix64.One;
 				s = Fix64.Zero;
 			}
-			else if (radians > -Fix64.PiOver2 - epsilon && radians < -Fix64.PiOver2 + epsilon)
+			else if (radians > -Fix64.PiOver2 - RotationEpsilon && radians < -Fix64.PiOver2 + RotationEpsilon)
 			{
 				// Exact case for 270 degree rotation.
 				c = Fix64.Zero;
@@ -376,8 +376,8 @@ namespace MoonWorks.Math.Fixed
 			else
 			{
 				// Arbitrary rotation.
-				c = (Fix64) Fix64.Cos(radians);
-				s = (Fix64) Fix64.Sin(radians);
+				c = Fix64.Cos(radians);
+				s = Fix64.Sin(radians);
 			}
 
 			// [  c  s ]
@@ -403,31 +403,29 @@ namespace MoonWorks.Math.Fixed
 		{
 			Matrix3x2 result;
 
-			radians = radians % Fix64.PiTimes2;
+			radians = Fix64.IEEERemainder(radians, Fix64.PiTimes2);
 
 			Fix64 c, s;
 
-            Fix64 epsilon = (Fix64) 0.001f * (Fix64.Pi / new Fix64(180));
-
-			if (radians > -epsilon && radians < epsilon)
+			if (radians > -RotationEpsilon && radians < RotationEpsilon)
 			{
 				// Exact case for zero rotation.
 				c = Fix64.One;
 				s = Fix64.Zero;
 			}
-			else if (radians > Fix64.PiOver2 - epsilon && radians < Fix64.PiOver2 + epsilon)
+			else if (radians > Fix64.PiOver2 - RotationEpsilon && radians < Fix64.PiOver2 + RotationEpsilon)
 			{
 				// Exact case for 90 degree rotation.
 				c = Fix64.Zero;
 				s = Fix64.One;
 			}
-			else if (radians < -Fix64.Pi + epsilon || radians > Fix64.Pi - epsilon)
+			else if (radians < -Fix64.Pi + RotationEpsilon || radians > Fix64.Pi - RotationEpsilon)
 			{
 				// Exact case for 180 degree rotation.
 				c = -Fix64.One;
 				s = Fix64.Zero;
 			}
-			else if (radians > -Fix64.PiOver2 - epsilon && radians < -Fix64.PiOver2 + epsilon)
+			else if (radians > -Fix64.PiOver2 - RotationEpsilon && radians < -Fix64.PiOver2 + RotationEpsilon)
 			{
 				// Exact case for 270 degree rotation.
 				c = Fix64.Zero;