aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/linden/indra/llmath/llquaternion.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'linden/indra/llmath/llquaternion.cpp')
-rw-r--r--linden/indra/llmath/llquaternion.cpp849
1 files changed, 849 insertions, 0 deletions
diff --git a/linden/indra/llmath/llquaternion.cpp b/linden/indra/llmath/llquaternion.cpp
new file mode 100644
index 0000000..1111ca5
--- /dev/null
+++ b/linden/indra/llmath/llquaternion.cpp
@@ -0,0 +1,849 @@
1/**
2 * @file qmath.cpp
3 * @brief LLQuaternion class implementation.
4 *
5 * Copyright (c) 2000-2007, Linden Research, Inc.
6 *
7 * The source code in this file ("Source Code") is provided by Linden Lab
8 * to you under the terms of the GNU General Public License, version 2.0
9 * ("GPL"), unless you have obtained a separate licensing agreement
10 * ("Other License"), formally executed by you and Linden Lab. Terms of
11 * the GPL can be found in doc/GPL-license.txt in this distribution, or
12 * online at http://secondlife.com/developers/opensource/gplv2
13 *
14 * There are special exceptions to the terms and conditions of the GPL as
15 * it is applied to this Source Code. View the full text of the exception
16 * in the file doc/FLOSS-exception.txt in this software distribution, or
17 * online at http://secondlife.com/developers/opensource/flossexception
18 *
19 * By copying, modifying or distributing this software, you acknowledge
20 * that you have read and understood your obligations described above,
21 * and agree to abide by those obligations.
22 *
23 * ALL LINDEN LAB SOURCE CODE IS PROVIDED "AS IS." LINDEN LAB MAKES NO
24 * WARRANTIES, EXPRESS, IMPLIED OR OTHERWISE, REGARDING ITS ACCURACY,
25 * COMPLETENESS OR PERFORMANCE.
26 */
27
28#include "linden_common.h"
29
30#include "llquaternion.h"
31
32#include "llmath.h" // for F_PI
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 normQuat();
51}
52
53LLQuaternion::LLQuaternion(const LLMatrix3 &mat)
54{
55 *this = mat.quaternion();
56 normQuat();
57}
58
59LLQuaternion::LLQuaternion(F32 angle, const LLVector4 &vec)
60{
61 LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
62 v.normVec();
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 normQuat();
73}
74
75LLQuaternion::LLQuaternion(F32 angle, const LLVector3 &vec)
76{
77 LLVector3 v(vec);
78 v.normVec();
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 normQuat();
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 normQuat();
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(x, lower, upper), lower, upper);
110 y = U16_to_F32(F32_to_U16(y, lower, upper), lower, upper);
111 z = U16_to_F32(F32_to_U16(z, lower, upper), lower, upper);
112 s = U16_to_F32(F32_to_U16(s, lower, upper), lower, upper);
113
114 mQ[VX] = x;
115 mQ[VY] = y;
116 mQ[VZ] = z;
117 mQ[VS] = s;
118}
119
120void LLQuaternion::quantize8(F32 lower, F32 upper)
121{
122 mQ[VX] = U8_to_F32(F32_to_U8(mQ[VX], lower, upper), lower, upper);
123 mQ[VY] = U8_to_F32(F32_to_U8(mQ[VY], lower, upper), lower, upper);
124 mQ[VZ] = U8_to_F32(F32_to_U8(mQ[VZ], lower, upper), lower, upper);
125 mQ[VS] = U8_to_F32(F32_to_U8(mQ[VS], lower, upper), lower, upper);
126}
127
128// LLVector3 Magnitude and Normalization Functions
129
130
131// Set LLQuaternion routines
132
133const LLQuaternion& LLQuaternion::setQuat(F32 angle, F32 x, F32 y, F32 z)
134{
135 LLVector3 vec(x, y, z);
136 vec.normVec();
137
138 angle *= 0.5f;
139 F32 c, s;
140 c = cosf(angle);
141 s = sinf(angle);
142
143 mQ[VX] = vec.mV[VX]*s;
144 mQ[VY] = vec.mV[VY]*s;
145 mQ[VZ] = vec.mV[VZ]*s;
146 mQ[VW] = c;
147
148 normQuat();
149 return (*this);
150}
151
152const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector3 &vec)
153{
154 LLVector3 v(vec);
155 v.normVec();
156
157 angle *= 0.5f;
158 F32 c, s;
159 c = cosf(angle);
160 s = sinf(angle);
161
162 mQ[VX] = v.mV[VX]*s;
163 mQ[VY] = v.mV[VY]*s;
164 mQ[VZ] = v.mV[VZ]*s;
165 mQ[VW] = c;
166
167 normQuat();
168 return (*this);
169}
170
171const LLQuaternion& LLQuaternion::setQuat(F32 angle, const LLVector4 &vec)
172{
173 LLVector3 v(vec.mV[VX], vec.mV[VY], vec.mV[VZ]);
174 v.normVec();
175
176 F32 c, s;
177 c = cosf(angle*0.5f);
178 s = sinf(angle*0.5f);
179
180 mQ[VX] = v.mV[VX]*s;
181 mQ[VY] = v.mV[VY]*s;
182 mQ[VZ] = v.mV[VZ]*s;
183 mQ[VW] = c;
184
185 normQuat();
186 return (*this);
187}
188
189const LLQuaternion& LLQuaternion::setQuat(F32 roll, F32 pitch, F32 yaw)
190{
191 LLMatrix3 rot_mat(roll, pitch, yaw);
192 rot_mat.orthogonalize();
193 *this = rot_mat.quaternion();
194
195 normQuat();
196 return (*this);
197//#if 1
198// // NOTE: LLQuaternion's are actually inverted with respect to
199// // the matrices, so this code also assumes inverted quaternions
200// // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
201// // in reverse order (yaw,pitch,roll).
202// F64 cosX = cos(roll);
203// F64 cosY = cos(pitch);
204// F64 cosZ = cos(yaw);
205//
206// F64 sinX = sin(roll);
207// F64 sinY = sin(pitch);
208// F64 sinZ = sin(yaw);
209//
210// mQ[VW] = (F32)sqrt(cosY*cosZ - sinX*sinY*sinZ + cosX*cosZ + cosX*cosY + 1.0)*.5;
211// if (fabs(mQ[VW]) < F_APPROXIMATELY_ZERO)
212// {
213// // null rotation, any axis will do
214// mQ[VX] = 0.0f;
215// mQ[VY] = 1.0f;
216// mQ[VZ] = 0.0f;
217// }
218// else
219// {
220// F32 inv_s = 1.0f / (4.0f * mQ[VW]);
221// mQ[VX] = (F32)-(-sinX*cosY - cosX*sinY*sinZ - sinX*cosZ) * inv_s;
222// mQ[VY] = (F32)-(-cosX*sinY*cosZ + sinX*sinZ - sinY) * inv_s;
223// mQ[VZ] = (F32)-(-cosY*sinZ - sinX*sinY*cosZ - cosX*sinZ) * inv_s;
224// }
225//
226//#else // This only works on a certain subset of roll/pitch/yaw
227//
228// F64 cosX = cosf(roll/2.0);
229// F64 cosY = cosf(pitch/2.0);
230// F64 cosZ = cosf(yaw/2.0);
231//
232// F64 sinX = sinf(roll/2.0);
233// F64 sinY = sinf(pitch/2.0);
234// F64 sinZ = sinf(yaw/2.0);
235//
236// mQ[VW] = (F32)(cosX*cosY*cosZ + sinX*sinY*sinZ);
237// mQ[VX] = (F32)(sinX*cosY*cosZ - cosX*sinY*sinZ);
238// mQ[VY] = (F32)(cosX*sinY*cosZ + sinX*cosY*sinZ);
239// mQ[VZ] = (F32)(cosX*cosY*sinZ - sinX*sinY*cosZ);
240//#endif
241//
242// normQuat();
243// return (*this);
244}
245
246// SJB: This code is correct for a logicly stored (non-transposed) matrix;
247// Our matrices are stored transposed, OpenGL style, so this generates the
248// INVERSE matrix, or the CORRECT matrix form an INVERSE quaternion.
249// Because we use similar logic in LLMatrix3::quaternion(),
250// we are internally consistant so everything works OK :)
251LLMatrix3 LLQuaternion::getMatrix3(void) const
252{
253 LLMatrix3 mat;
254 F32 xx, xy, xz, xw, yy, yz, yw, zz, zw;
255
256 xx = mQ[VX] * mQ[VX];
257 xy = mQ[VX] * mQ[VY];
258 xz = mQ[VX] * mQ[VZ];
259 xw = mQ[VX] * mQ[VW];
260
261 yy = mQ[VY] * mQ[VY];
262 yz = mQ[VY] * mQ[VZ];
263 yw = mQ[VY] * mQ[VW];
264
265 zz = mQ[VZ] * mQ[VZ];
266 zw = mQ[VZ] * mQ[VW];
267
268 mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz );
269 mat.mMatrix[0][1] = 2.f * ( xy + zw );
270 mat.mMatrix[0][2] = 2.f * ( xz - yw );
271
272 mat.mMatrix[1][0] = 2.f * ( xy - zw );
273 mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz );
274 mat.mMatrix[1][2] = 2.f * ( yz + xw );
275
276 mat.mMatrix[2][0] = 2.f * ( xz + yw );
277 mat.mMatrix[2][1] = 2.f * ( yz - xw );
278 mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy );
279
280 return mat;
281}
282
283LLMatrix4 LLQuaternion::getMatrix4(void) const
284{
285 LLMatrix4 mat;
286 F32 xx, xy, xz, xw, yy, yz, yw, zz, zw;
287
288 xx = mQ[VX] * mQ[VX];
289 xy = mQ[VX] * mQ[VY];
290 xz = mQ[VX] * mQ[VZ];
291 xw = mQ[VX] * mQ[VW];
292
293 yy = mQ[VY] * mQ[VY];
294 yz = mQ[VY] * mQ[VZ];
295 yw = mQ[VY] * mQ[VW];
296
297 zz = mQ[VZ] * mQ[VZ];
298 zw = mQ[VZ] * mQ[VW];
299
300 mat.mMatrix[0][0] = 1.f - 2.f * ( yy + zz );
301 mat.mMatrix[0][1] = 2.f * ( xy + zw );
302 mat.mMatrix[0][2] = 2.f * ( xz - yw );
303
304 mat.mMatrix[1][0] = 2.f * ( xy - zw );
305 mat.mMatrix[1][1] = 1.f - 2.f * ( xx + zz );
306 mat.mMatrix[1][2] = 2.f * ( yz + xw );
307
308 mat.mMatrix[2][0] = 2.f * ( xz + yw );
309 mat.mMatrix[2][1] = 2.f * ( yz - xw );
310 mat.mMatrix[2][2] = 1.f - 2.f * ( xx + yy );
311
312 // TODO -- should we set the translation portion to zero?
313
314 return mat;
315}
316
317
318
319
320// Other useful methods
321
322
323// calculate the shortest rotation from a to b
324void LLQuaternion::shortestArc(const LLVector3 &a, const LLVector3 &b)
325{
326 // Make a local copy of both vectors.
327 LLVector3 vec_a = a;
328 LLVector3 vec_b = b;
329
330 // Make sure neither vector is zero length. Also normalize
331 // the vectors while we are at it.
332 F32 vec_a_mag = vec_a.normVec();
333 F32 vec_b_mag = vec_b.normVec();
334 if (vec_a_mag < F_APPROXIMATELY_ZERO ||
335 vec_b_mag < F_APPROXIMATELY_ZERO)
336 {
337 // Can't calculate a rotation from this.
338 // Just return ZERO_ROTATION instead.
339 loadIdentity();
340 return;
341 }
342
343 // Create an axis to rotate around, and the cos of the angle to rotate.
344 LLVector3 axis = vec_a % vec_b;
345 F32 cos_theta = vec_a * vec_b;
346
347 // Check the angle between the vectors to see if they are parallel or anti-parallel.
348 if (cos_theta > 1.0 - F_APPROXIMATELY_ZERO)
349 {
350 // a and b are parallel. No rotation is necessary.
351 loadIdentity();
352 }
353 else if (cos_theta < -1.0 + F_APPROXIMATELY_ZERO)
354 {
355 // a and b are anti-parallel.
356 // Rotate 180 degrees around some orthogonal axis.
357 // Find the projection of the x-axis onto a, and try
358 // using the vector between the projection and the x-axis
359 // as the orthogonal axis.
360 LLVector3 proj = vec_a.mV[VX] / (vec_a * vec_a) * vec_a;
361 LLVector3 ortho_axis(1.f, 0.f, 0.f);
362 ortho_axis -= proj;
363
364 // Turn this into an orthonormal axis.
365 F32 ortho_length = ortho_axis.normVec();
366 // If the axis' length is 0, then our guess at an orthogonal axis
367 // was wrong (a is parallel to the x-axis).
368 if (ortho_length < F_APPROXIMATELY_ZERO)
369 {
370 // Use the z-axis instead.
371 ortho_axis.setVec(0.f, 0.f, 1.f);
372 }
373
374 // Construct a quaternion from this orthonormal axis.
375 mQ[VX] = ortho_axis.mV[VX];
376 mQ[VY] = ortho_axis.mV[VY];
377 mQ[VZ] = ortho_axis.mV[VZ];
378 mQ[VW] = 0.f;
379 }
380 else
381 {
382 // a and b are NOT parallel or anti-parallel.
383 // Return the rotation between these vectors.
384 F32 theta = (F32)acos(cos_theta);
385
386 setQuat(theta, axis);
387 }
388}
389
390// constrains rotation to a cone angle specified in radians
391const LLQuaternion &LLQuaternion::constrain(F32 radians)
392{
393 const F32 cos_angle_lim = cosf( radians/2 ); // mQ[VW] limit
394 const F32 sin_angle_lim = sinf( radians/2 ); // rotation axis length limit
395
396 if (mQ[VW] < 0.f)
397 {
398 mQ[VX] *= -1.f;
399 mQ[VY] *= -1.f;
400 mQ[VZ] *= -1.f;
401 mQ[VW] *= -1.f;
402 }
403
404 // if rotation angle is greater than limit (cos is less than limit)
405 if( mQ[VW] < cos_angle_lim )
406 {
407 mQ[VW] = cos_angle_lim;
408 F32 axis_len = sqrtf( mQ[VX]*mQ[VX] + mQ[VY]*mQ[VY] + mQ[VZ]*mQ[VZ] ); // sin(theta/2)
409 F32 axis_mult_fact = sin_angle_lim / axis_len;
410 mQ[VX] *= axis_mult_fact;
411 mQ[VY] *= axis_mult_fact;
412 mQ[VZ] *= axis_mult_fact;
413 }
414
415 return *this;
416}
417
418// Operators
419
420std::ostream& operator<<(std::ostream &s, const LLQuaternion &a)
421{
422 s << "{ "
423 << a.mQ[VX] << ", " << a.mQ[VY] << ", " << a.mQ[VZ] << ", " << a.mQ[VW]
424 << " }";
425 return s;
426}
427
428
429// Does NOT renormalize the result
430LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b)
431{
432// LLQuaternion::mMultCount++;
433
434 LLQuaternion q(
435 b.mQ[3] * a.mQ[0] + b.mQ[0] * a.mQ[3] + b.mQ[1] * a.mQ[2] - b.mQ[2] * a.mQ[1],
436 b.mQ[3] * a.mQ[1] + b.mQ[1] * a.mQ[3] + b.mQ[2] * a.mQ[0] - b.mQ[0] * a.mQ[2],
437 b.mQ[3] * a.mQ[2] + b.mQ[2] * a.mQ[3] + b.mQ[0] * a.mQ[1] - b.mQ[1] * a.mQ[0],
438 b.mQ[3] * a.mQ[3] - b.mQ[0] * a.mQ[0] - b.mQ[1] * a.mQ[1] - b.mQ[2] * a.mQ[2]
439 );
440 return q;
441}
442
443/*
444LLMatrix4 operator*(const LLMatrix4 &m, const LLQuaternion &q)
445{
446 LLMatrix4 qmat(q);
447 return (m*qmat);
448}
449*/
450
451
452
453LLVector4 operator*(const LLVector4 &a, const LLQuaternion &rot)
454{
455 F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
456 F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
457 F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
458 F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
459
460 F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
461 F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
462 F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
463
464 return LLVector4(nx, ny, nz, a.mV[VW]);
465}
466
467LLVector3 operator*(const LLVector3 &a, const LLQuaternion &rot)
468{
469 F32 rw = - rot.mQ[VX] * a.mV[VX] - rot.mQ[VY] * a.mV[VY] - rot.mQ[VZ] * a.mV[VZ];
470 F32 rx = rot.mQ[VW] * a.mV[VX] + rot.mQ[VY] * a.mV[VZ] - rot.mQ[VZ] * a.mV[VY];
471 F32 ry = rot.mQ[VW] * a.mV[VY] + rot.mQ[VZ] * a.mV[VX] - rot.mQ[VX] * a.mV[VZ];
472 F32 rz = rot.mQ[VW] * a.mV[VZ] + rot.mQ[VX] * a.mV[VY] - rot.mQ[VY] * a.mV[VX];
473
474 F32 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
475 F32 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
476 F32 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
477
478 return LLVector3(nx, ny, nz);
479}
480
481LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot)
482{
483 F64 rw = - rot.mQ[VX] * a.mdV[VX] - rot.mQ[VY] * a.mdV[VY] - rot.mQ[VZ] * a.mdV[VZ];
484 F64 rx = rot.mQ[VW] * a.mdV[VX] + rot.mQ[VY] * a.mdV[VZ] - rot.mQ[VZ] * a.mdV[VY];
485 F64 ry = rot.mQ[VW] * a.mdV[VY] + rot.mQ[VZ] * a.mdV[VX] - rot.mQ[VX] * a.mdV[VZ];
486 F64 rz = rot.mQ[VW] * a.mdV[VZ] + rot.mQ[VX] * a.mdV[VY] - rot.mQ[VY] * a.mdV[VX];
487
488 F64 nx = - rw * rot.mQ[VX] + rx * rot.mQ[VW] - ry * rot.mQ[VZ] + rz * rot.mQ[VY];
489 F64 ny = - rw * rot.mQ[VY] + ry * rot.mQ[VW] - rz * rot.mQ[VX] + rx * rot.mQ[VZ];
490 F64 nz = - rw * rot.mQ[VZ] + rz * rot.mQ[VW] - rx * rot.mQ[VY] + ry * rot.mQ[VX];
491
492 return LLVector3d(nx, ny, nz);
493}
494
495F32 dot(const LLQuaternion &a, const LLQuaternion &b)
496{
497 return a.mQ[VX] * b.mQ[VX] +
498 a.mQ[VY] * b.mQ[VY] +
499 a.mQ[VZ] * b.mQ[VZ] +
500 a.mQ[VW] * b.mQ[VW];
501}
502
503// DEMO HACK: This lerp is probably inocrrect now due intermediate normalization
504// it should look more like the lerp below
505#if 0
506// linear interpolation
507LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
508{
509 LLQuaternion r;
510 r = t * (q - p) + p;
511 r.normQuat();
512 return r;
513}
514#endif
515
516// lerp from identity to q
517LLQuaternion lerp(F32 t, const LLQuaternion &q)
518{
519 LLQuaternion r;
520 r.mQ[VX] = t * q.mQ[VX];
521 r.mQ[VY] = t * q.mQ[VY];
522 r.mQ[VZ] = t * q.mQ[VZ];
523 r.mQ[VW] = t * (q.mQ[VZ] - 1.f) + 1.f;
524 r.normQuat();
525 return r;
526}
527
528LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q)
529{
530 LLQuaternion r;
531 F32 inv_t;
532
533 inv_t = 1.f - t;
534
535 r.mQ[VX] = t * q.mQ[VX] + (inv_t * p.mQ[VX]);
536 r.mQ[VY] = t * q.mQ[VY] + (inv_t * p.mQ[VY]);
537 r.mQ[VZ] = t * q.mQ[VZ] + (inv_t * p.mQ[VZ]);
538 r.mQ[VW] = t * q.mQ[VW] + (inv_t * p.mQ[VW]);
539 r.normQuat();
540 return r;
541}
542
543
544// spherical linear interpolation
545LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b )
546{
547 // cosine theta = dot product of a and b
548 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];
549
550 // if b is on opposite hemisphere from a, use -a instead
551 int bflip;
552 if (cos_t < 0.0f)
553 {
554 cos_t = -cos_t;
555 bflip = TRUE;
556 }
557 else
558 bflip = FALSE;
559
560 // if B is (within precision limits) the same as A,
561 // just linear interpolate between A and B.
562 F32 alpha; // interpolant
563 F32 beta; // 1 - interpolant
564 if (1.0f - cos_t < 0.00001f)
565 {
566 beta = 1.0f - u;
567 alpha = u;
568 }
569 else
570 {
571 F32 theta = acosf(cos_t);
572 F32 sin_t = sinf(theta);
573 beta = sinf(theta - u*theta) / sin_t;
574 alpha = sinf(u*theta) / sin_t;
575 }
576
577 if (bflip)
578 beta = -beta;
579
580 // interpolate
581 LLQuaternion ret;
582 ret.mQ[0] = beta*a.mQ[0] + alpha*b.mQ[0];
583 ret.mQ[1] = beta*a.mQ[1] + alpha*b.mQ[1];
584 ret.mQ[2] = beta*a.mQ[2] + alpha*b.mQ[2];
585 ret.mQ[3] = beta*a.mQ[3] + alpha*b.mQ[3];
586
587 return ret;
588}
589
590// lerp whenever possible
591LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b)
592{
593 if (dot(a, b) < 0.f)
594 {
595 return slerp(t, a, b);
596 }
597 else
598 {
599 return lerp(t, a, b);
600 }
601}
602
603LLQuaternion nlerp(F32 t, const LLQuaternion &q)
604{
605 if (q.mQ[VW] < 0.f)
606 {
607 return slerp(t, q);
608 }
609 else
610 {
611 return lerp(t, q);
612 }
613}
614
615// slerp from identity quaternion to another quaternion
616LLQuaternion slerp(F32 t, const LLQuaternion &q)
617{
618 F32 c = q.mQ[VW];
619 if (1.0f == t || 1.0f == c)
620 {
621 // the trivial cases
622 return q;
623 }
624
625 LLQuaternion r;
626 F32 s, angle, stq, stp;
627
628 s = (F32) sqrt(1.f - c*c);
629
630 if (c < 0.0f)
631 {
632 // when c < 0.0 then theta > PI/2
633 // since quat and -quat are the same rotation we invert one of
634 // p or q to reduce unecessary spins
635 // A equivalent way to do it is to convert acos(c) as if it had been negative,
636 // and to negate stp
637 angle = (F32) acos(-c);
638 stp = -(F32) sin(angle * (1.f - t));
639 stq = (F32) sin(angle * t);
640 }
641 else
642 {
643 angle = (F32) acos(c);
644 stp = (F32) sin(angle * (1.f - t));
645 stq = (F32) sin(angle * t);
646 }
647
648 r.mQ[VX] = (q.mQ[VX] * stq) / s;
649 r.mQ[VY] = (q.mQ[VY] * stq) / s;
650 r.mQ[VZ] = (q.mQ[VZ] * stq) / s;
651 r.mQ[VW] = (stp + q.mQ[VW] * stq) / s;
652
653 return r;
654}
655
656LLQuaternion mayaQ(F32 xRot, F32 yRot, F32 zRot, LLQuaternion::Order order)
657{
658 LLQuaternion xQ( xRot*DEG_TO_RAD, LLVector3(1.0f, 0.0f, 0.0f) );
659 LLQuaternion yQ( yRot*DEG_TO_RAD, LLVector3(0.0f, 1.0f, 0.0f) );
660 LLQuaternion zQ( zRot*DEG_TO_RAD, LLVector3(0.0f, 0.0f, 1.0f) );
661 LLQuaternion ret;
662 switch( order )
663 {
664 case LLQuaternion::XYZ:
665 ret = xQ * yQ * zQ;
666 break;
667 case LLQuaternion::YZX:
668 ret = yQ * zQ * xQ;
669 break;
670 case LLQuaternion::ZXY:
671 ret = zQ * xQ * yQ;
672 break;
673 case LLQuaternion::XZY:
674 ret = xQ * zQ * yQ;
675 break;
676 case LLQuaternion::YXZ:
677 ret = yQ * xQ * zQ;
678 break;
679 case LLQuaternion::ZYX:
680 ret = zQ * yQ * xQ;
681 break;
682 }
683 return ret;
684}
685
686const char *OrderToString( const LLQuaternion::Order order )
687{
688 char *p = NULL;
689 switch( order )
690 {
691 default:
692 case LLQuaternion::XYZ:
693 p = "XYZ";
694 break;
695 case LLQuaternion::YZX:
696 p = "YZX";
697 break;
698 case LLQuaternion::ZXY:
699 p = "ZXY";
700 break;
701 case LLQuaternion::XZY:
702 p = "XZY";
703 break;
704 case LLQuaternion::YXZ:
705 p = "YXZ";
706 break;
707 case LLQuaternion::ZYX:
708 p = "ZYX";
709 break;
710 }
711 return p;
712}
713
714LLQuaternion::Order StringToOrder( const char *str )
715{
716 if (strncmp(str, "XYZ", 3)==0 || strncmp(str, "xyz", 3)==0)
717 return LLQuaternion::XYZ;
718
719 if (strncmp(str, "YZX", 3)==0 || strncmp(str, "yzx", 3)==0)
720 return LLQuaternion::YZX;
721
722 if (strncmp(str, "ZXY", 3)==0 || strncmp(str, "zxy", 3)==0)
723 return LLQuaternion::ZXY;
724
725 if (strncmp(str, "XZY", 3)==0 || strncmp(str, "xzy", 3)==0)
726 return LLQuaternion::XZY;
727
728 if (strncmp(str, "YXZ", 3)==0 || strncmp(str, "yxz", 3)==0)
729 return LLQuaternion::YXZ;
730
731 if (strncmp(str, "ZYX", 3)==0 || strncmp(str, "zyx", 3)==0)
732 return LLQuaternion::ZYX;
733
734 return LLQuaternion::XYZ;
735}
736
737const LLQuaternion& LLQuaternion::setQuat(const LLMatrix3 &mat)
738{
739 *this = mat.quaternion();
740 normQuat();
741 return (*this);
742}
743
744const LLQuaternion& LLQuaternion::setQuat(const LLMatrix4 &mat)
745{
746 *this = mat.quaternion();
747 normQuat();
748 return (*this);
749}
750
751void LLQuaternion::getAngleAxis(F32* angle, LLVector3 &vec) const
752{
753 F32 cos_a = mQ[VW];
754 if (cos_a > 1.0f) cos_a = 1.0f;
755 if (cos_a < -1.0f) cos_a = -1.0f;
756
757 F32 sin_a = (F32) sqrt( 1.0f - cos_a * cos_a );
758
759 if ( fabs( sin_a ) < 0.0005f )
760 sin_a = 1.0f;
761 else
762 sin_a = 1.f/sin_a;
763
764 *angle = 2.0f * (F32) acos( cos_a );
765 vec.mV[VX] = mQ[VX] * sin_a;
766 vec.mV[VY] = mQ[VY] * sin_a;
767 vec.mV[VZ] = mQ[VZ] * sin_a;
768}
769
770
771// quaternion does not need to be normalized
772void LLQuaternion::getEulerAngles(F32 *roll, F32 *pitch, F32 *yaw) const
773{
774 LLMatrix3 rot_mat(*this);
775 rot_mat.orthogonalize();
776 rot_mat.getEulerAngles(roll, pitch, yaw);
777
778// // NOTE: LLQuaternion's are actually inverted with respect to
779// // the matrices, so this code also assumes inverted quaternions
780// // (-x, -y, -z, w). The result is that roll,pitch,yaw are applied
781// // in reverse order (yaw,pitch,roll).
782// F32 x = -mQ[VX], y = -mQ[VY], z = -mQ[VZ], w = mQ[VW];
783// F64 m20 = 2.0*(x*z-y*w);
784// if (1.0f - fabsf(m20) < F_APPROXIMATELY_ZERO)
785// {
786// *roll = 0.0f;
787// *pitch = (F32)asin(m20);
788// *yaw = (F32)atan2(2.0*(x*y-z*w), 1.0 - 2.0*(x*x+z*z));
789// }
790// else
791// {
792// *roll = (F32)atan2(-2.0*(y*z+x*w), 1.0-2.0*(x*x+y*y));
793// *pitch = (F32)asin(m20);
794// *yaw = (F32)atan2(-2.0*(x*y+z*w), 1.0-2.0*(y*y+z*z));
795// }
796}
797
798// Saves space by using the fact that our quaternions are normalized
799LLVector3 LLQuaternion::packToVector3() const
800{
801 if( mQ[VW] >= 0 )
802 {
803 return LLVector3( mQ[VX], mQ[VY], mQ[VZ] );
804 }
805 else
806 {
807 return LLVector3( -mQ[VX], -mQ[VY], -mQ[VZ] );
808 }
809}
810
811// Saves space by using the fact that our quaternions are normalized
812void LLQuaternion::unpackFromVector3( const LLVector3& vec )
813{
814 mQ[VX] = vec.mV[VX];
815 mQ[VY] = vec.mV[VY];
816 mQ[VZ] = vec.mV[VZ];
817 F32 t = 1.f - vec.magVecSquared();
818 if( t > 0 )
819 {
820 mQ[VW] = sqrt( t );
821 }
822 else
823 {
824 // Need this to avoid trying to find the square root of a negative number due
825 // to floating point error.
826 mQ[VW] = 0;
827 }
828}
829
830BOOL LLQuaternion::parseQuat(const char* buf, LLQuaternion* value)
831{
832 if( buf == NULL || buf[0] == '\0' || value == NULL)
833 {
834 return FALSE;
835 }
836
837 LLQuaternion quat;
838 S32 count = sscanf( buf, "%f %f %f %f", quat.mQ + 0, quat.mQ + 1, quat.mQ + 2, quat.mQ + 3 );
839 if( 4 == count )
840 {
841 value->setQuat( quat );
842 return TRUE;
843 }
844
845 return FALSE;
846}
847
848
849// End
generated by cgit v1.1 at 2025-01-15 12:58:17 +0000