summaryrefslogtreecommitdiff
path: root/indra/llmath/llquaternion.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'indra/llmath/llquaternion.cpp')
-rw-r--r--indra/llmath/llquaternion.cpp1962
1 files changed, 981 insertions, 981 deletions
diff --git a/indra/llmath/llquaternion.cpp b/indra/llmath/llquaternion.cpp
index dd3d552832..90d908c07c 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
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-10 03:26:06 +0000