/*---------------------------------------------------------------------------* Project: matrix vector Library File: vec.c Copyright 1998-2002 Nintendo. All rights reserved. These coded instructions, statements, and computer programs contain proprietary information of Nintendo of America Inc. and/or Nintendo Company Ltd., and are protected by Federal copyright law. They may not be disclosed to third parties or copied or duplicated in any form, in whole or in part, without the prior written consent of Nintendo. $Log: vec.c,v $ Revision 1.2 02/20/2006 04:25:42 mitu changed include path from dolphin/ to revolution/. Revision 1.1.1.1 2005/05/12 02:15:49 yasuh-to transitioned from the Dolphin source tree 8 02/08/20 14:49 Hirose Workaround for divided-by-zero exceptions. 7 02/04/11 13:11 Hirose const type specifier support. (worked by Hiratsu@IRD) 6 9/12/01 10:40a Hirose Removed unnecessary possibilities of zero division exception. 5 8/27/01 6:45p Hirose PSVECMag/PSVECDistance implementations. Misc. updates. 4 7/30/01 11:00p Hirose Fixed MAC build errors regarding missing "#ifdef GEKKO". 3 7/30/01 10:23p Hirose Added PSVECMag, PSVECDistance. Some definition changes. 2 6/11/01 2:10p Hirose reduced number of operations in PSVECNormalize. 1 2/22/01 11:52p Hirose Integrated PSVEC functions made by Scott Perras@NOA Sdsg. 0 2/15/00 10:43p Hirose Vector section was moved from mtx.c $NoKeywords: $ *---------------------------------------------------------------------------*/ #ifdef WIN32 #include #endif #include #include #include "mtxAssert.h" /*---------------------------------------------------------------------* VECTOR SECTION *---------------------------------------------------------------------*/ #ifdef GEKKO // Register macros for Paired-Single assembler instructions #define RET_REG fp1 #define V1_XY fp2 #define V1_Z fp3 #define V2_XY fp4 #define V2_Z fp5 #define D1_XY fp6 #define D1_Z fp7 #define D2_XY fp8 #define D2_Z fp9 #define W1_XY fp10 #define W1_Z fp11 #define W2_XY fp12 #define W2_Z fp13 #endif /*---------------------------------------------------------------------* Name: VECAdd Description: add two vectors. Arguments: a first vector. b second vector. ab resultant vector (a + b). ok if ab == a or ab == b. Return : none. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_VECAdd ( const Vec *a, const Vec *b, Vec *ab ) { ASSERTMSG( ( a != 0), VEC_ADD_1 ); ASSERTMSG( ( b != 0), VEC_ADD_2 ); ASSERTMSG( ( ab != 0), VEC_ADD_3 ); ab->x = a->x + b->x; ab->y = a->y + b->y; ab->z = a->z + b->z; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO asm void PSVECAdd ( const register Vec *vec1, const register Vec *vec2, register Vec *dst ) { nofralloc; //load vectors XY psq_l V1_XY, 0(vec1), 0, 0; psq_l V2_XY, 0(vec2), 0, 0; //add vectors XY ps_add D1_XY, V1_XY, V2_XY; //store result XY psq_st D1_XY, 0(dst), 0, 0; //load vectors Z psq_l V1_Z, 8(vec1), 1, 0; psq_l V2_Z, 8(vec2), 1, 0; //add vectors Z ps_add D1_Z, V1_Z, V2_Z; //store result Z psq_st D1_Z, 8(dst), 1, 0; blr } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECSubtract Description: subtract one vector from another. Arguments: a first vector. b second vector. a_b resultant vector (a - b). ok if a_b == a or a_b == b. Return : none. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_VECSubtract ( const Vec *a, const Vec *b, Vec *a_b ) { ASSERTMSG( ( a != 0), VEC_SUBTRACT_1 ); ASSERTMSG( ( b != 0), VEC_SUBTRACT_2 ); ASSERTMSG( ( a_b != 0), VEC_SUBTRACT_3 ); a_b->x = a->x - b->x; a_b->y = a->y - b->y; a_b->z = a->z - b->z; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO asm void PSVECSubtract ( const register Vec *vec1, const register Vec *vec2, register Vec *dst ) { nofralloc; //load vectors XY psq_l V1_XY, 0(vec1), 0, 0; psq_l V2_XY, 0(vec2), 0, 0; //subtract vectors XY ps_sub D1_XY, V1_XY, V2_XY; //store vectors XY psq_st D1_XY, 0(dst), 0, 0; //load vectors Z psq_l V1_Z, 8(vec1), 1, 0; psq_l V2_Z, 8(vec2), 1, 0; //subtract vectors Z ps_sub D1_Z, V1_Z, V2_Z; //store vectors Z psq_st D1_Z, 8(dst), 1, 0; blr } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECScale Description: multiply a vector by a scalar. Arguments: src unscaled source vector. dst scaled resultant vector ( src * scale). ok if dst == src. scale scaling factor. Return : none. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_VECScale ( const Vec *src, Vec *dst, f32 scale ) { ASSERTMSG( ( src != 0), VEC_SCALE_1 ); ASSERTMSG( ( dst != 0), VEC_SCALE_2 ); dst->x = src->x * scale; dst->y = src->y * scale; dst->z = src->z * scale; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO void PSVECScale ( const register Vec *src, register Vec *dst, register f32 mult ) { register f32 vxy, vz, rxy, rz; asm { //load vector XY psq_l vxy, 0(src), 0, 0 //load vector Z psq_l vz, 8(src), 1, 0 //multiply vector XY ps_muls0 rxy, vxy, mult //store result XY psq_st rxy, 0(dst), 0, 0 //multiply vector Z ps_muls0 rz, vz, mult //store vector Z psq_st rz, 8(dst), 1, 0 } } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECNormalize Description: normalize a vector. Arguments: src non-unit source vector. unit resultant unit vector ( src / src magnitude ). ok if unit == src Return : none. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_VECNormalize ( const Vec *src, Vec *unit ) { f32 mag; ASSERTMSG( (src != 0 ), VEC_NORMALIZE_1 ); ASSERTMSG( (unit != 0), VEC_NORMALIZE_2 ); mag = (src->x * src->x) + (src->y * src->y) + (src->z * src->z); ASSERTMSG( (mag != 0), VEC_NORMALIZE_3 ); mag = 1.0f / sqrtf(mag); unit->x = src->x * mag; unit->y = src->y * mag; unit->z = src->z * mag; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO void PSVECNormalize ( const register Vec *vec1, // r3 register Vec *dst // r4 ) { register f32 c_half = 0.5F; register f32 c_three = 3.0F; register f32 v1_xy, v1_z; register f32 xx_zz, xx_yy; register f32 sqsum; register f32 rsqrt; register f32 nwork0, nwork1; asm { // X | Y psq_l v1_xy, 0(vec1), 0, 0; // X*X | Y*Y ps_mul xx_yy, v1_xy, v1_xy; // Z | 1 psq_l v1_z, 8(vec1), 1, 0; // X*X+Z*Z | Y*Y+1 ps_madd xx_zz, v1_z, v1_z, xx_yy; // X*X+Z*Z+Y*Y | Z ps_sum0 sqsum, xx_zz, v1_z, xx_yy; // 1.0/sqrt : estimation[E] frsqrte rsqrt, sqsum; // Newton's refinement x 1 // E' = (E/2)(3 - sqsum * E * E) fmuls nwork0, rsqrt, rsqrt; fmuls nwork1, rsqrt, c_half; fnmsubs nwork0, nwork0, sqsum, c_three; fmuls rsqrt, nwork0, nwork1; // X * mag | Y * mag ps_muls0 v1_xy, v1_xy, rsqrt; psq_st v1_xy, 0(dst), 0, 0; // Z * mag ps_muls0 v1_z, v1_z, rsqrt; psq_st v1_z, 8(dst), 1, 0; } } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECSquareMag Description: compute the square of the magnitude of a vector. Arguments: v source vector. Return : square magnitude of the vector. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ f32 C_VECSquareMag ( const Vec *v ) { f32 sqmag; ASSERTMSG( (v != 0), VEC_MAG_1 ); sqmag = (v->x * v->x) + (v->y * v->y) + (v->z * v->z); return sqmag; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO f32 PSVECSquareMag( const register Vec *vec1 ) { register f32 vxy, vzz, sqmag; asm { // load X | Y psq_l vxy, 0(vec1), 0, 0 // XX | YY ps_mul vxy, vxy, vxy // load Z | Z lfs vzz, 8(vec1) // XX + ZZ | YY + ZZ ps_madd sqmag, vzz, vzz, vxy ps_sum0 sqmag, sqmag, vxy, vxy } return sqmag; } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECMag Description: compute the magnitude of a vector. Arguments: v source vector. Return : magnitude of the vector. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ f32 C_VECMag ( const Vec *v ) { return sqrtf( C_VECSquareMag(v) ); } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------*/ #ifdef GEKKO f32 PSVECMag ( const register Vec *v ) { register f32 vxy, vzz, sqmag; register f32 rmag, nwork0, nwork1; register f32 c_three, c_half, c_zero; c_half = 0.5F; asm { // Square mag calculation psq_l vxy, 0(v), 0, 0 ps_mul vxy, vxy, vxy lfs vzz, 8(v) fsubs c_zero, c_half, c_half ps_madd sqmag, vzz, vzz, vxy // Square mag ps_sum0 sqmag, sqmag, vxy, vxy // Zero check fcmpu cr0, sqmag, c_zero beq- __exit // 1.0/sqrt : estimation[E] frsqrte rmag, sqmag } c_three = 3.0F; asm { // Refinement x 1 : E' = (E/2)(3 - X*E*E) fmuls nwork0, rmag, rmag fmuls nwork1, rmag, c_half fnmsubs nwork0, nwork0, sqmag, c_three fmuls rmag, nwork0, nwork1 // 1/sqrt(X) * X = sqrt(X) fmuls sqmag, sqmag, rmag __exit: } return sqmag; } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECDotProduct Description: compute the dot product of two vectors. Arguments: a first vector. b second vector. note: input vectors do not have to be normalized. input vectors are not normalized within the function. if direct cosine computation of the angle between a and b is desired, a and b should be normalized prior to calling VECDotProduct. Return : dot product value. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ f32 C_VECDotProduct ( const Vec *a, const Vec *b ) { f32 dot; ASSERTMSG( (a != 0), VEC_DOTPRODUCT_1 ); ASSERTMSG( (b != 0), VEC_DOTPRODUCT_2 ); dot = (a->x * b->x) + (a->y * b->y) + (a->z * b->z); return dot; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO asm f32 PSVECDotProduct(const register Vec *vec1, const register Vec *vec2) { nofralloc; psq_l fp2, 4(vec1), 0, 0; psq_l fp3, 4(vec2), 0, 0; ps_mul fp2, fp2, fp3; psq_l fp5, 0(vec1), 0, 0; psq_l fp4, 0(vec2), 0, 0; ps_madd fp3, fp5, fp4, fp2; ps_sum0 fp1, fp3, fp2, fp2; blr; } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECCrossProduct Description: compute the cross product of two vectors. Arguments: a first vector. b second vector. note: input vectors do not have to be normalized. axb resultant vector. ok if axb == a or axb == b. Return : none. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_VECCrossProduct ( const Vec *a, const Vec *b, Vec *axb ) { Vec vTmp; ASSERTMSG( (a != 0), VEC_CROSSPRODUCT_1 ); ASSERTMSG( (b != 0), VEC_CROSSPRODUCT_2 ); ASSERTMSG( (axb != 0), VEC_CROSSPRODUCT_3 ); vTmp.x = ( a->y * b->z ) - ( a->z * b->y ); vTmp.y = ( a->z * b->x ) - ( a->x * b->z ); vTmp.z = ( a->x * b->y ) - ( a->y * b->x ); axb->x = vTmp.x; axb->y = vTmp.y; axb->z = vTmp.z; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO asm void PSVECCrossProduct ( const register Vec *vec1, const register Vec *vec2, register Vec *dst ) { nofralloc; //x = a.n[VY]*b.n[VZ] - a.n[VZ]*b.n[VY]; //y = a.n[VZ]*b.n[VX] - a.n[VX]*b.n[VZ]; //z = a.n[VX]*b.n[VY] - a.n[VY]*b.n[VX]; // BX | BY psq_l fp1, 0(vec2), 0, 0 // AZ | AZ lfs fp2, 8(vec1) // AX | AY psq_l fp0, 0(vec1), 0, 0 // BY | BX ps_merge10 fp6, fp1, fp1 // BZ | BZ lfs fp3, 8(vec2) // BX*AZ | BY*AZ ps_mul fp4, fp1, fp2 // BX*AX | BY*AX ps_muls0 fp7, fp1, fp0 // AX*BZ-BX*AZ | AY*BZ-BY*AZ ps_msub fp5, fp0, fp3, fp4 // AX*BY-BX*AX | AY*BX-BY*AX ps_msub fp8, fp0, fp6, fp7 // AY*BZ-AZ*BY | AY*BZ-AZ*BY ps_merge11 fp9, fp5, fp5 // AX*BZ-AZ*BX | AY*BX-AX*BY ps_merge01 fp10, fp5, fp8 psq_st fp9, 0(dst), 1, 0 // AZ*BX-AX*BZ | AX*BY-AY*BX ps_neg fp10, fp10 psq_st fp10, 4(dst), 0, 0 blr; } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECHalfAngle Description: compute the vector halfway between two vectors. intended for use in computing specular highlights Arguments: a first vector. this must point FROM the light source (tail) TO the surface (head). b second vector. this must point FROM the viewer (tail) TO the surface (head). note: input vectors do not have to be normalized. half resultant normalized 'half-angle' vector. ok if half == a or half == b Return : none. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_VECHalfAngle ( const Vec *a, const Vec *b, Vec *half ) { Vec aTmp, bTmp, hTmp; ASSERTMSG( (a != 0), VEC_HALFANGLE_1 ); ASSERTMSG( (b != 0), VEC_HALFANGLE_2 ); ASSERTMSG( (half != 0), VEC_HALFANGLE_3 ); aTmp.x = -a->x; aTmp.y = -a->y; aTmp.z = -a->z; bTmp.x = -b->x; bTmp.y = -b->y; bTmp.z = -b->z; VECNormalize( &aTmp, &aTmp ); VECNormalize( &bTmp, &bTmp ); VECAdd( &aTmp, &bTmp, &hTmp ); if ( VECDotProduct( &hTmp, &hTmp ) > 0.0F ) { VECNormalize( &hTmp, half ); } else // The singular case returns zero vector { *half = hTmp; } } /*---------------------------------------------------------------------* Name: VECReflect Description: reflect a vector about a normal to a surface. Arguments: src incident vector. normal normal to surface. dst normalized reflected vector. ok if dst == src Return : none. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_VECReflect ( const Vec *src, const Vec *normal, Vec *dst ) { f32 cosA; Vec uI, uN; ASSERTMSG( (src != 0), VEC_REFLECT_1 ); ASSERTMSG( (normal != 0), VEC_REFLECT_2 ); ASSERTMSG( (dst != 0), VEC_REFLECT_3 ); // assume src is incident to a surface. // reverse direction of src so that src and normal // sit tail to tail. uI.x = -( src->x ); uI.y = -( src->y ); uI.z = -( src->z ); // VECNormalize will catch any zero magnitude vectors VECNormalize( &uI, &uI); VECNormalize( normal, &uN); // angle between the unit vectors cosA = VECDotProduct( &uI, &uN); // R = 2N(N.I) - I dst->x = (2.0f * uN.x * cosA) - uI.x; dst->y = (2.0f * uN.y * cosA) - uI.y; dst->z = (2.0f * uN.z * cosA) - uI.z; VECNormalize( dst, dst ); } /*---------------------------------------------------------------------* Name: VECSquareDistance Description: Returns the square of the distance between vectors a and b. Distance can be calculated using the square root of the returned value. Arguments: a first vector. b second vector. Return : square distance of between vectors. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ f32 C_VECSquareDistance( const Vec *a, const Vec *b ) { Vec diff; diff.x = a->x - b->x; diff.y = a->y - b->y; diff.z = a->z - b->z; return (diff.x * diff.x) + (diff.y * diff.y) + (diff.z * diff.z); } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------*/ #ifdef GEKKO f32 PSVECSquareDistance ( const register Vec *a, const register Vec *b ) { register f32 v0yz, v1yz, v0xy, v1xy; register f32 dyz, dxy, sqdist; asm { psq_l v0yz, 4(a), 0, 0 // [Y0][Z0] psq_l v1yz, 4(b), 0, 0 // [Y1][Z1] ps_sub dyz, v0yz, v1yz // [Y0-Y1][Z0-Z1] psq_l v0xy, 0(a), 0, 0 // [X0][Y0] psq_l v1xy, 0(b), 0, 0 // [X1][Y1] ps_mul dyz, dyz, dyz // [dYdY][dZdZ] ps_sub dxy, v0xy, v1xy // [X0-X1][Y0-Y1] ps_madd sqdist, dxy, dxy, dyz // [dXdX+dYdY][dYdY+dZdZ] ps_sum0 sqdist, sqdist, dyz, dyz // [dXdX+dYdY+dZdZ][N/A] } return sqdist; } #endif // GEKKO /*---------------------------------------------------------------------* Name: VECDistance Description: Returns the distance between vectors a and b. Arguments: a first vector. b second vector. Return : distance between the two vectors. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ f32 C_VECDistance( const Vec *a, const Vec *b ) { return sqrtf( C_VECSquareDistance( a, b ) ); } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------*/ #ifdef GEKKO f32 PSVECDistance( const register Vec *a, const register Vec *b ) { register f32 v0yz, v1yz, v0xy, v1xy; register f32 dyz, dxy, sqdist, rdist; register f32 nwork0, nwork1; register f32 c_half, c_three, c_zero; asm { psq_l v0yz, 4(a), 0, 0 // [Y0][Z0] psq_l v1yz, 4(b), 0, 0 // [Y1][Z1] ps_sub dyz, v0yz, v1yz // [Y0-Y1][Z0-Z1] psq_l v0xy, 0(a), 0, 0 // [X0][Y0] psq_l v1xy, 0(b), 0, 0 // [X1][Y1] ps_mul dyz, dyz, dyz // [dYdY][dZdZ] ps_sub dxy, v0xy, v1xy // [X0-X1][Y0-Y1] } c_half = 0.5F; asm { ps_madd sqdist, dxy, dxy, dyz // [dXdX+dYdY][dYdY+dZdZ] fsubs c_zero, c_half, c_half ps_sum0 sqdist, sqdist, dyz, dyz // [dXdX+dYdY+dZdZ][N/A] // Zero check fcmpu cr0, c_zero, sqdist beq- __exit } c_three = 3.0F; asm { // 1.0/sqrt : estimation[E] frsqrte rdist, sqdist // Refinement x 1 : E' = (E/2)(3 - X*E*E) fmuls nwork0, rdist, rdist fmuls nwork1, rdist, c_half fnmsubs nwork0, nwork0, sqdist, c_three fmuls rdist, nwork0, nwork1 // 1/sqrt(X) * X = sqrt(X) fmuls sqdist, sqdist, rdist __exit: } return sqdist; } #endif // GEKKO /*===========================================================================*/