1 /*---------------------------------------------------------------------------*
2 Project: Matrix vector Library
3 File: quat.c
4
5 Copyright 1998-2001 Nintendo. All rights reserved.
6
7 These coded instructions, statements, and computer programs contain
8 proprietary information of Nintendo of America Inc. and/or Nintendo
9 Company Ltd., and are protected by Federal copyright law. They may
10 not be disclosed to third parties or copied or duplicated in any form,
11 in whole or in part, without the prior written consent of Nintendo.
12
13 $Log: quat.c,v $
14 Revision 1.3 2007/01/11 00:45:26 aka
15 Removed win32.h.
16
17 Revision 1.2 2006/02/20 04:25:42 mitu
18 Changed include path from dolphin/ to revolution/.
19
20 Revision 1.1.1.1 2005/05/12 02:15:49 yasuh-to
21 Ported from dolphin source tree.
22
23
24 9 2002/10/15 16:01 Hirose
25 Added one more const to C_QUATMakeClosest( ).
26 Fixed implementation also.
27
28 8 2002/08/20 16:50 Hirose
29 Workaround for divided-by-zero exceptions.
30
31 7 2002/04/11 13:11 Hirose
32 const type specifier support. (by Hiratsu@IRD)
33
34 6 2002/01/04 6:03p Hirose
35 Fixed wrong implementation of QUATMtx.
36
37 5 2001/09/19 4:00p Hirose
38 Workaround for the WIN32 build.
39
40 4 2001/09/12 10:40a Hirose
41 Removed unnecessary possibilities of zero division exception.
42
43 3 2001/08/27 3:55p Hirose
44 Added some Paired-single assembler functions.
45
46 2 2001/08/20 10:52a Hirose
47 Fixed a function name bug. Used temporary variable in C_QUATMultiply.
48
49 1 2001/08/03 1:20a Hirose
50 Initial check in.
51
52 $NoKeywords: $
53
54 *---------------------------------------------------------------------------*/
55
56 #include <math.h>
57 #include <revolution/mtx.h>
58 #include "mtxAssert.h"
59
60 #ifdef WIN32
61 #define atan2f (f32)atan2
62 #endif
63
64 #define QUAT_EPSILON 0.00001F
65
66 /*---------------------------------------------------------------------------*
67 Name: QUATAdd
68
69 Description: Returns the sum of two quaternions.
70
71 Arguments: p: first quaternion
72 q: second quaternion
73 r: resultant quaternion p+q
74
75 Returns: none
76 *---------------------------------------------------------------------------*/
77 /*---------------------------------------------------------------------------*
78 C version
79 *---------------------------------------------------------------------------*/
C_QUATAdd(const Quaternion * p,const Quaternion * q,Quaternion * r)80 void C_QUATAdd( const Quaternion *p, const Quaternion *q, Quaternion *r )
81 {
82 ASSERTMSG( ( p != 0 ), QUAT_ADD_1 );
83 ASSERTMSG( ( q != 0 ), QUAT_ADD_2 );
84 ASSERTMSG( ( r != 0 ), QUAT_ADD_3 );
85
86 r->x = p->x + q->x;
87 r->y = p->y + q->y;
88 r->z = p->z + q->z;
89 r->w = p->w + q->w;
90 }
91
92 /*---------------------------------------------------------------------------*
93 Paired-Single assembler version
94 *---------------------------------------------------------------------------*
95 Note that this performs NO error checking.
96 *---------------------------------------------------------------------------*/
97 #ifdef GEKKO
PSQUATAdd(const register Quaternion * p,const register Quaternion * q,register Quaternion * r)98 void PSQUATAdd
99 (
100 const register Quaternion * p,
101 const register Quaternion * q,
102 register Quaternion * r
103 )
104 {
105 register f32 pxy, qxy, rxy, pzw, qzw, rzw;
106
107 asm
108 {
109 psq_l pxy, 0(p), 0, 0
110 psq_l qxy, 0(q), 0, 0
111 ps_add rxy, pxy, qxy
112 psq_st rxy, 0(r), 0, 0
113
114 psq_l pzw, 8(p), 0, 0
115 psq_l qzw, 8(q), 0, 0
116 ps_add rzw, pzw, qzw
117 psq_st rzw, 8(r), 0, 0
118 }
119 }
120 #endif
121
122 /*---------------------------------------------------------------------------*
123 Name: QUATSubtract
124
125 Description: Returns the difference of two quaternions p-q.
126
127 Arguments: p: left quaternion
128 q: right quaternion
129 r: resultant quaternion difference p-q
130
131 Returns: none
132 *---------------------------------------------------------------------------*/
133 /*---------------------------------------------------------------------------*
134 C version
135 *---------------------------------------------------------------------------*/
C_QUATSubtract(const Quaternion * p,const Quaternion * q,Quaternion * r)136 void C_QUATSubtract( const Quaternion *p, const Quaternion *q, Quaternion *r )
137 {
138 ASSERTMSG( ( p != 0 ), QUAT_SUBTRACT_1 );
139 ASSERTMSG( ( q != 0 ), QUAT_SUBTRACT_2 );
140 ASSERTMSG( ( r != 0 ), QUAT_SUBTRACT_3 );
141
142 r->x = p->x - q->x;
143 r->y = p->y - q->y;
144 r->z = p->z - q->z;
145 r->w = p->w - q->w;
146 }
147
148 /*---------------------------------------------------------------------------*
149 Paired-Single assembler version
150 *---------------------------------------------------------------------------*
151 Note that this performs NO error checking.
152 *---------------------------------------------------------------------------*/
153 #ifdef GEKKO
PSQUATSubtract(const register Quaternion * p,const register Quaternion * q,register Quaternion * r)154 void PSQUATSubtract
155 (
156 const register Quaternion * p,
157 const register Quaternion * q,
158 register Quaternion * r
159 )
160 {
161 register f32 pxy, qxy, rxy, pzw, qzw, rzw;
162
163 asm
164 {
165 psq_l pxy, 0(p), 0, 0
166 psq_l qxy, 0(q), 0, 0
167 ps_sub rxy, pxy, qxy
168 psq_st rxy, 0(r), 0, 0
169
170 psq_l pzw, 8(p), 0, 0
171 psq_l qzw, 8(q), 0, 0
172 ps_sub rzw, pzw, qzw
173 psq_st rzw, 8(r), 0, 0
174 }
175 }
176 #endif
177
178 /*---------------------------------------------------------------------------*
179 Name: QUATMultiply
180
181 Description: Returns the product of two quaternions. The order of
182 multiplication is important. (p*q != q*p)
183
184 Arguments: p: left quaternion
185 q: right quaternion
186 pq: resultant quaternion product p*q
187
188 Returns: none
189 *---------------------------------------------------------------------------*/
190 /*---------------------------------------------------------------------------*
191 C version
192 *---------------------------------------------------------------------------*/
C_QUATMultiply(const Quaternion * p,const Quaternion * q,Quaternion * pq)193 void C_QUATMultiply( const Quaternion *p, const Quaternion *q, Quaternion *pq )
194 {
195 Quaternion *r;
196 Quaternion pqTmp;
197
198 ASSERTMSG( ( p != 0 ), QUAT_MULTIPLY_1 );
199 ASSERTMSG( ( q != 0 ), QUAT_MULTIPLY_2 );
200 ASSERTMSG( ( pq != 0 ), QUAT_MULTIPLY_3 );
201
202 if ( p == pq || q == pq )
203 {
204 r = &pqTmp;
205 }
206 else
207 {
208 r = pq;
209 }
210
211 r->w = p->w*q->w - p->x*q->x - p->y*q->y - p->z*q->z;
212 r->x = p->w*q->x + p->x*q->w + p->y*q->z - p->z*q->y;
213 r->y = p->w*q->y + p->y*q->w + p->z*q->x - p->x*q->z;
214 r->z = p->w*q->z + p->z*q->w + p->x*q->y - p->y*q->x;
215
216 if ( r == &pqTmp )
217 {
218 *pq = pqTmp;
219 }
220 }
221
222 /*---------------------------------------------------------------------------*
223 Paired-Single assembler version
224 *---------------------------------------------------------------------------*
225 Note that this performs NO error checking.
226 *---------------------------------------------------------------------------*/
227 #ifdef GEKKO
PSQUATMultiply(const register Quaternion * p,const register Quaternion * q,register Quaternion * pq)228 void PSQUATMultiply
229 (
230 const register Quaternion *p,
231 const register Quaternion *q,
232 register Quaternion *pq
233 )
234 {
235 register f32 pxy, pzw, qxy, qzw;
236 register f32 pnxy, pnzw, pnxny, pnznw;
237 register f32 rxy, rzw, sxy, szw;
238
239 asm
240 {
241 // [px][py] : Load
242 psq_l pxy, 0(p), 0, 0
243 // [pz][pw] : Load
244 psq_l pzw, 8(p), 0, 0
245
246 // [qx][qy] : Load
247 psq_l qxy, 0(q), 0, 0
248 // [-px][-py]
249 ps_neg pnxny, pxy
250 // [qz][qw] : Load
251 psq_l qzw, 8(q), 0, 0
252 // [-pz][-pw]
253 ps_neg pnznw, pzw
254
255 // [-px][py]
256 ps_merge01 pnxy, pnxny, pxy
257
258 // [pz*qx][pw*qx]
259 ps_muls0 rxy, pzw, qxy
260 // [-px*qx][-py*qx]
261 ps_muls0 rzw, pnxny, qxy
262
263 // [-pz][pw]
264 ps_merge01 pnzw, pnznw, pzw
265
266 // [-px*qy][py*qy]
267 ps_muls1 szw, pnxy, qxy
268 // [pz*qx-px*qz][pw*qx+py*qz]
269 ps_madds0 rxy, pnxy, qzw, rxy
270 // [-pz*qy][pw*qy]
271 ps_muls1 sxy, pnzw, qxy
272 // [-px*qx-pz*qz][-py*qx+pw*qz]
273 ps_madds0 rzw, pnzw, qzw, rzw
274 // [-px*qy-pz*qw][py*qy-pw*qw]
275 ps_madds1 szw, pnznw, qzw, szw
276 // [pw*qx+py*qz][pz*qx-px*qz]
277 ps_merge10 rxy, rxy, rxy
278 // [-pz*qy+px*qw][pw*qy+py*qw]
279 ps_madds1 sxy, pxy, qzw, sxy
280 // [-py*qx+pw*qz][-px*qx-pz*qz]
281 ps_merge10 rzw, rzw, rzw
282
283 // [pw*qx+py*qz-pz*qy+px*qw][pz*qx-px*qz+pw*qy+py*qw] : [pqx][pqy]
284 ps_add rxy, rxy, sxy
285 // [pqx][pqy] : Store
286 psq_st rxy, 0(pq), 0, 0
287 // [-py*qx+pw*qz+px*qy+pz*qw][-px*qx-pz*qz-py*qy+pw*qw] : [pqz][pqw]
288 ps_sub rzw, rzw, szw
289 // [pqz][pqw] : Store
290 psq_st rzw, 8(pq), 0, 0
291 }
292 }
293 #endif
294
295 /*---------------------------------------------------------------------------*
296 Name: QUATScale
297
298 Description: Scales a quaternion.
299
300 Arguments: q: quaternion
301 r: resultant scaled quaternion
302 scale: float to scale by
303
304 Returns: none
305 *---------------------------------------------------------------------------*/
306 /*---------------------------------------------------------------------------*
307 C version
308 *---------------------------------------------------------------------------*/
C_QUATScale(const Quaternion * q,Quaternion * r,f32 scale)309 void C_QUATScale( const Quaternion *q, Quaternion *r, f32 scale )
310 {
311 ASSERTMSG( ( q != 0 ), QUAT_SCALE_1 );
312 ASSERTMSG( ( r != 0 ), QUAT_SCALE_2 );
313
314 r->x = q->x * scale;
315 r->y = q->y * scale;
316 r->z = q->z * scale;
317 r->w = q->w * scale;
318 }
319
320 /*---------------------------------------------------------------------------*
321 Paired-Single assembler version
322 *---------------------------------------------------------------------------*
323 Note that this performs NO error checking.
324 *---------------------------------------------------------------------------*/
325 #ifdef GEKKO
PSQUATScale(const register Quaternion * q,register Quaternion * r,register f32 scale)326 void PSQUATScale
327 (
328 const register Quaternion *q,
329 register Quaternion *r,
330 register f32 scale
331 )
332 {
333 register f32 rxy, rzw;
334
335 asm
336 {
337 psq_l rxy, 0(q), 0, 0
338 psq_l rzw, 8(q), 0, 0
339 ps_muls0 rxy, rxy, scale
340 psq_st rxy, 0(r), 0, 0
341 ps_muls0 rzw, rzw, scale
342 psq_st rzw, 8(r), 0, 0
343 }
344 }
345 #endif
346
347 /*---------------------------------------------------------------------------*
348 Name: QUATDotProduct
349
350 Description: Returns the dot product of the two quaternions.
351
352 Arguments: p: first quaternion
353 q: second quaternion
354
355 Returns: Dot product
356 *---------------------------------------------------------------------------*/
357 /*---------------------------------------------------------------------------*
358 C version
359 *---------------------------------------------------------------------------*/
C_QUATDotProduct(const Quaternion * p,const Quaternion * q)360 f32 C_QUATDotProduct( const Quaternion *p, const Quaternion *q )
361 {
362 ASSERTMSG( ( p != 0 ), QUAT_DOTPRODUCT_1 );
363 ASSERTMSG( ( q != 0 ), QUAT_DOTPRODUCT_2 );
364
365 return (q->x*p->x + q->y*p->y + q->z*p->z + q->w*p->w);
366 }
367
368 /*---------------------------------------------------------------------------*
369 Paired-Single assembler version
370 *---------------------------------------------------------------------------*
371 Note that this performs NO error checking.
372 *---------------------------------------------------------------------------*/
373 #ifdef GEKKO
PSQUATDotProduct(const register Quaternion * p,const register Quaternion * q)374 f32 PSQUATDotProduct( const register Quaternion *p, const register Quaternion *q )
375 {
376 register f32 pxy, pzw, qxy, qzw, dp;
377
378 asm
379 {
380 psq_l pxy, 0(p), 0, 0
381 psq_l qxy, 0(q), 0, 0
382 ps_mul dp, pxy, qxy
383
384 psq_l pzw, 8(p), 0, 0
385 psq_l qzw, 8(q), 0, 0
386 ps_madd dp, pzw, qzw, dp
387
388 ps_sum0 dp, dp, dp, dp
389 }
390
391 return dp;
392 }
393 #endif
394
395 /*---------------------------------------------------------------------------*
396 Name: QUATNormalize
397
398 Description: Normalizes a quaternion
399
400 Arguments: src: the source quaternion
401 unit: resulting unit quaternion
402
403 Returns: none
404 *---------------------------------------------------------------------------*/
405 /*---------------------------------------------------------------------------*
406 C version
407 *---------------------------------------------------------------------------*/
C_QUATNormalize(const Quaternion * src,Quaternion * unit)408 void C_QUATNormalize( const Quaternion *src, Quaternion *unit )
409 {
410 f32 mag;
411
412 ASSERTMSG( ( src != 0 ), QUAT_NORMALIZE_1 );
413 ASSERTMSG( ( unit != 0 ), QUAT_NORMALIZE_2 );
414
415 mag = (src->x * src->x) + (src->y * src->y) + (src->z * src->z) + (src->w * src->w);
416
417 if ( mag >= QUAT_EPSILON )
418 {
419 mag = 1.0F / sqrtf(mag);
420
421 unit->x = src->x * mag;
422 unit->y = src->y * mag;
423 unit->z = src->z * mag;
424 unit->w = src->w * mag;
425 }
426 else
427 {
428 unit->x = unit->y = unit->z = unit->w = 0.0F;
429 }
430 }
431
432 /*---------------------------------------------------------------------------*
433 Paired-Single assembler version
434 *---------------------------------------------------------------------------*
435 Note that this performs NO error checking.
436 *---------------------------------------------------------------------------*/
437 #ifdef GEKKO
PSQUATNormalize(const register Quaternion * src,register Quaternion * unit)438 void PSQUATNormalize( const register Quaternion *src, register Quaternion *unit )
439 {
440 register f32 sxy, szw;
441 register f32 mag, rsqmag, diff, c_zero;
442 register f32 nwork0, nwork1;
443 register f32 epsilon = QUAT_EPSILON;
444 register f32 c_half = 0.5F;
445 register f32 c_three = 3.0F;
446
447 asm
448 {
449 psq_l sxy, 0(src), 0, 0
450
451 // mag = [x*x][y*y]
452 ps_mul mag, sxy, sxy
453
454 psq_l szw, 8(src), 0, 0
455
456 // c_zero = [0.0F]
457 ps_sub c_zero, epsilon, epsilon
458 // mag = [x*x+z*z][y*y+w*w]
459 ps_madd mag, szw, szw, mag
460 // mag = [x*x+y*y+z*z+w*w][N/A]
461 ps_sum0 mag, mag, mag, mag
462
463 // rsqmag = 1.0F / sqrtf(mag) : estimation
464 frsqrte rsqmag, mag
465 // diff = mag - epsilon
466 ps_sub diff, mag, epsilon
467 // Newton-Rapson refinement (x1) : E' = (E/2)(3 - X * E * E)
468 fmul nwork0, rsqmag, rsqmag
469 fmul nwork1, rsqmag, c_half
470 fnmsub nwork0, nwork0, mag, c_three
471 fmul rsqmag, nwork0, nwork1
472
473 // rsqmag = ( mag >= epsilon ) ? rsqmag : 0
474 ps_sel rsqmag, diff, rsqmag, c_zero
475 // sxy = [x*rsqmag][y*rsqmag]
476 ps_muls0 sxy, sxy, rsqmag
477 // szw = [z*rsqmag][w*rsqmag]
478 ps_muls0 szw, szw, rsqmag
479
480 psq_st sxy, 0(unit), 0, 0
481 psq_st szw, 8(unit), 0, 0
482 }
483 }
484 #endif
485
486 /*---------------------------------------------------------------------------*
487 Name: QUATInverse
488
489 Description: Returns the inverse of the quaternion.
490
491 Arguments: src: the source quaternion
492 inv: resulting inverse quaternion
493
494 Returns: none
495 *---------------------------------------------------------------------------*/
496 /*---------------------------------------------------------------------------*
497 C version
498 *---------------------------------------------------------------------------*/
C_QUATInverse(const Quaternion * src,Quaternion * inv)499 void C_QUATInverse( const Quaternion *src, Quaternion *inv )
500 {
501 f32 mag, norminv;
502
503 ASSERTMSG( ( src != 0 ), QUAT_INVERSE_1 );
504 ASSERTMSG( ( inv != 0 ), QUAT_INVERSE_2 );
505
506 mag = ( src->x*src->x + src->y*src->y + src->z*src->z + src->w*src->w );
507
508 if ( mag == 0.0f )
509 {
510 mag = 1.0f;
511 }
512
513 // [Inverse] = [Conjugate] / [Magnitude]
514 norminv = 1.0f / mag;
515 inv->x = -src->x * norminv;
516 inv->y = -src->y * norminv;
517 inv->z = -src->z * norminv;
518 inv->w = src->w * norminv;
519 }
520
521 /*---------------------------------------------------------------------------*
522 Paired-Single assembler version
523 *---------------------------------------------------------------------------*
524 Note that this performs NO error checking.
525 *---------------------------------------------------------------------------*/
526 #ifdef GEKKO
PSQUATInverse(const register Quaternion * src,register Quaternion * inv)527 void PSQUATInverse( const register Quaternion *src, register Quaternion *inv )
528 {
529 register f32 sxy, szw, izz, iww;
530 register f32 mag, nmag, norminv, nninv, nwork0, c_two, c_zero;
531 register f32 c_one = 1.0F;
532
533 asm
534 {
535 // load xy
536 psq_l sxy, 0(src), 0, 0
537
538 // mag = [x*x][y*y]
539 ps_mul mag, sxy, sxy
540 // c_zero = [0.0F]
541 ps_sub c_zero, c_one, c_one
542
543 // load zw
544 psq_l szw, 8(src), 0, 0
545
546 // mag = [x*x+z*z][y*y+w*w]
547 ps_madd mag, szw, szw, mag
548 // c_two = [2.0F]
549 ps_add c_two, c_one, c_one
550 // mag = [x*x+y*y+z*z+w*w][N/A]
551 ps_sum0 mag, mag, mag, mag
552
553 // zero check
554 fcmpu cr0, mag, c_zero
555 beq- __zero
556
557 // norminv = 1.0F / mag
558 fres norminv, mag
559 // nmag = -mag
560 ps_neg nmag, mag
561 // Newton-Rapson refinment (x1) : E' = 2E-X*E*E
562 ps_nmsub nwork0, mag, norminv, c_two
563 ps_mul norminv, norminv, nwork0
564 b __mulnorm
565
566 __zero:
567 fmr norminv, c_one
568
569 __mulnorm:
570 // nninv = [ -norminv ]
571 ps_neg nninv, norminv
572
573 // iww = [ w*norminv ][ N/A ]
574 ps_muls1 iww, norminv, szw
575 // sxy = [ -x*norminv ][ -y*norminv ]
576 ps_muls0 sxy, sxy, nninv
577
578 // store w
579 psq_st iww, 12(inv), 1, 0
580
581 // izz = [ -z*norminv ][ N/A ]
582 ps_muls0 izz, szw, nninv
583
584 // store xy
585 psq_st sxy, 0(inv), 0, 0
586 // store z
587 psq_st izz, 8(inv), 1, 0
588 }
589 }
590 #endif
591
592 /*---------------------------------------------------------------------------*
593 Name: QUATDivide
594
595 Description: Returns the ratio of two quaternions. Creates a result
596 r = p/q such that q*r=p (order of multiplication is important).
597
598 Arguments: p: left quaternion
599 q: right quaternion
600 r: resultant ratio quaterion p/q
601
602 Returns: none
603 *---------------------------------------------------------------------------*/
604 /*---------------------------------------------------------------------------*
605 C version
606 *---------------------------------------------------------------------------*/
C_QUATDivide(const Quaternion * p,const Quaternion * q,Quaternion * r)607 void C_QUATDivide( const Quaternion *p, const Quaternion *q, Quaternion *r )
608 {
609 Quaternion qtmp;
610
611 ASSERTMSG( ( p != 0 ), QUAT_DIVIDE_1 );
612 ASSERTMSG( ( q != 0 ), QUAT_DIVIDE_2 );
613 ASSERTMSG( ( r != 0 ), QUAT_DIVIDE_3 );
614
615 C_QUATInverse(q, &qtmp);
616 C_QUATMultiply(&qtmp, p, r);
617 }
618
619 /*---------------------------------------------------------------------------*
620 Paired-Single assembler version
621 *---------------------------------------------------------------------------*
622 Note that this performs NO error checking.
623 *---------------------------------------------------------------------------*/
624 #ifdef GEKKO
PSQUATDivide(const Quaternion * p,const Quaternion * q,Quaternion * r)625 void PSQUATDivide( const Quaternion *p, const Quaternion *q, Quaternion *r )
626 {
627 Quaternion qtmp;
628
629 PSQUATInverse(q, &qtmp);
630 PSQUATMultiply(&qtmp, p, r);
631 }
632 #endif
633
634 /*---------------------------------------------------------------------------*
635 Name: QUATExp
636
637 Description: Exponentiate quaternion (where q.w == 0).
638
639 Arguments: q: pure quaternion
640 r: resultant exponentiate quaternion (an unit quaternion)
641
642 Returns: none
643 *---------------------------------------------------------------------------*/
C_QUATExp(const Quaternion * q,Quaternion * r)644 void C_QUATExp( const Quaternion *q, Quaternion *r )
645 {
646 f32 theta, scale;
647
648 ASSERTMSG( ( q != 0 ), QUAT_EXP_1 );
649 ASSERTMSG( ( r != 0 ), QUAT_EXP_2 );
650
651 // pure quaternion check
652 ASSERTMSG( ( q->w == 0.0F ), QUAT_EXP_3 );
653
654 theta = sqrtf( q->x*q->x + q->y*q->y + q->z*q->z );
655 scale = 1.0F;
656
657 if ( theta > QUAT_EPSILON )
658 scale = (f32)sinf(theta)/theta;
659
660 r->x = scale * q->x;
661 r->y = scale * q->y;
662 r->z = scale * q->z;
663 r->w = (f32)cosf(theta);
664 }
665
666
667 /*---------------------------------------------------------------------------*
668 Name: QUATLogN
669
670 Description: Returns the natural logarithm of a UNIT quaternion
671
672 Arguments: q: unit quaternion
673 r: resultant logarithm quaternion (an pure quaternion)
674
675 Returns: none
676 *---------------------------------------------------------------------------*/
C_QUATLogN(const Quaternion * q,Quaternion * r)677 void C_QUATLogN( const Quaternion *q, Quaternion *r )
678 {
679 f32 theta,scale;
680
681 ASSERTMSG( ( q != 0 ), QUAT_LOGN_1 );
682 ASSERTMSG( ( r != 0 ), QUAT_LOGN_2 );
683
684 scale = q->x*q->x + q->y*q->y + q->z*q->z;
685
686 // unit quaternion check
687 #ifdef _DEBUG
688 {
689 f32 mag;
690 mag = scale + q->z*q->z;
691 if ( mag < 1.0F - QUAT_EPSILON || mag > 1.0F + QUAT_EPSILON )
692 {
693 ASSERT( QUAT_LOGN_3 );
694 }
695 }
696 #endif
697
698 scale = sqrtf(scale);
699 theta = atan2f( scale, q->w );
700
701 if ( scale > 0.0F )
702 scale = theta/scale;
703
704 r->x = scale*q->x;
705 r->y = scale*q->y;
706 r->z = scale*q->z;
707 r->w = 0.0F;
708
709 }
710
711
712 /*---------------------------------------------------------------------------*
713 Name: QUATMakeClosest
714
715 Description: Modify q so it is on the same side of the hypersphere as qto
716
717 Arguments: q: quaternion
718 qto: quaternion to be close to
719 r: resultant modified quaternion
720
721 Returns: NONE
722 *---------------------------------------------------------------------------*/
C_QUATMakeClosest(const Quaternion * q,const Quaternion * qto,Quaternion * r)723 void C_QUATMakeClosest( const Quaternion *q, const Quaternion *qto, Quaternion *r )
724 {
725 f32 dot;
726
727 ASSERTMSG( ( q != 0 ), QUAT_MAKECLOSEST_1 );
728 ASSERTMSG( ( qto != 0 ), QUAT_MAKECLOSEST_2 );
729 ASSERTMSG( ( r != 0 ), QUAT_MAKECLOSEST_3 );
730
731 dot = q->x*qto->x + q->y*qto->y + q->z*qto->z + q->w*qto->w;
732
733 if ( dot < 0.0f )
734 {
735 r->x = -q->x;
736 r->y = -q->y;
737 r->z = -q->z;
738 r->w = -q->w;
739 }
740 else
741 {
742 *r = *q;
743 }
744 }
745
746
747 /*---------------------------------------------------------------------------*
748 Name: QUATRotAxisRad
749
750 Description: Returns rotation quaternion about arbitrary axis.
751
752 Arguments: r: resultant rotation quaternion
753 axis: rotation axis
754 rad: rotation angle in radians
755
756 Returns: NONE
757 *---------------------------------------------------------------------------*/
C_QUATRotAxisRad(Quaternion * r,const Vec * axis,f32 rad)758 void C_QUATRotAxisRad( Quaternion *r, const Vec *axis, f32 rad )
759 {
760 f32 half, sh, ch;
761 Vec nAxis;
762
763 ASSERTMSG( ( r != 0 ), QUAT_ROTAXISRAD_1 );
764 ASSERTMSG( ( axis != 0 ), QUAT_ROTAXISRAD_2 );
765
766 VECNormalize(axis, &nAxis);
767
768 half = rad * 0.50F;
769 sh = sinf(half);
770 ch = cosf(half);
771
772 r->x = sh * nAxis.x;
773 r->y = sh * nAxis.y;
774 r->z = sh * nAxis.z;
775 r->w = ch;
776 }
777
778
779 /*---------------------------------------------------------------------------*
780 Name: QUATMtx
781
782 Description: Converts a matrix to a unit quaternion.
783
784 Arguments: r: result quaternion
785 m: the matrix
786
787 Returns: NONE
788 *---------------------------------------------------------------------------*/
C_QUATMtx(Quaternion * r,const Mtx m)789 void C_QUATMtx( Quaternion *r, const Mtx m )
790 {
791 f32 tr,s;
792 s32 i,j,k;
793 s32 nxt[3] = {1,2,0};
794 f32 q[3];
795
796 ASSERTMSG( ( r != 0 ), QUAT_MTX_1 );
797 ASSERTMSG( ( m != 0 ), QUAT_MTX_2 );
798
799 tr = m[0][0] + m[1][1] + m[2][2];
800 if( tr > 0.0f )
801 {
802 s = (f32)sqrtf(tr + 1.0f);
803 r->w = s * 0.5f;
804 s = 0.5f / s;
805 r->x = (m[2][1] - m[1][2]) * s;
806 r->y = (m[0][2] - m[2][0]) * s;
807 r->z = (m[1][0] - m[0][1]) * s;
808 }
809 else
810 {
811 i = 0;
812 if (m[1][1] > m[0][0]) i = 1;
813 if (m[2][2] > m[i][i]) i = 2;
814 j = nxt[i];
815 k = nxt[j];
816 s = (f32)sqrtf( (m[i][i] - (m[j][j] + m[k][k])) + 1.0f );
817 q[i] = s * 0.5f;
818
819 if (s!=0.0f)
820 s = 0.5f / s;
821
822 r->w = (m[k][j] - m[j][k]) * s;
823 q[j] = (m[i][j] + m[j][i]) * s;
824 q[k] = (m[i][k] + m[k][i]) * s;
825
826 r->x = q[0];
827 r->y = q[1];
828 r->z = q[2];
829 }
830 }
831
832
833 /*---------------------------------------------------------------------------*
834 Name: QUATLerp
835
836 Description: Linear interpolation of two quaternions.
837
838 Arguments: p: first quaternion
839 q: second quaternion
840 r: resultant interpolated quaternion
841 t: interpolation parameter
842
843 Returns: NONE
844 *---------------------------------------------------------------------------*/
C_QUATLerp(const Quaternion * p,const Quaternion * q,Quaternion * r,f32 t)845 void C_QUATLerp( const Quaternion *p, const Quaternion *q, Quaternion *r, f32 t )
846 {
847 ASSERTMSG( ( p != 0 ), QUAT_LERP_1 );
848 ASSERTMSG( ( q != 0 ), QUAT_LERP_2 );
849 ASSERTMSG( ( r != 0 ), QUAT_LERP_3 );
850
851 r->x = t * ( q->x - p->x ) + p->x;
852 r->y = t * ( q->y - p->y ) + p->y;
853 r->z = t * ( q->z - p->z ) + p->z;
854 r->w = t * ( q->w - p->w ) + p->w;
855 }
856
857
858 /*---------------------------------------------------------------------------*
859 Name: QUATSlerp
860
861 Description: Spherical linear interpolation of two quaternions.
862
863 Arguments: p: first quaternion
864 q: second quaternion
865 r: resultant interpolated quaternion
866 t: interpolation parameter
867
868 Returns: NONE
869 *---------------------------------------------------------------------------*/
C_QUATSlerp(const Quaternion * p,const Quaternion * q,Quaternion * r,f32 t)870 void C_QUATSlerp( const Quaternion *p, const Quaternion *q, Quaternion *r, f32 t )
871 {
872 f32 theta, sin_th, cos_th, tp, tq;
873
874 ASSERTMSG( ( p != 0 ), QUAT_SLERP_1 );
875 ASSERTMSG( ( q != 0 ), QUAT_SLERP_2 );
876 ASSERTMSG( ( r != 0 ), QUAT_SLERP_3 );
877
878 cos_th = p->x * q->x + p->y * q->y + p->z * q->z + p->w * q->w;
879 tq = 1.0F;
880
881 if ( cos_th < 0.0F )
882 {
883 cos_th = -cos_th;
884 tq = -tq;
885 }
886
887 if ( cos_th <= 1.0F - QUAT_EPSILON )
888 {
889 theta = acosf(cos_th);
890 sin_th = sinf(theta);
891 tp = sinf((1.0F - t) * theta) / sin_th;
892 tq *= sinf( t * theta ) / sin_th;
893 }
894 else
895 {
896 // cos(theta) is close to 1.0F -> linear
897 tp = 1.0F - t;
898 tq = tq * t;
899 }
900
901 r->x = tp * p->x + tq * q->x;
902 r->y = tp * p->y + tq * q->y;
903 r->z = tp * p->z + tq * q->z;
904 r->w = tp * p->w + tq * q->w;
905
906 }
907
908
909 /*---------------------------------------------------------------------------*
910 Name: QUATSquad
911
912 Description: Spherical cubic quadrangle interpolation of two quaternions
913 with derrived inner-quadrangle quaternions.
914
915 This will be used with the function QUATCompA().
916
917 Arguments: p: first quaternion
918 a: derrived inner-quadrangle quaternion
919 b: derrived inner-quadrangle quaternion
920 q: second quaternion
921 r: resultant interpolated quaternion
922 t: interpolation parameter
923
924 Returns: NONE
925 *---------------------------------------------------------------------------*/
C_QUATSquad(const Quaternion * p,const Quaternion * a,const Quaternion * b,const Quaternion * q,Quaternion * r,f32 t)926 void C_QUATSquad( const Quaternion *p, const Quaternion *a, const Quaternion *b,
927 const Quaternion *q, Quaternion *r, f32 t )
928 {
929 Quaternion pq, ab;
930 f32 t2;
931
932 ASSERTMSG( ( p != 0 ), QUAT_SQUAD_1 );
933 ASSERTMSG( ( a != 0 ), QUAT_SQUAD_2 );
934 ASSERTMSG( ( b != 0 ), QUAT_SQUAD_3 );
935 ASSERTMSG( ( q != 0 ), QUAT_SQUAD_4 );
936 ASSERTMSG( ( r != 0 ), QUAT_SQUAD_5 );
937
938 t2 = 2 * t * ( 1.0F - t );
939 C_QUATSlerp(p, q, &pq, t);
940 C_QUATSlerp(a, b, &ab, t);
941 C_QUATSlerp(&pq, &ab, r, t2);
942 }
943
944
945 /*---------------------------------------------------------------------------*
946 Name: QUATCompA
947
948 Description: Compute a, the term used in Boehm-type interpolation
949 a[n] = q[n]* qexp(-(1/4)*( ln(qinv(q[n])*q[n+1]) +
950 ln( qinv(q[n])*q[n-1] )))
951
952 Arguments: qprev: previous quaternion
953 q: current quaternion
954 qnext: next quaternion
955 a: result quaternion A
956
957 Returns: none
958 /*---------------------------------------------------------------------------*/
C_QUATCompA(const Quaternion * qprev,const Quaternion * q,const Quaternion * qnext,Quaternion * a)959 void C_QUATCompA( const Quaternion *qprev, const Quaternion *q, const Quaternion *qnext, Quaternion *a )
960 {
961 Quaternion qm, qp, lqm, lqp, qpqm, exq;
962
963 ASSERTMSG( ( qprev != 0 ), QUAT_COMPA_1 );
964 ASSERTMSG( ( q != 0 ), QUAT_COMPA_2 );
965 ASSERTMSG( ( qnext != 0 ), QUAT_COMPA_3 );
966 ASSERTMSG( ( a != 0 ), QUAT_COMPA_4 );
967
968 C_QUATDivide(qprev, q, &qm);
969 C_QUATLogN(&qm, &lqm);
970 C_QUATDivide(qnext, q, &qp);
971 C_QUATLogN(&qp, &lqp);
972
973 C_QUATAdd(&lqp, &lqm, &qpqm);
974 C_QUATScale(&qpqm, &qpqm, -0.25F);
975
976 C_QUATExp(&qpqm, &exq);
977 C_QUATMultiply(q, &exq, a);
978 }
979
980
981 /*===========================================================================*/
982