From f47d80b62e018055f4e80c6662947a824cfbe11b Mon Sep 17 00:00:00 2001
From: Steve Streeting <steve@stevestreeting.com>
Date: Thu, 23 Nov 2023 16:46:38 +0000
Subject: [PATCH] Add GetDistanceToConvex2D function

---
 .../Public/StevesMathHelpers.h                | 52 +++++++++++++++++++
 1 file changed, 52 insertions(+)

diff --git a/Source/StevesUEHelpers/Public/StevesMathHelpers.h b/Source/StevesUEHelpers/Public/StevesMathHelpers.h
index 791f705..defbfa9 100644
--- a/Source/StevesUEHelpers/Public/StevesMathHelpers.h
+++ b/Source/StevesUEHelpers/Public/StevesMathHelpers.h
@@ -98,4 +98,56 @@ public:
 					   const FVector& ShapePos,
 					   const FQuat& ShapeRot,
 					   FMTDResult& OutResult);
+
+
+	/**
+	 * Return the distance to a convex polygon in 2D
+	 * @param ConvexPoints Points on the convex polygon, anti-clockwise order, in local space
+	 * @param ConvexTransform World transform for convex polygon
+	 * @param WorldPoint Point in world space
+	 * @return The distance to this convex polygon in 2D space. <= 0 if inside
+	 */
+	static float GetDistanceToConvex2D(const TArray<FVector2f>& ConvexPoints,
+	                                   const FTransform& ConvexTransform,
+	                                   const FVector& WorldPoint)
+	{
+		checkf(ConvexTransform.GetMaximumAxisScale() == ConvexTransform.GetMinimumAxisScale(), TEXT("Non-uniform scale not supported in GetDistanceToConvex2D"));
+		const FVector LocalPoint = ConvexTransform.InverseTransformPosition(WorldPoint);
+
+		// Assume inside until 1 or more tests show it's outside
+		bool bInside = true;
+		float ClosestOutside = 0;
+		float ClosestInside = -1e30f;
+		const int N = ConvexPoints.Num();
+		for (int i = 0; i < N; ++i)
+		{
+			const int OtherIdx = (i + 1) % N;
+			const FVector2f Edge = ConvexPoints[OtherIdx] - ConvexPoints[i];
+			// Simple cross procduct to get the normal
+			const FVector2f Normal = FVector2f(Edge.Y, -Edge.X).GetSafeNormal();
+			if (!Normal.IsNearlyZero())
+			{
+				const FVector2f ToPoint = FVector2f(LocalPoint.X, LocalPoint.Y) - ConvexPoints[i];
+				const float Dot = Normal.Dot(ToPoint);
+
+				if (Dot > 0)
+				{
+					// Outside
+					// When outside we want the largest separation because convex
+					ClosestOutside = FMath::Max(ClosestOutside, Dot);
+					bInside = false;
+				}
+				else if (bInside)
+				{
+					// Inside (don't bother if we've already found one outside)
+					// Max because negative and inside we want the closest dot
+					ClosestInside = FMath::Max(ClosestInside, Dot);
+				}
+			}
+		}
+
+		// Need to rescale distance back up to world scale, only uniform scale supported for simplicity
+		return (bInside ? ClosestInside : ClosestOutside) * ConvexTransform.GetScale3D().X;		
+	}
+
 };
\ No newline at end of file