From 49e05bc206ed9407e33f5fbcb0c0a3b1bca9471e Mon Sep 17 00:00:00 2001
From: PrecisionRender <austin08dean@gmail.com>
Date: Sat, 15 Apr 2023 12:53:19 -0500
Subject: [PATCH] Add `ClampLength` functions to C++ Vector3

---
 Source/Engine/Core/Math/Vector3.h | 52 +++++++++++++++++++++++++++++++
 1 file changed, 52 insertions(+)

diff --git a/Source/Engine/Core/Math/Vector3.h b/Source/Engine/Core/Math/Vector3.h
index f2547f47d..76b9aa2f8 100644
--- a/Source/Engine/Core/Math/Vector3.h
+++ b/Source/Engine/Core/Math/Vector3.h
@@ -566,6 +566,58 @@ public:
         result = Vector3Base(Math::Clamp(v.X, min.X, max.X), Math::Clamp(v.Y, min.Y, max.Y), Math::Clamp(v.Z, min.Z, max.Z));
     }
 
+    /// <summary>
+    /// Makes sure that Length of the output vector is always below max and above 0.
+    /// </summary>
+    /// <param name="vector">Input Vector.</param>
+    /// <param name="max">Max Length</param>
+    static Vector3Base ClampLength(const Vector3Base& v, float max)
+    {
+        return ClampLength(v, 0, max);
+    }
+
+    /// <summary>
+    /// Makes sure that Length of the output vector is always below max and above min.
+    /// </summary>
+    /// <param name="vector">Input Vector.</param>
+    /// <param name="min">Min Length</param>
+    /// <param name="max">Max Length</param>
+    static Vector3Base ClampLength(const Vector3Base& v, float min, float max)
+    {
+        Vector3Base result;
+        ClampLength(v, min, max, result);
+        return result;
+    }
+
+    /// <summary>
+    /// Makes sure that Length of the output vector is always below max and above min.
+    /// </summary>
+    /// <param name="vector">Input Vector.</param>
+    /// <param name="min">Min Length</param>
+    /// <param name="max">Max Length</param>
+    /// <param name="result">The result vector.</param>
+    static void ClampLength(const Vector3Base& v, float min, float max, Vector3Base& result)
+    {
+        result.X = v.X;
+        result.Y = v.Y;
+        result.Z = v.Z;
+        auto lenSq = result.LengthSquared();
+        if (lenSq > max * max)
+        {
+            auto scaleFactor = max / (float)Math::Sqrt(lenSq);
+            result.X *= scaleFactor;
+            result.Y *= scaleFactor;
+            result.Z *= scaleFactor;
+        }
+        if (lenSq < min * min)
+        {
+            auto scaleFactor = min / (float)Math::Sqrt(lenSq);
+            result.X *= scaleFactor;
+            result.Y *= scaleFactor;
+            result.Z *= scaleFactor;
+        }
+    }
+
     // Calculates the distance between two vectors
     // @param a The first vector
     // @param b The second vector