diff options
Diffstat (limited to 'indra/llmath')
| -rw-r--r-- | indra/llmath/llquaternion.cpp | 6 | ||||
| -rw-r--r-- | indra/llmath/llquaternion.h | 34 |
2 files changed, 27 insertions, 13 deletions
diff --git a/indra/llmath/llquaternion.cpp b/indra/llmath/llquaternion.cpp index cfd6183ec4..fdcc19d657 100644 --- a/indra/llmath/llquaternion.cpp +++ b/indra/llmath/llquaternion.cpp @@ -121,7 +121,7 @@ void LLQuaternion::quantize16(F32 lower, F32 upper) mQ[VZ] = z; mQ[VS] = s; - normQuat(); + normalize(); } void LLQuaternion::quantize8(F32 lower, F32 upper) @@ -131,7 +131,7 @@ void LLQuaternion::quantize8(F32 lower, F32 upper) mQ[VZ] = U8_to_F32(F32_to_U8_ROUND(mQ[VZ], lower, upper), lower, upper); mQ[VS] = U8_to_F32(F32_to_U8_ROUND(mQ[VS], lower, upper), lower, upper); - normQuat(); + normalize(); } // LLVector3 Magnitude and Normalization Functions @@ -346,7 +346,7 @@ const LLQuaternion& LLQuaternion::setQuat(const LLMatrix4 &mat) // mQ[VZ] = (F32)(cosX*cosY*sinZ - sinX*sinY*cosZ); //#endif // -// normQuat(); +// normalize(); // return (*this); } diff --git a/indra/llmath/llquaternion.h b/indra/llmath/llquaternion.h index 5db9c5be2e..0769f29f23 100644 --- a/indra/llmath/llquaternion.h +++ b/indra/llmath/llquaternion.h @@ -469,20 +469,30 @@ inline const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b) return a; } +const F32 ONE_PART_IN_A_MILLION = 0.000001f; + inline F32 LLQuaternion::normalize() { F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]); if (mag > FP_MAG_THRESHOLD) { - F32 oomag = 1.f/mag; - mQ[VX] *= oomag; - mQ[VY] *= oomag; - mQ[VZ] *= oomag; - mQ[VS] *= oomag; + // Floating point error can prevent some quaternions from achieving + // exact unity length. When trying to renormalize such quaternions we + // can oscillate between multiple quantized states. To prevent such + // drifts we only renomalize if the length is far enough from unity. + if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION) + { + F32 oomag = 1.f/mag; + mQ[VX] *= oomag; + mQ[VY] *= oomag; + mQ[VZ] *= oomag; + mQ[VS] *= oomag; + } } else { + // we were given a very bad quaternion so we set it to identity mQ[VX] = 0.f; mQ[VY] = 0.f; mQ[VZ] = 0.f; @@ -499,11 +509,15 @@ inline F32 LLQuaternion::normQuat() if (mag > FP_MAG_THRESHOLD) { - F32 oomag = 1.f/mag; - mQ[VX] *= oomag; - mQ[VY] *= oomag; - mQ[VZ] *= oomag; - mQ[VS] *= oomag; + if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION) + { + // only renormalize if length not close enough to 1.0 already + F32 oomag = 1.f/mag; + mQ[VX] *= oomag; + mQ[VY] *= oomag; + mQ[VZ] *= oomag; + mQ[VS] *= oomag; + } } else { |