/*---------------------------------------------------------------------------* Project: Matrix vector Library File: mtx44vec.c Copyright 1998 - 2001 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: mtx44vec.c,v $ Revision 1.4 2007/08/30 10:41:22 hirose Updated PSMTX44MultVecArray to make it Broadway EABI compliant. Revision 1.3 2007/01/11 00:45:26 aka Removed win32.h. Revision 1.2 2006/02/20 04:25:42 mitu Changed include path from dolphin/ to revolution/. Revision 1.1.1.1 2005/05/12 02:15:49 yasuh-to Ported from dolphin sheath tree. 2 2002/04/11 13:11 Hirose const type specifier support. (by Hiratsu@IRD) 1 2001/07/30 10:51p Hirose Initial check in. $NoKeywords: $ *---------------------------------------------------------------------------*/ #include #include #include #include "mtx44extAssert.h" /*---------------------------------------------------------------------* MODEL SECTION *---------------------------------------------------------------------*/ /* NOTE: Prototypes for these functions are defined in "mtx44ext.h". */ /*---------------------------------------------------------------------* Name: MTX44MultVec Description: Multiplies a vector by a matrix. m x src = dst. Arguments: m Matrix. src Source vector for multiply. dst Resultant vector from multiply. Note: OK if src == dst. Return : None. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_MTX44MultVec ( const Mtx44 m, const Vec *src, Vec *dst ) { Vec vTmp; f32 w; ASSERTMSG( (m != 0), MTX44_MULTVEC_1 ); ASSERTMSG( (src != 0), MTX44_MULTVEC_2 ); ASSERTMSG( (dst != 0), MTX44_MULTVEC_3 ); // A Vec has a 4th implicit 'w' coordinate of 1 vTmp.x = m[0][0]*src->x + m[0][1]*src->y + m[0][2]*src->z + m[0][3]; vTmp.y = m[1][0]*src->x + m[1][1]*src->y + m[1][2]*src->z + m[1][3]; vTmp.z = m[2][0]*src->x + m[2][1]*src->y + m[2][2]*src->z + m[2][3]; w = m[3][0]*src->x + m[3][1]*src->y + m[3][2]*src->z + m[3][3]; w = 1.0f/w; // Copy back dst->x = vTmp.x * w; dst->y = vTmp.y * w; dst->z = vTmp.z * w; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that NO error checking is performed. *---------------------------------------------------------------------*/ #ifdef GEKKO asm void PSMTX44MultVec ( const register Mtx44 m, const register Vec *src, register Vec *dst ) { nofralloc; psq_l fp0, 0(src), 0, 0; // fp0 <-src.x, src.y psq_l fp2, 48(m), 0, 0; psq_l fp1, 8(src), 1, 0; // fp1 <-src.z, 1.0 ps_mul fp4, fp0, fp2; psq_l fp3, 56(m), 0, 0; ps_madd fp5, fp1, fp3, fp4; ps_merge11 fp12, fp1, fp1; // fp12 = 1.0, 1.0 ps_sum0 fp13, fp5, fp5, fp5; // fp3 <- w psq_l fp4, 0(m), 0, 0; ps_merge00 fp13, fp13, fp13; psq_l fp5, 8(m), 0, 0; ps_div fp13, fp12, fp13; // fp13 <- 1/w psq_l fp6, 16(m), 0, 0; psq_l fp7, 24(m), 0, 0; psq_l fp8, 32(m), 0, 0; psq_l fp9, 40(m), 0, 0; ps_mul fp4, fp0, fp4; ps_madd fp2, fp1, fp5, fp4; ps_mul fp6, fp0, fp6; ps_madd fp3, fp1, fp7, fp6; ps_mul fp8, fp0, fp8; ps_sum0 fp2, fp2, fp2, fp2; // fp2 <- dst.x, -- ps_madd fp9, fp1, fp9, fp8; ps_sum1 fp2, fp3, fp2, fp3; // fp2 <- dst.x, dst.y ps_sum0 fp3, fp9, fp9, fp9; ps_mul fp2, fp2, fp13; psq_st fp2, 0(dst), 0, 0; ps_mul fp3, fp3, fp13; psq_st fp3, 8(dst), 1, 0; blr; } #endif // GEKKO /*---------------------------------------------------------------------* Name: MTX44MultVecArray Description: Multiplies an array of vectors by a matrix. Arguments: m Matrix. srcBase Start of source vector array. dstBase Start of resultant vector array. Note: OK if srcBase == dstBase. count Number of vectors in srcBase, dstBase arrays Note: Cannot check for array overflow Return : None. *---------------------------------------------------------------------*/ /*---------------------------------------------------------------------* C version *---------------------------------------------------------------------*/ void C_MTX44MultVecArray ( const Mtx44 m, const Vec *srcBase, Vec *dstBase, u32 count ) { u32 i; Vec vTmp; f32 w; ASSERTMSG( (m != 0), MTX44_MULTVECARRAY_1 ); ASSERTMSG( (srcBase != 0), MTX44_MULTVECARRAY_2 ); ASSERTMSG( (dstBase != 0), MTX44_MULTVECARRAY_3 ); for(i=0; i< count; i++) { // Vec has a 4th implicit 'w' coordinate of 1 vTmp.x = m[0][0]*srcBase->x + m[0][1]*srcBase->y + m[0][2]*srcBase->z + m[0][3]; vTmp.y = m[1][0]*srcBase->x + m[1][1]*srcBase->y + m[1][2]*srcBase->z + m[1][3]; vTmp.z = m[2][0]*srcBase->x + m[2][1]*srcBase->y + m[2][2]*srcBase->z + m[2][3]; w = m[3][0]*srcBase->x + m[3][1]*srcBase->y + m[3][2]*srcBase->z + m[3][3]; w = 1.0f/w; // Copy back dstBase->x = vTmp.x * w; dstBase->y = vTmp.y * w; dstBase->z = vTmp.z * w; srcBase++; dstBase++; } } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that NO error checking is performed. *---------------------------------------------------------------------*/ #ifdef GEKKO asm void PSMTX44MultVecArray ( const register Mtx44 m, const register Vec *srcBase, register Vec *dstBase, register u32 count ) { nofralloc; // cmpwi count, 1; // bne @array // b PSMTX44MultVec //@array: stwu rsp, -24(rsp); addi count, count, -1; psq_l fp6, 48(m), 0, 0; // fp6 <- m30, m31 mtctr count; psq_l fp8, 0(srcBase), 0, 0; // fp8 <- src.x, src.y addi dstBase, dstBase, -4; stfd fp14, 8(rsp); psq_l fp7, 56(m), 0, 0; // fp7 <- m32, m33 psq_lu fp9, 8(srcBase), 1, 0; // fp9 <- src.z, 1.0 ps_mul fp13, fp6, fp8; psq_l fp0, 0(m), 0, 0; // fp0 <- m00, m01 psq_st fp14, 16(rsp), 0, 0; ps_madd fp13, fp7, fp9, fp13; psq_l fp2, 16(m), 0, 0; // fp2 <- m10, m11 ps_merge11 fp14, fp9, fp9; // fp9 = 1.0F, 1.0F ps_mul fp10, fp0, fp8; psq_l fp4, 32(m), 0, 0; // fp4 <- m20, m21 ps_mul fp11, fp2, fp8; psq_l fp1, 8(m), 0, 0; // fp1 <- m02, m03 ps_mul fp12, fp4, fp8; psq_l fp3, 24(m), 0, 0; // fp3 <- m12, m13 ps_sum0 fp13, fp13, fp13, fp13; // fp13 <- w psq_l fp5, 40(m), 0, 0; // fp5 <- m22, m23 @loop: ps_madd fp10, fp1, fp9, fp10; ps_madd fp11, fp3, fp9, fp11; ps_madd fp12, fp5, fp9, fp12; ps_sum0 fp10, fp10, fp10, fp10; // fp10 <- x ps_sum0 fp11, fp11, fp11, fp11; // fp11 <- y ps_sum0 fp12, fp12, fp12, fp12; // fp12 <- z ps_div fp13, fp14, fp13; psq_lu fp8, 4(srcBase), 0, 0; psq_lu fp9, 8(srcBase), 1, 0; ps_mul fp10, fp10, fp13; psq_stu fp10, 4(dstBase), 1, 0; ps_mul fp11, fp11, fp13; psq_stu fp11, 4(dstBase), 1, 0; ps_mul fp12, fp12, fp13; psq_stu fp12, 4(dstBase), 1, 0; ps_mul fp13, fp6, fp8; ps_mul fp10, fp0, fp8; ps_mul fp11, fp2, fp8; ps_madd fp13, fp7, fp9, fp13; ps_mul fp12, fp4, fp8; ps_sum0 fp13, fp13, fp13, fp13; bdnz+ @loop ps_madd fp10, fp1, fp9, fp10; ps_madd fp11, fp3, fp9, fp11; ps_madd fp12, fp5, fp9, fp12; ps_sum0 fp10, fp10, fp10, fp10; // fp10 <- x ps_sum0 fp11, fp11, fp11, fp11; // fp11 <- y ps_sum0 fp12, fp12, fp12, fp12; // fp12 <- z ps_div fp13, fp14, fp13; ps_mul fp10, fp10, fp13; psq_st fp10, 4(dstBase), 1, 0; ps_mul fp11, fp11, fp13; psq_st fp11, 8(dstBase), 1, 0; ps_mul fp12, fp12, fp13; psq_st fp12, 12(dstBase), 1, 0; psq_l fp14, 16(rsp), 0, 0; lfd fp14, 8(rsp); addi rsp, rsp, 24; blr; } #endif // GEKKO /*---------------------------------------------------------------------* Name: MTX44MultVecSR Description: Multiplies a vector by a matrix 3x3 (Scaling and Rotation) component. m x src = dst. Arguments: m Matrix. src Source vector for multiply. dst Resultant vector from multiply. Note: OK if src == dst. Return : None. *---------------------------------------------------------------------*/ void C_MTX44MultVecSR ( const Mtx44 m, const Vec *src, Vec *dst ) { Vec vTmp; ASSERTMSG( (m != 0), MTX44_MULTVECSR_1 ); ASSERTMSG( (src != 0), MTX44_MULTVECSR_2 ); ASSERTMSG( (dst != 0), MTX44_MULTVECSR_3 ); // A Vec has a 4th implicit 'w' coordinate of 1 vTmp.x = m[0][0]*src->x + m[0][1]*src->y + m[0][2]*src->z; vTmp.y = m[1][0]*src->x + m[1][1]*src->y + m[1][2]*src->z; vTmp.z = m[2][0]*src->x + m[2][1]*src->y + m[2][2]*src->z; // Copy back dst->x = vTmp.x; dst->y = vTmp.y; dst->z = vTmp.z; } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO asm void PSMTX44MultVecSR( const register Mtx44 m, const register Vec *src, register Vec *dst ) { nofralloc psq_l fp0, 0(m), 0, 0 // m[0][0], m[0][1] GQR0 = 0 // fp6 - x y psq_l fp6, 0(src), 0, 0 psq_l fp2, 16(m), 0, 0 // m[1][0], m[1][1] // fp8 = m00x m01y // next X ps_mul fp8, fp0, fp6 psq_l fp4, 32(m), 0, 0 // m[2][0], m[2][1] // fp10 = m10x m11y // next Y ps_mul fp10, fp2, fp6 psq_l fp7, 8(src), 1, 0 // fp7 - z,1.0 // fp12 = m20x m21y // next Z ps_mul fp12, fp4, fp6 // YYY last FP6 usage psq_l fp3, 24(m), 0, 0 // m[1][2], m[1][3] ps_sum0 fp8, fp8, fp8, fp8 psq_l fp5, 40(m), 0, 0 // m[2][2], m[2][3] ps_sum0 fp10, fp10, fp10, fp10 psq_l fp1, 8(m), 0, 0 // m[0][2], m[0][3] ps_sum0 fp12, fp12, fp12, fp12 ps_madd fp9, fp1, fp7, fp8 psq_st fp9, 0(dst), 1, 0 // Store X ps_madd fp11, fp3, fp7, fp10 psq_st fp11, 4(dst), 1, 0 // Store Y ps_madd fp13, fp5, fp7, fp12 psq_st fp13, 8(dst), 1, 0 // Store Z blr } #endif // GEKKO /*---------------------------------------------------------------------* Name: MTX44MultVecArraySR Description: Multiplies an array of vectors by a matrix 3x3 (Scaling and Rotation) component. Arguments: m Matrix. srcBase Start of source vector array. dstBase Start of resultant vector array. Note: OK if srcBase == dstBase. count Number of vectors in srcBase, dstBase arrays Note: Cannot check for array overflow Return : None. *---------------------------------------------------------------------*/ void C_MTX44MultVecArraySR ( const Mtx44 m, const Vec *srcBase, Vec *dstBase, u32 count ) { u32 i; Vec vTmp; ASSERTMSG( (m != 0), MTX44_MULTVECARRAYSR_1 ); ASSERTMSG( (srcBase != 0), MTX44_MULTVECARRAYSR_2 ); ASSERTMSG( (dstBase != 0), MTX44_MULTVECARRAYSR_3 ); for ( i = 0; i < count; i ++ ) { // Vec has a 4th implicit 'w' coordinate of 1 vTmp.x = m[0][0]*srcBase->x + m[0][1]*srcBase->y + m[0][2]*srcBase->z; vTmp.y = m[1][0]*srcBase->x + m[1][1]*srcBase->y + m[1][2]*srcBase->z; vTmp.z = m[2][0]*srcBase->x + m[2][1]*srcBase->y + m[2][2]*srcBase->z; // Copy back dstBase->x = vTmp.x; dstBase->y = vTmp.y; dstBase->z = vTmp.z; srcBase++; dstBase++; } } /*---------------------------------------------------------------------* Paired-Single assembler version *---------------------------------------------------------------------* Note that this performs NO error checking. *---------------------------------------------------------------------*/ #ifdef GEKKO asm void PSMTX44MultVecArraySR( const register Mtx44 m, const register Vec *srcBase, register Vec *dstBase, register u32 count ) { nofralloc; psq_l fp0, 0(m), 0, 0; // fp0 <- m00, m01 addi count, count, -1; psq_l fp6, 0(srcBase), 0, 0; // fp6 <- src.x, src.y ps_mul fp8, fp0, fp6; psq_l fp2, 16(m), 0, 0; // fp2 <- m10, m11 ps_mul fp9, fp2, fp6; psq_l fp4, 32(m), 0, 0; // fp4 <- m20, m21 psq_lu fp7, 8(srcBase), 1, 0; // fp7 <- src.z, 1.0 ps_mul fp10, fp4, fp6; psq_l fp1, 8(m), 1, 0; // fp1 <- m02, 1.0 mtctr count; psq_l fp3, 24(m), 1, 0; // fp3 <- m12, 1.0 addi dstBase, dstBase, -4; psq_l fp5, 40(m), 1, 0; // fp5 <- m22, 1.0 @loop: ps_madd fp11, fp1, fp7, fp8; psq_lu fp6, 4(srcBase), 0, 0; ps_madd fp12, fp3, fp7, fp9; ps_madd fp13, fp5, fp7, fp10; psq_lu fp7, 8(srcBase), 1, 0; ps_sum0 fp11, fp11, fp8, fp8; psq_stu fp11, 4(dstBase), 1, 0; ps_sum0 fp12, fp12, fp9, fp9; psq_stu fp12, 4(dstBase), 1, 0; ps_sum0 fp13, fp13, fp10, fp10; psq_stu fp13, 4(dstBase), 1, 0; ps_mul fp8, fp0, fp6; ps_mul fp9, fp2, fp6; ps_mul fp10, fp4, fp6; bdnz+ @loop ps_madd fp11, fp1, fp7, fp8; ps_madd fp12, fp3, fp7, fp9; ps_madd fp13, fp5, fp7, fp10; ps_sum0 fp11, fp11, fp8, fp8; psq_stu fp11, 4(dstBase), 1, 0; ps_sum0 fp12, fp12, fp9, fp9; psq_stu fp12, 4(dstBase), 1, 0; ps_sum0 fp13, fp13, fp10, fp10; psq_stu fp13, 4(dstBase), 1, 0; blr; } #endif // GEKKO /*===========================================================================*/