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