Add utility function to clamp a vector to a maximum magnitude.

1 files changed, 12 insertions, 0 deletions

diff --git a/OpenSim/Framework/Util.cs b/OpenSim/Framework/Util.cs
index 4fd8a2d..7204279 100644
--- a/OpenSim/Framework/Util.cs
+++ b/OpenSim/Framework/Util.cs
@@ -299,6 +299,18 @@ namespace OpenSim.Framework
                 x;
         }
 
+        // Clamp the maximum magnitude of a vector
+        public static Vector3 ClampV(Vector3 x, float max)
+        {
+            Vector3 ret = x;
+            float lenSq = x.LengthSquared();
+            if (lenSq > (max * max))
+            {
+                x = x / x.Length() * max;
+            }
+            return x;
+        }
+
         // Inclusive, within range test (true if equal to the endpoints)
         public static bool InRange<T>(T x, T min, T max)
             where T : IComparable<T>