summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--indra/cmake/00-Common.cmake2
-rw-r--r--indra/llmath/CMakeLists.txt256
-rw-r--r--indra/llmath/llmath.h1018
-rw-r--r--indra/llmath/llquantize.h316
-rw-r--r--indra/llmath/llquaternion.cpp1922
-rw-r--r--indra/llmath/llquaternion.h1188
6 files changed, 2351 insertions, 2351 deletions
diff --git a/indra/cmake/00-Common.cmake b/indra/cmake/00-Common.cmake
index f10a61e1e7..8262462ced 100644
--- a/indra/cmake/00-Common.cmake
+++ b/indra/cmake/00-Common.cmake
@@ -68,7 +68,7 @@ if (WINDOWS)
add_definitions(
/Zc:wchar_t-
- /arch:SSE2
+ /arch:SSE2
)
endif (MSVC80 OR MSVC90)
diff --git a/indra/llmath/CMakeLists.txt b/indra/llmath/CMakeLists.txt
index 8d85765eb8..9dadad7dd3 100644
--- a/indra/llmath/CMakeLists.txt
+++ b/indra/llmath/CMakeLists.txt
@@ -1,128 +1,128 @@
-# -*- cmake -*-
-
-project(llmath)
-
-include(00-Common)
-include(LLCommon)
-
-include_directories(
- ${LLCOMMON_INCLUDE_DIRS}
- )
-
-set(llmath_SOURCE_FILES
- llbbox.cpp
- llbboxlocal.cpp
- llcamera.cpp
- llcoordframe.cpp
- llline.cpp
- llmatrix3a.cpp
- llmodularmath.cpp
- llperlin.cpp
- llquaternion.cpp
- llrect.cpp
- llsphere.cpp
- llvector4a.cpp
- llvolume.cpp
- llvolumemgr.cpp
- llvolumeoctree.cpp
- llsdutil_math.cpp
- m3math.cpp
- m4math.cpp
- raytrace.cpp
- v2math.cpp
- v3color.cpp
- v3dmath.cpp
- v3math.cpp
- v4color.cpp
- v4coloru.cpp
- v4math.cpp
- xform.cpp
- )
-
-set(llmath_HEADER_FILES
- CMakeLists.txt
-
- camera.h
- coordframe.h
- llbbox.h
- llbboxlocal.h
- llcamera.h
- llcoord.h
- llcoordframe.h
- llinterp.h
- llline.h
- llmath.h
- llmatrix3a.h
- llmatrix3a.inl
- llmodularmath.h
- lloctree.h
- llperlin.h
- llplane.h
- llquantize.h
- llquaternion.h
- llquaternion2.h
- llquaternion2.inl
- llrect.h
- llsimdmath.h
- llsimdtypes.h
- llsimdtypes.inl
- llsphere.h
- lltreenode.h
- llvector4a.h
- llvector4a.inl
- llvector4logical.h
- llv4math.h
- llv4matrix3.h
- llv4matrix4.h
- llv4vector3.h
- llvolume.h
- llvolumemgr.h
- llvolumeoctree.h
- llsdutil_math.h
- m3math.h
- m4math.h
- raytrace.h
- v2math.h
- v3color.h
- v3dmath.h
- v3math.h
- v4color.h
- v4coloru.h
- v4math.h
- xform.h
- )
-
-set_source_files_properties(${llmath_HEADER_FILES}
- PROPERTIES HEADER_FILE_ONLY TRUE)
-
-list(APPEND llmath_SOURCE_FILES ${llmath_HEADER_FILES})
-
-add_library (llmath ${llmath_SOURCE_FILES})
-
-# Add tests
-if (LL_TESTS)
- include(LLAddBuildTest)
- # UNIT TESTS
- SET(llmath_TEST_SOURCE_FILES
- llbboxlocal.cpp
- llmodularmath.cpp
- llrect.cpp
- v2math.cpp
- v3color.cpp
- v4color.cpp
- v4coloru.cpp
- )
- LL_ADD_PROJECT_UNIT_TESTS(llmath "${llmath_TEST_SOURCE_FILES}")
-
- # INTEGRATION TESTS
- set(test_libs llmath llcommon ${LLCOMMON_LIBRARIES} ${WINDOWS_LIBRARIES})
- # TODO: Some of these need refactoring to be proper Unit tests rather than Integration tests.
- LL_ADD_INTEGRATION_TEST(llbbox llbbox.cpp "${test_libs}")
- LL_ADD_INTEGRATION_TEST(llquaternion llquaternion.cpp "${test_libs}")
- LL_ADD_INTEGRATION_TEST(mathmisc "" "${test_libs}")
- LL_ADD_INTEGRATION_TEST(m3math "" "${test_libs}")
- LL_ADD_INTEGRATION_TEST(v3dmath v3dmath.cpp "${test_libs}")
- LL_ADD_INTEGRATION_TEST(v3math v3math.cpp "${test_libs}")
- LL_ADD_INTEGRATION_TEST(v4math v4math.cpp "${test_libs}")
- LL_ADD_INTEGRATION_TEST(xform xform.cpp "${test_libs}")
-endif (LL_TESTS)
+# -*- cmake -*-
+
+project(llmath)
+
+include(00-Common)
+include(LLCommon)
+
+include_directories(
+ ${LLCOMMON_INCLUDE_DIRS}
+ )
+
+set(llmath_SOURCE_FILES
+ llbbox.cpp
+ llbboxlocal.cpp
+ llcamera.cpp
+ llcoordframe.cpp
+ llline.cpp
+ llmatrix3a.cpp
+ llmodularmath.cpp
+ llperlin.cpp
+ llquaternion.cpp
+ llrect.cpp
+ llsphere.cpp
+ llvector4a.cpp
+ llvolume.cpp
+ llvolumemgr.cpp
+ llvolumeoctree.cpp
+ llsdutil_math.cpp
+ m3math.cpp
+ m4math.cpp
+ raytrace.cpp
+ v2math.cpp
+ v3color.cpp
+ v3dmath.cpp
+ v3math.cpp
+ v4color.cpp
+ v4coloru.cpp
+ v4math.cpp
+ xform.cpp
+ )
+
+set(llmath_HEADER_FILES
+ CMakeLists.txt
+
+ camera.h
+ coordframe.h
+ llbbox.h
+ llbboxlocal.h
+ llcamera.h
+ llcoord.h
+ llcoordframe.h
+ llinterp.h
+ llline.h
+ llmath.h
+ llmatrix3a.h
+ llmatrix3a.inl
+ llmodularmath.h
+ lloctree.h
+ llperlin.h
+ llplane.h
+ llquantize.h
+ llquaternion.h
+ llquaternion2.h
+ llquaternion2.inl
+ llrect.h
+ llsimdmath.h
+ llsimdtypes.h
+ llsimdtypes.inl
+ llsphere.h
+ lltreenode.h
+ llvector4a.h
+ llvector4a.inl
+ llvector4logical.h
+ llv4math.h
+ llv4matrix3.h
+ llv4matrix4.h
+ llv4vector3.h
+ llvolume.h
+ llvolumemgr.h
+ llvolumeoctree.h
+ llsdutil_math.h
+ m3math.h
+ m4math.h
+ raytrace.h
+ v2math.h
+ v3color.h
+ v3dmath.h
+ v3math.h
+ v4color.h
+ v4coloru.h
+ v4math.h
+ xform.h
+ )
+
+set_source_files_properties(${llmath_HEADER_FILES}
+ PROPERTIES HEADER_FILE_ONLY TRUE)
+
+list(APPEND llmath_SOURCE_FILES ${llmath_HEADER_FILES})
+
+add_library (llmath ${llmath_SOURCE_FILES})
+
+# Add tests
+if (LL_TESTS)
+ include(LLAddBuildTest)
+ # UNIT TESTS
+ SET(llmath_TEST_SOURCE_FILES
+ llbboxlocal.cpp
+ llmodularmath.cpp
+ llrect.cpp
+ v2math.cpp
+ v3color.cpp
+ v4color.cpp
+ v4coloru.cpp
+ )
+ LL_ADD_PROJECT_UNIT_TESTS(llmath "${llmath_TEST_SOURCE_FILES}")
+
+ # INTEGRATION TESTS
+ set(test_libs llmath llcommon ${LLCOMMON_LIBRARIES} ${WINDOWS_LIBRARIES})
+ # TODO: Some of these need refactoring to be proper Unit tests rather than Integration tests.
+ LL_ADD_INTEGRATION_TEST(llbbox llbbox.cpp "${test_libs}")
+ LL_ADD_INTEGRATION_TEST(llquaternion llquaternion.cpp "${test_libs}")
+ LL_ADD_INTEGRATION_TEST(mathmisc "" "${test_libs}")
+ LL_ADD_INTEGRATION_TEST(m3math "" "${test_libs}")
+ LL_ADD_INTEGRATION_TEST(v3dmath v3dmath.cpp "${test_libs}")
+ LL_ADD_INTEGRATION_TEST(v3math v3math.cpp "${test_libs}")
+ LL_ADD_INTEGRATION_TEST(v4math v4math.cpp "${test_libs}")
+ LL_ADD_INTEGRATION_TEST(xform xform.cpp "${test_libs}")
+endif (LL_TESTS)
diff --git a/indra/llmath/llmath.h b/indra/llmath/llmath.h
index 742bbc4751..e572381b1a 100644
--- a/indra/llmath/llmath.h
+++ b/indra/llmath/llmath.h
@@ -1,509 +1,509 @@
-/**
- * @file llmath.h
- * @brief Useful math constants and macros.
- *
- * $LicenseInfo:firstyear=2000&license=viewergpl$
- *
- * Copyright (c) 2000-2009, Linden Research, Inc.
- *
- * Second Life Viewer Source Code
- * The source code in this file ("Source Code") is provided by Linden Lab
- * to you under the terms of the GNU General Public License, version 2.0
- * ("GPL"), unless you have obtained a separate licensing agreement
- * ("Other License"), formally executed by you and Linden Lab. Terms of
- * the GPL can be found in doc/GPL-license.txt in this distribution, or
- * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
- *
- * There are special exceptions to the terms and conditions of the GPL as
- * it is applied to this Source Code. View the full text of the exception
- * in the file doc/FLOSS-exception.txt in this software distribution, or
- * online at
- * http://secondlifegrid.net/programs/open_source/licensing/flossexception
- *
- * By copying, modifying or distributing this software, you acknowledge
- * that you have read and understood your obligations described above,
- * and agree to abide by those obligations.
- *
- * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
- * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
- * COMPLETENESS OR PERFORMANCE.
- * $/LicenseInfo$
- */
-
-#ifndef LLMATH_H
-#define LLMATH_H
-
-#include <cmath>
-#include <cstdlib>
-#include "lldefs.h"
-//#include "llstl.h" // *TODO: Remove when LLString is gone
-//#include "llstring.h" // *TODO: Remove when LLString is gone
-// lltut.h uses is_approx_equal_fraction(). This was moved to its own header
-// file in llcommon so we can use lltut.h for llcommon tests without making
-// llcommon depend on llmath.
-#include "is_approx_equal_fraction.h"
-
-// work around for Windows & older gcc non-standard function names.
-#if LL_WINDOWS
-#include <float.h>
-#define llisnan(val) _isnan(val)
-#define llfinite(val) _finite(val)
-#elif (LL_LINUX && __GNUC__ <= 2)
-#define llisnan(val) isnan(val)
-#define llfinite(val) isfinite(val)
-#elif LL_SOLARIS
-#define llisnan(val) isnan(val)
-#define llfinite(val) (val <= std::numeric_limits<double>::max())
-#else
-#define llisnan(val) std::isnan(val)
-#define llfinite(val) std::isfinite(val)
-#endif
-
-// Single Precision Floating Point Routines
-// (There used to be more defined here, but they appeared to be redundant and
-// were breaking some other includes. Removed by Falcon, reviewed by Andrew, 11/25/09)
-/*#ifndef tanf
-#define tanf(x) ((F32)tan((F64)(x)))
-#endif*/
-
-const F32 GRAVITY = -9.8f;
-
-// mathematical constants
-const F32 F_PI = 3.1415926535897932384626433832795f;
-const F32 F_TWO_PI = 6.283185307179586476925286766559f;
-const F32 F_PI_BY_TWO = 1.5707963267948966192313216916398f;
-const F32 F_SQRT_TWO_PI = 2.506628274631000502415765284811f;
-const F32 F_E = 2.71828182845904523536f;
-const F32 F_SQRT2 = 1.4142135623730950488016887242097f;
-const F32 F_SQRT3 = 1.73205080756888288657986402541f;
-const F32 OO_SQRT2 = 0.7071067811865475244008443621049f;
-const F32 DEG_TO_RAD = 0.017453292519943295769236907684886f;
-const F32 RAD_TO_DEG = 57.295779513082320876798154814105f;
-const F32 F_APPROXIMATELY_ZERO = 0.00001f;
-const F32 F_LN2 = 0.69314718056f;
-const F32 OO_LN2 = 1.4426950408889634073599246810019f;
-
-const F32 F_ALMOST_ZERO = 0.0001f;
-const F32 F_ALMOST_ONE = 1.0f - F_ALMOST_ZERO;
-
-// BUG: Eliminate in favor of F_APPROXIMATELY_ZERO above?
-const F32 FP_MAG_THRESHOLD = 0.0000001f;
-
-// TODO: Replace with logic like is_approx_equal
-inline BOOL is_approx_zero( F32 f ) { return (-F_APPROXIMATELY_ZERO < f) && (f < F_APPROXIMATELY_ZERO); }
-
-// These functions work by interpreting sign+exp+mantissa as an unsigned
-// integer.
-// For example:
-// x = <sign>1 <exponent>00000010 <mantissa>00000000000000000000000
-// y = <sign>1 <exponent>00000001 <mantissa>11111111111111111111111
-//
-// interpreted as ints =
-// x = 10000001000000000000000000000000
-// y = 10000000111111111111111111111111
-// which is clearly a different of 1 in the least significant bit
-// Values with the same exponent can be trivially shown to work.
-//
-// WARNING: Denormals of opposite sign do not work
-// x = <sign>1 <exponent>00000000 <mantissa>00000000000000000000001
-// y = <sign>0 <exponent>00000000 <mantissa>00000000000000000000001
-// Although these values differ by 2 in the LSB, the sign bit makes
-// the int comparison fail.
-//
-// WARNING: NaNs can compare equal
-// There is no special treatment of exceptional values like NaNs
-//
-// WARNING: Infinity is comparable with F32_MAX and negative
-// infinity is comparable with F32_MIN
-
-inline BOOL is_approx_equal(F32 x, F32 y)
-{
- const S32 COMPARE_MANTISSA_UP_TO_BIT = 0x02;
- return (std::abs((S32) ((U32&)x - (U32&)y) ) < COMPARE_MANTISSA_UP_TO_BIT);
-}
-
-inline BOOL is_approx_equal(F64 x, F64 y)
-{
- const S64 COMPARE_MANTISSA_UP_TO_BIT = 0x02;
- return (std::abs((S32) ((U64&)x - (U64&)y) ) < COMPARE_MANTISSA_UP_TO_BIT);
-}
-
-inline S32 llabs(const S32 a)
-{
- return S32(std::labs(a));
-}
-
-inline F32 llabs(const F32 a)
-{
- return F32(std::fabs(a));
-}
-
-inline F64 llabs(const F64 a)
-{
- return F64(std::fabs(a));
-}
-
-inline S32 lltrunc( F32 f )
-{
-#if LL_WINDOWS && !defined( __INTEL_COMPILER )
- // Avoids changing the floating point control word.
- // Add or subtract 0.5 - epsilon and then round
- const static U32 zpfp[] = { 0xBEFFFFFF, 0x3EFFFFFF };
- S32 result;
- __asm {
- fld f
- mov eax, f
- shr eax, 29
- and eax, 4
- fadd dword ptr [zpfp + eax]
- fistp result
- }
- return result;
-#else
- return (S32)f;
-#endif
-}
-
-inline S32 lltrunc( F64 f )
-{
- return (S32)f;
-}
-
-inline S32 llfloor( F32 f )
-{
-#if LL_WINDOWS && !defined( __INTEL_COMPILER )
- // Avoids changing the floating point control word.
- // Accurate (unlike Stereopsis version) for all values between S32_MIN and S32_MAX and slightly faster than Stereopsis version.
- // Add -(0.5 - epsilon) and then round
- const U32 zpfp = 0xBEFFFFFF;
- S32 result;
- __asm {
- fld f
- fadd dword ptr [zpfp]
- fistp result
- }
- return result;
-#else
- return (S32)floor(f);
-#endif
-}
-
-
-inline S32 llceil( F32 f )
-{
- // This could probably be optimized, but this works.
- return (S32)ceil(f);
-}
-
-
-#ifndef BOGUS_ROUND
-// Use this round. Does an arithmetic round (0.5 always rounds up)
-inline S32 llround(const F32 val)
-{
- return llfloor(val + 0.5f);
-}
-
-#else // BOGUS_ROUND
-// Old llround implementation - does banker's round (toward nearest even in the case of a 0.5.
-// Not using this because we don't have a consistent implementation on both platforms, use
-// llfloor(val + 0.5f), which is consistent on all platforms.
-inline S32 llround(const F32 val)
-{
- #if LL_WINDOWS
- // Note: assumes that the floating point control word is set to rounding mode (the default)
- S32 ret_val;
- _asm fld val
- _asm fistp ret_val;
- return ret_val;
- #elif LL_LINUX
- // Note: assumes that the floating point control word is set
- // to rounding mode (the default)
- S32 ret_val;
- __asm__ __volatile__( "flds %1 \n\t"
- "fistpl %0 \n\t"
- : "=m" (ret_val)
- : "m" (val) );
- return ret_val;
- #else
- return llfloor(val + 0.5f);
- #endif
-}
-
-// A fast arithmentic round on intel, from Laurent de Soras http://ldesoras.free.fr
-inline int round_int(double x)
-{
- const float round_to_nearest = 0.5f;
- int i;
- __asm
- {
- fld x
- fadd st, st (0)
- fadd round_to_nearest
- fistp i
- sar i, 1
- }
- return (i);
-}
-#endif // BOGUS_ROUND
-
-inline F32 llround( F32 val, F32 nearest )
-{
- return F32(floor(val * (1.0f / nearest) + 0.5f)) * nearest;
-}
-
-inline F64 llround( F64 val, F64 nearest )
-{
- return F64(floor(val * (1.0 / nearest) + 0.5)) * nearest;
-}
-
-// these provide minimum peak error
-//
-// avg error = -0.013049
-// peak error = -31.4 dB
-// RMS error = -28.1 dB
-
-const F32 FAST_MAG_ALPHA = 0.960433870103f;
-const F32 FAST_MAG_BETA = 0.397824734759f;
-
-// these provide minimum RMS error
-//
-// avg error = 0.000003
-// peak error = -32.6 dB
-// RMS error = -25.7 dB
-//
-//const F32 FAST_MAG_ALPHA = 0.948059448969f;
-//const F32 FAST_MAG_BETA = 0.392699081699f;
-
-inline F32 fastMagnitude(F32 a, F32 b)
-{
- a = (a > 0) ? a : -a;
- b = (b > 0) ? b : -b;
- return(FAST_MAG_ALPHA * llmax(a,b) + FAST_MAG_BETA * llmin(a,b));
-}
-
-
-
-////////////////////
-//
-// Fast F32/S32 conversions
-//
-// Culled from www.stereopsis.com/FPU.html
-
-const F64 LL_DOUBLE_TO_FIX_MAGIC = 68719476736.0*1.5; //2^36 * 1.5, (52-_shiftamt=36) uses limited precisicion to floor
-const S32 LL_SHIFT_AMOUNT = 16; //16.16 fixed point representation,
-
-// Endian dependent code
-#ifdef LL_LITTLE_ENDIAN
- #define LL_EXP_INDEX 1
- #define LL_MAN_INDEX 0
-#else
- #define LL_EXP_INDEX 0
- #define LL_MAN_INDEX 1
-#endif
-
-/* Deprecated: use llround(), lltrunc(), or llfloor() instead
-// ================================================================================================
-// Real2Int
-// ================================================================================================
-inline S32 F64toS32(F64 val)
-{
- val = val + LL_DOUBLE_TO_FIX_MAGIC;
- return ((S32*)&val)[LL_MAN_INDEX] >> LL_SHIFT_AMOUNT;
-}
-
-// ================================================================================================
-// Real2Int
-// ================================================================================================
-inline S32 F32toS32(F32 val)
-{
- return F64toS32 ((F64)val);
-}
-*/
-
-////////////////////////////////////////////////
-//
-// Fast exp and log
-//
-
-// Implementation of fast exp() approximation (from a paper by Nicol N. Schraudolph
-// http://www.inf.ethz.ch/~schraudo/pubs/exp.pdf
-static union
-{
- double d;
- struct
- {
-#ifdef LL_LITTLE_ENDIAN
- S32 j, i;
-#else
- S32 i, j;
-#endif
- } n;
-} LLECO; // not sure what the name means
-
-#define LL_EXP_A (1048576 * OO_LN2) // use 1512775 for integer
-#define LL_EXP_C (60801) // this value of C good for -4 < y < 4
-
-#define LL_FAST_EXP(y) (LLECO.n.i = llround(F32(LL_EXP_A*(y))) + (1072693248 - LL_EXP_C), LLECO.d)
-
-
-
-inline F32 llfastpow(const F32 x, const F32 y)
-{
- return (F32)(LL_FAST_EXP(y * log(x)));
-}
-
-
-inline F32 snap_to_sig_figs(F32 foo, S32 sig_figs)
-{
- // compute the power of ten
- F32 bar = 1.f;
- for (S32 i = 0; i < sig_figs; i++)
- {
- bar *= 10.f;
- }
-
- //F32 new_foo = (F32)llround(foo * bar);
- // the llround() implementation sucks. Don't us it.
-
- F32 sign = (foo > 0.f) ? 1.f : -1.f;
- F32 new_foo = F32( S64(foo * bar + sign * 0.5f));
- new_foo /= bar;
-
- return new_foo;
-}
-
-inline F32 lerp(F32 a, F32 b, F32 u)
-{
- return a + ((b - a) * u);
-}
-
-inline F32 lerp2d(F32 x00, F32 x01, F32 x10, F32 x11, F32 u, F32 v)
-{
- F32 a = x00 + (x01-x00)*u;
- F32 b = x10 + (x11-x10)*u;
- F32 r = a + (b-a)*v;
- return r;
-}
-
-inline F32 ramp(F32 x, F32 a, F32 b)
-{
- return (a == b) ? 0.0f : ((a - x) / (a - b));
-}
-
-inline F32 rescale(F32 x, F32 x1, F32 x2, F32 y1, F32 y2)
-{
- return lerp(y1, y2, ramp(x, x1, x2));
-}
-
-inline F32 clamp_rescale(F32 x, F32 x1, F32 x2, F32 y1, F32 y2)
-{
- if (y1 < y2)
- {
- return llclamp(rescale(x,x1,x2,y1,y2),y1,y2);
- }
- else
- {
- return llclamp(rescale(x,x1,x2,y1,y2),y2,y1);
- }
-}
-
-
-inline F32 cubic_step( F32 x, F32 x0, F32 x1, F32 s0, F32 s1 )
-{
- if (x <= x0)
- return s0;
-
- if (x >= x1)
- return s1;
-
- F32 f = (x - x0) / (x1 - x0);
-
- return s0 + (s1 - s0) * (f * f) * (3.0f - 2.0f * f);
-}
-
-inline F32 cubic_step( F32 x )
-{
- x = llclampf(x);
-
- return (x * x) * (3.0f - 2.0f * x);
-}
-
-inline F32 quadratic_step( F32 x, F32 x0, F32 x1, F32 s0, F32 s1 )
-{
- if (x <= x0)
- return s0;
-
- if (x >= x1)
- return s1;
-
- F32 f = (x - x0) / (x1 - x0);
- F32 f_squared = f * f;
-
- return (s0 * (1.f - f_squared)) + ((s1 - s0) * f_squared);
-}
-
-inline F32 llsimple_angle(F32 angle)
-{
- while(angle <= -F_PI)
- angle += F_TWO_PI;
- while(angle > F_PI)
- angle -= F_TWO_PI;
- return angle;
-}
-
-//SDK - Renamed this to get_lower_power_two, since this is what this actually does.
-inline U32 get_lower_power_two(U32 val, U32 max_power_two)
-{
- if(!max_power_two)
- {
- max_power_two = 1 << 31 ;
- }
- if(max_power_two & (max_power_two - 1))
- {
- return 0 ;
- }
-
- for(; val < max_power_two ; max_power_two >>= 1) ;
-
- return max_power_two ;
-}
-
-// calculate next highest power of two, limited by max_power_two
-// This is taken from a brilliant little code snipped on http://acius2.blogspot.com/2007/11/calculating-next-power-of-2.html
-// Basically we convert the binary to a solid string of 1's with the same
-// number of digits, then add one. We subtract 1 initially to handle
-// the case where the number passed in is actually a power of two.
-// WARNING: this only works with 32 bit ints.
-inline U32 get_next_power_two(U32 val, U32 max_power_two)
-{
- if(!max_power_two)
- {
- max_power_two = 1 << 31 ;
- }
-
- if(val >= max_power_two)
- {
- return max_power_two;
- }
-
- val--;
- val = (val >> 1) | val;
- val = (val >> 2) | val;
- val = (val >> 4) | val;
- val = (val >> 8) | val;
- val = (val >> 16) | val;
- val++;
-
- return val;
-}
-
-//get the gaussian value given the linear distance from axis x and guassian value o
-inline F32 llgaussian(F32 x, F32 o)
-{
- return 1.f/(F_SQRT_TWO_PI*o)*powf(F_E, -(x*x)/(2*o*o));
-}
-
-// Include simd math header
-#include "llsimdmath.h"
-
-#endif
+/**
+ * @file llmath.h
+ * @brief Useful math constants and macros.
+ *
+ * $LicenseInfo:firstyear=2000&license=viewergpl$
+ *
+ * Copyright (c) 2000-2009, Linden Research, Inc.
+ *
+ * Second Life Viewer Source Code
+ * The source code in this file ("Source Code") is provided by Linden Lab
+ * to you under the terms of the GNU General Public License, version 2.0
+ * ("GPL"), unless you have obtained a separate licensing agreement
+ * ("Other License"), formally executed by you and Linden Lab. Terms of
+ * the GPL can be found in doc/GPL-license.txt in this distribution, or
+ * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
+ *
+ * There are special exceptions to the terms and conditions of the GPL as
+ * it is applied to this Source Code. View the full text of the exception
+ * in the file doc/FLOSS-exception.txt in this software distribution, or
+ * online at
+ * http://secondlifegrid.net/programs/open_source/licensing/flossexception
+ *
+ * By copying, modifying or distributing this software, you acknowledge
+ * that you have read and understood your obligations described above,
+ * and agree to abide by those obligations.
+ *
+ * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
+ * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
+ * COMPLETENESS OR PERFORMANCE.
+ * $/LicenseInfo$
+ */
+
+#ifndef LLMATH_H
+#define LLMATH_H
+
+#include <cmath>
+#include <cstdlib>
+#include "lldefs.h"
+//#include "llstl.h" // *TODO: Remove when LLString is gone
+//#include "llstring.h" // *TODO: Remove when LLString is gone
+// lltut.h uses is_approx_equal_fraction(). This was moved to its own header
+// file in llcommon so we can use lltut.h for llcommon tests without making
+// llcommon depend on llmath.
+#include "is_approx_equal_fraction.h"
+
+// work around for Windows & older gcc non-standard function names.
+#if LL_WINDOWS
+#include <float.h>
+#define llisnan(val) _isnan(val)
+#define llfinite(val) _finite(val)
+#elif (LL_LINUX && __GNUC__ <= 2)
+#define llisnan(val) isnan(val)
+#define llfinite(val) isfinite(val)
+#elif LL_SOLARIS
+#define llisnan(val) isnan(val)
+#define llfinite(val) (val <= std::numeric_limits<double>::max())
+#else
+#define llisnan(val) std::isnan(val)
+#define llfinite(val) std::isfinite(val)
+#endif
+
+// Single Precision Floating Point Routines
+// (There used to be more defined here, but they appeared to be redundant and
+// were breaking some other includes. Removed by Falcon, reviewed by Andrew, 11/25/09)
+/*#ifndef tanf
+#define tanf(x) ((F32)tan((F64)(x)))
+#endif*/
+
+const F32 GRAVITY = -9.8f;
+
+// mathematical constants
+const F32 F_PI = 3.1415926535897932384626433832795f;
+const F32 F_TWO_PI = 6.283185307179586476925286766559f;
+const F32 F_PI_BY_TWO = 1.5707963267948966192313216916398f;
+const F32 F_SQRT_TWO_PI = 2.506628274631000502415765284811f;
+const F32 F_E = 2.71828182845904523536f;
+const F32 F_SQRT2 = 1.4142135623730950488016887242097f;
+const F32 F_SQRT3 = 1.73205080756888288657986402541f;
+const F32 OO_SQRT2 = 0.7071067811865475244008443621049f;
+const F32 DEG_TO_RAD = 0.017453292519943295769236907684886f;
+const F32 RAD_TO_DEG = 57.295779513082320876798154814105f;
+const F32 F_APPROXIMATELY_ZERO = 0.00001f;
+const F32 F_LN2 = 0.69314718056f;
+const F32 OO_LN2 = 1.4426950408889634073599246810019f;
+
+const F32 F_ALMOST_ZERO = 0.0001f;
+const F32 F_ALMOST_ONE = 1.0f - F_ALMOST_ZERO;
+
+// BUG: Eliminate in favor of F_APPROXIMATELY_ZERO above?
+const F32 FP_MAG_THRESHOLD = 0.0000001f;
+
+// TODO: Replace with logic like is_approx_equal
+inline BOOL is_approx_zero( F32 f ) { return (-F_APPROXIMATELY_ZERO < f) && (f < F_APPROXIMATELY_ZERO); }
+
+// These functions work by interpreting sign+exp+mantissa as an unsigned
+// integer.
+// For example:
+// x = <sign>1 <exponent>00000010 <mantissa>00000000000000000000000
+// y = <sign>1 <exponent>00000001 <mantissa>11111111111111111111111
+//
+// interpreted as ints =
+// x = 10000001000000000000000000000000
+// y = 10000000111111111111111111111111
+// which is clearly a different of 1 in the least significant bit
+// Values with the same exponent can be trivially shown to work.
+//
+// WARNING: Denormals of opposite sign do not work
+// x = <sign>1 <exponent>00000000 <mantissa>00000000000000000000001
+// y = <sign>0 <exponent>00000000 <mantissa>00000000000000000000001
+// Although these values differ by 2 in the LSB, the sign bit makes
+// the int comparison fail.
+//
+// WARNING: NaNs can compare equal
+// There is no special treatment of exceptional values like NaNs
+//
+// WARNING: Infinity is comparable with F32_MAX and negative
+// infinity is comparable with F32_MIN
+
+inline BOOL is_approx_equal(F32 x, F32 y)
+{
+ const S32 COMPARE_MANTISSA_UP_TO_BIT = 0x02;
+ return (std::abs((S32) ((U32&)x - (U32&)y) ) < COMPARE_MANTISSA_UP_TO_BIT);
+}
+
+inline BOOL is_approx_equal(F64 x, F64 y)
+{
+ const S64 COMPARE_MANTISSA_UP_TO_BIT = 0x02;
+ return (std::abs((S32) ((U64&)x - (U64&)y) ) < COMPARE_MANTISSA_UP_TO_BIT);
+}
+
+inline S32 llabs(const S32 a)
+{
+ return S32(std::labs(a));
+}
+
+inline F32 llabs(const F32 a)
+{
+ return F32(std::fabs(a));
+}
+
+inline F64 llabs(const F64 a)
+{
+ return F64(std::fabs(a));
+}
+
+inline S32 lltrunc( F32 f )
+{
+#if LL_WINDOWS && !defined( __INTEL_COMPILER )
+ // Avoids changing the floating point control word.
+ // Add or subtract 0.5 - epsilon and then round
+ const static U32 zpfp[] = { 0xBEFFFFFF, 0x3EFFFFFF };
+ S32 result;
+ __asm {
+ fld f
+ mov eax, f
+ shr eax, 29
+ and eax, 4
+ fadd dword ptr [zpfp + eax]
+ fistp result
+ }
+ return result;
+#else
+ return (S32)f;
+#endif
+}
+
+inline S32 lltrunc( F64 f )
+{
+ return (S32)f;
+}
+
+inline S32 llfloor( F32 f )
+{
+#if LL_WINDOWS && !defined( __INTEL_COMPILER )
+ // Avoids changing the floating point control word.
+ // Accurate (unlike Stereopsis version) for all values between S32_MIN and S32_MAX and slightly faster than Stereopsis version.
+ // Add -(0.5 - epsilon) and then round
+ const U32 zpfp = 0xBEFFFFFF;
+ S32 result;
+ __asm {
+ fld f
+ fadd dword ptr [zpfp]
+ fistp result
+ }
+ return result;
+#else
+ return (S32)floor(f);
+#endif
+}
+
+
+inline S32 llceil( F32 f )
+{
+ // This could probably be optimized, but this works.
+ return (S32)ceil(f);
+}
+
+
+#ifndef BOGUS_ROUND
+// Use this round. Does an arithmetic round (0.5 always rounds up)
+inline S32 llround(const F32 val)
+{
+ return llfloor(val + 0.5f);
+}
+
+#else // BOGUS_ROUND
+// Old llround implementation - does banker's round (toward nearest even in the case of a 0.5.
+// Not using this because we don't have a consistent implementation on both platforms, use
+// llfloor(val + 0.5f), which is consistent on all platforms.
+inline S32 llround(const F32 val)
+{
+ #if LL_WINDOWS
+ // Note: assumes that the floating point control word is set to rounding mode (the default)
+ S32 ret_val;
+ _asm fld val
+ _asm fistp ret_val;
+ return ret_val;
+ #elif LL_LINUX
+ // Note: assumes that the floating point control word is set
+ // to rounding mode (the default)
+ S32 ret_val;
+ __asm__ __volatile__( "flds %1 \n\t"
+ "fistpl %0 \n\t"
+ : "=m" (ret_val)
+ : "m" (val) );
+ return ret_val;
+ #else
+ return llfloor(val + 0.5f);
+ #endif
+}
+
+// A fast arithmentic round on intel, from Laurent de Soras http://ldesoras.free.fr
+inline int round_int(double x)
+{
+ const float round_to_nearest = 0.5f;
+ int i;
+ __asm
+ {
+ fld x
+ fadd st, st (0)
+ fadd round_to_nearest
+ fistp i
+ sar i, 1
+ }
+ return (i);
+}
+#endif // BOGUS_ROUND
+
+inline F32 llround( F32 val, F32 nearest )
+{
+ return F32(floor(val * (1.0f / nearest) + 0.5f)) * nearest;
+}
+
+inline F64 llround( F64 val, F64 nearest )
+{
+ return F64(floor(val * (1.0 / nearest) + 0.5)) * nearest;
+}
+
+// these provide minimum peak error
+//
+// avg error = -0.013049
+// peak error = -31.4 dB
+// RMS error = -28.1 dB
+
+const F32 FAST_MAG_ALPHA = 0.960433870103f;
+const F32 FAST_MAG_BETA = 0.397824734759f;
+
+// these provide minimum RMS error
+//
+// avg error = 0.000003
+// peak error = -32.6 dB
+// RMS error = -25.7 dB
+//
+//const F32 FAST_MAG_ALPHA = 0.948059448969f;
+//const F32 FAST_MAG_BETA = 0.392699081699f;
+
+inline F32 fastMagnitude(F32 a, F32 b)
+{
+ a = (a > 0) ? a : -a;
+ b = (b > 0) ? b : -b;
+ return(FAST_MAG_ALPHA * llmax(a,b) + FAST_MAG_BETA * llmin(a,b));
+}
+
+
+
+////////////////////
+//
+// Fast F32/S32 conversions
+//
+// Culled from www.stereopsis.com/FPU.html
+
+const F64 LL_DOUBLE_TO_FIX_MAGIC = 68719476736.0*1.5; //2^36 * 1.5, (52-_shiftamt=36) uses limited precisicion to floor
+const S32 LL_SHIFT_AMOUNT = 16; //16.16 fixed point representation,
+
+// Endian dependent code
+#ifdef LL_LITTLE_ENDIAN
+ #define LL_EXP_INDEX 1
+ #define LL_MAN_INDEX 0
+#else
+ #define LL_EXP_INDEX 0
+ #define LL_MAN_INDEX 1
+#endif
+
+/* Deprecated: use llround(), lltrunc(), or llfloor() instead
+// ================================================================================================
+// Real2Int
+// ================================================================================================
+inline S32 F64toS32(F64 val)
+{
+ val = val + LL_DOUBLE_TO_FIX_MAGIC;
+ return ((S32*)&val)[LL_MAN_INDEX] >> LL_SHIFT_AMOUNT;
+}
+
+// ================================================================================================
+// Real2Int
+// ================================================================================================
+inline S32 F32toS32(F32 val)
+{
+ return F64toS32 ((F64)val);
+}
+*/
+
+////////////////////////////////////////////////
+//
+// Fast exp and log
+//
+
+// Implementation of fast exp() approximation (from a paper by Nicol N. Schraudolph
+// http://www.inf.ethz.ch/~schraudo/pubs/exp.pdf
+static union
+{
+ double d;
+ struct
+ {
+#ifdef LL_LITTLE_ENDIAN
+ S32 j, i;
+#else
+ S32 i, j;
+#endif
+ } n;
+} LLECO; // not sure what the name means
+
+#define LL_EXP_A (1048576 * OO_LN2) // use 1512775 for integer
+#define LL_EXP_C (60801) // this value of C good for -4 < y < 4
+
+#define LL_FAST_EXP(y) (LLECO.n.i = llround(F32(LL_EXP_A*(y))) + (1072693248 - LL_EXP_C), LLECO.d)
+
+
+
+inline F32 llfastpow(const F32 x, const F32 y)
+{
+ return (F32)(LL_FAST_EXP(y * log(x)));
+}
+
+
+inline F32 snap_to_sig_figs(F32 foo, S32 sig_figs)
+{
+ // compute the power of ten
+ F32 bar = 1.f;
+ for (S32 i = 0; i < sig_figs; i++)
+ {
+ bar *= 10.f;
+ }
+
+ //F32 new_foo = (F32)llround(foo * bar);
+ // the llround() implementation sucks. Don't us it.
+
+ F32 sign = (foo > 0.f) ? 1.f : -1.f;
+ F32 new_foo = F32( S64(foo * bar + sign * 0.5f));
+ new_foo /= bar;
+
+ return new_foo;
+}
+
+inline F32 lerp(F32 a, F32 b, F32 u)
+{
+ return a + ((b - a) * u);
+}
+
+inline F32 lerp2d(F32 x00, F32 x01, F32 x10, F32 x11, F32 u, F32 v)
+{
+ F32 a = x00 + (x01-x00)*u;
+ F32 b = x10 + (x11-x10)*u;
+ F32 r = a + (b-a)*v;
+ return r;
+}
+
+inline F32 ramp(F32 x, F32 a, F32 b)
+{
+ return (a == b) ? 0.0f : ((a - x) / (a - b));
+}
+
+inline F32 rescale(F32 x, F32 x1, F32 x2, F32 y1, F32 y2)
+{
+ return lerp(y1, y2, ramp(x, x1, x2));
+}
+
+inline F32 clamp_rescale(F32 x, F32 x1, F32 x2, F32 y1, F32 y2)
+{
+ if (y1 < y2)
+ {
+ return llclamp(rescale(x,x1,x2,y1,y2),y1,y2);
+ }
+ else
+ {
+ return llclamp(rescale(x,x1,x2,y1,y2),y2,y1);
+ }
+}
+
+
+inline F32 cubic_step( F32 x, F32 x0, F32 x1, F32 s0, F32 s1 )
+{
+ if (x <= x0)
+ return s0;
+
+ if (x >= x1)
+ return s1;
+
+ F32 f = (x - x0) / (x1 - x0);
+
+ return s0 + (s1 - s0) * (f * f) * (3.0f - 2.0f * f);
+}
+
+inline F32 cubic_step( F32 x )
+{
+ x = llclampf(x);
+
+ return (x * x) * (3.0f - 2.0f * x);
+}
+
+inline F32 quadratic_step( F32 x, F32 x0, F32 x1, F32 s0, F32 s1 )
+{
+ if (x <= x0)
+ return s0;
+
+ if (x >= x1)
+ return s1;
+
+ F32 f = (x - x0) / (x1 - x0);
+ F32 f_squared = f * f;
+
+ return (s0 * (1.f - f_squared)) + ((s1 - s0) * f_squared);
+}
+
+inline F32 llsimple_angle(F32 angle)
+{
+ while(angle <= -F_PI)
+ angle += F_TWO_PI;
+ while(angle > F_PI)
+ angle -= F_TWO_PI;
+ return angle;
+}
+
+//SDK - Renamed this to get_lower_power_two, since this is what this actually does.
+inline U32 get_lower_power_two(U32 val, U32 max_power_two)
+{
+ if(!max_power_two)
+ {
+ max_power_two = 1 << 31 ;
+ }
+ if(max_power_two & (max_power_two - 1))
+ {
+ return 0 ;
+ }
+
+ for(; val < max_power_two ; max_power_two >>= 1) ;
+
+ return max_power_two ;
+}
+
+// calculate next highest power of two, limited by max_power_two
+// This is taken from a brilliant little code snipped on http://acius2.blogspot.com/2007/11/calculating-next-power-of-2.html
+// Basically we convert the binary to a solid string of 1's with the same
+// number of digits, then add one. We subtract 1 initially to handle
+// the case where the number passed in is actually a power of two.
+// WARNING: this only works with 32 bit ints.
+inline U32 get_next_power_two(U32 val, U32 max_power_two)
+{
+ if(!max_power_two)
+ {
+ max_power_two = 1 << 31 ;
+ }
+
+ if(val >= max_power_two)
+ {
+ return max_power_two;
+ }
+
+ val--;
+ val = (val >> 1) | val;
+ val = (val >> 2) | val;
+ val = (val >> 4) | val;
+ val = (val >> 8) | val;
+ val = (val >> 16) | val;
+ val++;
+
+ return val;
+}
+
+//get the gaussian value given the linear distance from axis x and guassian value o
+inline F32 llgaussian(F32 x, F32 o)
+{
+ return 1.f/(F_SQRT_TWO_PI*o)*powf(F_E, -(x*x)/(2*o*o));
+}
+
+// Include simd math header
+#include "llsimdmath.h"
+
+#endif
diff --git a/indra/llmath/llquantize.h b/indra/llmath/llquantize.h
index 000d8a060f..c043f7f752 100644
--- a/indra/llmath/llquantize.h
+++ b/indra/llmath/llquantize.h
@@ -1,158 +1,158 @@
-/**
- * @file llquantize.h
- * @brief useful routines for quantizing floats to various length ints
- * and back out again
- *
- * $LicenseInfo:firstyear=2001&license=viewergpl$
- *
- * Copyright (c) 2001-2009, Linden Research, Inc.
- *
- * Second Life Viewer Source Code
- * The source code in this file ("Source Code") is provided by Linden Lab
- * to you under the terms of the GNU General Public License, version 2.0
- * ("GPL"), unless you have obtained a separate licensing agreement
- * ("Other License"), formally executed by you and Linden Lab. Terms of
- * the GPL can be found in doc/GPL-license.txt in this distribution, or
- * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
- *
- * There are special exceptions to the terms and conditions of the GPL as
- * it is applied to this Source Code. View the full text of the exception
- * in the file doc/FLOSS-exception.txt in this software distribution, or
- * online at
- * http://secondlifegrid.net/programs/open_source/licensing/flossexception
- *
- * By copying, modifying or distributing this software, you acknowledge
- * that you have read and understood your obligations described above,
- * and agree to abide by those obligations.
- *
- * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
- * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
- * COMPLETENESS OR PERFORMANCE.
- * $/LicenseInfo$
- */
-
-#ifndef LL_LLQUANTIZE_H
-#define LL_LLQUANTIZE_H
-
-const U16 U16MAX = 65535;
-LL_ALIGN_16( const F32 F_U16MAX_4A[4] ) = { 65535.f, 65535.f, 65535.f, 65535.f };
-
-const F32 OOU16MAX = 1.f/(F32)(U16MAX);
-LL_ALIGN_16( const F32 F_OOU16MAX_4A[4] ) = { OOU16MAX, OOU16MAX, OOU16MAX, OOU16MAX };
-
-const U8 U8MAX = 255;
-LL_ALIGN_16( const F32 F_U8MAX_4A[4] ) = { 255.f, 255.f, 255.f, 255.f };
-
-const F32 OOU8MAX = 1.f/(F32)(U8MAX);
-LL_ALIGN_16( const F32 F_OOU8MAX_4A[4] ) = { OOU8MAX, OOU8MAX, OOU8MAX, OOU8MAX };
-
-const U8 FIRSTVALIDCHAR = 54;
-const U8 MAXSTRINGVAL = U8MAX - FIRSTVALIDCHAR; //we don't allow newline or null
-
-
-inline U16 F32_to_U16_ROUND(F32 val, F32 lower, F32 upper)
-{
- val = llclamp(val, lower, upper);
- // make sure that the value is positive and normalized to <0, 1>
- val -= lower;
- val /= (upper - lower);
-
- // round the value. Sreturn the U16
- return (U16)(llround(val*U16MAX));
-}
-
-
-inline U16 F32_to_U16(F32 val, F32 lower, F32 upper)
-{
- val = llclamp(val, lower, upper);
- // make sure that the value is positive and normalized to <0, 1>
- val -= lower;
- val /= (upper - lower);
-
- // return the U16
- return (U16)(llfloor(val*U16MAX));
-}
-
-inline F32 U16_to_F32(U16 ival, F32 lower, F32 upper)
-{
- F32 val = ival*OOU16MAX;
- F32 delta = (upper - lower);
- val *= delta;
- val += lower;
-
- F32 max_error = delta*OOU16MAX;
-
- // make sure that zero's come through as zero
- if (fabsf(val) < max_error)
- val = 0.f;
-
- return val;
-}
-
-
-inline U8 F32_to_U8_ROUND(F32 val, F32 lower, F32 upper)
-{
- val = llclamp(val, lower, upper);
- // make sure that the value is positive and normalized to <0, 1>
- val -= lower;
- val /= (upper - lower);
-
- // return the rounded U8
- return (U8)(llround(val*U8MAX));
-}
-
-
-inline U8 F32_to_U8(F32 val, F32 lower, F32 upper)
-{
- val = llclamp(val, lower, upper);
- // make sure that the value is positive and normalized to <0, 1>
- val -= lower;
- val /= (upper - lower);
-
- // return the U8
- return (U8)(llfloor(val*U8MAX));
-}
-
-inline F32 U8_to_F32(U8 ival, F32 lower, F32 upper)
-{
- F32 val = ival*OOU8MAX;
- F32 delta = (upper - lower);
- val *= delta;
- val += lower;
-
- F32 max_error = delta*OOU8MAX;
-
- // make sure that zero's come through as zero
- if (fabsf(val) < max_error)
- val = 0.f;
-
- return val;
-}
-
-inline U8 F32_TO_STRING(F32 val, F32 lower, F32 upper)
-{
- val = llclamp(val, lower, upper); //[lower, upper]
- // make sure that the value is positive and normalized to <0, 1>
- val -= lower; //[0, upper-lower]
- val /= (upper - lower); //[0,1]
- val = val * MAXSTRINGVAL; //[0, MAXSTRINGVAL]
- val = floor(val + 0.5f); //[0, MAXSTRINGVAL]
-
- U8 stringVal = (U8)(val) + FIRSTVALIDCHAR; //[FIRSTVALIDCHAR, MAXSTRINGVAL + FIRSTVALIDCHAR]
- return stringVal;
-}
-
-inline F32 STRING_TO_F32(U8 ival, F32 lower, F32 upper)
-{
- // remove empty space left for NULL, newline, etc.
- ival -= FIRSTVALIDCHAR; //[0, MAXSTRINGVAL]
-
- F32 val = (F32)ival * (1.f / (F32)MAXSTRINGVAL); //[0, 1]
- F32 delta = (upper - lower);
- val *= delta; //[0, upper - lower]
- val += lower; //[lower, upper]
-
- return val;
-}
-
-#endif
+/**
+ * @file llquantize.h
+ * @brief useful routines for quantizing floats to various length ints
+ * and back out again
+ *
+ * $LicenseInfo:firstyear=2001&license=viewergpl$
+ *
+ * Copyright (c) 2001-2009, Linden Research, Inc.
+ *
+ * Second Life Viewer Source Code
+ * The source code in this file ("Source Code") is provided by Linden Lab
+ * to you under the terms of the GNU General Public License, version 2.0
+ * ("GPL"), unless you have obtained a separate licensing agreement
+ * ("Other License"), formally executed by you and Linden Lab. Terms of
+ * the GPL can be found in doc/GPL-license.txt in this distribution, or
+ * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
+ *
+ * There are special exceptions to the terms and conditions of the GPL as
+ * it is applied to this Source Code. View the full text of the exception
+ * in the file doc/FLOSS-exception.txt in this software distribution, or
+ * online at
+ * http://secondlifegrid.net/programs/open_source/licensing/flossexception
+ *
+ * By copying, modifying or distributing this software, you acknowledge
+ * that you have read and understood your obligations described above,
+ * and agree to abide by those obligations.
+ *
+ * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
+ * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
+ * COMPLETENESS OR PERFORMANCE.
+ * $/LicenseInfo$
+ */
+
+#ifndef LL_LLQUANTIZE_H
+#define LL_LLQUANTIZE_H
+
+const U16 U16MAX = 65535;
+LL_ALIGN_16( const F32 F_U16MAX_4A[4] ) = { 65535.f, 65535.f, 65535.f, 65535.f };
+
+const F32 OOU16MAX = 1.f/(F32)(U16MAX);
+LL_ALIGN_16( const F32 F_OOU16MAX_4A[4] ) = { OOU16MAX, OOU16MAX, OOU16MAX, OOU16MAX };
+
+const U8 U8MAX = 255;
+LL_ALIGN_16( const F32 F_U8MAX_4A[4] ) = { 255.f, 255.f, 255.f, 255.f };
+
+const F32 OOU8MAX = 1.f/(F32)(U8MAX);
+LL_ALIGN_16( const F32 F_OOU8MAX_4A[4] ) = { OOU8MAX, OOU8MAX, OOU8MAX, OOU8MAX };
+
+const U8 FIRSTVALIDCHAR = 54;
+const U8 MAXSTRINGVAL = U8MAX - FIRSTVALIDCHAR; //we don't allow newline or null
+
+
+inline U16 F32_to_U16_ROUND(F32 val, F32 lower, F32 upper)
+{
+ val = llclamp(val, lower, upper);
+ // make sure that the value is positive and normalized to <0, 1>
+ val -= lower;
+ val /= (upper - lower);
+
+ // round the value. Sreturn the U16
+ return (U16)(llround(val*U16MAX));
+}
+
+
+inline U16 F32_to_U16(F32 val, F32 lower, F32 upper)
+{
+ val = llclamp(val, lower, upper);
+ // make sure that the value is positive and normalized to <0, 1>
+ val -= lower;
+ val /= (upper - lower);
+
+ // return the U16
+ return (U16)(llfloor(val*U16MAX));
+}
+
+inline F32 U16_to_F32(U16 ival, F32 lower, F32 upper)
+{
+ F32 val = ival*OOU16MAX;
+ F32 delta = (upper - lower);
+ val *= delta;
+ val += lower;
+
+ F32 max_error = delta*OOU16MAX;
+
+ // make sure that zero's come through as zero
+ if (fabsf(val) < max_error)
+ val = 0.f;
+
+ return val;
+}
+
+
+inline U8 F32_to_U8_ROUND(F32 val, F32 lower, F32 upper)
+{
+ val = llclamp(val, lower, upper);
+ // make sure that the value is positive and normalized to <0, 1>
+ val -= lower;
+ val /= (upper - lower);
+
+ // return the rounded U8
+ return (U8)(llround(val*U8MAX));
+}
+
+
+inline U8 F32_to_U8(F32 val, F32 lower, F32 upper)
+{
+ val = llclamp(val, lower, upper);
+ // make sure that the value is positive and normalized to <0, 1>
+ val -= lower;
+ val /= (upper - lower);
+
+ // return the U8
+ return (U8)(llfloor(val*U8MAX));
+}
+
+inline F32 U8_to_F32(U8 ival, F32 lower, F32 upper)
+{
+ F32 val = ival*OOU8MAX;
+ F32 delta = (upper - lower);
+ val *= delta;
+ val += lower;
+
+ F32 max_error = delta*OOU8MAX;
+
+ // make sure that zero's come through as zero
+ if (fabsf(val) < max_error)
+ val = 0.f;
+
+ return val;
+}
+
+inline U8 F32_TO_STRING(F32 val, F32 lower, F32 upper)
+{
+ val = llclamp(val, lower, upper); //[lower, upper]
+ // make sure that the value is positive and normalized to <0, 1>
+ val -= lower; //[0, upper-lower]
+ val /= (upper - lower); //[0,1]
+ val = val * MAXSTRINGVAL; //[0, MAXSTRINGVAL]
+ val = floor(val + 0.5f); //[0, MAXSTRINGVAL]
+
+ U8 stringVal = (U8)(val) + FIRSTVALIDCHAR; //[FIRSTVALIDCHAR, MAXSTRINGVAL + FIRSTVALIDCHAR]
+ return stringVal;
+}
+
+inline F32 STRING_TO_F32(U8 ival, F32 lower, F32 upper)
+{
+ // remove empty space left for NULL, newline, etc.
+ ival -= FIRSTVALIDCHAR; //[0, MAXSTRINGVAL]
+
+ F32 val = (F32)ival * (1.f / (F32)MAXSTRINGVAL); //[0, 1]
+ F32 delta = (upper - lower);
+ val *= delta; //[0, upper - lower]
+ val += lower; //[lower, upper]
+
+ return val;
+}
+
+#endif
diff --git a/indra/llmath/llquaternion.cpp b/indra/llmath/llquaternion.cpp
index efdc10e2c6..73c5f4505e 100644
--- a/indra/llmath/llquaternion.cpp
+++ b/indra/llmath/llquaternion.cpp
@@ -1,961 +1,961 @@
-/**
- * @file llquaternion.cpp
- * @brief LLQuaternion class implementation.
- *
- * $LicenseInfo:firstyear=2000&license=viewergpl$
- *
- * Copyright (c) 2000-2009, Linden Research, Inc.
- *
- * Second Life Viewer Source Code
- * The source code in this file ("Source Code") is provided by Linden Lab
- * to you under the terms of the GNU General Public License, version 2.0
- * ("GPL"), unless you have obtained a separate licensing agreement
- * ("Other License"), formally executed by you and Linden Lab. Terms of
- * the GPL can be found in doc/GPL-license.txt in this distribution, or
- * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
- *
- * There are special exceptions to the terms and conditions of the GPL as
- * it is applied to this Source Code. View the full text of the exception
- * in the file doc/FLOSS-exception.txt in this software distribution, or
- * online at
- * http://secondlifegrid.net/programs/open_source/licensing/flossexception
- *
- * By copying, modifying or distributing this software, you acknowledge
- * that you have read and understood your obligations described above,
- * and agree to abide by those obligations.
- *
- * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
- * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
- * COMPLETENESS OR PERFORMANCE.
- * $/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)
-{
- LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
- v.normalize();
-
- F32 c, s;
- c = cosf(angle*0.5f);
- s = sinf(angle*0.5f);
-
- mQ[VX] = v.mV[VX] * s;
- mQ[VY] = v.mV[VY] * s;
- mQ[VZ] = v.mV[VZ] * s;
- mQ[VW] = c;
- normalize();
-}
-
-LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
-{
- LLVector3 v(vec);
- v.normalize();
-
- F32 c, s;
- c = cosf(angle*0.5f);
- s = sinf(angle*0.5f);
-
- mQ[VX] = v.mV[VX] * s;
- mQ[VY] = v.mV[VY] * s;
- mQ[VZ] = v.mV[VZ] * s;
- mQ[VW] = c;
- normalize();
-}
-
-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();
-}
-
-// 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)
-{
- LLVector3 vec(x, y, z);
- vec.normalize();
-
- angle *= 0.5f;
- F32 c, s;
- c = cosf(angle);
- s = sinf(angle);
-
- mQ[VX] = vec.mV[VX]*s;
- mQ[VY] = vec.mV[VY]*s;
- mQ[VZ] = vec.mV[VZ]*s;
- mQ[VW] = c;
-
- normalize();
- return (*this);
-}
-
-const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector3 &vec)
-{
- LLVector3 v(vec);
- v.normalize();
-
- angle *= 0.5f;
- F32 c, s;
- c = cosf(angle);
- s = sinf(angle);
-
- mQ[VX] = v.mV[VX]*s;
- mQ[VY] = v.mV[VY]*s;
- mQ[VZ] = v.mV[VZ]*s;
- mQ[VW] = c;
-
- normalize();
- return (*this);
-}
-
-const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector4 &vec)
-{
- LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
- v.normalize();
-
- F32 c, s;
- c = cosf(angle*0.5f);
- s = sinf(angle*0.5f);
-
- mQ[VX] = v.mV[VX]*s;
- mQ[VY] = v.mV[VY]*s;
- mQ[VZ] = v.mV[VZ]*s;
- mQ[VW] = c;
-
- normalize();
- 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)
-{
- LLVector3 vec(x, y, z);
- vec.normalize();
-
- angle *= 0.5f;
- F32 c, s;
- c = cosf(angle);
- s = sinf(angle);
-
- mQ[VX] = vec.mV[VX]*s;
- mQ[VY] = vec.mV[VY]*s;
- mQ[VZ] = vec.mV[VZ]*s;
- mQ[VW] = c;
-
- normalize();
- return (*this);
-}
-
-// deprecated
-const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
-{
- LLVector3 v(vec);
- v.normalize();
-
- angle *= 0.5f;
- F32 c, s;
- c = cosf(angle);
- s = sinf(angle);
-
- mQ[VX] = v.mV[VX]*s;
- mQ[VY] = v.mV[VY]*s;
- mQ[VZ] = v.mV[VZ]*s;
- mQ[VW] = c;
-
- normalize();
- return (*this);
-}
-
-const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
-{
- LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
- v.normalize();
-
- F32 c, s;
- c = cosf(angle*0.5f);
- s = sinf(angle*0.5f);
-
- mQ[VX] = v.mV[VX]*s;
- mQ[VY] = v.mV[VY]*s;
- mQ[VZ] = v.mV[VZ]*s;
- mQ[VW] = c;
-
- normalize();
- return (*this);
-}
-
-const LLQuaternion& LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
-{
- LLMatrix3 rot_mat(roll, pitch, yaw);
- rot_mat.orthogonalize();
- *this = rot_mat.quaternion();
-
- normalize();
- 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)
-{
- // Make a local copy of both vectors.
- LLVector3 vec_a = a;
- LLVector3 vec_b = b;
-
- // Make sure neither vector is zero length. Also normalize
- // the vectors while we are at it.
- F32 vec_a_mag = vec_a.normalize();
- F32 vec_b_mag = vec_b.normalize();
- if (vec_a_mag < F_APPROXIMATELY_ZERO ||
- vec_b_mag < F_APPROXIMATELY_ZERO)
- {
- // Can't calculate a rotation from this.
- // Just return ZERO_ROTATION instead.
- loadIdentity();
- return;
- }
-
- // Create an axis to rotate around, and the cos of the angle to rotate.
- LLVector3 axis = vec_a % vec_b;
- F32 cos_theta = vec_a * vec_b;
-
- // Check the angle between the vectors to see if they are parallel or anti-parallel.
- if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
- {
- // a and b are parallel. No rotation is necessary.
- loadIdentity();
- }
- else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
- {
- // a and b are anti-parallel.
- // Rotate 180 degrees around some orthogonal axis.
- // Find the projection of the x-axis onto a, and try
- // using the vector between the projection and the x-axis
- // as the orthogonal axis.
- LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
- LLVector3 ortho_axis(1.f, 0.f, 0.f);
- ortho_axis -= proj;
-
- // Turn this into an orthonormal axis.
- F32 ortho_length = ortho_axis.normalize();
- // If the axis' length is 0, then our guess at an orthogonal axis
- // was wrong (a is parallel to the x-axis).
- if (ortho_length < F_APPROXIMATELY_ZERO)
- {
- // Use the z-axis instead.
- ortho_axis.setVec(0.f, 0.f, 1.f);
- }
-
- // Construct a quaternion from this orthonormal axis.
- mQ[VX] = ortho_axis.mV[VX];
- mQ[VY] = ortho_axis.mV[VY];
- mQ[VZ] = ortho_axis.mV[VZ];
- mQ[VW] = 0.f;
- }
- else
- {
- // a and b are NOT parallel or anti-parallel.
- // Return the rotation between these vectors.
- F32 theta = (F32)acos(cos_theta);
-
- setAngleAxis(theta, axis);
- }
-}
-
-// 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
- int 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 cos_a = mQ[VW];
- if (cos_a > 1.0f) cos_a = 1.0f;
- if (cos_a < -1.0f) cos_a = -1.0f;
-
- F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
-
- if ( fabs( sin_a ) < 0.0005f )
- sin_a = 1.0f;
- else
- sin_a = 1.f/sin_a;
-
- F32 temp_angle = 2.0f * (F32) acos( cos_a );
- if (temp_angle > F_PI)
- {
- // The (angle,axis) pair should never have angles outside [PI, -PI]
- // since we want the _shortest_ (angle,axis) solution.
- // Since acos is defined for [0, PI], and we multiply by 2.0, we
- // can push the angle outside the acceptible range.
- // When this happens we set the angle to the other portion of a
- // full 2PI rotation, and negate the axis, which reverses the
- // direction of the rotation (by the right-hand rule).
- *angle = 2.f * F_PI - temp_angle;
- vec.mV[VX] = - mQ[VX] * sin_a;
- vec.mV[VY] = - mQ[VY] * sin_a;
- vec.mV[VZ] = - mQ[VZ] * sin_a;
- }
- else
- {
- *angle = temp_angle;
- vec.mV[VX] = mQ[VX] * sin_a;
- vec.mV[VY] = mQ[VY] * sin_a;
- vec.mV[VZ] = mQ[VZ] * sin_a;
- }
-}
-
-
-// quaternion does not need to be normalized
-void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
-{
- LLMatrix3 rot_mat(*this);
- rot_mat.orthogonalize();
- rot_mat.getEulerAngles(roll, pitch, yaw);
-
-// // NOTE: LLQuaternion's are actually inverted with respect to
-// // the matrices, so this code also assumes inverted quaternions
-// // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
-// // in reverse order (yaw,pitch,roll).
-// F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
-// F64 m20 = 2.0*(x*z-y*w);
-// if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
-// {
-// *roll = 0.0f;
-// *pitch = (F32)asin(m20);
-// *yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
-// }
-// else
-// {
-// *roll = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
-// *pitch = (F32)asin(m20);
-// *yaw = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
-// }
-}
-
-// Saves space by using the fact that our quaternions are normalized
-LLVector3 LLQuaternion::packToVector3() const
-{
- if( mQ[VW] >= 0 )
- {
- return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
- }
- else
- {
- return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
- }
-}
-
-// 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=viewergpl$
+ *
+ * Copyright (c) 2000-2009, Linden Research, Inc.
+ *
+ * Second Life Viewer Source Code
+ * The source code in this file ("Source Code") is provided by Linden Lab
+ * to you under the terms of the GNU General Public License, version 2.0
+ * ("GPL"), unless you have obtained a separate licensing agreement
+ * ("Other License"), formally executed by you and Linden Lab. Terms of
+ * the GPL can be found in doc/GPL-license.txt in this distribution, or
+ * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
+ *
+ * There are special exceptions to the terms and conditions of the GPL as
+ * it is applied to this Source Code. View the full text of the exception
+ * in the file doc/FLOSS-exception.txt in this software distribution, or
+ * online at
+ * http://secondlifegrid.net/programs/open_source/licensing/flossexception
+ *
+ * By copying, modifying or distributing this software, you acknowledge
+ * that you have read and understood your obligations described above,
+ * and agree to abide by those obligations.
+ *
+ * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
+ * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
+ * COMPLETENESS OR PERFORMANCE.
+ * $/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)
+{
+ LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
+ v.normalize();
+
+ F32 c, s;
+ c = cosf(angle*0.5f);
+ s = sinf(angle*0.5f);
+
+ mQ[VX] = v.mV[VX] * s;
+ mQ[VY] = v.mV[VY] * s;
+ mQ[VZ] = v.mV[VZ] * s;
+ mQ[VW] = c;
+ normalize();
+}
+
+LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
+{
+ LLVector3 v(vec);
+ v.normalize();
+
+ F32 c, s;
+ c = cosf(angle*0.5f);
+ s = sinf(angle*0.5f);
+
+ mQ[VX] = v.mV[VX] * s;
+ mQ[VY] = v.mV[VY] * s;
+ mQ[VZ] = v.mV[VZ] * s;
+ mQ[VW] = c;
+ normalize();
+}
+
+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();
+}
+
+// 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)
+{
+ LLVector3 vec(x, y, z);
+ vec.normalize();
+
+ angle *= 0.5f;
+ F32 c, s;
+ c = cosf(angle);
+ s = sinf(angle);
+
+ mQ[VX] = vec.mV[VX]*s;
+ mQ[VY] = vec.mV[VY]*s;
+ mQ[VZ] = vec.mV[VZ]*s;
+ mQ[VW] = c;
+
+ normalize();
+ return (*this);
+}
+
+const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector3 &vec)
+{
+ LLVector3 v(vec);
+ v.normalize();
+
+ angle *= 0.5f;
+ F32 c, s;
+ c = cosf(angle);
+ s = sinf(angle);
+
+ mQ[VX] = v.mV[VX]*s;
+ mQ[VY] = v.mV[VY]*s;
+ mQ[VZ] = v.mV[VZ]*s;
+ mQ[VW] = c;
+
+ normalize();
+ return (*this);
+}
+
+const LLQuaternion& LLQuaternion::setAngleAxis(F32 angle, const LLVector4 &vec)
+{
+ LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
+ v.normalize();
+
+ F32 c, s;
+ c = cosf(angle*0.5f);
+ s = sinf(angle*0.5f);
+
+ mQ[VX] = v.mV[VX]*s;
+ mQ[VY] = v.mV[VY]*s;
+ mQ[VZ] = v.mV[VZ]*s;
+ mQ[VW] = c;
+
+ normalize();
+ 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)
+{
+ LLVector3 vec(x, y, z);
+ vec.normalize();
+
+ angle *= 0.5f;
+ F32 c, s;
+ c = cosf(angle);
+ s = sinf(angle);
+
+ mQ[VX] = vec.mV[VX]*s;
+ mQ[VY] = vec.mV[VY]*s;
+ mQ[VZ] = vec.mV[VZ]*s;
+ mQ[VW] = c;
+
+ normalize();
+ return (*this);
+}
+
+// deprecated
+const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
+{
+ LLVector3 v(vec);
+ v.normalize();
+
+ angle *= 0.5f;
+ F32 c, s;
+ c = cosf(angle);
+ s = sinf(angle);
+
+ mQ[VX] = v.mV[VX]*s;
+ mQ[VY] = v.mV[VY]*s;
+ mQ[VZ] = v.mV[VZ]*s;
+ mQ[VW] = c;
+
+ normalize();
+ return (*this);
+}
+
+const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
+{
+ LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
+ v.normalize();
+
+ F32 c, s;
+ c = cosf(angle*0.5f);
+ s = sinf(angle*0.5f);
+
+ mQ[VX] = v.mV[VX]*s;
+ mQ[VY] = v.mV[VY]*s;
+ mQ[VZ] = v.mV[VZ]*s;
+ mQ[VW] = c;
+
+ normalize();
+ return (*this);
+}
+
+const LLQuaternion& LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
+{
+ LLMatrix3 rot_mat(roll, pitch, yaw);
+ rot_mat.orthogonalize();
+ *this = rot_mat.quaternion();
+
+ normalize();
+ 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)
+{
+ // Make a local copy of both vectors.
+ LLVector3 vec_a = a;
+ LLVector3 vec_b = b;
+
+ // Make sure neither vector is zero length. Also normalize
+ // the vectors while we are at it.
+ F32 vec_a_mag = vec_a.normalize();
+ F32 vec_b_mag = vec_b.normalize();
+ if (vec_a_mag < F_APPROXIMATELY_ZERO ||
+ vec_b_mag < F_APPROXIMATELY_ZERO)
+ {
+ // Can't calculate a rotation from this.
+ // Just return ZERO_ROTATION instead.
+ loadIdentity();
+ return;
+ }
+
+ // Create an axis to rotate around, and the cos of the angle to rotate.
+ LLVector3 axis = vec_a % vec_b;
+ F32 cos_theta = vec_a * vec_b;
+
+ // Check the angle between the vectors to see if they are parallel or anti-parallel.
+ if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
+ {
+ // a and b are parallel. No rotation is necessary.
+ loadIdentity();
+ }
+ else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
+ {
+ // a and b are anti-parallel.
+ // Rotate 180 degrees around some orthogonal axis.
+ // Find the projection of the x-axis onto a, and try
+ // using the vector between the projection and the x-axis
+ // as the orthogonal axis.
+ LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
+ LLVector3 ortho_axis(1.f, 0.f, 0.f);
+ ortho_axis -= proj;
+
+ // Turn this into an orthonormal axis.
+ F32 ortho_length = ortho_axis.normalize();
+ // If the axis' length is 0, then our guess at an orthogonal axis
+ // was wrong (a is parallel to the x-axis).
+ if (ortho_length < F_APPROXIMATELY_ZERO)
+ {
+ // Use the z-axis instead.
+ ortho_axis.setVec(0.f, 0.f, 1.f);
+ }
+
+ // Construct a quaternion from this orthonormal axis.
+ mQ[VX] = ortho_axis.mV[VX];
+ mQ[VY] = ortho_axis.mV[VY];
+ mQ[VZ] = ortho_axis.mV[VZ];
+ mQ[VW] = 0.f;
+ }
+ else
+ {
+ // a and b are NOT parallel or anti-parallel.
+ // Return the rotation between these vectors.
+ F32 theta = (F32)acos(cos_theta);
+
+ setAngleAxis(theta, axis);
+ }
+}
+
+// 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
+ int 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 cos_a = mQ[VW];
+ if (cos_a > 1.0f) cos_a = 1.0f;
+ if (cos_a < -1.0f) cos_a = -1.0f;
+
+ F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
+
+ if ( fabs( sin_a ) < 0.0005f )
+ sin_a = 1.0f;
+ else
+ sin_a = 1.f/sin_a;
+
+ F32 temp_angle = 2.0f * (F32) acos( cos_a );
+ if (temp_angle > F_PI)
+ {
+ // The (angle,axis) pair should never have angles outside [PI, -PI]
+ // since we want the _shortest_ (angle,axis) solution.
+ // Since acos is defined for [0, PI], and we multiply by 2.0, we
+ // can push the angle outside the acceptible range.
+ // When this happens we set the angle to the other portion of a
+ // full 2PI rotation, and negate the axis, which reverses the
+ // direction of the rotation (by the right-hand rule).
+ *angle = 2.f * F_PI - temp_angle;
+ vec.mV[VX] = - mQ[VX] * sin_a;
+ vec.mV[VY] = - mQ[VY] * sin_a;
+ vec.mV[VZ] = - mQ[VZ] * sin_a;
+ }
+ else
+ {
+ *angle = temp_angle;
+ vec.mV[VX] = mQ[VX] * sin_a;
+ vec.mV[VY] = mQ[VY] * sin_a;
+ vec.mV[VZ] = mQ[VZ] * sin_a;
+ }
+}
+
+
+// quaternion does not need to be normalized
+void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
+{
+ LLMatrix3 rot_mat(*this);
+ rot_mat.orthogonalize();
+ rot_mat.getEulerAngles(roll, pitch, yaw);
+
+// // NOTE: LLQuaternion's are actually inverted with respect to
+// // the matrices, so this code also assumes inverted quaternions
+// // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
+// // in reverse order (yaw,pitch,roll).
+// F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
+// F64 m20 = 2.0*(x*z-y*w);
+// if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
+// {
+// *roll = 0.0f;
+// *pitch = (F32)asin(m20);
+// *yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
+// }
+// else
+// {
+// *roll = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
+// *pitch = (F32)asin(m20);
+// *yaw = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
+// }
+}
+
+// Saves space by using the fact that our quaternions are normalized
+LLVector3 LLQuaternion::packToVector3() const
+{
+ if( mQ[VW] >= 0 )
+ {
+ return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
+ }
+ else
+ {
+ return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
+ }
+}
+
+// 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
diff --git a/indra/llmath/llquaternion.h b/indra/llmath/llquaternion.h
index bbd4326483..a7bb09fae3 100644
--- a/indra/llmath/llquaternion.h
+++ b/indra/llmath/llquaternion.h
@@ -1,594 +1,594 @@
-/**
- * @file llquaternion.h
- * @brief LLQuaternion class header file.
- *
- * $LicenseInfo:firstyear=2000&license=viewergpl$
- *
- * Copyright (c) 2000-2009, Linden Research, Inc.
- *
- * Second Life Viewer Source Code
- * The source code in this file ("Source Code") is provided by Linden Lab
- * to you under the terms of the GNU General Public License, version 2.0
- * ("GPL"), unless you have obtained a separate licensing agreement
- * ("Other License"), formally executed by you and Linden Lab. Terms of
- * the GPL can be found in doc/GPL-license.txt in this distribution, or
- * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
- *
- * There are special exceptions to the terms and conditions of the GPL as
- * it is applied to this Source Code. View the full text of the exception
- * in the file doc/FLOSS-exception.txt in this software distribution, or
- * online at
- * http://secondlifegrid.net/programs/open_source/licensing/flossexception
- *
- * By copying, modifying or distributing this software, you acknowledge
- * that you have read and understood your obligations described above,
- * and agree to abide by those obligations.
- *
- * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
- * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
- * COMPLETENESS OR PERFORMANCE.
- * $/LicenseInfo$
- */
-
-#ifndef LLQUATERNION_H
-#define LLQUATERNION_H
-
-#include <iostream>
-
-#ifndef LLMATH_H //enforce specific include order to avoid tangling inline dependencies
-#error "Please include llmath.h first."
-#endif
-
-class LLVector4;
-class LLVector3;
-class LLVector3d;
-class LLMatrix4;
-class LLMatrix3;
-
-// NOTA BENE: Quaternion code is written assuming Unit Quaternions!!!!
-// Moreover, it is written assuming that all vectors and matricies
-// passed as arguments are normalized and unitary respectively.
-// VERY VERY VERY VERY BAD THINGS will happen if these assumptions fail.
-
-static const U32 LENGTHOFQUAT = 4;
-
-class LLQuaternion
-{
-public:
- F32 mQ[LENGTHOFQUAT];
-
- static const LLQuaternion DEFAULT;
-
- LLQuaternion(); // Initializes Quaternion to (0,0,0,1)
- explicit LLQuaternion(const LLMatrix4 &mat); // Initializes Quaternion from Matrix4
- explicit LLQuaternion(const LLMatrix3 &mat); // Initializes Quaternion from Matrix3
- LLQuaternion(F32 x, F32 y, F32 z, F32 w); // Initializes Quaternion to normalize(x, y, z, w)
- LLQuaternion(F32 angle, const LLVector4 &vec); // Initializes Quaternion to axis_angle2quat(angle, vec)
- LLQuaternion(F32 angle, const LLVector3 &vec); // Initializes Quaternion to axis_angle2quat(angle, vec)
- LLQuaternion(const F32 *q); // Initializes Quaternion to normalize(x, y, z, w)
- LLQuaternion(const LLVector3 &x_axis,
- const LLVector3 &y_axis,
- const LLVector3 &z_axis); // Initializes Quaternion from Matrix3 = [x_axis ; y_axis ; z_axis]
-
- BOOL isIdentity() const;
- BOOL isNotIdentity() const;
- BOOL isFinite() const; // checks to see if all values of LLQuaternion are finite
- void quantize16(F32 lower, F32 upper); // changes the vector to reflect quatization
- void quantize8(F32 lower, F32 upper); // changes the vector to reflect quatization
- void loadIdentity(); // Loads the quaternion that represents the identity rotation
-
- const LLQuaternion& set(F32 x, F32 y, F32 z, F32 w); // Sets Quaternion to normalize(x, y, z, w)
- const LLQuaternion& set(const LLQuaternion &quat); // Copies Quaternion
- const LLQuaternion& set(const F32 *q); // Sets Quaternion to normalize(quat[VX], quat[VY], quat[VZ], quat[VW])
- const LLQuaternion& set(const LLMatrix3 &mat); // Sets Quaternion to mat2quat(mat)
- const LLQuaternion& set(const LLMatrix4 &mat); // Sets Quaternion to mat2quat(mat)
-
- const LLQuaternion& setAngleAxis(F32 angle, F32 x, F32 y, F32 z); // Sets Quaternion to axis_angle2quat(angle, x, y, z)
- const LLQuaternion& setAngleAxis(F32 angle, const LLVector3 &vec); // Sets Quaternion to axis_angle2quat(angle, vec)
- const LLQuaternion& setAngleAxis(F32 angle, const LLVector4 &vec); // Sets Quaternion to axis_angle2quat(angle, vec)
- const LLQuaternion& setEulerAngles(F32 roll, F32 pitch, F32 yaw); // Sets Quaternion to euler2quat(pitch, yaw, roll)
-
- const LLQuaternion& setQuatInit(F32 x, F32 y, F32 z, F32 w); // deprecated
- const LLQuaternion& setQuat(const LLQuaternion &quat); // deprecated
- const LLQuaternion& setQuat(const F32 *q); // deprecated
- const LLQuaternion& setQuat(const LLMatrix3 &mat); // deprecated
- const LLQuaternion& setQuat(const LLMatrix4 &mat); // deprecated
- const LLQuaternion& setQuat(F32 angle, F32 x, F32 y, F32 z); // deprecated
- const LLQuaternion& setQuat(F32 angle, const LLVector3 &vec); // deprecated
- const LLQuaternion& setQuat(F32 angle, const LLVector4 &vec); // deprecated
- const LLQuaternion& setQuat(F32 roll, F32 pitch, F32 yaw); // deprecated
-
- LLMatrix4 getMatrix4(void) const; // Returns the Matrix4 equivalent of Quaternion
- LLMatrix3 getMatrix3(void) const; // Returns the Matrix3 equivalent of Quaternion
- void getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const; // returns rotation in radians about axis x,y,z
- void getAngleAxis(F32* angle, LLVector3 &vec) const;
- void getEulerAngles(F32 *roll, F32* pitch, F32 *yaw) const;
-
- F32 normalize(); // Normalizes Quaternion and returns magnitude
- F32 normQuat(); // deprecated
-
- const LLQuaternion& conjugate(void); // Conjugates Quaternion and returns result
- const LLQuaternion& conjQuat(void); // deprecated
-
- // Other useful methods
- const LLQuaternion& transpose(); // transpose (same as conjugate)
- const LLQuaternion& transQuat(); // deprecated
-
- void shortestArc(const LLVector3 &a, const LLVector3 &b); // shortest rotation from a to b
- const LLQuaternion& constrain(F32 radians); // constrains rotation to a cone angle specified in radians
-
- // Standard operators
- friend std::ostream& operator<<(std::ostream &s, const LLQuaternion &a); // Prints a
- friend LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b); // Addition
- friend LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b); // Subtraction
- friend LLQuaternion operator-(const LLQuaternion &a); // Negation
- friend LLQuaternion operator*(F32 a, const LLQuaternion &q); // Scale
- friend LLQuaternion operator*(const LLQuaternion &q, F32 b); // Scale
- friend LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b); // Returns a * b
- friend LLQuaternion operator~(const LLQuaternion &a); // Returns a* (Conjugate of a)
- bool operator==(const LLQuaternion &b) const; // Returns a == b
- bool operator!=(const LLQuaternion &b) const; // Returns a != b
-
- friend const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b); // Returns a * b
-
- friend LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot); // Rotates a by rot
- friend LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot); // Rotates a by rot
- friend LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot); // Rotates a by rot
-
- // Non-standard operators
- friend F32 dot(const LLQuaternion &a, const LLQuaternion &b);
- friend LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q); // linear interpolation (t = 0 to 1) from p to q
- friend LLQuaternion lerp(F32 t, const LLQuaternion &q); // linear interpolation (t = 0 to 1) from identity to q
- friend LLQuaternion slerp(F32 t, const LLQuaternion &p, const LLQuaternion &q); // spherical linear interpolation from p to q
- friend LLQuaternion slerp(F32 t, const LLQuaternion &q); // spherical linear interpolation from identity to q
- friend LLQuaternion nlerp(F32 t, const LLQuaternion &p, const LLQuaternion &q); // normalized linear interpolation from p to q
- friend LLQuaternion nlerp(F32 t, const LLQuaternion &q); // normalized linear interpolation from p to q
-
- LLVector3 packToVector3() const; // Saves space by using the fact that our quaternions are normalized
- void unpackFromVector3(const LLVector3& vec); // Saves space by using the fact that our quaternions are normalized
-
- enum Order {
- XYZ = 0,
- YZX = 1,
- ZXY = 2,
- XZY = 3,
- YXZ = 4,
- ZYX = 5
- };
- // Creates a quaternions from maya's rotation representation,
- // which is 3 rotations (in DEGREES) in the specified order
- friend LLQuaternion mayaQ(F32 x, F32 y, F32 z, Order order);
-
- // Conversions between Order and strings like "xyz" or "ZYX"
- friend const char *OrderToString( const Order order );
- friend Order StringToOrder( const char *str );
-
- static BOOL parseQuat(const std::string& buf, LLQuaternion* value);
-
- // For debugging, only
- //static U32 mMultCount;
-};
-
-// checker
-inline BOOL LLQuaternion::isFinite() const
-{
- return (llfinite(mQ[VX]) && llfinite(mQ[VY]) && llfinite(mQ[VZ]) && llfinite(mQ[VS]));
-}
-
-inline BOOL LLQuaternion::isIdentity() const
-{
- return
- ( mQ[VX] == 0.f ) &&
- ( mQ[VY] == 0.f ) &&
- ( mQ[VZ] == 0.f ) &&
- ( mQ[VS] == 1.f );
-}
-
-inline BOOL LLQuaternion::isNotIdentity() const
-{
- return
- ( mQ[VX] != 0.f ) ||
- ( mQ[VY] != 0.f ) ||
- ( mQ[VZ] != 0.f ) ||
- ( mQ[VS] != 1.f );
-}
-
-
-
-inline LLQuaternion::LLQuaternion(void)
-{
- mQ[VX] = 0.f;
- mQ[VY] = 0.f;
- mQ[VZ] = 0.f;
- mQ[VS] = 1.f;
-}
-
-inline LLQuaternion::LLQuaternion(F32 x, F32 y, F32 z, F32 w)
-{
- mQ[VX] = x;
- mQ[VY] = y;
- mQ[VZ] = z;
- mQ[VS] = w;
-
- //RN: don't normalize this case as its used mainly for temporaries during calculations
- //normalize();
- /*
- F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
- mag -= 1.f;
- mag = fabs(mag);
- llassert(mag < 10.f*FP_MAG_THRESHOLD);
- */
-}
-
-inline LLQuaternion::LLQuaternion(const F32 *q)
-{
- mQ[VX] = q[VX];
- mQ[VY] = q[VY];
- mQ[VZ] = q[VZ];
- mQ[VS] = q[VW];
-
- normalize();
- /*
- F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
- mag -= 1.f;
- mag = fabs(mag);
- llassert(mag < FP_MAG_THRESHOLD);
- */
-}
-
-
-inline void LLQuaternion::loadIdentity()
-{
- mQ[VX] = 0.0f;
- mQ[VY] = 0.0f;
- mQ[VZ] = 0.0f;
- mQ[VW] = 1.0f;
-}
-
-
-inline const LLQuaternion& LLQuaternion::set(F32 x, F32 y, F32 z, F32 w)
-{
- mQ[VX] = x;
- mQ[VY] = y;
- mQ[VZ] = z;
- mQ[VS] = w;
- normalize();
- return (*this);
-}
-
-inline const LLQuaternion& LLQuaternion::set(const LLQuaternion &quat)
-{
- mQ[VX] = quat.mQ[VX];
- mQ[VY] = quat.mQ[VY];
- mQ[VZ] = quat.mQ[VZ];
- mQ[VW] = quat.mQ[VW];
- normalize();
- return (*this);
-}
-
-inline const LLQuaternion& LLQuaternion::set(const F32 *q)
-{
- mQ[VX] = q[VX];
- mQ[VY] = q[VY];
- mQ[VZ] = q[VZ];
- mQ[VS] = q[VW];
- normalize();
- return (*this);
-}
-
-
-// deprecated
-inline const LLQuaternion& LLQuaternion::setQuatInit(F32 x, F32 y, F32 z, F32 w)
-{
- mQ[VX] = x;
- mQ[VY] = y;
- mQ[VZ] = z;
- mQ[VS] = w;
- normalize();
- return (*this);
-}
-
-// deprecated
-inline const LLQuaternion& LLQuaternion::setQuat(const LLQuaternion &quat)
-{
- mQ[VX] = quat.mQ[VX];
- mQ[VY] = quat.mQ[VY];
- mQ[VZ] = quat.mQ[VZ];
- mQ[VW] = quat.mQ[VW];
- normalize();
- return (*this);
-}
-
-// deprecated
-inline const LLQuaternion& LLQuaternion::setQuat(const F32 *q)
-{
- mQ[VX] = q[VX];
- mQ[VY] = q[VY];
- mQ[VZ] = q[VZ];
- mQ[VS] = q[VW];
- normalize();
- return (*this);
-}
-
-// There may be a cheaper way that avoids the sqrt.
-// Does sin_a = VX*VX + VY*VY + VZ*VZ?
-// Copied from Matrix and Quaternion FAQ 1.12
-inline void LLQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const
-{
- F32 cos_a = mQ[VW];
- if (cos_a > 1.0f) cos_a = 1.0f;
- if (cos_a < -1.0f) cos_a = -1.0f;
-
- F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
-
- if ( fabs( sin_a ) < 0.0005f )
- sin_a = 1.0f;
- else
- sin_a = 1.f/sin_a;
-
- F32 temp_angle = 2.0f * (F32) acos( cos_a );
- if (temp_angle > F_PI)
- {
- // The (angle,axis) pair should never have angles outside [PI, -PI]
- // since we want the _shortest_ (angle,axis) solution.
- // Since acos is defined for [0, PI], and we multiply by 2.0, we
- // can push the angle outside the acceptible range.
- // When this happens we set the angle to the other portion of a
- // full 2PI rotation, and negate the axis, which reverses the
- // direction of the rotation (by the right-hand rule).
- *angle = 2.f * F_PI - temp_angle;
- *x = - mQ[VX] * sin_a;
- *y = - mQ[VY] * sin_a;
- *z = - mQ[VZ] * sin_a;
- }
- else
- {
- *angle = temp_angle;
- *x = mQ[VX] * sin_a;
- *y = mQ[VY] * sin_a;
- *z = mQ[VZ] * sin_a;
- }
-}
-
-inline const LLQuaternion& LLQuaternion::conjugate()
-{
- mQ[VX] *= -1.f;
- mQ[VY] *= -1.f;
- mQ[VZ] *= -1.f;
- return (*this);
-}
-
-inline const LLQuaternion& LLQuaternion::conjQuat()
-{
- mQ[VX] *= -1.f;
- mQ[VY] *= -1.f;
- mQ[VZ] *= -1.f;
- return (*this);
-}
-
-// Transpose
-inline const LLQuaternion& LLQuaternion::transpose()
-{
- mQ[VX] *= -1.f;
- mQ[VY] *= -1.f;
- mQ[VZ] *= -1.f;
- return (*this);
-}
-
-// deprecated
-inline const LLQuaternion& LLQuaternion::transQuat()
-{
- mQ[VX] *= -1.f;
- mQ[VY] *= -1.f;
- mQ[VZ] *= -1.f;
- return (*this);
-}
-
-
-inline LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b)
-{
- return LLQuaternion(
- a.mQ[VX] + b.mQ[VX],
- a.mQ[VY] + b.mQ[VY],
- a.mQ[VZ] + b.mQ[VZ],
- a.mQ[VW] + b.mQ[VW] );
-}
-
-
-inline LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b)
-{
- return LLQuaternion(
- a.mQ[VX] - b.mQ[VX],
- a.mQ[VY] - b.mQ[VY],
- a.mQ[VZ] - b.mQ[VZ],
- a.mQ[VW] - b.mQ[VW] );
-}
-
-
-inline LLQuaternion operator-(const LLQuaternion &a)
-{
- return LLQuaternion(
- -a.mQ[VX],
- -a.mQ[VY],
- -a.mQ[VZ],
- -a.mQ[VW] );
-}
-
-
-inline LLQuaternion operator*(F32 a, const LLQuaternion &q)
-{
- return LLQuaternion(
- a * q.mQ[VX],
- a * q.mQ[VY],
- a * q.mQ[VZ],
- a * q.mQ[VW] );
-}
-
-
-inline LLQuaternion operator*(const LLQuaternion &q, F32 a)
-{
- return LLQuaternion(
- a * q.mQ[VX],
- a * q.mQ[VY],
- a * q.mQ[VZ],
- a * q.mQ[VW] );
-}
-
-inline LLQuaternion operator~(const LLQuaternion &a)
-{
- LLQuaternion q(a);
- q.conjQuat();
- return q;
-}
-
-inline bool LLQuaternion::operator==(const LLQuaternion &b) const
-{
- return ( (mQ[VX] == b.mQ[VX])
- &&(mQ[VY] == b.mQ[VY])
- &&(mQ[VZ] == b.mQ[VZ])
- &&(mQ[VS] == b.mQ[VS]));
-}
-
-inline bool LLQuaternion::operator!=(const LLQuaternion &b) const
-{
- return ( (mQ[VX] != b.mQ[VX])
- ||(mQ[VY] != b.mQ[VY])
- ||(mQ[VZ] != b.mQ[VZ])
- ||(mQ[VS] != b.mQ[VS]));
-}
-
-inline const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b)
-{
-#if 1
- 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]
- );
- a = q;
-#else
- a = a * b;
-#endif
- return a;
-}
-
-const F32 ONE_PART_IN_A_MILLION = 0.000001f;
-
-inline F32 LLQuaternion::normalize()
-{
- F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
-
- if (mag > FP_MAG_THRESHOLD)
- {
- // Floating point error can prevent some quaternions from achieving
- // exact unity length. When trying to renormalize such quaternions we
- // can oscillate between multiple quantized states. To prevent such
- // drifts we only renomalize if the length is far enough from unity.
- if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
- {
- F32 oomag = 1.f/mag;
- mQ[VX] *= oomag;
- mQ[VY] *= oomag;
- mQ[VZ] *= oomag;
- mQ[VS] *= oomag;
- }
- }
- else
- {
- // we were given a very bad quaternion so we set it to identity
- mQ[VX] = 0.f;
- mQ[VY] = 0.f;
- mQ[VZ] = 0.f;
- mQ[VS] = 1.f;
- }
-
- return mag;
-}
-
-// deprecated
-inline F32 LLQuaternion::normQuat()
-{
- F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
-
- if (mag > FP_MAG_THRESHOLD)
- {
- if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
- {
- // only renormalize if length not close enough to 1.0 already
- F32 oomag = 1.f/mag;
- mQ[VX] *= oomag;
- mQ[VY] *= oomag;
- mQ[VZ] *= oomag;
- mQ[VS] *= oomag;
- }
- }
- else
- {
- mQ[VX] = 0.f;
- mQ[VY] = 0.f;
- mQ[VZ] = 0.f;
- mQ[VS] = 1.f;
- }
-
- return mag;
-}
-
-LLQuaternion::Order StringToOrder( const char *str );
-
-// Some notes about Quaternions
-
-// What is a Quaternion?
-// ---------------------
-// A quaternion is a point in 4-dimensional complex space.
-// Q = { Qx, Qy, Qz, Qw }
-//
-//
-// Why Quaternions?
-// ----------------
-// The set of quaternions that make up the the 4-D unit sphere
-// can be mapped to the set of all rotations in 3-D space. Sometimes
-// it is easier to describe/manipulate rotations in quaternion space
-// than rotation-matrix space.
-//
-//
-// How Quaternions?
-// ----------------
-// In order to take advantage of quaternions we need to know how to
-// go from rotation-matricies to quaternions and back. We also have
-// to agree what variety of rotations we're generating.
-//
-// Consider the equation... v' = v * R
-//
-// There are two ways to think about rotations of vectors.
-// 1) v' is the same vector in a different reference frame
-// 2) v' is a new vector in the same reference frame
-//
-// bookmark -- which way are we using?
-//
-//
-// Quaternion from Angle-Axis:
-// ---------------------------
-// Suppose we wanted to represent a rotation of some angle (theta)
-// about some axis ({Ax, Ay, Az})...
-//
-// axis of rotation = {Ax, Ay, Az}
-// angle_of_rotation = theta
-//
-// s = sin(0.5 * theta)
-// c = cos(0.5 * theta)
-// Q = { s * Ax, s * Ay, s * Az, c }
-//
-//
-// 3x3 Matrix from Quaternion
-// --------------------------
-//
-// | |
-// | 1 - 2 * (y^2 + z^2) 2 * (x * y + z * w) 2 * (y * w - x * z) |
-// | |
-// M = | 2 * (x * y - z * w) 1 - 2 * (x^2 + z^2) 2 * (y * z + x * w) |
-// | |
-// | 2 * (x * z + y * w) 2 * (y * z - x * w) 1 - 2 * (x^2 + y^2) |
-// | |
-
-#endif
+/**
+ * @file llquaternion.h
+ * @brief LLQuaternion class header file.
+ *
+ * $LicenseInfo:firstyear=2000&license=viewergpl$
+ *
+ * Copyright (c) 2000-2009, Linden Research, Inc.
+ *
+ * Second Life Viewer Source Code
+ * The source code in this file ("Source Code") is provided by Linden Lab
+ * to you under the terms of the GNU General Public License, version 2.0
+ * ("GPL"), unless you have obtained a separate licensing agreement
+ * ("Other License"), formally executed by you and Linden Lab. Terms of
+ * the GPL can be found in doc/GPL-license.txt in this distribution, or
+ * online at http://secondlifegrid.net/programs/open_source/licensing/gplv2
+ *
+ * There are special exceptions to the terms and conditions of the GPL as
+ * it is applied to this Source Code. View the full text of the exception
+ * in the file doc/FLOSS-exception.txt in this software distribution, or
+ * online at
+ * http://secondlifegrid.net/programs/open_source/licensing/flossexception
+ *
+ * By copying, modifying or distributing this software, you acknowledge
+ * that you have read and understood your obligations described above,
+ * and agree to abide by those obligations.
+ *
+ * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
+ * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
+ * COMPLETENESS OR PERFORMANCE.
+ * $/LicenseInfo$
+ */
+
+#ifndef LLQUATERNION_H
+#define LLQUATERNION_H
+
+#include <iostream>
+
+#ifndef LLMATH_H //enforce specific include order to avoid tangling inline dependencies
+#error "Please include llmath.h first."
+#endif
+
+class LLVector4;
+class LLVector3;
+class LLVector3d;
+class LLMatrix4;
+class LLMatrix3;
+
+// NOTA BENE: Quaternion code is written assuming Unit Quaternions!!!!
+// Moreover, it is written assuming that all vectors and matricies
+// passed as arguments are normalized and unitary respectively.
+// VERY VERY VERY VERY BAD THINGS will happen if these assumptions fail.
+
+static const U32 LENGTHOFQUAT = 4;
+
+class LLQuaternion
+{
+public:
+ F32 mQ[LENGTHOFQUAT];
+
+ static const LLQuaternion DEFAULT;
+
+ LLQuaternion(); // Initializes Quaternion to (0,0,0,1)
+ explicit LLQuaternion(const LLMatrix4 &mat); // Initializes Quaternion from Matrix4
+ explicit LLQuaternion(const LLMatrix3 &mat); // Initializes Quaternion from Matrix3
+ LLQuaternion(F32 x, F32 y, F32 z, F32 w); // Initializes Quaternion to normalize(x, y, z, w)
+ LLQuaternion(F32 angle, const LLVector4 &vec); // Initializes Quaternion to axis_angle2quat(angle, vec)
+ LLQuaternion(F32 angle, const LLVector3 &vec); // Initializes Quaternion to axis_angle2quat(angle, vec)
+ LLQuaternion(const F32 *q); // Initializes Quaternion to normalize(x, y, z, w)
+ LLQuaternion(const LLVector3 &x_axis,
+ const LLVector3 &y_axis,
+ const LLVector3 &z_axis); // Initializes Quaternion from Matrix3 = [x_axis ; y_axis ; z_axis]
+
+ BOOL isIdentity() const;
+ BOOL isNotIdentity() const;
+ BOOL isFinite() const; // checks to see if all values of LLQuaternion are finite
+ void quantize16(F32 lower, F32 upper); // changes the vector to reflect quatization
+ void quantize8(F32 lower, F32 upper); // changes the vector to reflect quatization
+ void loadIdentity(); // Loads the quaternion that represents the identity rotation
+
+ const LLQuaternion& set(F32 x, F32 y, F32 z, F32 w); // Sets Quaternion to normalize(x, y, z, w)
+ const LLQuaternion& set(const LLQuaternion &quat); // Copies Quaternion
+ const LLQuaternion& set(const F32 *q); // Sets Quaternion to normalize(quat[VX], quat[VY], quat[VZ], quat[VW])
+ const LLQuaternion& set(const LLMatrix3 &mat); // Sets Quaternion to mat2quat(mat)
+ const LLQuaternion& set(const LLMatrix4 &mat); // Sets Quaternion to mat2quat(mat)
+
+ const LLQuaternion& setAngleAxis(F32 angle, F32 x, F32 y, F32 z); // Sets Quaternion to axis_angle2quat(angle, x, y, z)
+ const LLQuaternion& setAngleAxis(F32 angle, const LLVector3 &vec); // Sets Quaternion to axis_angle2quat(angle, vec)
+ const LLQuaternion& setAngleAxis(F32 angle, const LLVector4 &vec); // Sets Quaternion to axis_angle2quat(angle, vec)
+ const LLQuaternion& setEulerAngles(F32 roll, F32 pitch, F32 yaw); // Sets Quaternion to euler2quat(pitch, yaw, roll)
+
+ const LLQuaternion& setQuatInit(F32 x, F32 y, F32 z, F32 w); // deprecated
+ const LLQuaternion& setQuat(const LLQuaternion &quat); // deprecated
+ const LLQuaternion& setQuat(const F32 *q); // deprecated
+ const LLQuaternion& setQuat(const LLMatrix3 &mat); // deprecated
+ const LLQuaternion& setQuat(const LLMatrix4 &mat); // deprecated
+ const LLQuaternion& setQuat(F32 angle, F32 x, F32 y, F32 z); // deprecated
+ const LLQuaternion& setQuat(F32 angle, const LLVector3 &vec); // deprecated
+ const LLQuaternion& setQuat(F32 angle, const LLVector4 &vec); // deprecated
+ const LLQuaternion& setQuat(F32 roll, F32 pitch, F32 yaw); // deprecated
+
+ LLMatrix4 getMatrix4(void) const; // Returns the Matrix4 equivalent of Quaternion
+ LLMatrix3 getMatrix3(void) const; // Returns the Matrix3 equivalent of Quaternion
+ void getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const; // returns rotation in radians about axis x,y,z
+ void getAngleAxis(F32* angle, LLVector3 &vec) const;
+ void getEulerAngles(F32 *roll, F32* pitch, F32 *yaw) const;
+
+ F32 normalize(); // Normalizes Quaternion and returns magnitude
+ F32 normQuat(); // deprecated
+
+ const LLQuaternion& conjugate(void); // Conjugates Quaternion and returns result
+ const LLQuaternion& conjQuat(void); // deprecated
+
+ // Other useful methods
+ const LLQuaternion& transpose(); // transpose (same as conjugate)
+ const LLQuaternion& transQuat(); // deprecated
+
+ void shortestArc(const LLVector3 &a, const LLVector3 &b); // shortest rotation from a to b
+ const LLQuaternion& constrain(F32 radians); // constrains rotation to a cone angle specified in radians
+
+ // Standard operators
+ friend std::ostream& operator<<(std::ostream &s, const LLQuaternion &a); // Prints a
+ friend LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b); // Addition
+ friend LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b); // Subtraction
+ friend LLQuaternion operator-(const LLQuaternion &a); // Negation
+ friend LLQuaternion operator*(F32 a, const LLQuaternion &q); // Scale
+ friend LLQuaternion operator*(const LLQuaternion &q, F32 b); // Scale
+ friend LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b); // Returns a * b
+ friend LLQuaternion operator~(const LLQuaternion &a); // Returns a* (Conjugate of a)
+ bool operator==(const LLQuaternion &b) const; // Returns a == b
+ bool operator!=(const LLQuaternion &b) const; // Returns a != b
+
+ friend const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b); // Returns a * b
+
+ friend LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot); // Rotates a by rot
+ friend LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot); // Rotates a by rot
+ friend LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot); // Rotates a by rot
+
+ // Non-standard operators
+ friend F32 dot(const LLQuaternion &a, const LLQuaternion &b);
+ friend LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q); // linear interpolation (t = 0 to 1) from p to q
+ friend LLQuaternion lerp(F32 t, const LLQuaternion &q); // linear interpolation (t = 0 to 1) from identity to q
+ friend LLQuaternion slerp(F32 t, const LLQuaternion &p, const LLQuaternion &q); // spherical linear interpolation from p to q
+ friend LLQuaternion slerp(F32 t, const LLQuaternion &q); // spherical linear interpolation from identity to q
+ friend LLQuaternion nlerp(F32 t, const LLQuaternion &p, const LLQuaternion &q); // normalized linear interpolation from p to q
+ friend LLQuaternion nlerp(F32 t, const LLQuaternion &q); // normalized linear interpolation from p to q
+
+ LLVector3 packToVector3() const; // Saves space by using the fact that our quaternions are normalized
+ void unpackFromVector3(const LLVector3& vec); // Saves space by using the fact that our quaternions are normalized
+
+ enum Order {
+ XYZ = 0,
+ YZX = 1,
+ ZXY = 2,
+ XZY = 3,
+ YXZ = 4,
+ ZYX = 5
+ };
+ // Creates a quaternions from maya's rotation representation,
+ // which is 3 rotations (in DEGREES) in the specified order
+ friend LLQuaternion mayaQ(F32 x, F32 y, F32 z, Order order);
+
+ // Conversions between Order and strings like "xyz" or "ZYX"
+ friend const char *OrderToString( const Order order );
+ friend Order StringToOrder( const char *str );
+
+ static BOOL parseQuat(const std::string& buf, LLQuaternion* value);
+
+ // For debugging, only
+ //static U32 mMultCount;
+};
+
+// checker
+inline BOOL LLQuaternion::isFinite() const
+{
+ return (llfinite(mQ[VX]) && llfinite(mQ[VY]) && llfinite(mQ[VZ]) && llfinite(mQ[VS]));
+}
+
+inline BOOL LLQuaternion::isIdentity() const
+{
+ return
+ ( mQ[VX] == 0.f ) &&
+ ( mQ[VY] == 0.f ) &&
+ ( mQ[VZ] == 0.f ) &&
+ ( mQ[VS] == 1.f );
+}
+
+inline BOOL LLQuaternion::isNotIdentity() const
+{
+ return
+ ( mQ[VX] != 0.f ) ||
+ ( mQ[VY] != 0.f ) ||
+ ( mQ[VZ] != 0.f ) ||
+ ( mQ[VS] != 1.f );
+}
+
+
+
+inline LLQuaternion::LLQuaternion(void)
+{
+ mQ[VX] = 0.f;
+ mQ[VY] = 0.f;
+ mQ[VZ] = 0.f;
+ mQ[VS] = 1.f;
+}
+
+inline LLQuaternion::LLQuaternion(F32 x, F32 y, F32 z, F32 w)
+{
+ mQ[VX] = x;
+ mQ[VY] = y;
+ mQ[VZ] = z;
+ mQ[VS] = w;
+
+ //RN: don't normalize this case as its used mainly for temporaries during calculations
+ //normalize();
+ /*
+ F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
+ mag -= 1.f;
+ mag = fabs(mag);
+ llassert(mag < 10.f*FP_MAG_THRESHOLD);
+ */
+}
+
+inline LLQuaternion::LLQuaternion(const F32 *q)
+{
+ mQ[VX] = q[VX];
+ mQ[VY] = q[VY];
+ mQ[VZ] = q[VZ];
+ mQ[VS] = q[VW];
+
+ normalize();
+ /*
+ F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
+ mag -= 1.f;
+ mag = fabs(mag);
+ llassert(mag < FP_MAG_THRESHOLD);
+ */
+}
+
+
+inline void LLQuaternion::loadIdentity()
+{
+ mQ[VX] = 0.0f;
+ mQ[VY] = 0.0f;
+ mQ[VZ] = 0.0f;
+ mQ[VW] = 1.0f;
+}
+
+
+inline const LLQuaternion& LLQuaternion::set(F32 x, F32 y, F32 z, F32 w)
+{
+ mQ[VX] = x;
+ mQ[VY] = y;
+ mQ[VZ] = z;
+ mQ[VS] = w;
+ normalize();
+ return (*this);
+}
+
+inline const LLQuaternion& LLQuaternion::set(const LLQuaternion &quat)
+{
+ mQ[VX] = quat.mQ[VX];
+ mQ[VY] = quat.mQ[VY];
+ mQ[VZ] = quat.mQ[VZ];
+ mQ[VW] = quat.mQ[VW];
+ normalize();
+ return (*this);
+}
+
+inline const LLQuaternion& LLQuaternion::set(const F32 *q)
+{
+ mQ[VX] = q[VX];
+ mQ[VY] = q[VY];
+ mQ[VZ] = q[VZ];
+ mQ[VS] = q[VW];
+ normalize();
+ return (*this);
+}
+
+
+// deprecated
+inline const LLQuaternion& LLQuaternion::setQuatInit(F32 x, F32 y, F32 z, F32 w)
+{
+ mQ[VX] = x;
+ mQ[VY] = y;
+ mQ[VZ] = z;
+ mQ[VS] = w;
+ normalize();
+ return (*this);
+}
+
+// deprecated
+inline const LLQuaternion& LLQuaternion::setQuat(const LLQuaternion &quat)
+{
+ mQ[VX] = quat.mQ[VX];
+ mQ[VY] = quat.mQ[VY];
+ mQ[VZ] = quat.mQ[VZ];
+ mQ[VW] = quat.mQ[VW];
+ normalize();
+ return (*this);
+}
+
+// deprecated
+inline const LLQuaternion& LLQuaternion::setQuat(const F32 *q)
+{
+ mQ[VX] = q[VX];
+ mQ[VY] = q[VY];
+ mQ[VZ] = q[VZ];
+ mQ[VS] = q[VW];
+ normalize();
+ return (*this);
+}
+
+// There may be a cheaper way that avoids the sqrt.
+// Does sin_a = VX*VX + VY*VY + VZ*VZ?
+// Copied from Matrix and Quaternion FAQ 1.12
+inline void LLQuaternion::getAngleAxis(F32* angle, F32* x, F32* y, F32* z) const
+{
+ F32 cos_a = mQ[VW];
+ if (cos_a > 1.0f) cos_a = 1.0f;
+ if (cos_a < -1.0f) cos_a = -1.0f;
+
+ F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
+
+ if ( fabs( sin_a ) < 0.0005f )
+ sin_a = 1.0f;
+ else
+ sin_a = 1.f/sin_a;
+
+ F32 temp_angle = 2.0f * (F32) acos( cos_a );
+ if (temp_angle > F_PI)
+ {
+ // The (angle,axis) pair should never have angles outside [PI, -PI]
+ // since we want the _shortest_ (angle,axis) solution.
+ // Since acos is defined for [0, PI], and we multiply by 2.0, we
+ // can push the angle outside the acceptible range.
+ // When this happens we set the angle to the other portion of a
+ // full 2PI rotation, and negate the axis, which reverses the
+ // direction of the rotation (by the right-hand rule).
+ *angle = 2.f * F_PI - temp_angle;
+ *x = - mQ[VX] * sin_a;
+ *y = - mQ[VY] * sin_a;
+ *z = - mQ[VZ] * sin_a;
+ }
+ else
+ {
+ *angle = temp_angle;
+ *x = mQ[VX] * sin_a;
+ *y = mQ[VY] * sin_a;
+ *z = mQ[VZ] * sin_a;
+ }
+}
+
+inline const LLQuaternion& LLQuaternion::conjugate()
+{
+ mQ[VX] *= -1.f;
+ mQ[VY] *= -1.f;
+ mQ[VZ] *= -1.f;
+ return (*this);
+}
+
+inline const LLQuaternion& LLQuaternion::conjQuat()
+{
+ mQ[VX] *= -1.f;
+ mQ[VY] *= -1.f;
+ mQ[VZ] *= -1.f;
+ return (*this);
+}
+
+// Transpose
+inline const LLQuaternion& LLQuaternion::transpose()
+{
+ mQ[VX] *= -1.f;
+ mQ[VY] *= -1.f;
+ mQ[VZ] *= -1.f;
+ return (*this);
+}
+
+// deprecated
+inline const LLQuaternion& LLQuaternion::transQuat()
+{
+ mQ[VX] *= -1.f;
+ mQ[VY] *= -1.f;
+ mQ[VZ] *= -1.f;
+ return (*this);
+}
+
+
+inline LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b)
+{
+ return LLQuaternion(
+ a.mQ[VX] + b.mQ[VX],
+ a.mQ[VY] + b.mQ[VY],
+ a.mQ[VZ] + b.mQ[VZ],
+ a.mQ[VW] + b.mQ[VW] );
+}
+
+
+inline LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b)
+{
+ return LLQuaternion(
+ a.mQ[VX] - b.mQ[VX],
+ a.mQ[VY] - b.mQ[VY],
+ a.mQ[VZ] - b.mQ[VZ],
+ a.mQ[VW] - b.mQ[VW] );
+}
+
+
+inline LLQuaternion operator-(const LLQuaternion &a)
+{
+ return LLQuaternion(
+ -a.mQ[VX],
+ -a.mQ[VY],
+ -a.mQ[VZ],
+ -a.mQ[VW] );
+}
+
+
+inline LLQuaternion operator*(F32 a, const LLQuaternion &q)
+{
+ return LLQuaternion(
+ a * q.mQ[VX],
+ a * q.mQ[VY],
+ a * q.mQ[VZ],
+ a * q.mQ[VW] );
+}
+
+
+inline LLQuaternion operator*(const LLQuaternion &q, F32 a)
+{
+ return LLQuaternion(
+ a * q.mQ[VX],
+ a * q.mQ[VY],
+ a * q.mQ[VZ],
+ a * q.mQ[VW] );
+}
+
+inline LLQuaternion operator~(const LLQuaternion &a)
+{
+ LLQuaternion q(a);
+ q.conjQuat();
+ return q;
+}
+
+inline bool LLQuaternion::operator==(const LLQuaternion &b) const
+{
+ return ( (mQ[VX] == b.mQ[VX])
+ &&(mQ[VY] == b.mQ[VY])
+ &&(mQ[VZ] == b.mQ[VZ])
+ &&(mQ[VS] == b.mQ[VS]));
+}
+
+inline bool LLQuaternion::operator!=(const LLQuaternion &b) const
+{
+ return ( (mQ[VX] != b.mQ[VX])
+ ||(mQ[VY] != b.mQ[VY])
+ ||(mQ[VZ] != b.mQ[VZ])
+ ||(mQ[VS] != b.mQ[VS]));
+}
+
+inline const LLQuaternion& operator*=(LLQuaternion &a, const LLQuaternion &b)
+{
+#if 1
+ 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]
+ );
+ a = q;
+#else
+ a = a * b;
+#endif
+ return a;
+}
+
+const F32 ONE_PART_IN_A_MILLION = 0.000001f;
+
+inline F32 LLQuaternion::normalize()
+{
+ F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
+
+ if (mag > FP_MAG_THRESHOLD)
+ {
+ // Floating point error can prevent some quaternions from achieving
+ // exact unity length. When trying to renormalize such quaternions we
+ // can oscillate between multiple quantized states. To prevent such
+ // drifts we only renomalize if the length is far enough from unity.
+ if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
+ {
+ F32 oomag = 1.f/mag;
+ mQ[VX] *= oomag;
+ mQ[VY] *= oomag;
+ mQ[VZ] *= oomag;
+ mQ[VS] *= oomag;
+ }
+ }
+ else
+ {
+ // we were given a very bad quaternion so we set it to identity
+ mQ[VX] = 0.f;
+ mQ[VY] = 0.f;
+ mQ[VZ] = 0.f;
+ mQ[VS] = 1.f;
+ }
+
+ return mag;
+}
+
+// deprecated
+inline F32 LLQuaternion::normQuat()
+{
+ F32 mag = sqrtf(mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] + mQ[VS]*mQ[VS]);
+
+ if (mag > FP_MAG_THRESHOLD)
+ {
+ if (fabs(1.f - mag) > ONE_PART_IN_A_MILLION)
+ {
+ // only renormalize if length not close enough to 1.0 already
+ F32 oomag = 1.f/mag;
+ mQ[VX] *= oomag;
+ mQ[VY] *= oomag;
+ mQ[VZ] *= oomag;
+ mQ[VS] *= oomag;
+ }
+ }
+ else
+ {
+ mQ[VX] = 0.f;
+ mQ[VY] = 0.f;
+ mQ[VZ] = 0.f;
+ mQ[VS] = 1.f;
+ }
+
+ return mag;
+}
+
+LLQuaternion::Order StringToOrder( const char *str );
+
+// Some notes about Quaternions
+
+// What is a Quaternion?
+// ---------------------
+// A quaternion is a point in 4-dimensional complex space.
+// Q = { Qx, Qy, Qz, Qw }
+//
+//
+// Why Quaternions?
+// ----------------
+// The set of quaternions that make up the the 4-D unit sphere
+// can be mapped to the set of all rotations in 3-D space. Sometimes
+// it is easier to describe/manipulate rotations in quaternion space
+// than rotation-matrix space.
+//
+//
+// How Quaternions?
+// ----------------
+// In order to take advantage of quaternions we need to know how to
+// go from rotation-matricies to quaternions and back. We also have
+// to agree what variety of rotations we're generating.
+//
+// Consider the equation... v' = v * R
+//
+// There are two ways to think about rotations of vectors.
+// 1) v' is the same vector in a different reference frame
+// 2) v' is a new vector in the same reference frame
+//
+// bookmark -- which way are we using?
+//
+//
+// Quaternion from Angle-Axis:
+// ---------------------------
+// Suppose we wanted to represent a rotation of some angle (theta)
+// about some axis ({Ax, Ay, Az})...
+//
+// axis of rotation = {Ax, Ay, Az}
+// angle_of_rotation = theta
+//
+// s = sin(0.5 * theta)
+// c = cos(0.5 * theta)
+// Q = { s * Ax, s * Ay, s * Az, c }
+//
+//
+// 3x3 Matrix from Quaternion
+// --------------------------
+//
+// | |
+// | 1 - 2 * (y^2 + z^2) 2 * (x * y + z * w) 2 * (y * w - x * z) |
+// | |
+// M = | 2 * (x * y - z * w) 1 - 2 * (x^2 + z^2) 2 * (y * z + x * w) |
+// | |
+// | 2 * (x * z + y * w) 2 * (y * z - x * w) 1 - 2 * (x^2 + y^2) |
+// | |
+
+#endif
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-31 21:17:55 +0000