Merge. Refresh from viewer-release after 3.7.11 release.

5 files changed, 321 insertions, 283 deletions

diff --git a/indra/llmath/llmath.h b/indra/llmath/llmath.h
index 29db799154..a8b27ad189 100755
--- a/indra/llmath/llmath.h
+++ b/indra/llmath/llmath.h
@@ -1,4 +1,4 @@
-/** 
+/**
  * @file llmath.h
  * @brief Useful math constants and macros.
  *
@@ -73,6 +73,7 @@ const F32	F_E			= 2.71828182845904523536f;
 const F32	F_SQRT2		= 1.4142135623730950488016887242097f;
 const F32	F_SQRT3		= 1.73205080756888288657986402541f;
 const F32	OO_SQRT2	= 0.7071067811865475244008443621049f;
+const F32	OO_SQRT3	= 0.577350269189625764509f;
 const F32	DEG_TO_RAD	= 0.017453292519943295769236907684886f;
 const F32	RAD_TO_DEG	= 57.295779513082320876798154814105f;
 const F32	F_APPROXIMATELY_ZERO = 0.00001f;
@@ -84,6 +85,9 @@ const F32	OO_LN2		= 1.4426950408889634073599246810019f;
 const F32	F_ALMOST_ZERO	= 0.0001f;
 const F32	F_ALMOST_ONE	= 1.0f - F_ALMOST_ZERO;
 
+const F32	GIMBAL_THRESHOLD = 0.000436f; // sets the gimballock threshold 0.025 away from +/-90 degrees
+// formula: GIMBAL_THRESHOLD = sin(DEG_TO_RAD * gimbal_threshold_angle);
+
 // BUG: Eliminate in favor of F_APPROXIMATELY_ZERO above?
 const F32 FP_MAG_THRESHOLD = 0.0000001f;
 
diff --git a/indra/llmath/llquaternion.cpp b/indra/llmath/llquaternion.cpp
index 7381d5eb99..47374c287f 100755
--- a/indra/llmath/llquaternion.cpp
+++ b/indra/llmath/llquaternion.cpp
@@ -1,4 +1,4 @@
-/** 
+/**
  * @file llquaternion.cpp
  * @brief LLQuaternion class implementation.
  *
@@ -58,34 +58,40 @@ LLQuaternion::LLQuaternion(const LLMatrix3 &mat)
 
 LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec)
 {
-	LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
-	v.normalize();
-
-	F32 c, s;
-	c = cosf(angle*0.5f);
-	s = sinf(angle*0.5f);
-
-	mQ[VX] = v.mV[VX] * s;
-	mQ[VY] = v.mV[VY] * s;
-	mQ[VZ] = v.mV[VZ] * s;
-	mQ[VW] = c;
-	normalize();
+	F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		angle *= 0.5;
+		F32 c = cosf(angle);
+		F32 s = sinf(angle) / mag;
+		mQ[VX] = vec.mV[VX] * s;
+		mQ[VY] = vec.mV[VY] * s;
+		mQ[VZ] = vec.mV[VZ] * s;
+		mQ[VW] = c;
+	}
+	else
+	{
+		loadIdentity();
+	}
 }
 
 LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
 {
-	LLVector3 v(vec);
-	v.normalize();
-
-	F32 c, s;
-	c = cosf(angle*0.5f);
-	s = sinf(angle*0.5f);
-
-	mQ[VX] = v.mV[VX] * s;
-	mQ[VY] = v.mV[VY] * s;
-	mQ[VZ] = v.mV[VZ] * s;
-	mQ[VW] = c;
-	normalize();
+	F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		angle *= 0.5;
+		F32 c = cosf(angle);
+		F32 s = sinf(angle) / mag;
+		mQ[VX] = vec.mV[VX] * s;
+		mQ[VY] = vec.mV[VY] * s;
+		mQ[VZ] = vec.mV[VZ] * s;
+		mQ[VW] = c;
+	}
+	else
+	{
+		loadIdentity();
+	}
 }
 
 LLQuaternion::LLQuaternion(const LLVector3 &x_axis,
@@ -136,57 +142,61 @@ void	LLQuaternion::quantize8(F32 lower, F32 upper)
 
 const LLQuaternion&	LLQuaternion::setAngleAxis(F32 angle, F32 x, F32 y, F32 z)
 {
-	LLVector3 vec(x, y, z);
-	vec.normalize();
-
-	angle *= 0.5f;
-	F32 c, s;
-	c = cosf(angle);
-	s = sinf(angle);
-
-	mQ[VX] = vec.mV[VX]*s;
-	mQ[VY] = vec.mV[VY]*s;
-	mQ[VZ] = vec.mV[VZ]*s;
-	mQ[VW] = c;
-
-	normalize();
+	F32 mag = sqrtf(x * x + y * y + z * z);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		angle *= 0.5;
+		F32 c = cosf(angle);
+		F32 s = sinf(angle) / mag;
+		mQ[VX] = x * s;
+		mQ[VY] = y * s;
+		mQ[VZ] = z * s;
+		mQ[VW] = c;
+	}
+	else
+	{
+		loadIdentity();
+	}
 	return (*this);
 }
 
 const LLQuaternion&	LLQuaternion::setAngleAxis(F32 angle, const LLVector3 &vec)
 {
-	LLVector3 v(vec);
-	v.normalize();
-
-	angle *= 0.5f;
-	F32 c, s;
-	c = cosf(angle);
-	s = sinf(angle);
-
-	mQ[VX] = v.mV[VX]*s;
-	mQ[VY] = v.mV[VY]*s;
-	mQ[VZ] = v.mV[VZ]*s;
-	mQ[VW] = c;
-
-	normalize();
+	F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		angle *= 0.5;
+		F32 c = cosf(angle);
+		F32 s = sinf(angle) / mag;
+		mQ[VX] = vec.mV[VX] * s;
+		mQ[VY] = vec.mV[VY] * s;
+		mQ[VZ] = vec.mV[VZ] * s;
+		mQ[VW] = c;
+	}
+	else
+	{
+		loadIdentity();
+	}
 	return (*this);
 }
 
 const LLQuaternion&	LLQuaternion::setAngleAxis(F32 angle, const LLVector4 &vec)
 {
-	LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
-	v.normalize();
-
-	F32 c, s;
-	c = cosf(angle*0.5f);
-	s = sinf(angle*0.5f);
-
-	mQ[VX] = v.mV[VX]*s;
-	mQ[VY] = v.mV[VY]*s;
-	mQ[VZ] = v.mV[VZ]*s;
-	mQ[VW] = c;
-
-	normalize();
+	F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		angle *= 0.5;
+		F32 c = cosf(angle);
+		F32 s = sinf(angle) / mag;
+		mQ[VX] = vec.mV[VX] * s;
+		mQ[VY] = vec.mV[VY] * s;
+		mQ[VZ] = vec.mV[VZ] * s;
+		mQ[VW] = c;
+	}
+	else
+	{
+		loadIdentity();
+	}
 	return (*this);
 }
 
@@ -219,68 +229,80 @@ const LLQuaternion&	LLQuaternion::set(const LLMatrix4 &mat)
 // deprecated
 const LLQuaternion&	LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z)
 {
-	LLVector3 vec(x, y, z);
-	vec.normalize();
-
-	angle *= 0.5f;
-	F32 c, s;
-	c = cosf(angle);
-	s = sinf(angle);
-
-	mQ[VX] = vec.mV[VX]*s;
-	mQ[VY] = vec.mV[VY]*s;
-	mQ[VZ] = vec.mV[VZ]*s;
-	mQ[VW] = c;
-
-	normalize();
+	F32 mag = sqrtf(x * x + y * y + z * z);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		angle *= 0.5;
+		F32 c = cosf(angle);
+		F32 s = sinf(angle) / mag;
+		mQ[VX] = x * s;
+		mQ[VY] = y * s;
+		mQ[VZ] = z * s;
+		mQ[VW] = c;
+	}
+	else
+	{
+		loadIdentity();
+	}
 	return (*this);
 }
 
 // deprecated
 const LLQuaternion&	LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
 {
-	LLVector3 v(vec);
-	v.normalize();
-
-	angle *= 0.5f;
-	F32 c, s;
-	c = cosf(angle);
-	s = sinf(angle);
-
-	mQ[VX] = v.mV[VX]*s;
-	mQ[VY] = v.mV[VY]*s;
-	mQ[VZ] = v.mV[VZ]*s;
-	mQ[VW] = c;
-
-	normalize();
+	F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		angle *= 0.5;
+		F32 c = cosf(angle);
+		F32 s = sinf(angle) / mag;
+		mQ[VX] = vec.mV[VX] * s;
+		mQ[VY] = vec.mV[VY] * s;
+		mQ[VZ] = vec.mV[VZ] * s;
+		mQ[VW] = c;
+	}
+	else
+	{
+		loadIdentity();
+	}
 	return (*this);
 }
 
 const LLQuaternion&	LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
 {
-	LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
-	v.normalize();
-
-	F32 c, s;
-	c = cosf(angle*0.5f);
-	s = sinf(angle*0.5f);
-
-	mQ[VX] = v.mV[VX]*s;
-	mQ[VY] = v.mV[VY]*s;
-	mQ[VZ] = v.mV[VZ]*s;
-	mQ[VW] = c;
-
-	normalize();
+	F32 mag = sqrtf(vec.mV[VX] * vec.mV[VX] + vec.mV[VY] * vec.mV[VY] + vec.mV[VZ] * vec.mV[VZ]);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		angle *= 0.5;
+		F32 c = cosf(angle);
+		F32 s = sinf(angle) / mag;
+		mQ[VX] = vec.mV[VX] * s;
+		mQ[VY] = vec.mV[VY] * s;
+		mQ[VZ] = vec.mV[VZ] * s;
+		mQ[VW] = c;
+	}
+	else
+	{
+		loadIdentity();
+	}
 	return (*this);
 }
 
 const LLQuaternion&	LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
 {
-	LLMatrix3 rot_mat(roll, pitch, yaw);
-	rot_mat.orthogonalize();
-	*this = rot_mat.quaternion();
-		
-	normalize();
+	roll  *= 0.5f;
+	pitch *= 0.5f;
+	yaw   *= 0.5f;
+	F32 sinX = sinf(roll);
+	F32 cosX = cosf(roll);
+	F32 sinY = sinf(pitch);
+	F32 cosY = cosf(pitch);
+	F32 sinZ = sinf(yaw);
+	F32 cosZ = cosf(yaw);
+	mQ[VW] = cosX * cosY * cosZ - sinX * sinY * sinZ;
+	mQ[VX] = sinX * cosY * cosZ + cosX * sinY * sinZ;
+	mQ[VY] = cosX * sinY * cosZ - sinX * cosY * sinZ;
+	mQ[VZ] = cosX * cosY * sinZ + sinX * sinY * cosZ;
 	return (*this);
 }
 
@@ -425,68 +447,44 @@ LLMatrix4	LLQuaternion::getMatrix4(void) const
 // calculate the shortest rotation from a to b
 void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b)
 {
-	// Make a local copy of both vectors.
-	LLVector3 vec_a = a;
-	LLVector3 vec_b = b;
-
-	// Make sure neither vector is zero length.  Also normalize
-	// the vectors while we are at it.
-	F32 vec_a_mag = vec_a.normalize();
-	F32 vec_b_mag = vec_b.normalize();
-	if (vec_a_mag < F_APPROXIMATELY_ZERO ||
-		vec_b_mag < F_APPROXIMATELY_ZERO)
-	{
-		// Can't calculate a rotation from this.
-		// Just return ZERO_ROTATION instead.
-		loadIdentity();
-		return;
-	}
-
-	// Create an axis to rotate around, and the cos of the angle to rotate.
-	LLVector3 axis = vec_a % vec_b;
-	F32 cos_theta  = vec_a * vec_b;
-
-	// Check the angle between the vectors to see if they are parallel or anti-parallel.
-	if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
-	{
-		// a and b are parallel.  No rotation is necessary.
-		loadIdentity();
-	}
-	else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
+	F32 ab = a * b; // dotproduct
+	LLVector3 c = a % b; // crossproduct
+	F32 cc = c * c; // squared length of the crossproduct
+	if (ab * ab + cc) // test if the arguments have sufficient magnitude
 	{
-		// a and b are anti-parallel.
-		// Rotate 180 degrees around some orthogonal axis.
-		// Find the projection of the x-axis onto a, and try
-		// using the vector between the projection and the x-axis
-		// as the orthogonal axis.
-		LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
-		LLVector3 ortho_axis(1.f, 0.f, 0.f);
-		ortho_axis -= proj;
-		
-		// Turn this into an orthonormal axis.
-		F32 ortho_length = ortho_axis.normalize();
-		// If the axis' length is 0, then our guess at an orthogonal axis
-		// was wrong (a is parallel to the x-axis).
-		if (ortho_length < F_APPROXIMATELY_ZERO)
+		if (cc > 0.0f) // test if the arguments are (anti)parallel
 		{
-			// Use the z-axis instead.
-			ortho_axis.setVec(0.f, 0.f, 1.f);
+			F32 s = sqrtf(ab * ab + cc) + ab; // note: don't try to optimize this line
+			F32 m = 1.0f / sqrtf(cc + s * s); // the inverted magnitude of the quaternion
+			mQ[VX] = c.mV[VX] * m;
+			mQ[VY] = c.mV[VY] * m;
+			mQ[VZ] = c.mV[VZ] * m;
+			mQ[VW] = s * m;
+			return;
+		}
+		if (ab < 0.0f) // test if the angle is bigger than PI/2 (anti parallel)
+		{
+			c = a - b; // the arguments are anti-parallel, we have to choose an axis
+			F32 m = sqrtf(c.mV[VX] * c.mV[VX] +  c.mV[VY] * c.mV[VY]); // the length projected on the XY-plane
+			if (m > FP_MAG_THRESHOLD)
+			{
+				mQ[VX] = -c.mV[VY] / m; // return the quaternion with the axis in the XY-plane
+				mQ[VY] =  c.mV[VX] / m;
+				mQ[VZ] = 0.0f;
+				mQ[VW] = 0.0f;
+				return;
+			}
+			else // the vectors are parallel to the Z-axis
+			{
+				mQ[VX] = 1.0f; // rotate around the X-axis
+				mQ[VY] = 0.0f;
+				mQ[VZ] = 0.0f;
+				mQ[VW] = 0.0f;
+				return;
+			}
 		}
-
-		// Construct a quaternion from this orthonormal axis.
-		mQ[VX] = ortho_axis.mV[VX];
-		mQ[VY] = ortho_axis.mV[VY];
-		mQ[VZ] = ortho_axis.mV[VZ];
-		mQ[VW] = 0.f;
-	}
-	else
-	{
-		// a and b are NOT parallel or anti-parallel.
-		// Return the rotation between these vectors.
-		F32 theta = (F32)acos(cos_theta);
-
-		setAngleAxis(theta, axis);
 	}
+	loadIdentity();
 }
 
 // constrains rotation to a cone angle specified in radians
@@ -838,79 +836,82 @@ LLQuaternion::Order StringToOrder( const char *str )
 
 void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const
 {
-	F32 cos_a = mQ[VW];
-	if (cos_a > 1.0f) cos_a = 1.0f;
-	if (cos_a < -1.0f) cos_a = -1.0f;
-
-    F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
-
-    if ( fabs( sin_a ) < 0.0005f )
-		sin_a = 1.0f;
-	else
-		sin_a = 1.f/sin_a;
-
-    F32 temp_angle = 2.0f * (F32) acos( cos_a );
-	if (temp_angle > F_PI)
-	{
-		// The (angle,axis) pair should never have angles outside [PI, -PI]
-		// since we want the _shortest_ (angle,axis) solution.
-		// Since acos is defined for [0, PI], and we multiply by 2.0, we
-		// can push the angle outside the acceptible range.
-		// When this happens we set the angle to the other portion of a 
-		// full 2PI rotation, and negate the axis, which reverses the 
-		// direction of the rotation (by the right-hand rule).
-		*angle = 2.f * F_PI - temp_angle;
-    	vec.mV[VX] = - mQ[VX] * sin_a;
-    	vec.mV[VY] = - mQ[VY] * sin_a;
-    	vec.mV[VZ] = - mQ[VZ] * sin_a;
+	F32 v = sqrtf(mQ[VX] * mQ[VX] + mQ[VY] * mQ[VY] + mQ[VZ] * mQ[VZ]); // length of the vector-component
+	if (v > FP_MAG_THRESHOLD)
+	{
+		F32 oomag = 1.0f / v;
+		F32 w = mQ[VW];
+		if (mQ[VW] < 0.0f)
+		{
+			w = -w; // make VW positive
+			oomag = -oomag; // invert the axis
+		}
+		vec.mV[VX] = mQ[VX] * oomag; // normalize the axis
+		vec.mV[VY] = mQ[VY] * oomag;
+		vec.mV[VZ] = mQ[VZ] * oomag;
+		*angle = 2.0f * atan2f(v, w); // get the angle
 	}
 	else
 	{
-		*angle = temp_angle;
-    	vec.mV[VX] = mQ[VX] * sin_a;
-    	vec.mV[VY] = mQ[VY] * sin_a;
-    	vec.mV[VZ] = mQ[VZ] * sin_a;
+		*angle = 0.0f; // no rotation
+		vec.mV[VX] = 0.0f; // around some dummy axis
+		vec.mV[VY] = 0.0f;
+		vec.mV[VZ] = 1.0f;
 	}
 }
 
-
 // quaternion does not need to be normalized
 void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
 {
-	LLMatrix3 rot_mat(*this);
-	rot_mat.orthogonalize();
-	rot_mat.getEulerAngles(roll, pitch, yaw);
-
-//	// NOTE: LLQuaternion's are actually inverted with respect to
-//	// the matrices, so this code also assumes inverted quaternions
-//	// (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
-//	// in reverse order (yaw,pitch,roll).
-//	F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
-//	F64 m20 = 2.0*(x*z-y*w);
-//	if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
-//	{
-//		*roll = 0.0f;
-//		*pitch = (F32)asin(m20);
-//		*yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
-//	}
-//	else
-//	{
-//		*roll  = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
-//		*pitch = (F32)asin(m20);
-//		*yaw   = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
-//	}
+	F32 sx = 2 * (mQ[VX] * mQ[VW] - mQ[VY] * mQ[VZ]); // sine of the roll
+	F32 sy = 2 * (mQ[VY] * mQ[VW] + mQ[VX] * mQ[VZ]); // sine of the pitch
+	F32 ys = mQ[VW] * mQ[VW] - mQ[VY] * mQ[VY]; // intermediate cosine 1
+	F32 xz = mQ[VX] * mQ[VX] - mQ[VZ] * mQ[VZ]; // intermediate cosine 2
+	F32 cx = ys - xz; // cosine of the roll
+	F32 cy = sqrtf(sx * sx + cx * cx); // cosine of the pitch
+	if (cy > GIMBAL_THRESHOLD) // no gimbal lock
+	{
+		*roll  = atan2f(sx, cx);
+		*pitch = atan2f(sy, cy);
+		*yaw   = atan2f(2 * (mQ[VZ] * mQ[VW] - mQ[VX] * mQ[VY]), ys + xz);
+	}
+	else // gimbal lock
+	{
+		if (sy > 0)
+		{
+			*pitch = F_PI_BY_TWO;
+			*yaw = 2 * atan2f(mQ[VZ] + mQ[VX], mQ[VW] + mQ[VY]);
+		}
+		else
+		{
+			*pitch = -F_PI_BY_TWO;
+			*yaw = 2 * atan2f(mQ[VZ] - mQ[VX], mQ[VW] - mQ[VY]);
+		}
+		*roll = 0;
+	}
 }
 
 // Saves space by using the fact that our quaternions are normalized
 LLVector3 LLQuaternion::packToVector3() const
 {
+	F32 x = mQ[VX];
+	F32 y = mQ[VY];
+	F32 z = mQ[VZ];
+	F32 w = mQ[VW];
+	F32 mag = sqrtf(x * x + y * y + z * z + w * w);
+	if (mag > FP_MAG_THRESHOLD)
+	{
+		x /= mag;
+		y /= mag;
+		z /= mag; // no need to normalize w, it's not used
+	}
 	if( mQ[VW] >= 0 )
 	{
-		return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
+		return LLVector3( x, y , z );
 	}
 	else
 	{
-		return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
+		return LLVector3( -x, -y, -z );
 	}
 }
 
diff --git a/indra/llmath/llquaternion.h b/indra/llmath/llquaternion.h
index ca0dfe206b..e56929ed0f 100755
--- a/indra/llmath/llquaternion.h
+++ b/indra/llmath/llquaternion.h
@@ -1,4 +1,4 @@
-/** 
+/**
  * @file llquaternion.h
  * @brief LLQuaternion class header file.
  *
@@ -304,43 +304,29 @@ inline const LLQuaternion&	LLQuaternion::setQuat(const F32 *q)
 	return (*this);
 }
 
-// There may be a cheaper way that avoids the sqrt.
-// Does sin_a = VX*VX + VY*VY + VZ*VZ?
-// Copied from Matrix and Quaternion FAQ 1.12
 inline void LLQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const
 {
-	F32 cos_a = mQ[VW];
-	if (cos_a > 1.0f) cos_a = 1.0f;
-	if (cos_a < -1.0f) cos_a = -1.0f;
-
-    F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
-
-    if ( fabs( sin_a ) < 0.0005f )
-		sin_a = 1.0f;
-	else
-		sin_a = 1.f/sin_a;
-
-    F32 temp_angle = 2.0f * (F32) acos( cos_a );
-	if (temp_angle > F_PI)
+	F32 v = sqrtf(mQ[VX] * mQ[VX] + mQ[VY] * mQ[VY] + mQ[VZ] * mQ[VZ]); // length of the vector-component
+	if (v > FP_MAG_THRESHOLD)
 	{
-		// The (angle,axis) pair should never have angles outside [PI, -PI]
-		// since we want the _shortest_ (angle,axis) solution.
-		// Since acos is defined for [0, PI], and we multiply by 2.0, we
-		// can push the angle outside the acceptible range.
-		// When this happens we set the angle to the other portion of a 
-		// full 2PI rotation, and negate the axis, which reverses the 
-		// direction of the rotation (by the right-hand rule).
-		*angle = 2.f * F_PI - temp_angle;
-    	*x = - mQ[VX] * sin_a;
-    	*y = - mQ[VY] * sin_a;
-    	*z = - mQ[VZ] * sin_a;
+		F32 oomag = 1.0f / v;
+		F32 w = mQ[VW];
+		if (w < 0.0f)
+		{
+			w = -w; // make VW positive
+			oomag = -oomag; // invert the axis
+		}
+		*x = mQ[VX] * oomag; // normalize the axis
+		*y = mQ[VY] * oomag;
+		*z = mQ[VZ] * oomag;
+		*angle = 2.0f * atan2f(v, w); // get the angle
 	}
 	else
 	{
-		*angle = temp_angle;
-    	*x = mQ[VX] * sin_a;
-    	*y = mQ[VY] * sin_a;
-    	*z = mQ[VZ] * sin_a;
+		*angle = 0.0f; // no rotation
+		*x = 0.0f; // around some dummy axis
+		*y = 0.0f;
+		*z = 1.0f;
 	}
 }
 
diff --git a/indra/llmath/v3dmath.h b/indra/llmath/v3dmath.h
index cab4c93a9f..4938273d5b 100755
--- a/indra/llmath/v3dmath.h
+++ b/indra/llmath/v3dmath.h
@@ -72,17 +72,22 @@ class LLVector3d
 		BOOL		clamp(const F64 min, const F64 max);		// Clamps all values to (min,max), returns TRUE if data changed
 		BOOL		abs();						// sets all values to absolute value of original value (first octant), returns TRUE if changed
 
-		inline const LLVector3d&	clearVec();		// Clears LLVector3d to (0, 0, 0, 1)
+		inline const LLVector3d&	clear();		// Clears LLVector3d to (0, 0, 0, 1)
+		inline const LLVector3d&	clearVec();		// deprecated
 		inline const LLVector3d&	setZero();		// Zero LLVector3d to (0, 0, 0, 0)
 		inline const LLVector3d&	zeroVec();		// deprecated
-		inline const LLVector3d&	setVec(const F64 x, const F64 y, const F64 z);	// Sets LLVector3d to (x, y, z, 1)
-		inline const LLVector3d&	setVec(const LLVector3d &vec);	// Sets LLVector3d to vec
-		inline const LLVector3d&	setVec(const F64 *vec);			// Sets LLVector3d to vec
-		inline const LLVector3d&	setVec(const LLVector3 &vec);
-
-		F64		magVec() const;				// Returns magnitude of LLVector3d
-		F64		magVecSquared() const;		// Returns magnitude squared of LLVector3d
-		inline F64		normVec();					// Normalizes and returns the magnitude of LLVector3d
+		inline const LLVector3d&	set(const F64 x, const F64 y, const F64 z);	// Sets LLVector3d to (x, y, z, 1)
+		inline const LLVector3d&	set(const LLVector3d &vec);	// Sets LLVector3d to vec
+		inline const LLVector3d&	set(const F64 *vec);		// Sets LLVector3d to vec
+		inline const LLVector3d&	set(const LLVector3 &vec);
+		inline const LLVector3d&	setVec(const F64 x, const F64 y, const F64 z);	// deprecated
+		inline const LLVector3d&	setVec(const LLVector3d &vec);	// deprecated
+		inline const LLVector3d&	setVec(const F64 *vec);			// deprecated
+		inline const LLVector3d&	setVec(const LLVector3 &vec);	// deprecated
+
+		F64		magVec() const;				// deprecated
+		F64		magVecSquared() const;		// deprecated
+		inline F64		normVec();					// deprecated
 
 		F64 length() const;			// Returns magnitude of LLVector3d
 		F64 lengthSquared() const;	// Returns magnitude squared of LLVector3d
@@ -127,7 +132,15 @@ class LLVector3d
 
 typedef LLVector3d LLGlobalVec;
 
-const LLVector3d &LLVector3d::setVec(const LLVector3 &vec)
+inline const LLVector3d &LLVector3d::set(const LLVector3 &vec)
+{
+	mdV[0] = vec.mV[0];
+	mdV[1] = vec.mV[1];
+	mdV[2] = vec.mV[2];
+	return *this;
+}
+
+inline const LLVector3d &LLVector3d::setVec(const LLVector3 &vec)
 {
 	mdV[0] = vec.mV[0];
 	mdV[1] = vec.mV[1];
@@ -184,6 +197,14 @@ inline BOOL LLVector3d::isFinite() const
 
 // Clear and Assignment Functions
 
+inline const LLVector3d&	LLVector3d::clear(void)
+{
+	mdV[0] = 0.f;
+	mdV[1] = 0.f;
+	mdV[2]= 0.f;
+	return (*this);
+}
+
 inline const LLVector3d&	LLVector3d::clearVec(void)
 {
 	mdV[0] = 0.f;
@@ -208,6 +229,30 @@ inline const LLVector3d&	LLVector3d::zeroVec(void)
 	return (*this);
 }
 
+inline const LLVector3d&	LLVector3d::set(const F64 x, const F64 y, const F64 z)
+{
+	mdV[VX] = x;
+	mdV[VY] = y;
+	mdV[VZ] = z;
+	return (*this);
+}
+
+inline const LLVector3d&	LLVector3d::set(const LLVector3d &vec)
+{
+	mdV[0] = vec.mdV[0];
+	mdV[1] = vec.mdV[1];
+	mdV[2] = vec.mdV[2];
+	return (*this);
+}
+
+inline const LLVector3d&	LLVector3d::set(const F64 *vec)
+{
+	mdV[0] = vec[0];
+	mdV[1] = vec[1];
+	mdV[2] = vec[2];
+	return (*this);
+}
+
 inline const LLVector3d&	LLVector3d::setVec(const F64 x, const F64 y, const F64 z)
 {
 	mdV[VX] = x;
diff --git a/indra/llmath/v3math.h b/indra/llmath/v3math.h
index c807a30f7b..f3fbce4843 100755
--- a/indra/llmath/v3math.h
+++ b/indra/llmath/v3math.h
@@ -1,4 +1,4 @@
-/** 
+/**
  * @file v3math.h
  * @brief LLVector3 class header file.
  *
@@ -159,9 +159,7 @@ F32	dist_vec(const LLVector3 &a, const LLVector3 &b);		// Returns distance betwe
 F32	dist_vec_squared(const LLVector3 &a, const LLVector3 &b);// Returns distance squared between a and b
 F32	dist_vec_squared2D(const LLVector3 &a, const LLVector3 &b);// Returns distance squared between a and b ignoring Z component
 LLVector3 projected_vec(const LLVector3 &a, const LLVector3 &b); // Returns vector a projected on vector b
-// Returns a vector in direction of a, such that when projected onto b, gives you the same value as b
-// in other words: projected_vec(inverse_projected_vec(a, b), b) == b;
-LLVector3 inverse_projected_vec(const LLVector3 &a, const LLVector3 &b); 
+LLVector3 inverse_projected_vec(const LLVector3 &a, const LLVector3 &b); // Returns vector a scaled such that projected_vec(inverse_projected_vec(a, b), b) == b;
 LLVector3 parallel_component(const LLVector3 &a, const LLVector3 &b); // Returns vector a projected on vector b (same as projected_vec)
 LLVector3 orthogonal_component(const LLVector3 &a, const LLVector3 &b); // Returns component of vector a not parallel to vector b (same as projected_vec)
 LLVector3 lerp(const LLVector3 &a, const LLVector3 &b, F32 u); // Returns a vector that is a linear interpolation between a and b
@@ -493,9 +491,15 @@ inline F32	dist_vec_squared2D(const LLVector3 &a, const LLVector3 &b)
 
 inline LLVector3 projected_vec(const LLVector3 &a, const LLVector3 &b)
 {
-	LLVector3 project_axis = b;
-	project_axis.normalize();
-	return project_axis * (a * project_axis);
+	F32 bb = b * b;
+	if (bb > FP_MAG_THRESHOLD * FP_MAG_THRESHOLD)
+	{
+		return ((a * b) / bb) * b;
+	}
+	else
+	{
+		return b.zero;
+	}
 }
 
 inline LLVector3 inverse_projected_vec(const LLVector3& a, const LLVector3& b)
@@ -571,15 +575,13 @@ inline void update_min_max(LLVector3& min, LLVector3& max, const F32* pos)
 
 inline F32 angle_between(const LLVector3& a, const LLVector3& b)
 {
-	LLVector3 an = a;
-	LLVector3 bn = b;
-	an.normalize();
-	bn.normalize();
-	F32 cosine = an * bn;
-	F32 angle = (cosine >= 1.0f) ? 0.0f :
-				(cosine <= -1.0f) ? F_PI :
-				(F32)acos(cosine);
-	return angle;
+	F32 ab = a * b; // dotproduct
+	if (ab == -0.0f)
+	{
+		ab = 0.0f; // get rid of negative zero
+	}
+	LLVector3 c = a % b; // crossproduct
+	return atan2f(sqrtf(c * c), ab); // return the angle
 }
 
 inline BOOL are_parallel(const LLVector3 &a, const LLVector3 &b, F32 epsilon)