From 70f8dc7a4f4be217fea5439e474fc75e567c23c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Lars=20N=C3=A6sbye=20Christensen?= Date: Sat, 10 Feb 2024 22:37:52 +0100 Subject: miscellaneous: BOOL (int) to real bool --- indra/llmath/llquaternion.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'indra/llmath/llquaternion.cpp') diff --git a/indra/llmath/llquaternion.cpp b/indra/llmath/llquaternion.cpp index 57a976b57a..1e7c39df65 100644 --- a/indra/llmath/llquaternion.cpp +++ b/indra/llmath/llquaternion.cpp @@ -963,7 +963,7 @@ BOOL LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value) { if( buf.empty() || value == NULL) { - return FALSE; + return false; } LLQuaternion quat; @@ -971,10 +971,10 @@ BOOL LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value) if( 4 == count ) { value->set( quat ); - return TRUE; + return true; } - return FALSE; + return false; } -- cgit v1.2.3 From 9480a98cffaafa5826b8daad20020cf399bbbefc Mon Sep 17 00:00:00 2001 From: Ansariel Date: Fri, 16 Feb 2024 00:07:58 +0100 Subject: Replace most of BOOL with bool in llmath --- indra/llmath/llquaternion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'indra/llmath/llquaternion.cpp') diff --git a/indra/llmath/llquaternion.cpp b/indra/llmath/llquaternion.cpp index 1e7c39df65..0d1ffa5857 100644 --- a/indra/llmath/llquaternion.cpp +++ b/indra/llmath/llquaternion.cpp @@ -959,7 +959,7 @@ void LLQuaternion::unpackFromVector3( const LLVector3& vec ) } } -BOOL LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value) +bool LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value) { if( buf.empty() || value == NULL) { -- cgit v1.2.3 From c3e6f7b1643076df35a6960f735024078ddbb353 Mon Sep 17 00:00:00 2001 From: Ansariel Date: Mon, 19 Feb 2024 21:33:38 +0100 Subject: Convert remaining cases of BOOL to bool in llmath and llprimitive Changed the return values for LLPrimitive::packTEMessage methods from FALSE to true - these seemed to be strange and wrong, especially considering the following statement in LLVOAvatarSelf: bool success = packTEMessage(mesgsys); --- indra/llmath/llquaternion.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'indra/llmath/llquaternion.cpp') diff --git a/indra/llmath/llquaternion.cpp b/indra/llmath/llquaternion.cpp index 0d1ffa5857..dd3d552832 100644 --- a/indra/llmath/llquaternion.cpp +++ b/indra/llmath/llquaternion.cpp @@ -653,14 +653,14 @@ LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b ) F32 cos_t = a.mQ[0]*b.mQ[0] + a.mQ[1]*b.mQ[1] + a.mQ[2]*b.mQ[2] + a.mQ[3]*b.mQ[3]; // if b is on opposite hemisphere from a, use -a instead - int bflip; + bool bflip; if (cos_t < 0.0f) { cos_t = -cos_t; - bflip = TRUE; + bflip = true; } else - bflip = FALSE; + bflip = false; // if B is (within precision limits) the same as A, // just linear interpolate between A and B. -- cgit v1.2.3 From e2e37cced861b98de8c1a7c9c0d3a50d2d90e433 Mon Sep 17 00:00:00 2001 From: Ansariel Date: Wed, 22 May 2024 21:25:21 +0200 Subject: Fix line endlings --- indra/llmath/llquaternion.cpp | 1962 ++++++++++++++++++++--------------------- 1 file changed, 981 insertions(+), 981 deletions(-) (limited to 'indra/llmath/llquaternion.cpp') diff --git a/indra/llmath/llquaternion.cpp b/indra/llmath/llquaternion.cpp index 90d908c07c..aefb82b2f0 100644 --- a/indra/llmath/llquaternion.cpp +++ b/indra/llmath/llquaternion.cpp @@ -1,981 +1,981 @@ -/** - * @file llquaternion.cpp - * @brief LLQuaternion class implementation. - * - * $LicenseInfo:firstyear=2000&license=viewerlgpl$ - * Second Life Viewer Source Code - * 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. - * - * 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. - * - * 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 - * - * Linden Research, Inc., 945 Battery Street, San Francisco, CA 94111 USA - * $/LicenseInfo$ - */ - -#include "linden_common.h" - -#include "llmath.h" // for F_PI - -#include "llquaternion.h" - -//#include "vmath.h" -#include "v3math.h" -#include "v3dmath.h" -#include "v4math.h" -#include "m4math.h" -#include "m3math.h" -#include "llquantize.h" - -// WARNING: Don't use this for global const definitions! using this -// at the top of a *.cpp file might not give you what you think. -const LLQuaternion LLQuaternion::DEFAULT; - -// Constructors - -LLQuaternion::LLQuaternion(const LLMatrix4 &mat) -{ - *this = mat.quaternion(); - normalize(); -} - -LLQuaternion::LLQuaternion(const LLMatrix3 &mat) -{ - *this = mat.quaternion(); - normalize(); -} - -LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec) -{ - 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) -{ - 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, - const LLVector3 &y_axis, - const LLVector3 &z_axis) -{ - LLMatrix3 mat; - mat.setRows(x_axis, y_axis, z_axis); - *this = mat.quaternion(); - normalize(); -} - -LLQuaternion::LLQuaternion(const LLSD &sd) -{ - setValue(sd); -} - -// Quatizations -void LLQuaternion::quantize16(F32 lower, F32 upper) -{ - F32 x = mQ[VX]; - F32 y = mQ[VY]; - F32 z = mQ[VZ]; - F32 s = mQ[VS]; - - x = U16_to_F32(F32_to_U16_ROUND(x, lower, upper), lower, upper); - y = U16_to_F32(F32_to_U16_ROUND(y, lower, upper), lower, upper); - z = U16_to_F32(F32_to_U16_ROUND(z, lower, upper), lower, upper); - s = U16_to_F32(F32_to_U16_ROUND(s, lower, upper), lower, upper); - - mQ[VX] = x; - mQ[VY] = y; - mQ[VZ] = z; - mQ[VS] = s; - - normalize(); -} - -void LLQuaternion::quantize8(F32 lower, F32 upper) -{ - mQ[VX] = U8_to_F32(F32_to_U8_ROUND(mQ[VX], lower, upper), lower, upper); - mQ[VY] = U8_to_F32(F32_to_U8_ROUND(mQ[VY], lower, upper), lower, 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); - - normalize(); -} - -// LLVector3 Magnitude and Normalization Functions - - -// Set LLQuaternion routines - -const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, F32 x, F32 y, F32 z) -{ - 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) -{ - 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) -{ - 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::setEulerAngles(F32 roll, F32 pitch, F32 yaw) -{ - LLMatrix3 rot_mat(roll, pitch, yaw); - rot_mat.orthogonalize(); - *this = rot_mat.quaternion(); - - normalize(); - return (*this); -} - -// deprecated -const LLQuaternion& LLQuaternion::set(const LLMatrix3 &mat) -{ - *this = mat.quaternion(); - normalize(); - return (*this); -} - -// deprecated -const LLQuaternion& LLQuaternion::set(const LLMatrix4 &mat) -{ - *this = mat.quaternion(); - normalize(); - return (*this); -} - -// deprecated -const LLQuaternion& LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z) -{ - 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) -{ - 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) -{ - 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) -{ - 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); -} - -const LLQuaternion& LLQuaternion::setQuat(const LLMatrix3 &mat) -{ - *this = mat.quaternion(); - normalize(); - return (*this); -} - -const LLQuaternion& LLQuaternion::setQuat(const LLMatrix4 &mat) -{ - *this = mat.quaternion(); - normalize(); - return (*this); -//#if 1 -// // 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). -// F64 cosX = cos(roll); -// F64 cosY = cos(pitch); -// F64 cosZ = cos(yaw); -// -// F64 sinX = sin(roll); -// F64 sinY = sin(pitch); -// F64 sinZ = sin(yaw); -// -// mQ[VW] = (F32)sqrt(cosY*cosZ - sinX*sinY*sinZ + cosX*cosZ + cosX*cosY + 1.0)*.5; -// if (fabs(mQ[VW]) < F_APPROXIMATELY_ZERO) -// { -// // null rotation, any axis will do -// mQ[VX] = 0.0f; -// mQ[VY] = 1.0f; -// mQ[VZ] = 0.0f; -// } -// else -// { -// F32 inv_s = 1.0f / (4.0f * mQ[VW]); -// mQ[VX] = (F32)-(-sinX*cosY - cosX*sinY*sinZ - sinX*cosZ) * inv_s; -// mQ[VY] = (F32)-(-cosX*sinY*cosZ + sinX*sinZ - sinY) * inv_s; -// mQ[VZ] = (F32)-(-cosY*sinZ - sinX*sinY*cosZ - cosX*sinZ) * inv_s; -// } -// -//#else // This only works on a certain subset of roll/pitch/yaw -// -// F64 cosX = cosf(roll/2.0); -// F64 cosY = cosf(pitch/2.0); -// F64 cosZ = cosf(yaw/2.0); -// -// F64 sinX = sinf(roll/2.0); -// F64 sinY = sinf(pitch/2.0); -// F64 sinZ = sinf(yaw/2.0); -// -// mQ[VW] = (F32)(cosX*cosY*cosZ + sinX*sinY*sinZ); -// mQ[VX] = (F32)(sinX*cosY*cosZ - cosX*sinY*sinZ); -// mQ[VY] = (F32)(cosX*sinY*cosZ + sinX*cosY*sinZ); -// mQ[VZ] = (F32)(cosX*cosY*sinZ - sinX*sinY*cosZ); -//#endif -// -// normalize(); -// return (*this); -} - -// SJB: This code is correct for a logicly stored (non-transposed) matrix; -// Our matrices are stored transposed, OpenGL style, so this generates the -// INVERSE matrix, or the CORRECT matrix form an INVERSE quaternion. -// Because we use similar logic in LLMatrix3::quaternion(), -// we are internally consistant so everything works OK :) -LLMatrix3 LLQuaternion::getMatrix3(void) const -{ - LLMatrix3 mat; - F32 xx, xy, xz, xw, yy, yz, yw, zz, zw; - - xx = mQ[VX] * mQ[VX]; - xy = mQ[VX] * mQ[VY]; - xz = mQ[VX] * mQ[VZ]; - xw = mQ[VX] * mQ[VW]; - - yy = mQ[VY] * mQ[VY]; - yz = mQ[VY] * mQ[VZ]; - yw = mQ[VY] * mQ[VW]; - - zz = mQ[VZ] * mQ[VZ]; - zw = mQ[VZ] * mQ[VW]; - - mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz ); - mat.mMatrix[0][1] = 2.f * ( xy + zw ); - mat.mMatrix[0][2] = 2.f * ( xz - yw ); - - mat.mMatrix[1][0] = 2.f * ( xy - zw ); - mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz ); - mat.mMatrix[1][2] = 2.f * ( yz + xw ); - - mat.mMatrix[2][0] = 2.f * ( xz + yw ); - mat.mMatrix[2][1] = 2.f * ( yz - xw ); - mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy ); - - return mat; -} - -LLMatrix4 LLQuaternion::getMatrix4(void) const -{ - LLMatrix4 mat; - F32 xx, xy, xz, xw, yy, yz, yw, zz, zw; - - xx = mQ[VX] * mQ[VX]; - xy = mQ[VX] * mQ[VY]; - xz = mQ[VX] * mQ[VZ]; - xw = mQ[VX] * mQ[VW]; - - yy = mQ[VY] * mQ[VY]; - yz = mQ[VY] * mQ[VZ]; - yw = mQ[VY] * mQ[VW]; - - zz = mQ[VZ] * mQ[VZ]; - zw = mQ[VZ] * mQ[VW]; - - mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz ); - mat.mMatrix[0][1] = 2.f * ( xy + zw ); - mat.mMatrix[0][2] = 2.f * ( xz - yw ); - - mat.mMatrix[1][0] = 2.f * ( xy - zw ); - mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz ); - mat.mMatrix[1][2] = 2.f * ( yz + xw ); - - mat.mMatrix[2][0] = 2.f * ( xz + yw ); - mat.mMatrix[2][1] = 2.f * ( yz - xw ); - mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy ); - - // TODO -- should we set the translation portion to zero? - - return mat; -} - - - - -// Other useful methods - - -// calculate the shortest rotation from a to b -void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b) -{ - 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 - { - if (cc > 0.0f) // test if the arguments are (anti)parallel - { - 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; - } - } - } - loadIdentity(); -} - -// constrains rotation to a cone angle specified in radians -const LLQuaternion &LLQuaternion::constrain(F32 radians) -{ - const F32 cos_angle_lim = cosf( radians/2 ); // mQ[VW] limit - const F32 sin_angle_lim = sinf( radians/2 ); // rotation axis length limit - - if (mQ[VW] < 0.f) - { - mQ[VX] *= -1.f; - mQ[VY] *= -1.f; - mQ[VZ] *= -1.f; - mQ[VW] *= -1.f; - } - - // if rotation angle is greater than limit (cos is less than limit) - if( mQ[VW] < cos_angle_lim ) - { - mQ[VW] = cos_angle_lim; - F32 axis_len = sqrtf( mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] ); // sin(theta/2) - F32 axis_mult_fact = sin_angle_lim / axis_len; - mQ[VX] *= axis_mult_fact; - mQ[VY] *= axis_mult_fact; - mQ[VZ] *= axis_mult_fact; - } - - return *this; -} - -// Operators - -std::ostream& operator<<(std::ostream &s, const LLQuaternion &a) -{ - s << "{ " - << a.mQ[VX] << ", " << a.mQ[VY] << ", " << a.mQ[VZ] << ", " << a.mQ[VW] - << " }"; - return s; -} - - -// Does NOT renormalize the result -LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b) -{ -// LLQuaternion::mMultCount++; - - LLQuaternion q( - b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1], - b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2], - b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0], - b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2] - ); - return q; -} - -/* -LLMatrix4 operator*(const LLMatrix4 &m, const LLQuaternion &q) -{ - LLMatrix4 qmat(q); - return (m*qmat); -} -*/ - - - -LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot) -{ - F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ]; - F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY]; - F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ]; - F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX]; - - F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY]; - F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ]; - F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX]; - - return LLVector4(nx, ny, nz, a.mV[VW]); -} - -LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot) -{ - F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ]; - F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY]; - F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ]; - F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX]; - - F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY]; - F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ]; - F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX]; - - return LLVector3(nx, ny, nz); -} - -LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot) -{ - F64 rw = - rot.mQ[VX] * a.mdV[VX] - rot.mQ[VY] * a.mdV[VY] - rot.mQ[VZ] * a.mdV[VZ]; - F64 rx = rot.mQ[VW] * a.mdV[VX] + rot.mQ[VY] * a.mdV[VZ] - rot.mQ[VZ] * a.mdV[VY]; - F64 ry = rot.mQ[VW] * a.mdV[VY] + rot.mQ[VZ] * a.mdV[VX] - rot.mQ[VX] * a.mdV[VZ]; - F64 rz = rot.mQ[VW] * a.mdV[VZ] + rot.mQ[VX] * a.mdV[VY] - rot.mQ[VY] * a.mdV[VX]; - - F64 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY]; - F64 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ]; - F64 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX]; - - return LLVector3d(nx, ny, nz); -} - -F32 dot(const LLQuaternion &a, const LLQuaternion &b) -{ - return a.mQ[VX] * b.mQ[VX] + - a.mQ[VY] * b.mQ[VY] + - a.mQ[VZ] * b.mQ[VZ] + - a.mQ[VW] * b.mQ[VW]; -} - -// DEMO HACK: This lerp is probably inocrrect now due intermediate normalization -// it should look more like the lerp below -#if 0 -// linear interpolation -LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q) -{ - LLQuaternion r; - r = t * (q - p) + p; - r.normalize(); - return r; -} -#endif - -// lerp from identity to q -LLQuaternion lerp(F32 t, const LLQuaternion &q) -{ - LLQuaternion r; - r.mQ[VX] = t * q.mQ[VX]; - r.mQ[VY] = t * q.mQ[VY]; - r.mQ[VZ] = t * q.mQ[VZ]; - r.mQ[VW] = t * (q.mQ[VZ] - 1.f) + 1.f; - r.normalize(); - return r; -} - -LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q) -{ - LLQuaternion r; - F32 inv_t; - - inv_t = 1.f - t; - - r.mQ[VX] = t * q.mQ[VX] + (inv_t * p.mQ[VX]); - r.mQ[VY] = t * q.mQ[VY] + (inv_t * p.mQ[VY]); - r.mQ[VZ] = t * q.mQ[VZ] + (inv_t * p.mQ[VZ]); - r.mQ[VW] = t * q.mQ[VW] + (inv_t * p.mQ[VW]); - r.normalize(); - return r; -} - - -// spherical linear interpolation -LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b ) -{ - // cosine theta = dot product of a and b - F32 cos_t = a.mQ[0]*b.mQ[0] + a.mQ[1]*b.mQ[1] + a.mQ[2]*b.mQ[2] + a.mQ[3]*b.mQ[3]; - - // if b is on opposite hemisphere from a, use -a instead - bool bflip; - if (cos_t < 0.0f) - { - cos_t = -cos_t; - bflip = true; - } - else - bflip = false; - - // if B is (within precision limits) the same as A, - // just linear interpolate between A and B. - F32 alpha; // interpolant - F32 beta; // 1 - interpolant - if (1.0f - cos_t < 0.00001f) - { - beta = 1.0f - u; - alpha = u; - } - else - { - F32 theta = acosf(cos_t); - F32 sin_t = sinf(theta); - beta = sinf(theta - u*theta) / sin_t; - alpha = sinf(u*theta) / sin_t; - } - - if (bflip) - beta = -beta; - - // interpolate - LLQuaternion ret; - ret.mQ[0] = beta*a.mQ[0] + alpha*b.mQ[0]; - ret.mQ[1] = beta*a.mQ[1] + alpha*b.mQ[1]; - ret.mQ[2] = beta*a.mQ[2] + alpha*b.mQ[2]; - ret.mQ[3] = beta*a.mQ[3] + alpha*b.mQ[3]; - - return ret; -} - -// lerp whenever possible -LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b) -{ - if (dot(a, b) < 0.f) - { - return slerp(t, a, b); - } - else - { - return lerp(t, a, b); - } -} - -LLQuaternion nlerp(F32 t, const LLQuaternion &q) -{ - if (q.mQ[VW] < 0.f) - { - return slerp(t, q); - } - else - { - return lerp(t, q); - } -} - -// slerp from identity quaternion to another quaternion -LLQuaternion slerp(F32 t, const LLQuaternion &q) -{ - F32 c = q.mQ[VW]; - if (1.0f == t || 1.0f == c) - { - // the trivial cases - return q; - } - - LLQuaternion r; - F32 s, angle, stq, stp; - - s = (F32) sqrt(1.f - c*c); - - if (c < 0.0f) - { - // when c < 0.0 then theta > PI/2 - // since quat and -quat are the same rotation we invert one of - // p or q to reduce unecessary spins - // A equivalent way to do it is to convert acos(c) as if it had - // been negative, and to negate stp - angle = (F32) acos(-c); - stp = -(F32) sin(angle * (1.f - t)); - stq = (F32) sin(angle * t); - } - else - { - angle = (F32) acos(c); - stp = (F32) sin(angle * (1.f - t)); - stq = (F32) sin(angle * t); - } - - r.mQ[VX] = (q.mQ[VX] * stq) / s; - r.mQ[VY] = (q.mQ[VY] * stq) / s; - r.mQ[VZ] = (q.mQ[VZ] * stq) / s; - r.mQ[VW] = (stp + q.mQ[VW] * stq) / s; - - return r; -} - -LLQuaternion mayaQ(F32 xRot, F32 yRot, F32 zRot, LLQuaternion::Order order) -{ - LLQuaternion xQ( xRot*DEG_TO_RAD, LLVector3(1.0f, 0.0f, 0.0f) ); - LLQuaternion yQ( yRot*DEG_TO_RAD, LLVector3(0.0f, 1.0f, 0.0f) ); - LLQuaternion zQ( zRot*DEG_TO_RAD, LLVector3(0.0f, 0.0f, 1.0f) ); - LLQuaternion ret; - switch( order ) - { - case LLQuaternion::XYZ: - ret = xQ * yQ * zQ; - break; - case LLQuaternion::YZX: - ret = yQ * zQ * xQ; - break; - case LLQuaternion::ZXY: - ret = zQ * xQ * yQ; - break; - case LLQuaternion::XZY: - ret = xQ * zQ * yQ; - break; - case LLQuaternion::YXZ: - ret = yQ * xQ * zQ; - break; - case LLQuaternion::ZYX: - ret = zQ * yQ * xQ; - break; - } - return ret; -} - -const char *OrderToString( const LLQuaternion::Order order ) -{ - const char *p = NULL; - switch( order ) - { - default: - case LLQuaternion::XYZ: - p = "XYZ"; - break; - case LLQuaternion::YZX: - p = "YZX"; - break; - case LLQuaternion::ZXY: - p = "ZXY"; - break; - case LLQuaternion::XZY: - p = "XZY"; - break; - case LLQuaternion::YXZ: - p = "YXZ"; - break; - case LLQuaternion::ZYX: - p = "ZYX"; - break; - } - return p; -} - -LLQuaternion::Order StringToOrder( const char *str ) -{ - if (strncmp(str, "XYZ", 3)==0 || strncmp(str, "xyz", 3)==0) - return LLQuaternion::XYZ; - - if (strncmp(str, "YZX", 3)==0 || strncmp(str, "yzx", 3)==0) - return LLQuaternion::YZX; - - if (strncmp(str, "ZXY", 3)==0 || strncmp(str, "zxy", 3)==0) - return LLQuaternion::ZXY; - - if (strncmp(str, "XZY", 3)==0 || strncmp(str, "xzy", 3)==0) - return LLQuaternion::XZY; - - if (strncmp(str, "YXZ", 3)==0 || strncmp(str, "yxz", 3)==0) - return LLQuaternion::YXZ; - - if (strncmp(str, "ZYX", 3)==0 || strncmp(str, "zyx", 3)==0) - return LLQuaternion::ZYX; - - return LLQuaternion::XYZ; -} - -void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const -{ - 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 = 0.0f; // no rotation - vec.mV[VX] = 0.0f; // around some dummy axis - vec.mV[VY] = 0.0f; - vec.mV[VZ] = 1.0f; - } -} - -const LLQuaternion& LLQuaternion::setFromAzimuthAndAltitude(F32 azimuthRadians, F32 altitudeRadians) -{ - // euler angle inputs are complements of azimuth/altitude which are measured from zenith - F32 pitch = llclamp(F_PI_BY_TWO - altitudeRadians, 0.0f, F_PI_BY_TWO); - F32 yaw = llclamp(F_PI_BY_TWO - azimuthRadians, 0.0f, F_PI_BY_TWO); - setEulerAngles(0.0f, pitch, yaw); - return *this; -} - -void LLQuaternion::getAzimuthAndAltitude(F32 &azimuthRadians, F32 &altitudeRadians) -{ - F32 rick_roll; - F32 pitch; - F32 yaw; - getEulerAngles(&rick_roll, &pitch, &yaw); - // make these measured from zenith - altitudeRadians = llclamp(F_PI_BY_TWO - pitch, 0.0f, F_PI_BY_TWO); - azimuthRadians = llclamp(F_PI_BY_TWO - yaw, 0.0f, F_PI_BY_TWO); -} - -// quaternion does not need to be normalized -void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const -{ - 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( x, y , z ); - } - else - { - return LLVector3( -x, -y, -z ); - } -} - -// Saves space by using the fact that our quaternions are normalized -void LLQuaternion::unpackFromVector3( const LLVector3& vec ) -{ - mQ[VX] = vec.mV[VX]; - mQ[VY] = vec.mV[VY]; - mQ[VZ] = vec.mV[VZ]; - F32 t = 1.f - vec.magVecSquared(); - if( t > 0 ) - { - mQ[VW] = sqrt( t ); - } - else - { - // Need this to avoid trying to find the square root of a negative number due - // to floating point error. - mQ[VW] = 0; - } -} - -bool LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value) -{ - if( buf.empty() || value == NULL) - { - return false; - } - - LLQuaternion quat; - S32 count = sscanf( buf.c_str(), "%f %f %f %f", quat.mQ + 0, quat.mQ + 1, quat.mQ + 2, quat.mQ + 3 ); - if( 4 == count ) - { - value->set( quat ); - return true; - } - - return false; -} - - -// End +/** + * @file llquaternion.cpp + * @brief LLQuaternion class implementation. + * + * $LicenseInfo:firstyear=2000&license=viewerlgpl$ + * Second Life Viewer Source Code + * 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. + * + * 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. + * + * 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 + * + * Linden Research, Inc., 945 Battery Street, San Francisco, CA 94111 USA + * $/LicenseInfo$ + */ + +#include "linden_common.h" + +#include "llmath.h" // for F_PI + +#include "llquaternion.h" + +//#include "vmath.h" +#include "v3math.h" +#include "v3dmath.h" +#include "v4math.h" +#include "m4math.h" +#include "m3math.h" +#include "llquantize.h" + +// WARNING: Don't use this for global const definitions! using this +// at the top of a *.cpp file might not give you what you think. +const LLQuaternion LLQuaternion::DEFAULT; + +// Constructors + +LLQuaternion::LLQuaternion(const LLMatrix4 &mat) +{ + *this = mat.quaternion(); + normalize(); +} + +LLQuaternion::LLQuaternion(const LLMatrix3 &mat) +{ + *this = mat.quaternion(); + normalize(); +} + +LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec) +{ + 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) +{ + 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, + const LLVector3 &y_axis, + const LLVector3 &z_axis) +{ + LLMatrix3 mat; + mat.setRows(x_axis, y_axis, z_axis); + *this = mat.quaternion(); + normalize(); +} + +LLQuaternion::LLQuaternion(const LLSD &sd) +{ + setValue(sd); +} + +// Quatizations +void LLQuaternion::quantize16(F32 lower, F32 upper) +{ + F32 x = mQ[VX]; + F32 y = mQ[VY]; + F32 z = mQ[VZ]; + F32 s = mQ[VS]; + + x = U16_to_F32(F32_to_U16_ROUND(x, lower, upper), lower, upper); + y = U16_to_F32(F32_to_U16_ROUND(y, lower, upper), lower, upper); + z = U16_to_F32(F32_to_U16_ROUND(z, lower, upper), lower, upper); + s = U16_to_F32(F32_to_U16_ROUND(s, lower, upper), lower, upper); + + mQ[VX] = x; + mQ[VY] = y; + mQ[VZ] = z; + mQ[VS] = s; + + normalize(); +} + +void LLQuaternion::quantize8(F32 lower, F32 upper) +{ + mQ[VX] = U8_to_F32(F32_to_U8_ROUND(mQ[VX], lower, upper), lower, upper); + mQ[VY] = U8_to_F32(F32_to_U8_ROUND(mQ[VY], lower, upper), lower, 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); + + normalize(); +} + +// LLVector3 Magnitude and Normalization Functions + + +// Set LLQuaternion routines + +const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, F32 x, F32 y, F32 z) +{ + 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) +{ + 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) +{ + 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::setEulerAngles(F32 roll, F32 pitch, F32 yaw) +{ + LLMatrix3 rot_mat(roll, pitch, yaw); + rot_mat.orthogonalize(); + *this = rot_mat.quaternion(); + + normalize(); + return (*this); +} + +// deprecated +const LLQuaternion& LLQuaternion::set(const LLMatrix3 &mat) +{ + *this = mat.quaternion(); + normalize(); + return (*this); +} + +// deprecated +const LLQuaternion& LLQuaternion::set(const LLMatrix4 &mat) +{ + *this = mat.quaternion(); + normalize(); + return (*this); +} + +// deprecated +const LLQuaternion& LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z) +{ + 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) +{ + 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) +{ + 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) +{ + 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); +} + +const LLQuaternion& LLQuaternion::setQuat(const LLMatrix3 &mat) +{ + *this = mat.quaternion(); + normalize(); + return (*this); +} + +const LLQuaternion& LLQuaternion::setQuat(const LLMatrix4 &mat) +{ + *this = mat.quaternion(); + normalize(); + return (*this); +//#if 1 +// // 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). +// F64 cosX = cos(roll); +// F64 cosY = cos(pitch); +// F64 cosZ = cos(yaw); +// +// F64 sinX = sin(roll); +// F64 sinY = sin(pitch); +// F64 sinZ = sin(yaw); +// +// mQ[VW] = (F32)sqrt(cosY*cosZ - sinX*sinY*sinZ + cosX*cosZ + cosX*cosY + 1.0)*.5; +// if (fabs(mQ[VW]) < F_APPROXIMATELY_ZERO) +// { +// // null rotation, any axis will do +// mQ[VX] = 0.0f; +// mQ[VY] = 1.0f; +// mQ[VZ] = 0.0f; +// } +// else +// { +// F32 inv_s = 1.0f / (4.0f * mQ[VW]); +// mQ[VX] = (F32)-(-sinX*cosY - cosX*sinY*sinZ - sinX*cosZ) * inv_s; +// mQ[VY] = (F32)-(-cosX*sinY*cosZ + sinX*sinZ - sinY) * inv_s; +// mQ[VZ] = (F32)-(-cosY*sinZ - sinX*sinY*cosZ - cosX*sinZ) * inv_s; +// } +// +//#else // This only works on a certain subset of roll/pitch/yaw +// +// F64 cosX = cosf(roll/2.0); +// F64 cosY = cosf(pitch/2.0); +// F64 cosZ = cosf(yaw/2.0); +// +// F64 sinX = sinf(roll/2.0); +// F64 sinY = sinf(pitch/2.0); +// F64 sinZ = sinf(yaw/2.0); +// +// mQ[VW] = (F32)(cosX*cosY*cosZ + sinX*sinY*sinZ); +// mQ[VX] = (F32)(sinX*cosY*cosZ - cosX*sinY*sinZ); +// mQ[VY] = (F32)(cosX*sinY*cosZ + sinX*cosY*sinZ); +// mQ[VZ] = (F32)(cosX*cosY*sinZ - sinX*sinY*cosZ); +//#endif +// +// normalize(); +// return (*this); +} + +// SJB: This code is correct for a logicly stored (non-transposed) matrix; +// Our matrices are stored transposed, OpenGL style, so this generates the +// INVERSE matrix, or the CORRECT matrix form an INVERSE quaternion. +// Because we use similar logic in LLMatrix3::quaternion(), +// we are internally consistant so everything works OK :) +LLMatrix3 LLQuaternion::getMatrix3(void) const +{ + LLMatrix3 mat; + F32 xx, xy, xz, xw, yy, yz, yw, zz, zw; + + xx = mQ[VX] * mQ[VX]; + xy = mQ[VX] * mQ[VY]; + xz = mQ[VX] * mQ[VZ]; + xw = mQ[VX] * mQ[VW]; + + yy = mQ[VY] * mQ[VY]; + yz = mQ[VY] * mQ[VZ]; + yw = mQ[VY] * mQ[VW]; + + zz = mQ[VZ] * mQ[VZ]; + zw = mQ[VZ] * mQ[VW]; + + mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz ); + mat.mMatrix[0][1] = 2.f * ( xy + zw ); + mat.mMatrix[0][2] = 2.f * ( xz - yw ); + + mat.mMatrix[1][0] = 2.f * ( xy - zw ); + mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz ); + mat.mMatrix[1][2] = 2.f * ( yz + xw ); + + mat.mMatrix[2][0] = 2.f * ( xz + yw ); + mat.mMatrix[2][1] = 2.f * ( yz - xw ); + mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy ); + + return mat; +} + +LLMatrix4 LLQuaternion::getMatrix4(void) const +{ + LLMatrix4 mat; + F32 xx, xy, xz, xw, yy, yz, yw, zz, zw; + + xx = mQ[VX] * mQ[VX]; + xy = mQ[VX] * mQ[VY]; + xz = mQ[VX] * mQ[VZ]; + xw = mQ[VX] * mQ[VW]; + + yy = mQ[VY] * mQ[VY]; + yz = mQ[VY] * mQ[VZ]; + yw = mQ[VY] * mQ[VW]; + + zz = mQ[VZ] * mQ[VZ]; + zw = mQ[VZ] * mQ[VW]; + + mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz ); + mat.mMatrix[0][1] = 2.f * ( xy + zw ); + mat.mMatrix[0][2] = 2.f * ( xz - yw ); + + mat.mMatrix[1][0] = 2.f * ( xy - zw ); + mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz ); + mat.mMatrix[1][2] = 2.f * ( yz + xw ); + + mat.mMatrix[2][0] = 2.f * ( xz + yw ); + mat.mMatrix[2][1] = 2.f * ( yz - xw ); + mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy ); + + // TODO -- should we set the translation portion to zero? + + return mat; +} + + + + +// Other useful methods + + +// calculate the shortest rotation from a to b +void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b) +{ + 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 + { + if (cc > 0.0f) // test if the arguments are (anti)parallel + { + 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; + } + } + } + loadIdentity(); +} + +// constrains rotation to a cone angle specified in radians +const LLQuaternion &LLQuaternion::constrain(F32 radians) +{ + const F32 cos_angle_lim = cosf( radians/2 ); // mQ[VW] limit + const F32 sin_angle_lim = sinf( radians/2 ); // rotation axis length limit + + if (mQ[VW] < 0.f) + { + mQ[VX] *= -1.f; + mQ[VY] *= -1.f; + mQ[VZ] *= -1.f; + mQ[VW] *= -1.f; + } + + // if rotation angle is greater than limit (cos is less than limit) + if( mQ[VW] < cos_angle_lim ) + { + mQ[VW] = cos_angle_lim; + F32 axis_len = sqrtf( mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] ); // sin(theta/2) + F32 axis_mult_fact = sin_angle_lim / axis_len; + mQ[VX] *= axis_mult_fact; + mQ[VY] *= axis_mult_fact; + mQ[VZ] *= axis_mult_fact; + } + + return *this; +} + +// Operators + +std::ostream& operator<<(std::ostream &s, const LLQuaternion &a) +{ + s << "{ " + << a.mQ[VX] << ", " << a.mQ[VY] << ", " << a.mQ[VZ] << ", " << a.mQ[VW] + << " }"; + return s; +} + + +// Does NOT renormalize the result +LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b) +{ +// LLQuaternion::mMultCount++; + + LLQuaternion q( + b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1], + b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2], + b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0], + b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2] + ); + return q; +} + +/* +LLMatrix4 operator*(const LLMatrix4 &m, const LLQuaternion &q) +{ + LLMatrix4 qmat(q); + return (m*qmat); +} +*/ + + + +LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot) +{ + F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ]; + F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY]; + F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ]; + F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX]; + + F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY]; + F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ]; + F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX]; + + return LLVector4(nx, ny, nz, a.mV[VW]); +} + +LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot) +{ + F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ]; + F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY]; + F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ]; + F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX]; + + F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY]; + F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ]; + F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX]; + + return LLVector3(nx, ny, nz); +} + +LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot) +{ + F64 rw = - rot.mQ[VX] * a.mdV[VX] - rot.mQ[VY] * a.mdV[VY] - rot.mQ[VZ] * a.mdV[VZ]; + F64 rx = rot.mQ[VW] * a.mdV[VX] + rot.mQ[VY] * a.mdV[VZ] - rot.mQ[VZ] * a.mdV[VY]; + F64 ry = rot.mQ[VW] * a.mdV[VY] + rot.mQ[VZ] * a.mdV[VX] - rot.mQ[VX] * a.mdV[VZ]; + F64 rz = rot.mQ[VW] * a.mdV[VZ] + rot.mQ[VX] * a.mdV[VY] - rot.mQ[VY] * a.mdV[VX]; + + F64 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY]; + F64 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ]; + F64 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX]; + + return LLVector3d(nx, ny, nz); +} + +F32 dot(const LLQuaternion &a, const LLQuaternion &b) +{ + return a.mQ[VX] * b.mQ[VX] + + a.mQ[VY] * b.mQ[VY] + + a.mQ[VZ] * b.mQ[VZ] + + a.mQ[VW] * b.mQ[VW]; +} + +// DEMO HACK: This lerp is probably inocrrect now due intermediate normalization +// it should look more like the lerp below +#if 0 +// linear interpolation +LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q) +{ + LLQuaternion r; + r = t * (q - p) + p; + r.normalize(); + return r; +} +#endif + +// lerp from identity to q +LLQuaternion lerp(F32 t, const LLQuaternion &q) +{ + LLQuaternion r; + r.mQ[VX] = t * q.mQ[VX]; + r.mQ[VY] = t * q.mQ[VY]; + r.mQ[VZ] = t * q.mQ[VZ]; + r.mQ[VW] = t * (q.mQ[VZ] - 1.f) + 1.f; + r.normalize(); + return r; +} + +LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q) +{ + LLQuaternion r; + F32 inv_t; + + inv_t = 1.f - t; + + r.mQ[VX] = t * q.mQ[VX] + (inv_t * p.mQ[VX]); + r.mQ[VY] = t * q.mQ[VY] + (inv_t * p.mQ[VY]); + r.mQ[VZ] = t * q.mQ[VZ] + (inv_t * p.mQ[VZ]); + r.mQ[VW] = t * q.mQ[VW] + (inv_t * p.mQ[VW]); + r.normalize(); + return r; +} + + +// spherical linear interpolation +LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b ) +{ + // cosine theta = dot product of a and b + F32 cos_t = a.mQ[0]*b.mQ[0] + a.mQ[1]*b.mQ[1] + a.mQ[2]*b.mQ[2] + a.mQ[3]*b.mQ[3]; + + // if b is on opposite hemisphere from a, use -a instead + bool bflip; + if (cos_t < 0.0f) + { + cos_t = -cos_t; + bflip = true; + } + else + bflip = false; + + // if B is (within precision limits) the same as A, + // just linear interpolate between A and B. + F32 alpha; // interpolant + F32 beta; // 1 - interpolant + if (1.0f - cos_t < 0.00001f) + { + beta = 1.0f - u; + alpha = u; + } + else + { + F32 theta = acosf(cos_t); + F32 sin_t = sinf(theta); + beta = sinf(theta - u*theta) / sin_t; + alpha = sinf(u*theta) / sin_t; + } + + if (bflip) + beta = -beta; + + // interpolate + LLQuaternion ret; + ret.mQ[0] = beta*a.mQ[0] + alpha*b.mQ[0]; + ret.mQ[1] = beta*a.mQ[1] + alpha*b.mQ[1]; + ret.mQ[2] = beta*a.mQ[2] + alpha*b.mQ[2]; + ret.mQ[3] = beta*a.mQ[3] + alpha*b.mQ[3]; + + return ret; +} + +// lerp whenever possible +LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b) +{ + if (dot(a, b) < 0.f) + { + return slerp(t, a, b); + } + else + { + return lerp(t, a, b); + } +} + +LLQuaternion nlerp(F32 t, const LLQuaternion &q) +{ + if (q.mQ[VW] < 0.f) + { + return slerp(t, q); + } + else + { + return lerp(t, q); + } +} + +// slerp from identity quaternion to another quaternion +LLQuaternion slerp(F32 t, const LLQuaternion &q) +{ + F32 c = q.mQ[VW]; + if (1.0f == t || 1.0f == c) + { + // the trivial cases + return q; + } + + LLQuaternion r; + F32 s, angle, stq, stp; + + s = (F32) sqrt(1.f - c*c); + + if (c < 0.0f) + { + // when c < 0.0 then theta > PI/2 + // since quat and -quat are the same rotation we invert one of + // p or q to reduce unecessary spins + // A equivalent way to do it is to convert acos(c) as if it had + // been negative, and to negate stp + angle = (F32) acos(-c); + stp = -(F32) sin(angle * (1.f - t)); + stq = (F32) sin(angle * t); + } + else + { + angle = (F32) acos(c); + stp = (F32) sin(angle * (1.f - t)); + stq = (F32) sin(angle * t); + } + + r.mQ[VX] = (q.mQ[VX] * stq) / s; + r.mQ[VY] = (q.mQ[VY] * stq) / s; + r.mQ[VZ] = (q.mQ[VZ] * stq) / s; + r.mQ[VW] = (stp + q.mQ[VW] * stq) / s; + + return r; +} + +LLQuaternion mayaQ(F32 xRot, F32 yRot, F32 zRot, LLQuaternion::Order order) +{ + LLQuaternion xQ( xRot*DEG_TO_RAD, LLVector3(1.0f, 0.0f, 0.0f) ); + LLQuaternion yQ( yRot*DEG_TO_RAD, LLVector3(0.0f, 1.0f, 0.0f) ); + LLQuaternion zQ( zRot*DEG_TO_RAD, LLVector3(0.0f, 0.0f, 1.0f) ); + LLQuaternion ret; + switch( order ) + { + case LLQuaternion::XYZ: + ret = xQ * yQ * zQ; + break; + case LLQuaternion::YZX: + ret = yQ * zQ * xQ; + break; + case LLQuaternion::ZXY: + ret = zQ * xQ * yQ; + break; + case LLQuaternion::XZY: + ret = xQ * zQ * yQ; + break; + case LLQuaternion::YXZ: + ret = yQ * xQ * zQ; + break; + case LLQuaternion::ZYX: + ret = zQ * yQ * xQ; + break; + } + return ret; +} + +const char *OrderToString( const LLQuaternion::Order order ) +{ + const char *p = NULL; + switch( order ) + { + default: + case LLQuaternion::XYZ: + p = "XYZ"; + break; + case LLQuaternion::YZX: + p = "YZX"; + break; + case LLQuaternion::ZXY: + p = "ZXY"; + break; + case LLQuaternion::XZY: + p = "XZY"; + break; + case LLQuaternion::YXZ: + p = "YXZ"; + break; + case LLQuaternion::ZYX: + p = "ZYX"; + break; + } + return p; +} + +LLQuaternion::Order StringToOrder( const char *str ) +{ + if (strncmp(str, "XYZ", 3)==0 || strncmp(str, "xyz", 3)==0) + return LLQuaternion::XYZ; + + if (strncmp(str, "YZX", 3)==0 || strncmp(str, "yzx", 3)==0) + return LLQuaternion::YZX; + + if (strncmp(str, "ZXY", 3)==0 || strncmp(str, "zxy", 3)==0) + return LLQuaternion::ZXY; + + if (strncmp(str, "XZY", 3)==0 || strncmp(str, "xzy", 3)==0) + return LLQuaternion::XZY; + + if (strncmp(str, "YXZ", 3)==0 || strncmp(str, "yxz", 3)==0) + return LLQuaternion::YXZ; + + if (strncmp(str, "ZYX", 3)==0 || strncmp(str, "zyx", 3)==0) + return LLQuaternion::ZYX; + + return LLQuaternion::XYZ; +} + +void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const +{ + 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 = 0.0f; // no rotation + vec.mV[VX] = 0.0f; // around some dummy axis + vec.mV[VY] = 0.0f; + vec.mV[VZ] = 1.0f; + } +} + +const LLQuaternion& LLQuaternion::setFromAzimuthAndAltitude(F32 azimuthRadians, F32 altitudeRadians) +{ + // euler angle inputs are complements of azimuth/altitude which are measured from zenith + F32 pitch = llclamp(F_PI_BY_TWO - altitudeRadians, 0.0f, F_PI_BY_TWO); + F32 yaw = llclamp(F_PI_BY_TWO - azimuthRadians, 0.0f, F_PI_BY_TWO); + setEulerAngles(0.0f, pitch, yaw); + return *this; +} + +void LLQuaternion::getAzimuthAndAltitude(F32 &azimuthRadians, F32 &altitudeRadians) +{ + F32 rick_roll; + F32 pitch; + F32 yaw; + getEulerAngles(&rick_roll, &pitch, &yaw); + // make these measured from zenith + altitudeRadians = llclamp(F_PI_BY_TWO - pitch, 0.0f, F_PI_BY_TWO); + azimuthRadians = llclamp(F_PI_BY_TWO - yaw, 0.0f, F_PI_BY_TWO); +} + +// quaternion does not need to be normalized +void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const +{ + 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( x, y , z ); + } + else + { + return LLVector3( -x, -y, -z ); + } +} + +// Saves space by using the fact that our quaternions are normalized +void LLQuaternion::unpackFromVector3( const LLVector3& vec ) +{ + mQ[VX] = vec.mV[VX]; + mQ[VY] = vec.mV[VY]; + mQ[VZ] = vec.mV[VZ]; + F32 t = 1.f - vec.magVecSquared(); + if( t > 0 ) + { + mQ[VW] = sqrt( t ); + } + else + { + // Need this to avoid trying to find the square root of a negative number due + // to floating point error. + mQ[VW] = 0; + } +} + +bool LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value) +{ + if( buf.empty() || value == NULL) + { + return false; + } + + LLQuaternion quat; + S32 count = sscanf( buf.c_str(), "%f %f %f %f", quat.mQ + 0, quat.mQ + 1, quat.mQ + 2, quat.mQ + 3 ); + if( 4 == count ) + { + value->set( quat ); + return true; + } + + return false; +} + + +// End -- cgit v1.2.3