PageRenderTime 105ms CodeModel.GetById 27ms app.highlight 70ms RepoModel.GetById 1ms app.codeStats 0ms

/indra/llmath/llquaternion.cpp

https://bitbucket.org/lindenlab/viewer-beta/
C++ | 955 lines | 633 code | 150 blank | 172 comment | 40 complexity | 523f68e86d7a99745b9ba816cd8467bb MD5 | raw file
  1/** 
  2 * @file llquaternion.cpp
  3 * @brief LLQuaternion class implementation.
  4 *
  5 * $LicenseInfo:firstyear=2000&license=viewerlgpl$
  6 * Second Life Viewer Source Code
  7 * Copyright (C) 2010, Linden Research, Inc.
  8 * 
  9 * This library is free software; you can redistribute it and/or
 10 * modify it under the terms of the GNU Lesser General Public
 11 * License as published by the Free Software Foundation;
 12 * version 2.1 of the License only.
 13 * 
 14 * This library is distributed in the hope that it will be useful,
 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 17 * Lesser General Public License for more details.
 18 * 
 19 * You should have received a copy of the GNU Lesser General Public
 20 * License along with this library; if not, write to the Free Software
 21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 22 * 
 23 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 24 * $/LicenseInfo$
 25 */
 26
 27#include "linden_common.h"
 28
 29#include "llmath.h"	// for F_PI
 30
 31#include "llquaternion.h"
 32
 33//#include "vmath.h"
 34#include "v3math.h"
 35#include "v3dmath.h"
 36#include "v4math.h"
 37#include "m4math.h"
 38#include "m3math.h"
 39#include "llquantize.h"
 40
 41// WARNING: Don't use this for global const definitions!  using this
 42// at the top of a *.cpp file might not give you what you think.
 43const LLQuaternion LLQuaternion::DEFAULT;
 44 
 45// Constructors
 46
 47LLQuaternion::LLQuaternion(const LLMatrix4 &mat)
 48{
 49	*this = mat.quaternion();
 50	normalize();
 51}
 52
 53LLQuaternion::LLQuaternion(const LLMatrix3 &mat)
 54{
 55	*this = mat.quaternion();
 56	normalize();
 57}
 58
 59LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec)
 60{
 61	LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
 62	v.normalize();
 63
 64	F32 c, s;
 65	c = cosf(angle*0.5f);
 66	s = sinf(angle*0.5f);
 67
 68	mQ[VX] = v.mV[VX] * s;
 69	mQ[VY] = v.mV[VY] * s;
 70	mQ[VZ] = v.mV[VZ] * s;
 71	mQ[VW] = c;
 72	normalize();
 73}
 74
 75LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
 76{
 77	LLVector3 v(vec);
 78	v.normalize();
 79
 80	F32 c, s;
 81	c = cosf(angle*0.5f);
 82	s = sinf(angle*0.5f);
 83
 84	mQ[VX] = v.mV[VX] * s;
 85	mQ[VY] = v.mV[VY] * s;
 86	mQ[VZ] = v.mV[VZ] * s;
 87	mQ[VW] = c;
 88	normalize();
 89}
 90
 91LLQuaternion::LLQuaternion(const LLVector3 &x_axis,
 92						   const LLVector3 &y_axis,
 93						   const LLVector3 &z_axis)
 94{
 95	LLMatrix3 mat;
 96	mat.setRows(x_axis, y_axis, z_axis);
 97	*this = mat.quaternion();
 98	normalize();
 99}
100
101// Quatizations
102void	LLQuaternion::quantize16(F32 lower, F32 upper)
103{
104	F32 x = mQ[VX];
105	F32 y = mQ[VY];
106	F32 z = mQ[VZ];
107	F32 s = mQ[VS];
108
109	x = U16_to_F32(F32_to_U16_ROUND(x, lower, upper), lower, upper);
110	y = U16_to_F32(F32_to_U16_ROUND(y, lower, upper), lower, upper);
111	z = U16_to_F32(F32_to_U16_ROUND(z, lower, upper), lower, upper);
112	s = U16_to_F32(F32_to_U16_ROUND(s, lower, upper), lower, upper);
113
114	mQ[VX] = x;
115	mQ[VY] = y;
116	mQ[VZ] = z;
117	mQ[VS] = s;
118
119	normalize();
120}
121
122void	LLQuaternion::quantize8(F32 lower, F32 upper)
123{
124	mQ[VX] = U8_to_F32(F32_to_U8_ROUND(mQ[VX], lower, upper), lower, upper);
125	mQ[VY] = U8_to_F32(F32_to_U8_ROUND(mQ[VY], lower, upper), lower, upper);
126	mQ[VZ] = U8_to_F32(F32_to_U8_ROUND(mQ[VZ], lower, upper), lower, upper);
127	mQ[VS] = U8_to_F32(F32_to_U8_ROUND(mQ[VS], lower, upper), lower, upper);
128
129	normalize();
130}
131
132// LLVector3 Magnitude and Normalization Functions
133
134
135// Set LLQuaternion routines
136
137const LLQuaternion&	LLQuaternion::setAngleAxis(F32 angle, F32 x, F32 y, F32 z)
138{
139	LLVector3 vec(x, y, z);
140	vec.normalize();
141
142	angle *= 0.5f;
143	F32 c, s;
144	c = cosf(angle);
145	s = sinf(angle);
146
147	mQ[VX] = vec.mV[VX]*s;
148	mQ[VY] = vec.mV[VY]*s;
149	mQ[VZ] = vec.mV[VZ]*s;
150	mQ[VW] = c;
151
152	normalize();
153	return (*this);
154}
155
156const LLQuaternion&	LLQuaternion::setAngleAxis(F32 angle, const LLVector3 &vec)
157{
158	LLVector3 v(vec);
159	v.normalize();
160
161	angle *= 0.5f;
162	F32 c, s;
163	c = cosf(angle);
164	s = sinf(angle);
165
166	mQ[VX] = v.mV[VX]*s;
167	mQ[VY] = v.mV[VY]*s;
168	mQ[VZ] = v.mV[VZ]*s;
169	mQ[VW] = c;
170
171	normalize();
172	return (*this);
173}
174
175const LLQuaternion&	LLQuaternion::setAngleAxis(F32 angle, const LLVector4 &vec)
176{
177	LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
178	v.normalize();
179
180	F32 c, s;
181	c = cosf(angle*0.5f);
182	s = sinf(angle*0.5f);
183
184	mQ[VX] = v.mV[VX]*s;
185	mQ[VY] = v.mV[VY]*s;
186	mQ[VZ] = v.mV[VZ]*s;
187	mQ[VW] = c;
188
189	normalize();
190	return (*this);
191}
192
193const LLQuaternion&	LLQuaternion::setEulerAngles(F32 roll, F32 pitch, F32 yaw)
194{
195	LLMatrix3 rot_mat(roll, pitch, yaw);
196	rot_mat.orthogonalize();
197	*this = rot_mat.quaternion();
198		
199	normalize();
200	return (*this);
201}
202
203// deprecated
204const LLQuaternion&	LLQuaternion::set(const LLMatrix3 &mat)
205{
206	*this = mat.quaternion();
207	normalize();
208	return (*this);
209}
210
211// deprecated
212const LLQuaternion&	LLQuaternion::set(const LLMatrix4 &mat)
213{
214	*this = mat.quaternion();
215	normalize();
216	return (*this);
217}
218
219// deprecated
220const LLQuaternion&	LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z)
221{
222	LLVector3 vec(x, y, z);
223	vec.normalize();
224
225	angle *= 0.5f;
226	F32 c, s;
227	c = cosf(angle);
228	s = sinf(angle);
229
230	mQ[VX] = vec.mV[VX]*s;
231	mQ[VY] = vec.mV[VY]*s;
232	mQ[VZ] = vec.mV[VZ]*s;
233	mQ[VW] = c;
234
235	normalize();
236	return (*this);
237}
238
239// deprecated
240const LLQuaternion&	LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
241{
242	LLVector3 v(vec);
243	v.normalize();
244
245	angle *= 0.5f;
246	F32 c, s;
247	c = cosf(angle);
248	s = sinf(angle);
249
250	mQ[VX] = v.mV[VX]*s;
251	mQ[VY] = v.mV[VY]*s;
252	mQ[VZ] = v.mV[VZ]*s;
253	mQ[VW] = c;
254
255	normalize();
256	return (*this);
257}
258
259const LLQuaternion&	LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
260{
261	LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
262	v.normalize();
263
264	F32 c, s;
265	c = cosf(angle*0.5f);
266	s = sinf(angle*0.5f);
267
268	mQ[VX] = v.mV[VX]*s;
269	mQ[VY] = v.mV[VY]*s;
270	mQ[VZ] = v.mV[VZ]*s;
271	mQ[VW] = c;
272
273	normalize();
274	return (*this);
275}
276
277const LLQuaternion&	LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
278{
279	LLMatrix3 rot_mat(roll, pitch, yaw);
280	rot_mat.orthogonalize();
281	*this = rot_mat.quaternion();
282		
283	normalize();
284	return (*this);
285}
286
287const LLQuaternion&	LLQuaternion::setQuat(const LLMatrix3 &mat)
288{
289	*this = mat.quaternion();
290	normalize();
291	return (*this);
292}
293
294const LLQuaternion&	LLQuaternion::setQuat(const LLMatrix4 &mat)
295{
296	*this = mat.quaternion();
297	normalize();
298	return (*this);
299//#if 1
300//	// NOTE: LLQuaternion's are actually inverted with respect to
301//	// the matrices, so this code also assumes inverted quaternions
302//	// (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
303//	// in reverse order (yaw,pitch,roll).
304//	F64 cosX = cos(roll);
305//    F64 cosY = cos(pitch);
306//    F64 cosZ = cos(yaw);
307//
308//    F64 sinX = sin(roll);
309//    F64 sinY = sin(pitch);
310//    F64 sinZ = sin(yaw);
311//
312//    mQ[VW] = (F32)sqrt(cosY*cosZ - sinX*sinY*sinZ + cosX*cosZ + cosX*cosY + 1.0)*.5;
313//	if (fabs(mQ[VW]) < F_APPROXIMATELY_ZERO)
314//	{
315//		// null rotation, any axis will do
316//		mQ[VX] = 0.0f;
317//		mQ[VY] = 1.0f;
318//		mQ[VZ] = 0.0f;
319//	}
320//	else
321//	{
322//		F32 inv_s = 1.0f / (4.0f * mQ[VW]);
323//		mQ[VX] = (F32)-(-sinX*cosY - cosX*sinY*sinZ - sinX*cosZ) * inv_s;
324//		mQ[VY] = (F32)-(-cosX*sinY*cosZ + sinX*sinZ - sinY) * inv_s;
325//		mQ[VZ] = (F32)-(-cosY*sinZ - sinX*sinY*cosZ - cosX*sinZ) * inv_s;		
326//	}
327//
328//#else // This only works on a certain subset of roll/pitch/yaw
329//	
330//	F64 cosX = cosf(roll/2.0);
331//    F64 cosY = cosf(pitch/2.0);
332//    F64 cosZ = cosf(yaw/2.0);
333//
334//    F64 sinX = sinf(roll/2.0);
335//    F64 sinY = sinf(pitch/2.0);
336//    F64 sinZ = sinf(yaw/2.0);
337//
338//    mQ[VW] = (F32)(cosX*cosY*cosZ + sinX*sinY*sinZ);
339//    mQ[VX] = (F32)(sinX*cosY*cosZ - cosX*sinY*sinZ);
340//    mQ[VY] = (F32)(cosX*sinY*cosZ + sinX*cosY*sinZ);
341//    mQ[VZ] = (F32)(cosX*cosY*sinZ - sinX*sinY*cosZ);
342//#endif
343//
344//	normalize();
345//	return (*this);
346}
347
348// SJB: This code is correct for a logicly stored (non-transposed) matrix;
349//		Our matrices are stored transposed, OpenGL style, so this generates the
350//		INVERSE matrix, or the CORRECT matrix form an INVERSE quaternion.
351//		Because we use similar logic in LLMatrix3::quaternion(),
352//		we are internally consistant so everything works OK :)
353LLMatrix3	LLQuaternion::getMatrix3(void) const
354{
355	LLMatrix3	mat;
356	F32		xx, xy, xz, xw, yy, yz, yw, zz, zw;
357
358    xx      = mQ[VX] * mQ[VX];
359    xy      = mQ[VX] * mQ[VY];
360    xz      = mQ[VX] * mQ[VZ];
361    xw      = mQ[VX] * mQ[VW];
362
363    yy      = mQ[VY] * mQ[VY];
364    yz      = mQ[VY] * mQ[VZ];
365    yw      = mQ[VY] * mQ[VW];
366
367    zz      = mQ[VZ] * mQ[VZ];
368    zw      = mQ[VZ] * mQ[VW];
369
370    mat.mMatrix[0][0]  = 1.f - 2.f * ( yy + zz );
371    mat.mMatrix[0][1]  =	   2.f * ( xy + zw );
372    mat.mMatrix[0][2]  =	   2.f * ( xz - yw );
373
374    mat.mMatrix[1][0]  =	   2.f * ( xy - zw );
375    mat.mMatrix[1][1]  = 1.f - 2.f * ( xx + zz );
376    mat.mMatrix[1][2]  =	   2.f * ( yz + xw );
377
378    mat.mMatrix[2][0]  =	   2.f * ( xz + yw );
379    mat.mMatrix[2][1]  =	   2.f * ( yz - xw );
380    mat.mMatrix[2][2]  = 1.f - 2.f * ( xx + yy );
381
382	return mat;
383}
384
385LLMatrix4	LLQuaternion::getMatrix4(void) const
386{
387	LLMatrix4	mat;
388	F32		xx, xy, xz, xw, yy, yz, yw, zz, zw;
389
390    xx      = mQ[VX] * mQ[VX];
391    xy      = mQ[VX] * mQ[VY];
392    xz      = mQ[VX] * mQ[VZ];
393    xw      = mQ[VX] * mQ[VW];
394
395    yy      = mQ[VY] * mQ[VY];
396    yz      = mQ[VY] * mQ[VZ];
397    yw      = mQ[VY] * mQ[VW];
398
399    zz      = mQ[VZ] * mQ[VZ];
400    zw      = mQ[VZ] * mQ[VW];
401
402    mat.mMatrix[0][0]  = 1.f - 2.f * ( yy + zz );
403    mat.mMatrix[0][1]  =	   2.f * ( xy + zw );
404    mat.mMatrix[0][2]  =	   2.f * ( xz - yw );
405
406    mat.mMatrix[1][0]  =	   2.f * ( xy - zw );
407    mat.mMatrix[1][1]  = 1.f - 2.f * ( xx + zz );
408    mat.mMatrix[1][2]  =	   2.f * ( yz + xw );
409
410    mat.mMatrix[2][0]  =	   2.f * ( xz + yw );
411    mat.mMatrix[2][1]  =	   2.f * ( yz - xw );
412    mat.mMatrix[2][2]  = 1.f - 2.f * ( xx + yy );
413
414	// TODO -- should we set the translation portion to zero?
415
416	return mat;
417}
418
419
420
421
422// Other useful methods
423
424
425// calculate the shortest rotation from a to b
426void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b)
427{
428	// Make a local copy of both vectors.
429	LLVector3 vec_a = a;
430	LLVector3 vec_b = b;
431
432	// Make sure neither vector is zero length.  Also normalize
433	// the vectors while we are at it.
434	F32 vec_a_mag = vec_a.normalize();
435	F32 vec_b_mag = vec_b.normalize();
436	if (vec_a_mag < F_APPROXIMATELY_ZERO ||
437		vec_b_mag < F_APPROXIMATELY_ZERO)
438	{
439		// Can't calculate a rotation from this.
440		// Just return ZERO_ROTATION instead.
441		loadIdentity();
442		return;
443	}
444
445	// Create an axis to rotate around, and the cos of the angle to rotate.
446	LLVector3 axis = vec_a % vec_b;
447	F32 cos_theta  = vec_a * vec_b;
448
449	// Check the angle between the vectors to see if they are parallel or anti-parallel.
450	if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
451	{
452		// a and b are parallel.  No rotation is necessary.
453		loadIdentity();
454	}
455	else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
456	{
457		// a and b are anti-parallel.
458		// Rotate 180 degrees around some orthogonal axis.
459		// Find the projection of the x-axis onto a, and try
460		// using the vector between the projection and the x-axis
461		// as the orthogonal axis.
462		LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
463		LLVector3 ortho_axis(1.f, 0.f, 0.f);
464		ortho_axis -= proj;
465		
466		// Turn this into an orthonormal axis.
467		F32 ortho_length = ortho_axis.normalize();
468		// If the axis' length is 0, then our guess at an orthogonal axis
469		// was wrong (a is parallel to the x-axis).
470		if (ortho_length < F_APPROXIMATELY_ZERO)
471		{
472			// Use the z-axis instead.
473			ortho_axis.setVec(0.f, 0.f, 1.f);
474		}
475
476		// Construct a quaternion from this orthonormal axis.
477		mQ[VX] = ortho_axis.mV[VX];
478		mQ[VY] = ortho_axis.mV[VY];
479		mQ[VZ] = ortho_axis.mV[VZ];
480		mQ[VW] = 0.f;
481	}
482	else
483	{
484		// a and b are NOT parallel or anti-parallel.
485		// Return the rotation between these vectors.
486		F32 theta = (F32)acos(cos_theta);
487
488		setAngleAxis(theta, axis);
489	}
490}
491
492// constrains rotation to a cone angle specified in radians
493const LLQuaternion &LLQuaternion::constrain(F32 radians)
494{
495	const F32 cos_angle_lim = cosf( radians/2 );	// mQ[VW] limit
496	const F32 sin_angle_lim = sinf( radians/2 );	// rotation axis length	limit
497
498	if (mQ[VW] < 0.f)
499	{
500		mQ[VX] *= -1.f;
501		mQ[VY] *= -1.f;
502		mQ[VZ] *= -1.f;
503		mQ[VW] *= -1.f;
504	}
505
506	// if rotation angle is greater than limit (cos is less than limit)
507	if( mQ[VW] < cos_angle_lim )
508	{
509		mQ[VW] = cos_angle_lim;
510		F32 axis_len = sqrtf( mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] ); // sin(theta/2)
511		F32 axis_mult_fact = sin_angle_lim / axis_len;
512		mQ[VX] *= axis_mult_fact;
513		mQ[VY] *= axis_mult_fact;
514		mQ[VZ] *= axis_mult_fact;
515	}
516
517	return *this;
518}
519
520// Operators
521
522std::ostream& operator<<(std::ostream &s, const LLQuaternion &a)
523{
524	s << "{ " 
525		<< a.mQ[VX] << ", " << a.mQ[VY] << ", " << a.mQ[VZ] << ", " << a.mQ[VW] 
526	<< " }";
527	return s;
528}
529
530
531// Does NOT renormalize the result
532LLQuaternion	operator*(const LLQuaternion &a, const LLQuaternion &b)
533{
534//	LLQuaternion::mMultCount++;
535
536	LLQuaternion q(
537		b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1],
538		b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2],
539		b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0],
540		b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2]
541	);
542	return q;
543}
544
545/*
546LLMatrix4	operator*(const LLMatrix4 &m, const LLQuaternion &q)
547{
548	LLMatrix4 qmat(q);
549	return (m*qmat);
550}
551*/
552
553
554
555LLVector4		operator*(const LLVector4 &a, const LLQuaternion &rot)
556{
557    F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
558    F32 rx =   rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
559    F32 ry =   rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
560    F32 rz =   rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
561
562    F32 nx = - rw * rot.mQ[VX] +  rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
563    F32 ny = - rw * rot.mQ[VY] +  ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
564    F32 nz = - rw * rot.mQ[VZ] +  rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
565
566    return LLVector4(nx, ny, nz, a.mV[VW]);
567}
568
569LLVector3		operator*(const LLVector3 &a, const LLQuaternion &rot)
570{
571    F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
572    F32 rx =   rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
573    F32 ry =   rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
574    F32 rz =   rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
575
576    F32 nx = - rw * rot.mQ[VX] +  rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
577    F32 ny = - rw * rot.mQ[VY] +  ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
578    F32 nz = - rw * rot.mQ[VZ] +  rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
579
580    return LLVector3(nx, ny, nz);
581}
582
583LLVector3d		operator*(const LLVector3d &a, const LLQuaternion &rot)
584{
585    F64 rw = - rot.mQ[VX] * a.mdV[VX] - rot.mQ[VY] * a.mdV[VY] - rot.mQ[VZ] * a.mdV[VZ];
586    F64 rx =   rot.mQ[VW] * a.mdV[VX] + rot.mQ[VY] * a.mdV[VZ] - rot.mQ[VZ] * a.mdV[VY];
587    F64 ry =   rot.mQ[VW] * a.mdV[VY] + rot.mQ[VZ] * a.mdV[VX] - rot.mQ[VX] * a.mdV[VZ];
588    F64 rz =   rot.mQ[VW] * a.mdV[VZ] + rot.mQ[VX] * a.mdV[VY] - rot.mQ[VY] * a.mdV[VX];
589
590    F64 nx = - rw * rot.mQ[VX] +  rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
591    F64 ny = - rw * rot.mQ[VY] +  ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
592    F64 nz = - rw * rot.mQ[VZ] +  rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
593
594    return LLVector3d(nx, ny, nz);
595}
596
597F32 dot(const LLQuaternion &a, const LLQuaternion &b)
598{
599	return a.mQ[VX] * b.mQ[VX] + 
600		   a.mQ[VY] * b.mQ[VY] + 
601		   a.mQ[VZ] * b.mQ[VZ] + 
602		   a.mQ[VW] * b.mQ[VW]; 
603}
604
605// DEMO HACK: This lerp is probably inocrrect now due intermediate normalization
606// it should look more like the lerp below
607#if 0
608// linear interpolation
609LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
610{
611	LLQuaternion r;
612	r = t * (q - p) + p;
613	r.normalize();
614	return r;
615}
616#endif
617
618// lerp from identity to q
619LLQuaternion lerp(F32 t, const LLQuaternion &q)
620{
621	LLQuaternion r;
622	r.mQ[VX] = t * q.mQ[VX];
623	r.mQ[VY] = t * q.mQ[VY];
624	r.mQ[VZ] = t * q.mQ[VZ];
625	r.mQ[VW] = t * (q.mQ[VZ] - 1.f) + 1.f;
626	r.normalize();
627	return r;
628}
629
630LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
631{
632	LLQuaternion r;
633	F32 inv_t;
634
635	inv_t = 1.f - t;
636
637	r.mQ[VX] = t * q.mQ[VX] + (inv_t * p.mQ[VX]);
638	r.mQ[VY] = t * q.mQ[VY] + (inv_t * p.mQ[VY]);
639	r.mQ[VZ] = t * q.mQ[VZ] + (inv_t * p.mQ[VZ]);
640	r.mQ[VW] = t * q.mQ[VW] + (inv_t * p.mQ[VW]);
641	r.normalize();
642	return r;
643}
644
645
646// spherical linear interpolation
647LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b )
648{
649	// cosine theta = dot product of a and b
650	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];
651	
652	// if b is on opposite hemisphere from a, use -a instead
653	int bflip;
654 	if (cos_t < 0.0f)
655	{
656		cos_t = -cos_t;
657		bflip = TRUE;
658	}
659	else
660		bflip = FALSE;
661
662	// if B is (within precision limits) the same as A,
663	// just linear interpolate between A and B.
664	F32 alpha;	// interpolant
665	F32 beta;		// 1 - interpolant
666	if (1.0f - cos_t < 0.00001f)
667	{
668		beta = 1.0f - u;
669		alpha = u;
670 	}
671	else
672	{
673 		F32 theta = acosf(cos_t);
674 		F32 sin_t = sinf(theta);
675 		beta = sinf(theta - u*theta) / sin_t;
676 		alpha = sinf(u*theta) / sin_t;
677 	}
678
679	if (bflip)
680		beta = -beta;
681
682	// interpolate
683	LLQuaternion ret;
684	ret.mQ[0] = beta*a.mQ[0] + alpha*b.mQ[0];
685 	ret.mQ[1] = beta*a.mQ[1] + alpha*b.mQ[1];
686 	ret.mQ[2] = beta*a.mQ[2] + alpha*b.mQ[2];
687 	ret.mQ[3] = beta*a.mQ[3] + alpha*b.mQ[3];
688
689	return ret;
690}
691
692// lerp whenever possible
693LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b)
694{
695	if (dot(a, b) < 0.f)
696	{
697		return slerp(t, a, b);
698	}
699	else
700	{
701		return lerp(t, a, b);
702	}
703}
704
705LLQuaternion nlerp(F32 t, const LLQuaternion &q)
706{
707	if (q.mQ[VW] < 0.f)
708	{
709		return slerp(t, q);
710	}
711	else
712	{
713		return lerp(t, q);
714	}
715}
716
717// slerp from identity quaternion to another quaternion
718LLQuaternion slerp(F32 t, const LLQuaternion &q)
719{
720	F32 c = q.mQ[VW];
721	if (1.0f == t  ||  1.0f == c)
722	{
723		// the trivial cases
724		return q;
725	}
726
727	LLQuaternion r;
728	F32 s, angle, stq, stp;
729
730	s = (F32) sqrt(1.f - c*c);
731
732    if (c < 0.0f)
733    {
734        // when c < 0.0 then theta > PI/2 
735        // since quat and -quat are the same rotation we invert one of  
736        // p or q to reduce unecessary spins
737        // A equivalent way to do it is to convert acos(c) as if it had 
738		// been negative, and to negate stp 
739        angle   = (F32) acos(-c); 
740        stp     = -(F32) sin(angle * (1.f - t));
741        stq     = (F32) sin(angle * t);
742    }   
743    else
744    {
745		angle 	= (F32) acos(c);
746        stp     = (F32) sin(angle * (1.f - t));
747        stq     = (F32) sin(angle * t);
748    }
749
750	r.mQ[VX] = (q.mQ[VX] * stq) / s;
751	r.mQ[VY] = (q.mQ[VY] * stq) / s;
752	r.mQ[VZ] = (q.mQ[VZ] * stq) / s;
753	r.mQ[VW] = (stp + q.mQ[VW] * stq) / s;
754
755	return r;
756}
757
758LLQuaternion mayaQ(F32 xRot, F32 yRot, F32 zRot, LLQuaternion::Order order)
759{
760	LLQuaternion xQ( xRot*DEG_TO_RAD, LLVector3(1.0f, 0.0f, 0.0f) );
761	LLQuaternion yQ( yRot*DEG_TO_RAD, LLVector3(0.0f, 1.0f, 0.0f) );
762	LLQuaternion zQ( zRot*DEG_TO_RAD, LLVector3(0.0f, 0.0f, 1.0f) );
763	LLQuaternion ret;
764	switch( order )
765	{
766	case LLQuaternion::XYZ:
767		ret = xQ * yQ * zQ;
768		break;
769	case LLQuaternion::YZX:
770		ret = yQ * zQ * xQ;
771		break;
772	case LLQuaternion::ZXY:
773		ret = zQ * xQ * yQ;
774		break;
775	case LLQuaternion::XZY:
776		ret = xQ * zQ * yQ;
777		break;
778	case LLQuaternion::YXZ:
779		ret = yQ * xQ * zQ;
780		break;
781	case LLQuaternion::ZYX:
782		ret = zQ * yQ * xQ;
783		break;
784	}
785	return ret;
786}
787
788const char *OrderToString( const LLQuaternion::Order order )
789{
790	const char *p = NULL;
791	switch( order )
792	{
793	default:
794	case LLQuaternion::XYZ:
795		p = "XYZ";
796		break;
797	case LLQuaternion::YZX:
798		p = "YZX";
799		break;
800	case LLQuaternion::ZXY:
801		p = "ZXY";
802		break;
803	case LLQuaternion::XZY:
804		p = "XZY";
805		break;
806	case LLQuaternion::YXZ:
807		p = "YXZ";
808		break;
809	case LLQuaternion::ZYX:
810		p = "ZYX";
811		break;
812	}
813	return p;
814}
815
816LLQuaternion::Order StringToOrder( const char *str )
817{
818	if (strncmp(str, "XYZ", 3)==0 || strncmp(str, "xyz", 3)==0)
819		return LLQuaternion::XYZ;
820
821	if (strncmp(str, "YZX", 3)==0 || strncmp(str, "yzx", 3)==0)
822		return LLQuaternion::YZX;
823
824	if (strncmp(str, "ZXY", 3)==0 || strncmp(str, "zxy", 3)==0)
825		return LLQuaternion::ZXY;
826
827	if (strncmp(str, "XZY", 3)==0 || strncmp(str, "xzy", 3)==0)
828		return LLQuaternion::XZY;
829
830	if (strncmp(str, "YXZ", 3)==0 || strncmp(str, "yxz", 3)==0)
831		return LLQuaternion::YXZ;
832
833	if (strncmp(str, "ZYX", 3)==0 || strncmp(str, "zyx", 3)==0)
834		return LLQuaternion::ZYX;
835
836	return LLQuaternion::XYZ;
837}
838
839void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const
840{
841	F32 cos_a = mQ[VW];
842	if (cos_a > 1.0f) cos_a = 1.0f;
843	if (cos_a < -1.0f) cos_a = -1.0f;
844
845    F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
846
847    if ( fabs( sin_a ) < 0.0005f )
848		sin_a = 1.0f;
849	else
850		sin_a = 1.f/sin_a;
851
852    F32 temp_angle = 2.0f * (F32) acos( cos_a );
853	if (temp_angle > F_PI)
854	{
855		// The (angle,axis) pair should never have angles outside [PI, -PI]
856		// since we want the _shortest_ (angle,axis) solution.
857		// Since acos is defined for [0, PI], and we multiply by 2.0, we
858		// can push the angle outside the acceptible range.
859		// When this happens we set the angle to the other portion of a 
860		// full 2PI rotation, and negate the axis, which reverses the 
861		// direction of the rotation (by the right-hand rule).
862		*angle = 2.f * F_PI - temp_angle;
863    	vec.mV[VX] = - mQ[VX] * sin_a;
864    	vec.mV[VY] = - mQ[VY] * sin_a;
865    	vec.mV[VZ] = - mQ[VZ] * sin_a;
866	}
867	else
868	{
869		*angle = temp_angle;
870    	vec.mV[VX] = mQ[VX] * sin_a;
871    	vec.mV[VY] = mQ[VY] * sin_a;
872    	vec.mV[VZ] = mQ[VZ] * sin_a;
873	}
874}
875
876
877// quaternion does not need to be normalized
878void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
879{
880	LLMatrix3 rot_mat(*this);
881	rot_mat.orthogonalize();
882	rot_mat.getEulerAngles(roll, pitch, yaw);
883
884//	// NOTE: LLQuaternion's are actually inverted with respect to
885//	// the matrices, so this code also assumes inverted quaternions
886//	// (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
887//	// in reverse order (yaw,pitch,roll).
888//	F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
889//	F64 m20 = 2.0*(x*z-y*w);
890//	if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
891//	{
892//		*roll = 0.0f;
893//		*pitch = (F32)asin(m20);
894//		*yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
895//	}
896//	else
897//	{
898//		*roll  = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
899//		*pitch = (F32)asin(m20);
900//		*yaw   = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
901//	}
902}
903
904// Saves space by using the fact that our quaternions are normalized
905LLVector3 LLQuaternion::packToVector3() const
906{
907	if( mQ[VW] >= 0 )
908	{
909		return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
910	}
911	else
912	{
913		return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
914	}
915}
916
917// Saves space by using the fact that our quaternions are normalized
918void LLQuaternion::unpackFromVector3( const LLVector3& vec )
919{
920	mQ[VX] = vec.mV[VX];
921	mQ[VY] = vec.mV[VY];
922	mQ[VZ] = vec.mV[VZ];
923	F32 t = 1.f - vec.magVecSquared();
924	if( t > 0 )
925	{
926		mQ[VW] = sqrt( t );
927	}
928	else
929	{
930		// Need this to avoid trying to find the square root of a negative number due
931		// to floating point error.
932		mQ[VW] = 0;
933	}
934}
935
936BOOL LLQuaternion::parseQuat(const std::string& buf, LLQuaternion* value)
937{
938	if( buf.empty() || value == NULL)
939	{
940		return FALSE;
941	}
942
943	LLQuaternion quat;
944	S32 count = sscanf( buf.c_str(), "%f %f %f %f", quat.mQ + 0, quat.mQ + 1, quat.mQ + 2, quat.mQ + 3 );
945	if( 4 == count )
946	{
947		value->set( quat );
948		return TRUE;
949	}
950
951	return FALSE;
952}
953
954
955// End