1 /*---------------------------------------------------------------------------*
2 Project: Matrix vector Library
3 File: vec.c
4
5 Copyright 1998 - 2002 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
14 $Log: vec.c,v $
15 Revision 1.3 2007/01/11 00:45:26 aka
16 Removed win32.h.
17
18 Revision 1.2 2006/02/20 04:25:42 mitu
19 Changed include path from dolphin/ to revolution/.
20
21 Revision 1.1.1.1 2005/05/12 02:15:49 yasuh-to
22 Ported from dolphin source tree.
23
24
25 8 2002/08/20 14:49 Hirose
26 Workaround for divided-by-zero exceptions.
27
28 7 2002/04/11 13:11 Hirose
29 const type specifier support. (by Hiratsu@IRD)
30
31 6 2001/09/12 10:40a Hirose
32 Removed unnecessary possibilities of zero division exception.
33
34 5 2001/08/27 6:45p Hirose
35 PSVECMag/PSVECDistance implementations. Misc. updates.
36
37 4 2001/07/30 11:00p Hirose
38 Fixed MAC build errors regarding missing "#ifdef GEKKO".
39
40 3 2001/07/30 10:23p Hirose
41 Added PSVECMag, PSVECDistance. Some definition changes.
42
43 2 2001/06/11 2:10p Hirose
44 Reduced number of operations in PSVECNormalize.
45
46 1 2001/02/22 11:52p Hirose
47 Integrated PSVEC functions made by Scott Perras@NOA Sdsg.
48
49 0 2000/02/15 10:43p Hirose
50 Vector section was moved from mtx.c
51
52 $NoKeywords: $
53 *---------------------------------------------------------------------------*/
54
55 #include <math.h>
56 #include <revolution/mtx.h>
57 #include "mtxAssert.h"
58
59 /*---------------------------------------------------------------------*
60
61
62
63
64
65 VECTOR SECTION
66
67
68
69
70
71 *---------------------------------------------------------------------*/
72
73 #ifdef GEKKO
74 // Register macros for Paired-Single assembler instructions
75 #define RET_REG fp1
76 #define V1_XY fp2
77 #define V1_Z fp3
78 #define V2_XY fp4
79 #define V2_Z fp5
80 #define D1_XY fp6
81 #define D1_Z fp7
82 #define D2_XY fp8
83 #define D2_Z fp9
84 #define W1_XY fp10
85 #define W1_Z fp11
86 #define W2_XY fp12
87 #define W2_Z fp13
88 #endif
89
90 /*---------------------------------------------------------------------*
91
92 Name: VECAdd
93
94 Description: add two vectors.
95
96
97 Arguments: a: first vector.
98
99 b: second vector.
100
101 ab: resultant vector (a + b).
102 ok if ab == a or ab == b.
103
104
105 Return: none.
106
107 *---------------------------------------------------------------------*/
108 /*---------------------------------------------------------------------*
109 C version
110 *---------------------------------------------------------------------*/
C_VECAdd(const Vec * a,const Vec * b,Vec * ab)111 void C_VECAdd ( const Vec *a, const Vec *b, Vec *ab )
112 {
113
114 ASSERTMSG( ( a != 0), VEC_ADD_1 );
115 ASSERTMSG( ( b != 0), VEC_ADD_2 );
116 ASSERTMSG( ( ab != 0), VEC_ADD_3 );
117
118
119 ab->x = a->x + b->x;
120 ab->y = a->y + b->y;
121 ab->z = a->z + b->z;
122
123 }
124
125 /*---------------------------------------------------------------------*
126 Paired-Single assembler version
127 *---------------------------------------------------------------------*
128 Note that this performs NO error checking.
129 *---------------------------------------------------------------------*/
130 #ifdef GEKKO
PSVECAdd(const register Vec * vec1,const register Vec * vec2,register Vec * dst)131 asm void PSVECAdd
132 (
133 const register Vec *vec1,
134 const register Vec *vec2,
135 register Vec *dst
136 )
137 {
138 nofralloc;
139
140 //Load vectors XY
141 psq_l V1_XY, 0(vec1), 0, 0;
142 psq_l V2_XY, 0(vec2), 0, 0;
143 //Add vectors XY
144 ps_add D1_XY, V1_XY, V2_XY;
145 //Store result XY
146 psq_st D1_XY, 0(dst), 0, 0;
147 //Load vectors Z
148 psq_l V1_Z, 8(vec1), 1, 0;
149 psq_l V2_Z, 8(vec2), 1, 0;
150 //Add vectors Z
151 ps_add D1_Z, V1_Z, V2_Z;
152 //Store result Z
153 psq_st D1_Z, 8(dst), 1, 0;
154
155 blr
156 }
157 #endif // GEKKO
158
159 /*---------------------------------------------------------------------*
160
161 Name: VECSubtract
162
163 Description: subtract one vector from another.
164
165
166 Arguments: a: first vector.
167
168 b: second vector.
169
170 a_b: resultant vector (a - b).
171 ok if a_b == a or a_b == b.
172
173
174 Return: none.
175
176 *---------------------------------------------------------------------*/
177 /*---------------------------------------------------------------------*
178 C version
179 *---------------------------------------------------------------------*/
C_VECSubtract(const Vec * a,const Vec * b,Vec * a_b)180 void C_VECSubtract ( const Vec *a, const Vec *b, Vec *a_b )
181 {
182
183 ASSERTMSG( ( a != 0), VEC_SUBTRACT_1 );
184 ASSERTMSG( ( b != 0), VEC_SUBTRACT_2 );
185 ASSERTMSG( ( a_b != 0), VEC_SUBTRACT_3 );
186
187
188 a_b->x = a->x - b->x;
189 a_b->y = a->y - b->y;
190 a_b->z = a->z - b->z;
191
192 }
193
194 /*---------------------------------------------------------------------*
195 Paired-Single assembler version
196 *---------------------------------------------------------------------*
197 Note that this performs NO error checking.
198 *---------------------------------------------------------------------*/
199 #ifdef GEKKO
PSVECSubtract(const register Vec * vec1,const register Vec * vec2,register Vec * dst)200 asm void PSVECSubtract
201 (
202 const register Vec *vec1,
203 const register Vec *vec2,
204 register Vec *dst
205 )
206 {
207 nofralloc;
208
209 //Load vectors XY
210 psq_l V1_XY, 0(vec1), 0, 0;
211 psq_l V2_XY, 0(vec2), 0, 0;
212 //Subtract vectors XY
213 ps_sub D1_XY, V1_XY, V2_XY;
214 //Store vectors XY
215 psq_st D1_XY, 0(dst), 0, 0;
216
217 //Load vectors Z
218 psq_l V1_Z, 8(vec1), 1, 0;
219 psq_l V2_Z, 8(vec2), 1, 0;
220 //Subtract vectors Z
221 ps_sub D1_Z, V1_Z, V2_Z;
222 //Store vectors Z
223 psq_st D1_Z, 8(dst), 1, 0;
224
225 blr
226 }
227 #endif // GEKKO
228
229 /*---------------------------------------------------------------------*
230
231 Name: VECScale
232
233 Description: multiply a vector by a scalar.
234
235
236 Arguments: src: unscaled source vector.
237
238 dst: scaled resultant vector ( src * scale).
239 ok if dst == src.
240
241 scale: scaling factor.
242
243
244 Return: none.
245
246 *---------------------------------------------------------------------*/
247 /*---------------------------------------------------------------------*
248 C version
249 *---------------------------------------------------------------------*/
C_VECScale(const Vec * src,Vec * dst,f32 scale)250 void C_VECScale ( const Vec *src, Vec *dst, f32 scale )
251 {
252
253 ASSERTMSG( ( src != 0), VEC_SCALE_1 );
254 ASSERTMSG( ( dst != 0), VEC_SCALE_2 );
255
256
257 dst->x = src->x * scale;
258 dst->y = src->y * scale;
259 dst->z = src->z * scale;
260
261 }
262
263 /*---------------------------------------------------------------------*
264 Paired-Single assembler version
265 *---------------------------------------------------------------------*
266 Note that this performs NO error checking.
267 *---------------------------------------------------------------------*/
268 #ifdef GEKKO
PSVECScale(const register Vec * src,register Vec * dst,register f32 mult)269 void PSVECScale
270 (
271 const register Vec *src,
272 register Vec *dst,
273 register f32 mult
274 )
275 {
276 register f32 vxy, vz, rxy, rz;
277
278 asm
279 {
280 //Load vector XY
281 psq_l vxy, 0(src), 0, 0
282 //Load vector Z
283 psq_l vz, 8(src), 1, 0
284 //Multiply vector XY
285 ps_muls0 rxy, vxy, mult
286 //Store result XY
287 psq_st rxy, 0(dst), 0, 0
288 //Multiply vector Z
289 ps_muls0 rz, vz, mult
290 //Store vector Z
291 psq_st rz, 8(dst), 1, 0
292 }
293
294 }
295 #endif // GEKKO
296
297 /*---------------------------------------------------------------------*
298
299 Name: VECNormalize
300
301 Description: normalize a vector.
302
303
304 Arguments: src: non-unit source vector.
305
306 unit: resultant unit vector ( src / src magnitude ).
307 ok if unit == src
308
309
310 Return: none.
311
312 *---------------------------------------------------------------------*/
313 /*---------------------------------------------------------------------*
314 C version
315 *---------------------------------------------------------------------*/
C_VECNormalize(const Vec * src,Vec * unit)316 void C_VECNormalize ( const Vec *src, Vec *unit )
317 {
318 f32 mag;
319
320
321 ASSERTMSG( (src != 0 ), VEC_NORMALIZE_1 );
322 ASSERTMSG( (unit != 0), VEC_NORMALIZE_2 );
323
324
325 mag = (src->x * src->x) + (src->y * src->y) + (src->z * src->z);
326
327 ASSERTMSG( (mag != 0), VEC_NORMALIZE_3 );
328
329 mag = 1.0f / sqrtf(mag);
330
331 unit->x = src->x * mag;
332 unit->y = src->y * mag;
333 unit->z = src->z * mag;
334
335 }
336
337 /*---------------------------------------------------------------------*
338 Paired-Single assembler version
339 *---------------------------------------------------------------------*
340 Note that this performs NO error checking.
341 *---------------------------------------------------------------------*/
342 #ifdef GEKKO
PSVECNormalize(const register Vec * vec1,register Vec * dst)343 void PSVECNormalize
344 (
345 const register Vec *vec1, // r3
346 register Vec *dst // r4
347 )
348 {
349 register f32 c_half = 0.5F;
350 register f32 c_three = 3.0F;
351 register f32 v1_xy, v1_z;
352 register f32 xx_zz, xx_yy;
353 register f32 sqsum;
354 register f32 rsqrt;
355 register f32 nwork0, nwork1;
356
357 asm
358 {
359 // X | Y
360 psq_l v1_xy, 0(vec1), 0, 0;
361 // X*X | Y*Y
362 ps_mul xx_yy, v1_xy, v1_xy;
363 // Z | 1
364 psq_l v1_z, 8(vec1), 1, 0;
365 // X*X+Z*Z | Y*Y+1
366 ps_madd xx_zz, v1_z, v1_z, xx_yy;
367 // X*X+Z*Z+Y*Y | Z
368 ps_sum0 sqsum, xx_zz, v1_z, xx_yy;
369
370 // 1.0/sqrt : estimation[E]
371 frsqrte rsqrt, sqsum;
372 // Newton's refinement x 1
373 // E' = (E/2)(3 - sqsum * E * E)
374 fmuls nwork0, rsqrt, rsqrt;
375 fmuls nwork1, rsqrt, c_half;
376 fnmsubs nwork0, nwork0, sqsum, c_three;
377 fmuls rsqrt, nwork0, nwork1;
378
379 // X * mag | Y * mag
380 ps_muls0 v1_xy, v1_xy, rsqrt;
381 psq_st v1_xy, 0(dst), 0, 0;
382
383 // Z * mag
384 ps_muls0 v1_z, v1_z, rsqrt;
385 psq_st v1_z, 8(dst), 1, 0;
386 }
387
388 }
389 #endif // GEKKO
390
391 /*---------------------------------------------------------------------*
392
393 Name: VECSquareMag
394
395 Description: compute the square of the magnitude of a vector.
396
397
398 Arguments: v: source vector.
399
400
401 Return: square magnitude of the vector.
402
403 *---------------------------------------------------------------------*/
404 /*---------------------------------------------------------------------*
405 C version
406 *---------------------------------------------------------------------*/
C_VECSquareMag(const Vec * v)407 f32 C_VECSquareMag ( const Vec *v )
408 {
409 f32 sqmag;
410
411 ASSERTMSG( (v != 0), VEC_MAG_1 );
412
413 sqmag = (v->x * v->x) + (v->y * v->y) + (v->z * v->z);
414
415 return sqmag;
416 }
417
418 /*---------------------------------------------------------------------*
419 Paired-Single assembler version
420 *---------------------------------------------------------------------*
421 Note that this performs NO error checking.
422 *---------------------------------------------------------------------*/
423 #ifdef GEKKO
PSVECSquareMag(const register Vec * vec1)424 f32 PSVECSquareMag( const register Vec *vec1 )
425 {
426 register f32 vxy, vzz, sqmag;
427
428 asm
429 {
430 // Load X | Y
431 psq_l vxy, 0(vec1), 0, 0
432 // XX | YY
433 ps_mul vxy, vxy, vxy
434 // Load Z | Z
435 lfs vzz, 8(vec1)
436 // XX + ZZ | YY + ZZ
437 ps_madd sqmag, vzz, vzz, vxy
438 ps_sum0 sqmag, sqmag, vxy, vxy
439 }
440
441 return sqmag;
442 }
443 #endif // GEKKO
444
445 /*---------------------------------------------------------------------*
446
447 Name: VECMag
448
449 Description: compute the magnitude of a vector.
450
451
452 Arguments: v: source vector.
453
454
455 Return: magnitude of the vector.
456
457 *---------------------------------------------------------------------*/
458 /*---------------------------------------------------------------------*
459 C version
460 *---------------------------------------------------------------------*/
C_VECMag(const Vec * v)461 f32 C_VECMag ( const Vec *v )
462 {
463 return sqrtf( C_VECSquareMag(v) );
464 }
465
466 /*---------------------------------------------------------------------*
467 Paired-Single assembler version
468 *---------------------------------------------------------------------*/
469 #ifdef GEKKO
PSVECMag(const register Vec * v)470 f32 PSVECMag ( const register Vec *v )
471 {
472 register f32 vxy, vzz, sqmag;
473 register f32 rmag, nwork0, nwork1;
474 register f32 c_three, c_half, c_zero;
475
476 c_half = 0.5F;
477
478 asm
479 {
480 // Square mag calculation
481 psq_l vxy, 0(v), 0, 0
482 ps_mul vxy, vxy, vxy
483 lfs vzz, 8(v)
484 fsubs c_zero, c_half, c_half
485 ps_madd sqmag, vzz, vzz, vxy
486
487 // Square mag
488 ps_sum0 sqmag, sqmag, vxy, vxy
489
490 // Zero check
491 fcmpu cr0, sqmag, c_zero
492 beq- __exit
493
494 // 1.0/sqrt : estimation[E]
495 frsqrte rmag, sqmag
496 }
497 c_three = 3.0F;
498
499 asm
500 {
501 // Refinement x 1 : E' = (E/2)(3 - X*E*E)
502 fmuls nwork0, rmag, rmag
503 fmuls nwork1, rmag, c_half
504 fnmsubs nwork0, nwork0, sqmag, c_three
505 fmuls rmag, nwork0, nwork1
506
507 // 1/sqrt(X) * X = sqrt(X)
508 fmuls sqmag, sqmag, rmag
509
510 __exit:
511 }
512
513 return sqmag;
514 }
515 #endif // GEKKO
516
517 /*---------------------------------------------------------------------*
518
519 Name: VECDotProduct
520
521 Description: compute the dot product of two vectors.
522
523
524 Arguments: a: first vector.
525
526 b: second vector.
527
528 Note: input vectors do not have to be normalized.
529 input vectors are not normalized within the function.
530
531 If direct cosine computation of the angle
532 between a and b is desired, a and b should be
533 normalized prior to calling VECDotProduct.
534
535
536 Return: dot product value.
537
538 *---------------------------------------------------------------------*/
539 /*---------------------------------------------------------------------*
540 C version
541 *---------------------------------------------------------------------*/
C_VECDotProduct(const Vec * a,const Vec * b)542 f32 C_VECDotProduct ( const Vec *a, const Vec *b )
543 {
544 f32 dot;
545
546 ASSERTMSG( (a != 0), VEC_DOTPRODUCT_1 );
547 ASSERTMSG( (b != 0), VEC_DOTPRODUCT_2 );
548
549 dot = (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
550
551 return dot;
552 }
553
554 /*---------------------------------------------------------------------*
555 Paired-Single assembler version
556 *---------------------------------------------------------------------*
557 Note that this performs NO error checking.
558 *---------------------------------------------------------------------*/
559 #ifdef GEKKO
PSVECDotProduct(const register Vec * vec1,const register Vec * vec2)560 asm f32 PSVECDotProduct(const register Vec *vec1, const register Vec *vec2)
561 {
562 nofralloc;
563
564 psq_l fp2, 4(vec1), 0, 0;
565 psq_l fp3, 4(vec2), 0, 0;
566
567 ps_mul fp2, fp2, fp3;
568
569 psq_l fp5, 0(vec1), 0, 0;
570 psq_l fp4, 0(vec2), 0, 0;
571
572 ps_madd fp3, fp5, fp4, fp2;
573 ps_sum0 fp1, fp3, fp2, fp2;
574
575 blr;
576 }
577 #endif // GEKKO
578
579 /*---------------------------------------------------------------------*
580
581 Name: VECCrossProduct
582
583 Description: compute the cross product of two vectors.
584
585
586 Arguments: a: first vector.
587
588 b: second vector.
589
590 Note: input vectors do not have to be normalized.
591
592
593 axb: resultant vector.
594 ok if axb == a or axb == b.
595
596
597 Return: none.
598
599 *---------------------------------------------------------------------*/
600 /*---------------------------------------------------------------------*
601 C version
602 *---------------------------------------------------------------------*/
C_VECCrossProduct(const Vec * a,const Vec * b,Vec * axb)603 void C_VECCrossProduct ( const Vec *a, const Vec *b, Vec *axb )
604 {
605 Vec vTmp;
606
607
608 ASSERTMSG( (a != 0), VEC_CROSSPRODUCT_1 );
609 ASSERTMSG( (b != 0), VEC_CROSSPRODUCT_2 );
610 ASSERTMSG( (axb != 0), VEC_CROSSPRODUCT_3 );
611
612
613 vTmp.x = ( a->y * b->z ) - ( a->z * b->y );
614 vTmp.y = ( a->z * b->x ) - ( a->x * b->z );
615 vTmp.z = ( a->x * b->y ) - ( a->y * b->x );
616
617
618 axb->x = vTmp.x;
619 axb->y = vTmp.y;
620 axb->z = vTmp.z;
621
622 }
623
624 /*---------------------------------------------------------------------*
625 Paired-Single assembler version
626 *---------------------------------------------------------------------*
627 Note that this performs NO error checking.
628 *---------------------------------------------------------------------*/
629 #ifdef GEKKO
PSVECCrossProduct(const register Vec * vec1,const register Vec * vec2,register Vec * dst)630 asm void PSVECCrossProduct
631 (
632 const register Vec *vec1,
633 const register Vec *vec2,
634 register Vec *dst
635 )
636 {
637 nofralloc;
638
639 //x = a.n[VY]*b.n[VZ] - a.n[VZ]*b.n[VY];
640 //y = a.n[VZ]*b.n[VX] - a.n[VX]*b.n[VZ];
641 //z = a.n[VX]*b.n[VY] - a.n[VY]*b.n[VX];
642
643 // BX | BY
644 psq_l fp1, 0(vec2), 0, 0
645 // AZ | AZ
646 lfs fp2, 8(vec1)
647 // AX | AY
648 psq_l fp0, 0(vec1), 0, 0
649 // BY | BX
650 ps_merge10 fp6, fp1, fp1
651 // BZ | BZ
652 lfs fp3, 8(vec2)
653
654 // BX*AZ | BY*AZ
655 ps_mul fp4, fp1, fp2
656 // BX*AX | BY*AX
657 ps_muls0 fp7, fp1, fp0
658 // AX*BZ-BX*AZ | AY*BZ-BY*AZ
659 ps_msub fp5, fp0, fp3, fp4
660 // AX*BY-BX*AX | AY*BX-BY*AX
661 ps_msub fp8, fp0, fp6, fp7
662
663 // AY*BZ-AZ*BY | AY*BZ-AZ*BY
664 ps_merge11 fp9, fp5, fp5
665 // AX*BZ-AZ*BX | AY*BX-AX*BY
666 ps_merge01 fp10, fp5, fp8
667
668 psq_st fp9, 0(dst), 1, 0
669
670 // AZ*BX-AX*BZ | AX*BY-AY*BX
671 ps_neg fp10, fp10
672
673 psq_st fp10, 4(dst), 0, 0
674
675 blr;
676 }
677 #endif // GEKKO
678
679 /*---------------------------------------------------------------------*
680
681 Name: VECHalfAngle
682
683 Description: compute the vector halfway between two vectors.
684 Intended for use in computing specular highlights
685
686
687 Arguments: a: first vector.
688 This must point FROM the light source (tail)
689 TO the surface (head).
690
691 b: second vector.
692 This must point FROM the viewer (tail)
693 TO the surface (head).
694
695 Note: input vectors do not have to be normalized.
696
697
698 half: resultant normalized 'half-angle' vector.
699 ok if half == a or half == b
700
701
702 Return: none.
703
704 *---------------------------------------------------------------------*/
705 /*---------------------------------------------------------------------*
706 C version
707 *---------------------------------------------------------------------*/
C_VECHalfAngle(const Vec * a,const Vec * b,Vec * half)708 void C_VECHalfAngle ( const Vec *a, const Vec *b, Vec *half )
709 {
710 Vec aTmp, bTmp, hTmp;
711
712
713 ASSERTMSG( (a != 0), VEC_HALFANGLE_1 );
714 ASSERTMSG( (b != 0), VEC_HALFANGLE_2 );
715 ASSERTMSG( (half != 0), VEC_HALFANGLE_3 );
716
717
718 aTmp.x = -a->x;
719 aTmp.y = -a->y;
720 aTmp.z = -a->z;
721
722 bTmp.x = -b->x;
723 bTmp.y = -b->y;
724 bTmp.z = -b->z;
725
726 VECNormalize( &aTmp, &aTmp );
727 VECNormalize( &bTmp, &bTmp );
728
729 VECAdd( &aTmp, &bTmp, &hTmp );
730
731 if ( VECDotProduct( &hTmp, &hTmp ) > 0.0F )
732 {
733 VECNormalize( &hTmp, half );
734 }
735 else // The singular case returns zero vector
736 {
737 *half = hTmp;
738 }
739
740 }
741
742 /*---------------------------------------------------------------------*
743
744 Name: VECReflect
745
746 Description: reflect a vector about a normal to a surface.
747
748
749 Arguments: src: incident vector.
750
751 normal: normal to surface.
752
753 dst: normalized reflected vector.
754 ok if dst == src
755
756
757 Return: none.
758
759 *---------------------------------------------------------------------*/
760 /*---------------------------------------------------------------------*
761 C version
762 *---------------------------------------------------------------------*/
C_VECReflect(const Vec * src,const Vec * normal,Vec * dst)763 void C_VECReflect ( const Vec *src, const Vec *normal, Vec *dst )
764 {
765 f32 cosA;
766 Vec uI, uN;
767
768
769 ASSERTMSG( (src != 0), VEC_REFLECT_1 );
770 ASSERTMSG( (normal != 0), VEC_REFLECT_2 );
771 ASSERTMSG( (dst != 0), VEC_REFLECT_3 );
772
773
774 // Assume src is incident to a surface.
775 // Reverse direction of src so that src and normal
776 // sit tail to tail.
777 uI.x = -( src->x );
778 uI.y = -( src->y );
779 uI.z = -( src->z );
780
781
782 // VECNormalize will catch any zero magnitude vectors
783 VECNormalize( &uI, &uI);
784 VECNormalize( normal, &uN);
785
786 // Angle between the unit vectors
787 cosA = VECDotProduct( &uI, &uN);
788
789
790 // R = 2N(N.I) - I
791 dst->x = (2.0f * uN.x * cosA) - uI.x;
792 dst->y = (2.0f * uN.y * cosA) - uI.y;
793 dst->z = (2.0f * uN.z * cosA) - uI.z;
794
795 VECNormalize( dst, dst );
796
797 }
798
799 /*---------------------------------------------------------------------*
800
801 Name: VECSquareDistance
802
803 Description: Returns the square of the distance between vectors
804 a and b. Distance can be calculated using the
805 square root of the returned value.
806
807
808 Arguments: a: first vector.
809
810 b: second vector.
811
812
813 Return: square distance of between vectors.
814
815 *---------------------------------------------------------------------*/
816 /*---------------------------------------------------------------------*
817 C version
818 *---------------------------------------------------------------------*/
C_VECSquareDistance(const Vec * a,const Vec * b)819 f32 C_VECSquareDistance( const Vec *a, const Vec *b )
820 {
821 Vec diff;
822
823 diff.x = a->x - b->x;
824 diff.y = a->y - b->y;
825 diff.z = a->z - b->z;
826
827 return (diff.x * diff.x) + (diff.y * diff.y) + (diff.z * diff.z);
828 }
829
830 /*---------------------------------------------------------------------*
831 Paired-Single assembler version
832 *---------------------------------------------------------------------*/
833 #ifdef GEKKO
PSVECSquareDistance(const register Vec * a,const register Vec * b)834 f32 PSVECSquareDistance
835 (
836 const register Vec *a,
837 const register Vec *b
838 )
839 {
840 register f32 v0yz, v1yz, v0xy, v1xy;
841 register f32 dyz, dxy, sqdist;
842
843 asm
844 {
845 psq_l v0yz, 4(a), 0, 0 // [Y0][Z0]
846 psq_l v1yz, 4(b), 0, 0 // [Y1][Z1]
847 ps_sub dyz, v0yz, v1yz // [Y0-Y1][Z0-Z1]
848
849 psq_l v0xy, 0(a), 0, 0 // [X0][Y0]
850 psq_l v1xy, 0(b), 0, 0 // [X1][Y1]
851 ps_mul dyz, dyz, dyz // [dYdY][dZdZ]
852 ps_sub dxy, v0xy, v1xy // [X0-X1][Y0-Y1]
853
854 ps_madd sqdist, dxy, dxy, dyz // [dXdX+dYdY][dYdY+dZdZ]
855 ps_sum0 sqdist, sqdist, dyz, dyz // [dXdX+dYdY+dZdZ][N/A]
856 }
857
858 return sqdist;
859 }
860 #endif // GEKKO
861
862 /*---------------------------------------------------------------------*
863
864 Name: VECDistance
865
866 Description: Returns the distance between vectors a and b.
867
868
869 Arguments: a: first vector.
870
871 b: second vector.
872
873
874 Return: distance between the two vectors.
875
876 *---------------------------------------------------------------------*/
877 /*---------------------------------------------------------------------*
878 C version
879 *---------------------------------------------------------------------*/
C_VECDistance(const Vec * a,const Vec * b)880 f32 C_VECDistance( const Vec *a, const Vec *b )
881 {
882 return sqrtf( C_VECSquareDistance( a, b ) );
883 }
884
885 /*---------------------------------------------------------------------*
886 Paired-Single assembler version
887 *---------------------------------------------------------------------*/
888 #ifdef GEKKO
PSVECDistance(const register Vec * a,const register Vec * b)889 f32 PSVECDistance( const register Vec *a, const register Vec *b )
890 {
891 register f32 v0yz, v1yz, v0xy, v1xy;
892 register f32 dyz, dxy, sqdist, rdist;
893 register f32 nwork0, nwork1;
894 register f32 c_half, c_three, c_zero;
895
896 asm
897 {
898 psq_l v0yz, 4(a), 0, 0 // [Y0][Z0]
899 psq_l v1yz, 4(b), 0, 0 // [Y1][Z1]
900 ps_sub dyz, v0yz, v1yz // [Y0-Y1][Z0-Z1]
901
902 psq_l v0xy, 0(a), 0, 0 // [X0][Y0]
903 psq_l v1xy, 0(b), 0, 0 // [X1][Y1]
904 ps_mul dyz, dyz, dyz // [dYdY][dZdZ]
905 ps_sub dxy, v0xy, v1xy // [X0-X1][Y0-Y1]
906 }
907 c_half = 0.5F;
908
909 asm
910 {
911 ps_madd sqdist, dxy, dxy, dyz // [dXdX+dYdY][dYdY+dZdZ]
912 fsubs c_zero, c_half, c_half
913 ps_sum0 sqdist, sqdist, dyz, dyz // [dXdX+dYdY+dZdZ][N/A]
914
915 // Zero check
916 fcmpu cr0, c_zero, sqdist
917 beq- __exit
918 }
919 c_three = 3.0F;
920
921 asm
922 {
923 // 1.0/sqrt : estimation[E]
924 frsqrte rdist, sqdist
925 // Refinement x 1 : E' = (E/2)(3 - X*E*E)
926 fmuls nwork0, rdist, rdist
927 fmuls nwork1, rdist, c_half
928 fnmsubs nwork0, nwork0, sqdist, c_three
929 fmuls rdist, nwork0, nwork1
930
931 // 1/sqrt(X) * X = sqrt(X)
932 fmuls sqdist, sqdist, rdist
933
934 __exit:
935 }
936
937 return sqdist;
938 }
939 #endif // GEKKO
940
941 /*===========================================================================*/
942