From 681305e9bf725872af906acb707627b26b3c8e67 Mon Sep 17 00:00:00 2001
From: MartinRJ Fayray <fuerholz@gmx.net>
Date: Thu, 25 Oct 2012 07:36:33 +0200
Subject: BUG-540: Math updates by Moon Metty. Reviewed by Chieron Tenk.
---
indra/llmath/llmath.h | 5 +-
indra/llmath/llquaternion.cpp | 449 +++++++++++++++++++++---------------------
indra/llmath/llquaternion.h | 50 ++---
indra/llmath/v3math.h | 30 +--
4 files changed, 264 insertions(+), 270 deletions(-)
(limited to 'indra/llmath')
diff --git a/indra/llmath/llmath.h b/indra/llmath/llmath.h
index b93f89d674..95e6f68895 100644
--- a/indra/llmath/llmath.h
+++ b/indra/llmath/llmath.h
@@ -1,4 +1,4 @@
-/**
+/**
* @file llmath.h
* @brief Useful math constants and macros.
*
@@ -81,6 +81,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 100644
--- 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 100644
--- 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/v3math.h b/indra/llmath/v3math.h
index 0432aeba4c..a269ed1b79 100644
--- a/indra/llmath/v3math.h
+++ b/indra/llmath/v3math.h
@@ -1,4 +1,4 @@
-/**
+/**
* @file v3math.h
* @brief LLVector3 class header file.
*
@@ -490,9 +490,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 parallel_component(const LLVector3 &a, const LLVector3 &b)
@@ -556,15 +562,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)
--
cgit v1.2.3