2 files changed, 67 insertions, 0 deletions

diff --git a/indra/llmath/v3math.cpp b/indra/llmath/v3math.cpp
index f1fe1a780e..6299bbd1b7 100644
--- a/indra/llmath/v3math.cpp
+++ b/indra/llmath/v3math.cpp
@@ -73,6 +73,72 @@ BOOL LLVector3::clamp(F32 min, F32 max)
 	return ret;
 }
 
+// Clamps length to an upper limit.  
+// Returns TRUE if the data changed
+BOOL LLVector3::clampLength( F32 length_limit )
+{
+	BOOL changed = FALSE;
+
+	F32 len = length();
+	if (llfinite(len))
+	{
+		if ( len > length_limit)
+		{
+			normalize();
+			if (length_limit < 0.f)
+			{
+				length_limit = 0.f;
+			}
+			mV[0] *= length_limit;
+			mV[1] *= length_limit;
+			mV[2] *= length_limit;
+			changed = TRUE;
+		}
+	}
+	else
+	{	// this vector may still be salvagable
+		F32 max_abs_component = 0.f;
+		for (S32 i = 0; i < 3; ++i)
+		{
+			F32 abs_component = fabs(mV[i]);
+			if (llfinite(abs_component))
+			{
+				if (abs_component > max_abs_component)
+				{
+					max_abs_component = abs_component;
+				}
+			}
+			else
+			{
+				// no it can't be salvaged --> clear it
+				clear();
+				changed = TRUE;
+				break;
+			}
+		}
+		if (!changed)
+		{
+			// yes it can be salvaged -->
+			// bring the components down before we normalize
+			mV[0] /= max_abs_component;
+			mV[1] /= max_abs_component;
+			mV[2] /= max_abs_component;
+			normalize();
+
+			if (length_limit < 0.f)
+			{
+				length_limit = 0.f;
+			}
+			mV[0] *= length_limit;
+			mV[1] *= length_limit;
+			mV[2] *= length_limit;
+		}
+	}
+
+	return changed;
+}
+
+
 // Sets all values to absolute value of their original values
 // Returns TRUE if data changed
 BOOL LLVector3::abs()
diff --git a/indra/llmath/v3math.h b/indra/llmath/v3math.h
index 03c780a1f4..d8f3bec193 100644
--- a/indra/llmath/v3math.h
+++ b/indra/llmath/v3math.h
@@ -74,6 +74,7 @@ class LLVector3
 
 		inline BOOL isFinite() const;									// checks to see if all values of LLVector3 are finite
 		BOOL		clamp(F32 min, F32 max);		// Clamps all values to (min,max), returns TRUE if data changed
+		BOOL		clampLength( F32 length_limit );					// Scales vector to limit length to a value
 
 		void		quantize16(F32 lowerxy, F32 upperxy, F32 lowerz, F32 upperz);	// changes the vector to reflect quatization
 		void		quantize8(F32 lowerxy, F32 upperxy, F32 lowerz, F32 upperz);	// changes the vector to reflect quatization