diff options
Diffstat (limited to 'indra/llmath/llquaternion.h')
| -rw-r--r-- | indra/llmath/llquaternion.h | 225 |
1 files changed, 172 insertions, 53 deletions
diff --git a/indra/llmath/llquaternion.h b/indra/llmath/llquaternion.h index 01ddae94cb..26da14ae20 100644 --- a/indra/llmath/llquaternion.h +++ b/indra/llmath/llquaternion.h @@ -2,30 +2,25 @@ * @file llquaternion.h * @brief LLQuaternion class header file. * - * $LicenseInfo:firstyear=2000&license=viewergpl$ - * - * Copyright (c) 2000-2007, Linden Research, Inc. - * + * $LicenseInfo:firstyear=2000&license=viewerlgpl$ * Second Life Viewer Source Code - * The source code in this file ("Source Code") is provided by Linden Lab - * to you under the terms of the GNU General Public License, version 2.0 - * ("GPL"), unless you have obtained a separate licensing agreement - * ("Other License"), formally executed by you and Linden Lab. Terms of - * the GPL can be found in doc/GPL-license.txt in this distribution, or - * online at http://secondlife.com/developers/opensource/gplv2 + * Copyright (C) 2010, Linden Research, Inc. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; + * version 2.1 of the License only. * - * There are special exceptions to the terms and conditions of the GPL as - * it is applied to this Source Code. View the full text of the exception - * in the file doc/FLOSS-exception.txt in this software distribution, or - * online at http://secondlife.com/developers/opensource/flossexception + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. * - * By copying, modifying or distributing this software, you acknowledge - * that you have read and understood your obligations described above, - * and agree to abide by those obligations. + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA * - * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO - * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY, - * COMPLETENESS OR PERFORMANCE. + * Linden Research, Inc., 945 Battery Street, San Francisco, CA 94111 USA * $/LicenseInfo$ */ @@ -57,10 +52,10 @@ public: LLQuaternion(); // Initializes Quaternion to (0,0,0,1) explicit LLQuaternion(const LLMatrix4 &mat); // Initializes Quaternion from Matrix4 explicit LLQuaternion(const LLMatrix3 &mat); // Initializes Quaternion from Matrix3 - LLQuaternion(F32 x, F32 y, F32 z, F32 w); // Initializes Quaternion to normQuat(x, y, z, w) + LLQuaternion(F32 x, F32 y, F32 z, F32 w); // Initializes Quaternion to normalize(x, y, z, w) LLQuaternion(F32 angle, const LLVector4 &vec); // Initializes Quaternion to axis_angle2quat(angle, vec) LLQuaternion(F32 angle, const LLVector3 &vec); // Initializes Quaternion to axis_angle2quat(angle, vec) - LLQuaternion(const F32 *q); // Initializes Quaternion to normQuat(x, y, z, w) + LLQuaternion(const F32 *q); // Initializes Quaternion to normalize(x, y, z, w) LLQuaternion(const LLVector3 &x_axis, const LLVector3 &y_axis, const LLVector3 &z_axis); // Initializes Quaternion from Matrix3 = [x_axis ; y_axis ; z_axis] @@ -71,15 +66,27 @@ public: void quantize16(F32 lower, F32 upper); // changes the vector to reflect quatization void quantize8(F32 lower, F32 upper); // changes the vector to reflect quatization void loadIdentity(); // Loads the quaternion that represents the identity rotation - const LLQuaternion& setQuatInit(F32 x, F32 y, F32 z, F32 w); // Sets Quaternion to normQuat(x, y, z, w) - const LLQuaternion& setQuat(const LLQuaternion &quat); // Copies Quaternion - const LLQuaternion& setQuat(const F32 *q); // Sets Quaternion to normQuat(quat[VX], quat[VY], quat[VZ], quat[VW]) - const LLQuaternion& setQuat(const LLMatrix3 &mat); // Sets Quaternion to mat2quat(mat) - const LLQuaternion& setQuat(const LLMatrix4 &mat); // Sets Quaternion to mat2quat(mat) - const LLQuaternion& setQuat(F32 angle, F32 x, F32 y, F32 z); // Sets Quaternion to axis_angle2quat(angle, x, y, z) - const LLQuaternion& setQuat(F32 angle, const LLVector3 &vec); // Sets Quaternion to axis_angle2quat(angle, vec) - const LLQuaternion& setQuat(F32 angle, const LLVector4 &vec); // Sets Quaternion to axis_angle2quat(angle, vec) - const LLQuaternion& setQuat(F32 roll, F32 pitch, F32 yaw); // Sets Quaternion to euler2quat(pitch, yaw, roll) + + const LLQuaternion& set(F32 x, F32 y, F32 z, F32 w); // Sets Quaternion to normalize(x, y, z, w) + const LLQuaternion& set(const LLQuaternion &quat); // Copies Quaternion + const LLQuaternion& set(const F32 *q); // Sets Quaternion to normalize(quat[VX], quat[VY], quat[VZ], quat[VW]) + const LLQuaternion& set(const LLMatrix3 &mat); // Sets Quaternion to mat2quat(mat) + const LLQuaternion& set(const LLMatrix4 &mat); // Sets Quaternion to mat2quat(mat) + + const LLQuaternion& setAngleAxis(F32 angle, F32 x, F32 y, F32 z); // Sets Quaternion to axis_angle2quat(angle, x, y, z) + const LLQuaternion& setAngleAxis(F32 angle, const LLVector3 &vec); // Sets Quaternion to axis_angle2quat(angle, vec) + const LLQuaternion& setAngleAxis(F32 angle, const LLVector4 &vec); // Sets Quaternion to axis_angle2quat(angle, vec) + const LLQuaternion& setEulerAngles(F32 roll, F32 pitch, F32 yaw); // Sets Quaternion to euler2quat(pitch, yaw, roll) + + const LLQuaternion& setQuatInit(F32 x, F32 y, F32 z, F32 w); // deprecated + const LLQuaternion& setQuat(const LLQuaternion &quat); // deprecated + const LLQuaternion& setQuat(const F32 *q); // deprecated + const LLQuaternion& setQuat(const LLMatrix3 &mat); // deprecated + const LLQuaternion& setQuat(const LLMatrix4 &mat); // deprecated + const LLQuaternion& setQuat(F32 angle, F32 x, F32 y, F32 z); // deprecated + const LLQuaternion& setQuat(F32 angle, const LLVector3 &vec); // deprecated + const LLQuaternion& setQuat(F32 angle, const LLVector4 &vec); // deprecated + const LLQuaternion& setQuat(F32 roll, F32 pitch, F32 yaw); // deprecated LLMatrix4 getMatrix4(void) const; // Returns the Matrix4 equivalent of Quaternion LLMatrix3 getMatrix3(void) const; // Returns the Matrix3 equivalent of Quaternion @@ -87,11 +94,16 @@ public: void getAngleAxis(F32* angle, LLVector3 &vec) const; void getEulerAngles(F32 *roll, F32* pitch, F32 *yaw) const; - F32 normQuat(); // Normalizes Quaternion and returns magnitude - const LLQuaternion& conjQuat(void); // Conjugates Quaternion and returns result + F32 normalize(); // Normalizes Quaternion and returns magnitude + F32 normQuat(); // deprecated + + const LLQuaternion& conjugate(void); // Conjugates Quaternion and returns result + const LLQuaternion& conjQuat(void); // deprecated // Other useful methods - const LLQuaternion& transQuat(); // Transpose + const LLQuaternion& transpose(); // transpose (same as conjugate) + const LLQuaternion& transQuat(); // deprecated + void shortestArc(const LLVector3 &a, const LLVector3 &b); // shortest rotation from a to b const LLQuaternion& constrain(F32 radians); // constrains rotation to a cone angle specified in radians @@ -141,7 +153,7 @@ public: friend const char *OrderToString( const Order order ); friend Order StringToOrder( const char *str ); - static BOOL parseQuat(const char* buf, LLQuaternion* value); + static BOOL parseQuat(const std::string& buf, LLQuaternion* value); // For debugging, only //static U32 mMultCount; @@ -189,7 +201,7 @@ inline LLQuaternion::LLQuaternion(F32 x, F32 y, F32 z, F32 w) mQ[VS] = w; //RN: don't normalize this case as its used mainly for temporaries during calculations - //normQuat(); + //normalize(); /* F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]); mag -= 1.f; @@ -205,7 +217,7 @@ inline LLQuaternion::LLQuaternion(const F32 *q) mQ[VZ] = q[VZ]; mQ[VS] = q[VW]; - normQuat(); + normalize(); /* F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]); mag -= 1.f; @@ -224,33 +236,67 @@ inline void LLQuaternion::loadIdentity() } +inline const LLQuaternion& LLQuaternion::set(F32 x, F32 y, F32 z, F32 w) +{ + mQ[VX] = x; + mQ[VY] = y; + mQ[VZ] = z; + mQ[VS] = w; + normalize(); + return (*this); +} + +inline const LLQuaternion& LLQuaternion::set(const LLQuaternion &quat) +{ + mQ[VX] = quat.mQ[VX]; + mQ[VY] = quat.mQ[VY]; + mQ[VZ] = quat.mQ[VZ]; + mQ[VW] = quat.mQ[VW]; + normalize(); + return (*this); +} + +inline const LLQuaternion& LLQuaternion::set(const F32 *q) +{ + mQ[VX] = q[VX]; + mQ[VY] = q[VY]; + mQ[VZ] = q[VZ]; + mQ[VS] = q[VW]; + normalize(); + return (*this); +} + + +// deprecated inline const LLQuaternion& LLQuaternion::setQuatInit(F32 x, F32 y, F32 z, F32 w) { mQ[VX] = x; mQ[VY] = y; mQ[VZ] = z; mQ[VS] = w; - normQuat(); + normalize(); return (*this); } +// deprecated inline const LLQuaternion& LLQuaternion::setQuat(const LLQuaternion &quat) { mQ[VX] = quat.mQ[VX]; mQ[VY] = quat.mQ[VY]; mQ[VZ] = quat.mQ[VZ]; mQ[VW] = quat.mQ[VW]; - normQuat(); + normalize(); return (*this); } +// deprecated inline const LLQuaternion& LLQuaternion::setQuat(const F32 *q) { mQ[VX] = q[VX]; mQ[VY] = q[VY]; mQ[VZ] = q[VZ]; mQ[VS] = q[VW]; - normQuat(); + normalize(); return (*this); } @@ -270,10 +316,36 @@ inline void LLQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const else sin_a = 1.f/sin_a; - *angle = 2.0f * (F32) acos( cos_a ); - *x = mQ[VX] * sin_a; - *y = mQ[VY] * sin_a; - *z = mQ[VZ] * 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; + *x = - mQ[VX] * sin_a; + *y = - mQ[VY] * sin_a; + *z = - mQ[VZ] * sin_a; + } + else + { + *angle = temp_angle; + *x = mQ[VX] * sin_a; + *y = mQ[VY] * sin_a; + *z = mQ[VZ] * sin_a; + } +} + +inline const LLQuaternion& LLQuaternion::conjugate() +{ + mQ[VX] *= -1.f; + mQ[VY] *= -1.f; + mQ[VZ] *= -1.f; + return (*this); } inline const LLQuaternion& LLQuaternion::conjQuat() @@ -285,12 +357,21 @@ inline const LLQuaternion& LLQuaternion::conjQuat() } // Transpose +inline const LLQuaternion& LLQuaternion::transpose() +{ + mQ[VX] *= -1.f; + mQ[VY] *= -1.f; + mQ[VZ] *= -1.f; + return (*this); +} + +// deprecated inline const LLQuaternion& LLQuaternion::transQuat() { - mQ[VX] = -mQ[VX]; - mQ[VY] = -mQ[VY]; - mQ[VZ] = -mQ[VZ]; - return *this; + mQ[VX] *= -1.f; + mQ[VY] *= -1.f; + mQ[VZ] *= -1.f; + return (*this); } @@ -382,17 +463,55 @@ 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) + { + // 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; + mQ[VS] = 1.f; + } + + return mag; +} + +// deprecated inline F32 LLQuaternion::normQuat() { 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; + 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 { |