From 43aec46b2fc4dd44523708b15448505759faf01c Mon Sep 17 00:00:00 2001
From: cosmonaut <evan@moonside.games>
Date: Sun, 15 Jan 2023 12:41:02 -0800
Subject: [PATCH] AABB transform shortcut + sweep test

---
 src/Collision/Fixed/AABB2D.cs | 104 ++++++++++++++++++++++++++++++++--
 1 file changed, 100 insertions(+), 4 deletions(-)

diff --git a/src/Collision/Fixed/AABB2D.cs b/src/Collision/Fixed/AABB2D.cs
index 025ddaf..4a3e8d2 100644
--- a/src/Collision/Fixed/AABB2D.cs
+++ b/src/Collision/Fixed/AABB2D.cs
@@ -66,6 +66,8 @@ namespace MoonWorks.Collision.Fixed
 			);
 		}
 
+		static readonly Fix64 Half = Fix64.FromFraction(1, 2);
+
 		/// <summary>
 		/// Efficiently transforms the AABB by a Transform2D.
 		/// </summary>
@@ -74,11 +76,42 @@ namespace MoonWorks.Collision.Fixed
 		/// <returns></returns>
 		public static AABB2D Transformed(AABB2D aabb, Transform2D transform)
 		{
-			var two = new Fix64(2);
-			var center = (aabb.Min + aabb.Max) / two;
-            var extent = (aabb.Max - aabb.Min) / two;
+			if (transform.IsAxisAligned)
+			{
+				var min = aabb.Min * transform.Scale + transform.Position;
+				var max = aabb.Max * transform.Scale + transform.Position;
 
-            var newCenter = Vector2.Transform(center, transform.TransformMatrix);
+				Fix64 minX, minY, maxX, maxY;
+
+				if (min.X <= max.X)
+				{
+					minX = min.X;
+					maxX = max.X;
+				}
+				else
+				{
+					minX = max.X;
+					maxX = min.X;
+				}
+
+				if (min.Y <= max.Y)
+				{
+					minY = min.Y;
+					maxY = max.Y;
+				}
+				else
+				{
+					minY = max.Y;
+					maxY = min.Y;
+				}
+
+				return new AABB2D(minX, minY, maxX, maxY);
+			}
+
+			var center = (aabb.Min + aabb.Max) * Half;
+			var extent = aabb.Max - center;
+
+			var newCenter = Vector2.Transform(center, transform.TransformMatrix);
             var newExtent = Vector2.TransformNormal(extent, AbsoluteMatrix(transform.TransformMatrix));
 
             return new AABB2D(newCenter - newExtent, newCenter + newExtent);
@@ -152,6 +185,69 @@ namespace MoonWorks.Collision.Fixed
 			return a.Left < b.Right && a.Right > b.Left && a.Top < b.Bottom && a.Bottom > b.Top;
 		}
 
+		// FIXME: this is broken
+		public static bool SweepTest(AABB2D a, AABB2D b, Vector2 aMovement, Vector2 bMovement, out Fix64 normalizedTime)
+		{
+			if (TestOverlap(a, b))
+			{
+				normalizedTime = Fix64.Zero;
+				return true;
+			}
+
+			normalizedTime = Fix64.One;
+
+			var relativeVelocity = bMovement - aMovement;
+
+			Vector2 entry = Vector2.Zero;
+			if (a.Max.X < b.Min.X && relativeVelocity.X < 0)
+			{
+				entry.X = (a.Max.X - b.Min.X) / relativeVelocity.X;
+			}
+			else if (b.Max.X < a.Min.X && relativeVelocity.X > 0)
+			{
+				entry.X = (a.Min.X - b.Max.X) / relativeVelocity.X;
+			}
+
+			if (a.Max.Y < b.Min.Y && relativeVelocity.Y < 0)
+			{
+				entry.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y;
+			}
+			else if (b.Max.Y > a.Min.Y && relativeVelocity.Y > 0)
+			{
+				entry.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y;
+			}
+
+			Vector2 exit = new Vector2(Fix64.MaxValue, Fix64.MaxValue);
+			if (b.Max.X > a.Min.X && relativeVelocity.X < 0)
+			{
+				exit.X = (a.Min.X - b.Max.X) / relativeVelocity.X;
+			}
+			else if (a.Max.X > b.Min.X && relativeVelocity.X > 0)
+			{
+				exit.Y = (a.Max.X - b.Min.X) / relativeVelocity.X;
+			}
+
+			if (b.Max.Y > a.Min.Y && relativeVelocity.Y < 0)
+			{
+				exit.Y = (a.Min.Y - b.Max.Y) / relativeVelocity.Y;
+			}
+			else if (a.Max.Y > b.Min.Y && relativeVelocity.Y > 0)
+			{
+				exit.Y = (a.Max.Y - b.Min.Y) / relativeVelocity.Y;
+			}
+
+			Fix64 firstTime = Fix64.Max(entry.X, entry.Y);
+			Fix64 lastTime = Fix64.Min(exit.X, exit.Y);
+
+			if (firstTime <= lastTime && firstTime > 0)
+			{
+				normalizedTime = firstTime;
+				return true;
+			}
+
+			return false;
+		}
+
 		public override bool Equals(object obj)
 		{
 			return obj is AABB2D aabb && Equals(aabb);