diff options
author | Made <made111@gmx.de> | 2020-05-04 17:07:22 +0200 |
---|---|---|
committer | Made <made111@gmx.de> | 2020-05-04 17:07:22 +0200 |
commit | ab28b0b37f2fd9fba6010e48e15bd31170d678d7 (patch) | |
tree | e144d94307707a51ccfe9e109c9c10c9dd0b01e7 /arm9/lib/src | |
parent | 246d8051ac7b7535c53d5d180eaf1d97451d31f2 (diff) |
Add typedefs for fixed point types
Diffstat (limited to 'arm9/lib/src')
-rw-r--r-- | arm9/lib/src/FX.c | 2 | ||||
-rw-r--r-- | arm9/lib/src/FX_atan.c | 12 | ||||
-rw-r--r-- | arm9/lib/src/FX_cp.c | 28 | ||||
-rw-r--r-- | arm9/lib/src/FX_mtx22.c | 12 | ||||
-rw-r--r-- | arm9/lib/src/FX_mtx33.c | 56 | ||||
-rw-r--r-- | arm9/lib/src/FX_mtx43.c | 100 | ||||
-rw-r--r-- | arm9/lib/src/FX_mtx44.c | 52 | ||||
-rw-r--r-- | arm9/lib/src/FX_vec.c | 50 |
8 files changed, 156 insertions, 156 deletions
diff --git a/arm9/lib/src/FX.c b/arm9/lib/src/FX.c index fa6fa6ba..f39b1995 100644 --- a/arm9/lib/src/FX.c +++ b/arm9/lib/src/FX.c @@ -6,7 +6,7 @@ void FX_Init(){ return; } -s32 FX_Modf(s32 x, s32 *iptr){ +fx32 FX_Modf(fx32 x, fx32 *iptr){ if (x >= 0) { *iptr = x & 0x7FFFF000; diff --git a/arm9/lib/src/FX_atan.c b/arm9/lib/src/FX_atan.c index 684db81e..085763f8 100644 --- a/arm9/lib/src/FX_atan.c +++ b/arm9/lib/src/FX_atan.c @@ -2,15 +2,15 @@ #include "main.h" #include "fx.h" -extern s16 FX_AtanTable_[]; +extern fx16 FX_AtanTable_[]; -u16 FX_Atan(s32 x){ +u16 FX_Atan(fx32 x){ if (x >= 0) { if (x > 0x1000) { x = FX_Inv(x); - s16 y = FX_AtanTable_[x >> 5]; + fx16 y = FX_AtanTable_[x >> 5]; return 0x4000 - y; } else if (x < 0x1000) @@ -27,7 +27,7 @@ u16 FX_Atan(s32 x){ if (x < -0x1000) { x = FX_Inv(-x); - s16 y = FX_AtanTable_[x >> 5]; + fx16 y = FX_AtanTable_[x >> 5]; return y - 0x4000; } else if (x > -0x1000) @@ -41,8 +41,8 @@ u16 FX_Atan(s32 x){ } } -u16 FX_Atan2(s32 x, s32 y){ - s32 result; +u16 FX_Atan2(fx32 x, fx32 y){ + fx32 result; u32 positive, bias, denominator, numerator; if (x > 0) { diff --git a/arm9/lib/src/FX_cp.c b/arm9/lib/src/FX_cp.c index a2d8307b..76dea4b4 100644 --- a/arm9/lib/src/FX_cp.c +++ b/arm9/lib/src/FX_cp.c @@ -1,23 +1,23 @@ #include "global.h" #include "main.h" #include "fx.h" - -s32 FX_Div(s32 numerator, s32 denominator){ + +fx32 FX_Div(fx32 numerator, fx32 denominator){ FX_DivAsync(numerator, denominator); return FX_GetDivResult(); } -s32 FX_Inv(s32 x){ +fx32 FX_Inv(fx32 x){ FX_InvAsync(x); return FX_GetDivResult(); } -s32 FX_Sqrt(s32 x){ +fx32 FX_Sqrt(fx32 x){ if (x > 0) { SETREG16(HW_REG_SQRTCNT, 0x1); - SETREG64(HW_REG_SQRT_PARAM, (s64)x << 32); + SETREG64(HW_REG_SQRT_PARAM, (fx64)x << 32); return FX_GetSqrtResult(); } else @@ -26,34 +26,34 @@ s32 FX_Sqrt(s32 x){ } } -s64 FX_GetDivResultFx64c(){ +fx64c FX_GetDivResultFx64c(){ while (READREG16(HW_REG_DIVCNT) & 0x8000); return READREG64(HW_REG_DIV_RESULT); } -s32 FX_GetDivResult(){ +fx32 FX_GetDivResult(){ while (READREG16(HW_REG_DIVCNT) & 0x8000); return (READREG64(HW_REG_DIV_RESULT) + (1 << (0x14 - 1))) >> 0x14; } -void FX_InvAsync(s32 x){ +void FX_InvAsync(fx32 x){ SETREG16(HW_REG_DIVCNT, 0x1); - SETREG64(HW_REG_DIV_NUMER, (s64)0x00001000 << 32); + SETREG64(HW_REG_DIV_NUMER, (fx64)0x00001000 << 32); SETREG64(HW_REG_DIV_DENOM, (u32)x); } -s32 FX_GetSqrtResult(){ +fx32 FX_GetSqrtResult(){ while (READREG16(HW_REG_SQRTCNT) & 0x8000); return (READREG32(HW_REG_SQRT_RESULT) + (1 << (0xA - 1))) >> 0xA; } -void FX_DivAsync(s32 numerator, s32 denominator){ +void FX_DivAsync(fx32 numerator, fx32 denominator){ SETREG16(HW_REG_DIVCNT, 0x1); - SETREG64(HW_REG_DIV_NUMER, (s64)numerator << 32); + SETREG64(HW_REG_DIV_NUMER, (fx64)numerator << 32); SETREG64(HW_REG_DIV_DENOM, (u32)denominator); } -s32 FX_DivS32(s32 numerator, s32 denominator){ +fx32 FX_Divfx32(fx32 numerator, fx32 denominator){ SETREG16(HW_REG_DIVCNT, 0x0); SETREG32(HW_REG_DIV_NUMER, (u32)numerator); //32bit write for some reason SETREG64(HW_REG_DIV_DENOM, (u32)denominator); @@ -61,7 +61,7 @@ s32 FX_DivS32(s32 numerator, s32 denominator){ return READREG32(HW_REG_DIV_RESULT); } -s32 FX_ModS32(s32 num, s32 mod){ +fx32 FX_Modfx32(fx32 num, fx32 mod){ SETREG16(HW_REG_DIVCNT, 0x0); SETREG32(HW_REG_DIV_NUMER, (u32)num); //32bit write for some reason SETREG64(HW_REG_DIV_DENOM, (u32)mod); diff --git a/arm9/lib/src/FX_mtx22.c b/arm9/lib/src/FX_mtx22.c index fcd634b1..19504574 100644 --- a/arm9/lib/src/FX_mtx22.c +++ b/arm9/lib/src/FX_mtx22.c @@ -2,11 +2,11 @@ #include "main.h" #include "fx.h" -void MTX_ScaleApply22(struct Mtx22 *mtx, struct Mtx22 *dst, s32 x, s32 y){ - dst->_[0] = ((s64)x * mtx->_[0]) >> FX32_INT_SHIFT; - dst->_[1] = ((s64)x * mtx->_[1]) >> FX32_INT_SHIFT; - dst->_[2] = ((s64)y * mtx->_[2]) >> FX32_INT_SHIFT; - dst->_[3] = ((s64)y * mtx->_[3]) >> FX32_INT_SHIFT; +void MTX_ScaleApply22(struct Mtx22 *mtx, struct Mtx22 *dst, fx32 x, fx32 y){ + dst->_[0] = ((fx64)x * mtx->_[0]) >> FX32_INT_SHIFT; + dst->_[1] = ((fx64)x * mtx->_[1]) >> FX32_INT_SHIFT; + dst->_[2] = ((fx64)y * mtx->_[2]) >> FX32_INT_SHIFT; + dst->_[3] = ((fx64)y * mtx->_[3]) >> FX32_INT_SHIFT; } asm void MTX_Identity22_(struct Mtx22 *mtx){ @@ -19,7 +19,7 @@ asm void MTX_Identity22_(struct Mtx22 *mtx){ } #pragma thumb on -asm void MTX_Rot22_(struct Mtx22 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_Rot22_(struct Mtx22 *mtx, fx32 sinphi, fx32 cosphi){ str r2, [r0, #0x0] str r1, [r0, #0x4] neg r1, r1 diff --git a/arm9/lib/src/FX_mtx33.c b/arm9/lib/src/FX_mtx33.c index 32b1ad2e..391a6104 100644 --- a/arm9/lib/src/FX_mtx33.c +++ b/arm9/lib/src/FX_mtx33.c @@ -2,23 +2,23 @@ #include "main.h" #include "fx.h" -void MTX_ScaleApply33(struct Mtx33 *mtx, struct Mtx33 *dst, s32 x, s32 y, s32 z){ - dst->_[0] = ((s64)x * mtx->_[0]) >> FX32_INT_SHIFT; - dst->_[1] = ((s64)x * mtx->_[1]) >> FX32_INT_SHIFT; - dst->_[2] = ((s64)x * mtx->_[2]) >> FX32_INT_SHIFT; - dst->_[3] = ((s64)y * mtx->_[3]) >> FX32_INT_SHIFT; - dst->_[4] = ((s64)y * mtx->_[4]) >> FX32_INT_SHIFT; - dst->_[5] = ((s64)y * mtx->_[5]) >> FX32_INT_SHIFT; - dst->_[6] = ((s64)z * mtx->_[6]) >> FX32_INT_SHIFT; - dst->_[7] = ((s64)z * mtx->_[7]) >> FX32_INT_SHIFT; - dst->_[8] = ((s64)z * mtx->_[8]) >> FX32_INT_SHIFT; +void MTX_ScaleApply33(struct Mtx33 *mtx, struct Mtx33 *dst, fx32 x, fx32 y, fx32 z){ + dst->_[0] = ((fx64)x * mtx->_[0]) >> FX32_INT_SHIFT; + dst->_[1] = ((fx64)x * mtx->_[1]) >> FX32_INT_SHIFT; + dst->_[2] = ((fx64)x * mtx->_[2]) >> FX32_INT_SHIFT; + dst->_[3] = ((fx64)y * mtx->_[3]) >> FX32_INT_SHIFT; + dst->_[4] = ((fx64)y * mtx->_[4]) >> FX32_INT_SHIFT; + dst->_[5] = ((fx64)y * mtx->_[5]) >> FX32_INT_SHIFT; + dst->_[6] = ((fx64)z * mtx->_[6]) >> FX32_INT_SHIFT; + dst->_[7] = ((fx64)z * mtx->_[7]) >> FX32_INT_SHIFT; + dst->_[8] = ((fx64)z * mtx->_[8]) >> FX32_INT_SHIFT; } void MTX_Concat33(struct Mtx33 *a, struct Mtx33 *b, struct Mtx33 *c){ struct Mtx33 temp; struct Mtx33 *dst; - s32 a0, a1, a2; - s32 b0, b1, b2; + fx32 a0, a1, a2; + fx32 b0, b1, b2; if (c == b) dst = &temp; @@ -28,43 +28,43 @@ void MTX_Concat33(struct Mtx33 *a, struct Mtx33 *b, struct Mtx33 *c){ a0 = a->_[0]; a1 = a->_[1]; a2 = a->_[2]; - dst->_[0] = (((s64)a0 * b->_[0] + (s64)a1 * b->_[3] + (s64)a2 * b->_[6] ) >> FX32_INT_SHIFT); - dst->_[1] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[4] + (s64)a2 * b->_[7] ) >> FX32_INT_SHIFT); + dst->_[0] = (((fx64)a0 * b->_[0] + (fx64)a1 * b->_[3] + (fx64)a2 * b->_[6] ) >> FX32_INT_SHIFT); + dst->_[1] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[7] ) >> FX32_INT_SHIFT); b0 = b->_[2]; b1 = b->_[5]; b2 = b->_[8]; - dst->_[2] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[2] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); a0 = a->_[3]; a1 = a->_[4]; a2 = a->_[5]; - dst->_[5] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); - dst->_[4] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[4] + (s64)a2 * b->_[7] ) >> FX32_INT_SHIFT); + dst->_[5] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[4] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[7] ) >> FX32_INT_SHIFT); b0 = b->_[0]; b1 = b->_[3]; b2 = b->_[6]; - dst->_[3] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[3] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); a0 = a->_[6]; a1 = a->_[7]; a2 = a->_[8]; - dst->_[6] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); - dst->_[7] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[4] + (s64)a2 * b->_[7] ) >> FX32_INT_SHIFT); + dst->_[6] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[7] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[7] ) >> FX32_INT_SHIFT); b0 = b->_[2]; b1 = b->_[5]; b2 = b->_[8]; - dst->_[8] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[8] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); if (dst == &temp) *c = temp; } void MTX_MultVec33(struct Vecx32 *vec, struct Mtx33 *mtx, struct Vecx32 *dst){ - s32 x, y, z; + fx32 x, y, z; x = vec->x; y = vec->y; z = vec->z; - dst->x = ((s64)x * mtx->_[0] + (s64)y * mtx->_[3] + (s64)z * mtx->_[6]) >> FX32_INT_SHIFT; - dst->y = ((s64)x * mtx->_[1] + (s64)y * mtx->_[4] + (s64)z * mtx->_[7]) >> FX32_INT_SHIFT; - dst->z = ((s64)x * mtx->_[2] + (s64)y * mtx->_[5] + (s64)z * mtx->_[8]) >> FX32_INT_SHIFT; + dst->x = ((fx64)x * mtx->_[0] + (fx64)y * mtx->_[3] + (fx64)z * mtx->_[6]) >> FX32_INT_SHIFT; + dst->y = ((fx64)x * mtx->_[1] + (fx64)y * mtx->_[4] + (fx64)z * mtx->_[7]) >> FX32_INT_SHIFT; + dst->z = ((fx64)x * mtx->_[2] + (fx64)y * mtx->_[5] + (fx64)z * mtx->_[8]) >> FX32_INT_SHIFT; } asm void MTX_Identity33_(struct Mtx33 *mtx){ @@ -80,7 +80,7 @@ asm void MTX_Identity33_(struct Mtx33 *mtx){ } #pragma thumb on -asm void MTX_RotX33_(struct Mtx33 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_RotX33_(struct Mtx33 *mtx, fx32 sinphi, fx32 cosphi){ mov r3, #0x1 lsl r3, r3, #0xc str r3, [r0, #0x0] @@ -99,7 +99,7 @@ asm void MTX_RotX33_(struct Mtx33 *mtx, s32 sinphi, s32 cosphi){ #pragma thumb off #pragma thumb on -asm void MTX_RotY33_(struct Mtx33 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_RotY33_(struct Mtx33 *mtx, fx32 sinphi, fx32 cosphi){ str r2, [r0, #0x0] str r2, [r0, #0x20] mov r3, #0x0 @@ -118,7 +118,7 @@ asm void MTX_RotY33_(struct Mtx33 *mtx, s32 sinphi, s32 cosphi){ #pragma thumb off #pragma thumb on -asm void MTX_RotZ33_(struct Mtx33 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_RotZ33_(struct Mtx33 *mtx, fx32 sinphi, fx32 cosphi){ stmia r0!, {r2} mov r3, #0x0 stmia r0!, {r1,r3} diff --git a/arm9/lib/src/FX_mtx43.c b/arm9/lib/src/FX_mtx43.c index 9cb8aa6e..862bd6f2 100644 --- a/arm9/lib/src/FX_mtx43.c +++ b/arm9/lib/src/FX_mtx43.c @@ -3,7 +3,7 @@ #include "fx.h" -void MTX_ScaleApply43(struct Mtx43 *mtx, struct Mtx43 *dst, s32 x, s32 y, s32 z){ +void MTX_ScaleApply43(struct Mtx43 *mtx, struct Mtx43 *dst, fx32 x, fx32 y, fx32 z){ //this works because matrices are indexed columns first MTX_ScaleApply33((struct Mtx33 *)mtx, (struct Mtx33 *)dst, x, y, z); dst->_[9] = mtx->_[9]; @@ -11,48 +11,48 @@ void MTX_ScaleApply43(struct Mtx43 *mtx, struct Mtx43 *dst, s32 x, s32 y, s32 z) dst->_[11] = mtx->_[11]; } -s32 MTX_Inverse43(struct Mtx43 *mtx, struct Mtx43 *inv){ +fx32 MTX_Inverse43(struct Mtx43 *mtx, struct Mtx43 *inv){ struct Mtx43 tempmat; struct Mtx43 *dst; - s32 det0, det1, det2, det; - s32 var0, var1, var2, var3; + fx32 det0, det1, det2, det; + fx32 var0, var1, var2, var3; if (mtx == inv) dst = &tempmat; else dst = inv; //subdeterminants - det0 = ((s64)mtx->_[4] * mtx->_[8] - (s64)mtx->_[5] * mtx->_[7] + (s64)(1 << (FX32_INT_SHIFT - 1))) >> FX32_INT_SHIFT; - det1 = ((s64)mtx->_[3] * mtx->_[8] - (s64)mtx->_[5] * mtx->_[6] + (s64)(1 << (FX32_INT_SHIFT - 1))) >> FX32_INT_SHIFT; - det2 = ((s64)mtx->_[3] * mtx->_[7] - (s64)mtx->_[4] * mtx->_[6] + (s64)(1 << (FX32_INT_SHIFT - 1))) >> FX32_INT_SHIFT; + det0 = ((fx64)mtx->_[4] * mtx->_[8] - (fx64)mtx->_[5] * mtx->_[7] + (fx64)(1 << (FX32_INT_SHIFT - 1))) >> FX32_INT_SHIFT; + det1 = ((fx64)mtx->_[3] * mtx->_[8] - (fx64)mtx->_[5] * mtx->_[6] + (fx64)(1 << (FX32_INT_SHIFT - 1))) >> FX32_INT_SHIFT; + det2 = ((fx64)mtx->_[3] * mtx->_[7] - (fx64)mtx->_[4] * mtx->_[6] + (fx64)(1 << (FX32_INT_SHIFT - 1))) >> FX32_INT_SHIFT; //matrix determinant - det = ((s64)mtx->_[0] * det0 - (s64)mtx->_[1] * det1 + (s64)mtx->_[2] * det2 + (s64)(1 << (FX32_INT_SHIFT - 1))) >> FX32_INT_SHIFT; + det = ((fx64)mtx->_[0] * det0 - (fx64)mtx->_[1] * det1 + (fx64)mtx->_[2] * det2 + (fx64)(1 << (FX32_INT_SHIFT - 1))) >> FX32_INT_SHIFT; if (det == 0) return -1; //not invertible FX_InvAsync(det); - var0 = ((s64)mtx->_[1] * mtx->_[8] - (s64)mtx->_[7] * mtx->_[2]) >> FX32_INT_SHIFT; - var1 = ((s64)mtx->_[1] * mtx->_[5] - (s64)mtx->_[4] * mtx->_[2]) >> FX32_INT_SHIFT; - var2 = ((s64)mtx->_[0] * mtx->_[8] - (s64)mtx->_[6] * mtx->_[2]) >> FX32_INT_SHIFT; - var3 = ((s64)mtx->_[0] * mtx->_[5] - (s64)mtx->_[3] * mtx->_[2]) >> FX32_INT_SHIFT; - - s32 ret = FX_GetDivResult(); - dst->_[0] = (s32)(((s64)ret * det0) >> FX32_INT_SHIFT); - dst->_[1] = -(s32)(((s64)ret * var0) >> FX32_INT_SHIFT); - dst->_[2] = (s32)(((s64)ret * var1) >> FX32_INT_SHIFT); - dst->_[3] = -(s32)(((s64)ret * det1) >> FX32_INT_SHIFT); - dst->_[4] = (s32)(((s64)ret * var2) >> FX32_INT_SHIFT); - dst->_[5] = -(s32)(((s64)ret * var3) >> FX32_INT_SHIFT); - - dst->_[6] = (s32)(((s64)ret * det2) >> FX32_INT_SHIFT); - s32 temp = (s32)(((s64)mtx->_[0] * mtx->_[7] - (s64)mtx->_[6] * mtx->_[1]) >> FX32_INT_SHIFT); - dst->_[7] = -(s32)(((s64)ret * temp) >> FX32_INT_SHIFT); - s32 temp1 = (s32)(((s64)mtx->_[0] * mtx->_[4] - (s64)mtx->_[3] * mtx->_[1]) >> FX32_INT_SHIFT); - dst->_[8] = (s32)(((s64)ret * temp1) >> FX32_INT_SHIFT); - dst->_[9] = -(s32)(((s64)dst->_[0] * mtx->_[9] + (s64)dst->_[3] * mtx->_[10] + (s64)dst->_[6] * mtx->_[11]) >> FX32_INT_SHIFT); - dst->_[10] = -(s32)(((s64)dst->_[1] * mtx->_[9] + (s64)dst->_[4] * mtx->_[10] + (s64)dst->_[7] * mtx->_[11]) >> FX32_INT_SHIFT); - dst->_[11] = -(s32)(((s64)dst->_[2] * mtx->_[9] + (s64)dst->_[5] * mtx->_[10] + (s64)dst->_[8] * mtx->_[11]) >> FX32_INT_SHIFT); + var0 = ((fx64)mtx->_[1] * mtx->_[8] - (fx64)mtx->_[7] * mtx->_[2]) >> FX32_INT_SHIFT; + var1 = ((fx64)mtx->_[1] * mtx->_[5] - (fx64)mtx->_[4] * mtx->_[2]) >> FX32_INT_SHIFT; + var2 = ((fx64)mtx->_[0] * mtx->_[8] - (fx64)mtx->_[6] * mtx->_[2]) >> FX32_INT_SHIFT; + var3 = ((fx64)mtx->_[0] * mtx->_[5] - (fx64)mtx->_[3] * mtx->_[2]) >> FX32_INT_SHIFT; + + fx32 ret = FX_GetDivResult(); + dst->_[0] = (fx32)(((fx64)ret * det0) >> FX32_INT_SHIFT); + dst->_[1] = -(fx32)(((fx64)ret * var0) >> FX32_INT_SHIFT); + dst->_[2] = (fx32)(((fx64)ret * var1) >> FX32_INT_SHIFT); + dst->_[3] = -(fx32)(((fx64)ret * det1) >> FX32_INT_SHIFT); + dst->_[4] = (fx32)(((fx64)ret * var2) >> FX32_INT_SHIFT); + dst->_[5] = -(fx32)(((fx64)ret * var3) >> FX32_INT_SHIFT); + + dst->_[6] = (fx32)(((fx64)ret * det2) >> FX32_INT_SHIFT); + fx32 temp = (fx32)(((fx64)mtx->_[0] * mtx->_[7] - (fx64)mtx->_[6] * mtx->_[1]) >> FX32_INT_SHIFT); + dst->_[7] = -(fx32)(((fx64)ret * temp) >> FX32_INT_SHIFT); + fx32 temp1 = (fx32)(((fx64)mtx->_[0] * mtx->_[4] - (fx64)mtx->_[3] * mtx->_[1]) >> FX32_INT_SHIFT); + dst->_[8] = (fx32)(((fx64)ret * temp1) >> FX32_INT_SHIFT); + dst->_[9] = -(fx32)(((fx64)dst->_[0] * mtx->_[9] + (fx64)dst->_[3] * mtx->_[10] + (fx64)dst->_[6] * mtx->_[11]) >> FX32_INT_SHIFT); + dst->_[10] = -(fx32)(((fx64)dst->_[1] * mtx->_[9] + (fx64)dst->_[4] * mtx->_[10] + (fx64)dst->_[7] * mtx->_[11]) >> FX32_INT_SHIFT); + dst->_[11] = -(fx32)(((fx64)dst->_[2] * mtx->_[9] + (fx64)dst->_[5] * mtx->_[10] + (fx64)dst->_[8] * mtx->_[11]) >> FX32_INT_SHIFT); if (dst == &tempmat) MI_Copy48B(&tempmat, inv); @@ -62,8 +62,8 @@ s32 MTX_Inverse43(struct Mtx43 *mtx, struct Mtx43 *inv){ void MTX_Concat43(struct Mtx43 *a, struct Mtx43 *b, struct Mtx43 *c){ struct Mtx43 temp; struct Mtx43 *dst; - s32 a0, a1, a2; - s32 b0, b1, b2; + fx32 a0, a1, a2; + fx32 b0, b1, b2; if (c == b) dst = &temp; @@ -73,50 +73,50 @@ void MTX_Concat43(struct Mtx43 *a, struct Mtx43 *b, struct Mtx43 *c){ a0 = a->_[0]; a1 = a->_[1]; a2 = a->_[2]; - dst->_[0] = (((s64)a0 * b->_[0] + (s64)a1 * b->_[3] + (s64)a2 * b->_[6] ) >> FX32_INT_SHIFT); - dst->_[1] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[4] + (s64)a2 * b->_[7] ) >> FX32_INT_SHIFT); + dst->_[0] = (((fx64)a0 * b->_[0] + (fx64)a1 * b->_[3] + (fx64)a2 * b->_[6] ) >> FX32_INT_SHIFT); + dst->_[1] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[7] ) >> FX32_INT_SHIFT); b0 = b->_[2]; b1 = b->_[5]; b2 = b->_[8]; - dst->_[2] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[2] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); a0 = a->_[3]; a1 = a->_[4]; a2 = a->_[5]; - dst->_[5] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); - dst->_[4] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[4] + (s64)a2 * b->_[7] ) >> FX32_INT_SHIFT); + dst->_[5] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[4] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[7] ) >> FX32_INT_SHIFT); b0 = b->_[0]; b1 = b->_[3]; b2 = b->_[6]; - dst->_[3] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[3] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); a0 = a->_[6]; a1 = a->_[7]; a2 = a->_[8]; - dst->_[6] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); - dst->_[7] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[4] + (s64)a2 * b->_[7] ) >> FX32_INT_SHIFT); + dst->_[6] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[7] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[7] ) >> FX32_INT_SHIFT); b0 = b->_[2]; b1 = b->_[5]; b2 = b->_[8]; - dst->_[8] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT); + dst->_[8] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT); a0 = a->_[9]; a1 = a->_[10]; a2 = a->_[11]; - dst->_[11] = ((((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2) >> FX32_INT_SHIFT) + b->_[11]); - dst->_[10] = ((((s64)a0 * b->_[1] + (s64)a1 * b->_[4] + (s64)a2 * b->_[7]) >> FX32_INT_SHIFT) + b->_[10]); - dst->_[9] = ((((s64)a0 * b->_[0] + (s64)a1 * b->_[3] + (s64)a2 * b->_[6]) >> FX32_INT_SHIFT) + b->_[9]); + dst->_[11] = ((((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT) + b->_[11]); + dst->_[10] = ((((fx64)a0 * b->_[1] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[7]) >> FX32_INT_SHIFT) + b->_[10]); + dst->_[9] = ((((fx64)a0 * b->_[0] + (fx64)a1 * b->_[3] + (fx64)a2 * b->_[6]) >> FX32_INT_SHIFT) + b->_[9]); if (dst == &temp) *c = temp; } void MTX_MultVec43(struct Vecx32 *vec, struct Mtx43 *mtx, struct Vecx32 *dst){ - s32 x, y, z; + fx32 x, y, z; x = vec->x; y = vec->y; z = vec->z; - dst->x = ((s64)x * mtx->_[0] + (s64)y * mtx->_[3] + (s64)z * mtx->_[6]) >> FX32_INT_SHIFT; + dst->x = ((fx64)x * mtx->_[0] + (fx64)y * mtx->_[3] + (fx64)z * mtx->_[6]) >> FX32_INT_SHIFT; dst->x += mtx->_[9]; - dst->y = ((s64)x * mtx->_[1] + (s64)y * mtx->_[4] + (s64)z * mtx->_[7]) >> FX32_INT_SHIFT; + dst->y = ((fx64)x * mtx->_[1] + (fx64)y * mtx->_[4] + (fx64)z * mtx->_[7]) >> FX32_INT_SHIFT; dst->y += mtx->_[10]; - dst->z = ((s64)x * mtx->_[2] + (s64)y * mtx->_[5] + (s64)z * mtx->_[8]) >> FX32_INT_SHIFT; + dst->z = ((fx64)x * mtx->_[2] + (fx64)y * mtx->_[5] + (fx64)z * mtx->_[8]) >> FX32_INT_SHIFT; dst->z += mtx->_[11]; } @@ -150,7 +150,7 @@ asm void MTX_Copy43To44_(struct Mtx43 *src, struct Mtx44 *dst){ } #pragma thumb on -asm void MTX_Scale43_(struct Mtx43 *dst, s32 x, s32 y, s32 z){ +asm void MTX_Scale43_(struct Mtx43 *dst, fx32 x, fx32 y, fx32 z){ stmia r0!, {r1} mov r1, #0x0 str r3, [r0, #0x1c] @@ -166,7 +166,7 @@ asm void MTX_Scale43_(struct Mtx43 *dst, s32 x, s32 y, s32 z){ #pragma thumb off #pragma thumb on -asm void MTX_RotX43_(struct Mtx43 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_RotX43_(struct Mtx43 *mtx, fx32 sinphi, fx32 cosphi){ str r1, [r0, #0x14] neg r1, r1 str r1, [r0, #0x1c] @@ -186,7 +186,7 @@ asm void MTX_RotX43_(struct Mtx43 *mtx, s32 sinphi, s32 cosphi){ #pragma thumb off #pragma thumb on -asm void MTX_RotY43_(struct Mtx43 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_RotY43_(struct Mtx43 *mtx, fx32 sinphi, fx32 cosphi){ str r1, [r0, #0x18] mov r3, #0x0 stmia r0!, {r2-r3} diff --git a/arm9/lib/src/FX_mtx44.c b/arm9/lib/src/FX_mtx44.c index 3c91b4df..8d158602 100644 --- a/arm9/lib/src/FX_mtx44.c +++ b/arm9/lib/src/FX_mtx44.c @@ -5,20 +5,20 @@ void MI_Copy48B(void *src, void *dst); -void MTX_TransApply44(struct Mtx44 *mtx, struct Mtx44 *dst, s32 x, s32 y, s32 z){ +void MTX_TransApply44(struct Mtx44 *mtx, struct Mtx44 *dst, fx32 x, fx32 y, fx32 z){ if(mtx != dst) MI_Copy48B(mtx, dst); - dst->_[12] = mtx->_[12] + (s32)(((s64)x * mtx->_[0] + (s64)y * mtx->_[4] + (s64)z * mtx->_[8] ) >> FX32_INT_SHIFT); - dst->_[13] = mtx->_[13] + (s32)(((s64)x * mtx->_[1] + (s64)y * mtx->_[5] + (s64)z * mtx->_[9] ) >> FX32_INT_SHIFT); - dst->_[14] = mtx->_[14] + (s32)(((s64)x * mtx->_[2] + (s64)y * mtx->_[6] + (s64)z * mtx->_[10]) >> FX32_INT_SHIFT); - dst->_[15] = mtx->_[15] + (s32)(((s64)x * mtx->_[3] + (s64)y * mtx->_[7] + (s64)z * mtx->_[11]) >> FX32_INT_SHIFT); + dst->_[12] = mtx->_[12] + (fx32)(((fx64)x * mtx->_[0] + (fx64)y * mtx->_[4] + (fx64)z * mtx->_[8] ) >> FX32_INT_SHIFT); + dst->_[13] = mtx->_[13] + (fx32)(((fx64)x * mtx->_[1] + (fx64)y * mtx->_[5] + (fx64)z * mtx->_[9] ) >> FX32_INT_SHIFT); + dst->_[14] = mtx->_[14] + (fx32)(((fx64)x * mtx->_[2] + (fx64)y * mtx->_[6] + (fx64)z * mtx->_[10]) >> FX32_INT_SHIFT); + dst->_[15] = mtx->_[15] + (fx32)(((fx64)x * mtx->_[3] + (fx64)y * mtx->_[7] + (fx64)z * mtx->_[11]) >> FX32_INT_SHIFT); } void MTX_Concat44(struct Mtx44 *a, struct Mtx44 *b, struct Mtx44 *c){ struct Mtx44 temp; struct Mtx44 *dst; - s32 a0, a1, a2, a3; - s32 b0, b1, b2, b3; + fx32 a0, a1, a2, a3; + fx32 b0, b1, b2, b3; if (c == b) dst = &temp; @@ -29,46 +29,46 @@ void MTX_Concat44(struct Mtx44 *a, struct Mtx44 *b, struct Mtx44 *c){ a1 = a->_[1]; a2 = a->_[2]; a3 = a->_[3]; - dst->_[0] = (((s64)a0 * b->_[0] + (s64)a1 * b->_[4] + (s64)a2 * b->_[8] + (s64)a3 * b->_[12]) >> FX32_INT_SHIFT); - dst->_[1] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[5] + (s64)a2 * b->_[9] + (s64)a3 * b->_[13]) >> FX32_INT_SHIFT); - dst->_[3] = (((s64)a0 * b->_[3] + (s64)a1 * b->_[7] + (s64)a2 * b->_[11] + (s64)a3 * b->_[15]) >> FX32_INT_SHIFT); + dst->_[0] = (((fx64)a0 * b->_[0] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[8] + (fx64)a3 * b->_[12]) >> FX32_INT_SHIFT); + dst->_[1] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[5] + (fx64)a2 * b->_[9] + (fx64)a3 * b->_[13]) >> FX32_INT_SHIFT); + dst->_[3] = (((fx64)a0 * b->_[3] + (fx64)a1 * b->_[7] + (fx64)a2 * b->_[11] + (fx64)a3 * b->_[15]) >> FX32_INT_SHIFT); b0 = b->_[2]; b1 = b->_[6]; b2 = b->_[10]; b3 = b->_[14]; - dst->_[2] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2 + (s64)a3 * b3) >> FX32_INT_SHIFT); + dst->_[2] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT); a0 = a->_[4]; a1 = a->_[5]; a2 = a->_[6]; a3 = a->_[7]; - dst->_[6] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2 + (s64)a3 * b3) >> FX32_INT_SHIFT); - dst->_[5] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[5] + (s64)a2 * b->_[9] + (s64)a3 * b->_[13]) >> FX32_INT_SHIFT); - dst->_[7] = (((s64)a0 * b->_[3] + (s64)a1 * b->_[7] + (s64)a2 * b->_[11] + (s64)a3 * b->_[15]) >> FX32_INT_SHIFT); + dst->_[6] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT); + dst->_[5] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[5] + (fx64)a2 * b->_[9] + (fx64)a3 * b->_[13]) >> FX32_INT_SHIFT); + dst->_[7] = (((fx64)a0 * b->_[3] + (fx64)a1 * b->_[7] + (fx64)a2 * b->_[11] + (fx64)a3 * b->_[15]) >> FX32_INT_SHIFT); b0 = b->_[0]; b1 = b->_[4]; b2 = b->_[8]; b3 = b->_[12]; - dst->_[4] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2 + (s64)a3 * b3) >> FX32_INT_SHIFT); + dst->_[4] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT); a0 = a->_[8]; a1 = a->_[9]; a2 = a->_[10]; a3 = a->_[11]; - dst->_[8] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2 + (s64)a3 * b3) >> FX32_INT_SHIFT); - dst->_[9] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[5] + (s64)a2 * b->_[9] + (s64)a3 * b->_[13]) >> FX32_INT_SHIFT); - dst->_[11] = (((s64)a0 * b->_[3] + (s64)a1 * b->_[7] + (s64)a2 * b->_[11] + (s64)a3 * b->_[15]) >> FX32_INT_SHIFT); + dst->_[8] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT); + dst->_[9] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[5] + (fx64)a2 * b->_[9] + (fx64)a3 * b->_[13]) >> FX32_INT_SHIFT); + dst->_[11] = (((fx64)a0 * b->_[3] + (fx64)a1 * b->_[7] + (fx64)a2 * b->_[11] + (fx64)a3 * b->_[15]) >> FX32_INT_SHIFT); b0 = b->_[2]; b1 = b->_[6]; b2 = b->_[10]; b3 = b->_[14]; - dst->_[10] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2 + (s64)a3 * b3) >> FX32_INT_SHIFT); + dst->_[10] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT); a0 = a->_[12]; a1 = a->_[13]; a2 = a->_[14]; a3 = a->_[15]; - dst->_[14] = (((s64)a0 * b0 + (s64)a1 * b1 + (s64)a2 * b2 + (s64)a3 * b3) >> FX32_INT_SHIFT); - dst->_[13] = (((s64)a0 * b->_[1] + (s64)a1 * b->_[5] + (s64)a2 * b->_[9] + (s64)a3 * b->_[13]) >> FX32_INT_SHIFT); - dst->_[12] = (((s64)a0 * b->_[0] + (s64)a1 * b->_[4] + (s64)a2 * b->_[8] + (s64)a3 * b->_[12]) >> FX32_INT_SHIFT); - dst->_[15] = (((s64)a0 * b->_[3] + (s64)a1 * b->_[7] + (s64)a2 * b->_[11] + (s64)a3 * b->_[15]) >> FX32_INT_SHIFT); + dst->_[14] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT); + dst->_[13] = (((fx64)a0 * b->_[1] + (fx64)a1 * b->_[5] + (fx64)a2 * b->_[9] + (fx64)a3 * b->_[13]) >> FX32_INT_SHIFT); + dst->_[12] = (((fx64)a0 * b->_[0] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[8] + (fx64)a3 * b->_[12]) >> FX32_INT_SHIFT); + dst->_[15] = (((fx64)a0 * b->_[3] + (fx64)a1 * b->_[7] + (fx64)a2 * b->_[11] + (fx64)a3 * b->_[15]) >> FX32_INT_SHIFT); if (dst == &temp) *c = temp; } @@ -104,7 +104,7 @@ asm void MTX_Copy44To43_(struct Mtx44 *src, struct Mtx43 *dst){ } #pragma thumb on -asm void MTX_RotX44_(struct Mtx44 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_RotX44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){ str r2, [r0, #0x14] str r2, [r0, #0x28] str r1, [r0, #0x18] @@ -127,7 +127,7 @@ asm void MTX_RotX44_(struct Mtx44 *mtx, s32 sinphi, s32 cosphi){ #pragma thumb off #pragma thumb on -asm void MTX_RotY44_(struct Mtx44 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_RotY44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){ str r2, [r0, #0x0] str r2, [r0, #0x28] str r1, [r0, #0x20] @@ -150,7 +150,7 @@ asm void MTX_RotY44_(struct Mtx44 *mtx, s32 sinphi, s32 cosphi){ #pragma thumb off #pragma thumb on -asm void MTX_RotZ44_(struct Mtx44 *mtx, s32 sinphi, s32 cosphi){ +asm void MTX_RotZ44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){ str r2, [r0, #0x0] str r2, [r0, #0x14] str r1, [r0, #0x4] diff --git a/arm9/lib/src/FX_vec.c b/arm9/lib/src/FX_vec.c index 7b838829..10a792be 100644 --- a/arm9/lib/src/FX_vec.c +++ b/arm9/lib/src/FX_vec.c @@ -20,29 +20,29 @@ void VEC_Fx16Add(struct Vecx16 *a, struct Vecx16 *b, struct Vecx16 *dst){ dst->z = a->z + b->z; } -s32 VEC_DotProduct(struct Vecx32 *a, struct Vecx32 *b){ - return ((s64)a->x * b->x + (s64)a->y * b->y + (s64)a->z * b->z + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT; +fx32 VEC_DotProduct(struct Vecx32 *a, struct Vecx32 *b){ + return ((fx64)a->x * b->x + (fx64)a->y * b->y + (fx64)a->z * b->z + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT; } -s32 VEC_Fx16DotProduct(struct Vecx16 *a, struct Vecx16 *b){ - s32 temp1, temp2; +fx32 VEC_Fx16DotProduct(struct Vecx16 *a, struct Vecx16 *b){ + fx32 temp1, temp2; temp1 = (a->x * b->x) + (a->y * b->y); temp2 = (a->z * b->z) + (1 << (FX64_INT_SHIFT - 1)); - return (s32)(((s64)temp1 + temp2) >> FX64_INT_SHIFT); + return (fx32)(((fx64)temp1 + temp2) >> FX64_INT_SHIFT); } void VEC_CrossProduct(struct Vecx32 *a, struct Vecx32 *b, struct Vecx32 *dst){ - s32 x, y, z; - x = (s32)(((s64)a->y * b->z - (s64)a->z * b->y + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); - y = (s32)(((s64)a->z * b->x - (s64)a->x * b->z + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); - z = (s32)(((s64)a->x * b->y - (s64)a->y * b->x + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); + fx32 x, y, z; + x = (fx32)(((fx64)a->y * b->z - (fx64)a->z * b->y + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); + y = (fx32)(((fx64)a->z * b->x - (fx64)a->x * b->z + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); + z = (fx32)(((fx64)a->x * b->y - (fx64)a->y * b->x + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); dst->x = x; dst->y = y; dst->z = z; } void VEC_Fx16CrossProduct(struct Vecx16 *a, struct Vecx16 *b, struct Vecx16 *dst){ - s32 x, y, z; + fx32 x, y, z; x = ((a->y * b->z - a->z * b->y + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); y = ((a->z * b->x - a->x * b->z + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); z = ((a->x * b->y - a->y * b->x + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT); @@ -51,20 +51,20 @@ void VEC_Fx16CrossProduct(struct Vecx16 *a, struct Vecx16 *b, struct Vecx16 *dst dst->z = z; } -s32 VEC_Mag(struct Vecx32 *a){ - s64 l2 = (s64)a->x * a->x; - l2 += (s64)a->y * a->y; - l2 += (s64)a->z * a->z; +fx32 VEC_Mag(struct Vecx32 *a){ + fx64 l2 = (fx64)a->x * a->x; + l2 += (fx64)a->y * a->y; + l2 += (fx64)a->z * a->z; SETREG16(HW_REG_SQRTCNT, 0x1); SETREG64(HW_REG_SQRT_PARAM, l2 * 4); while (READREG16(HW_REG_SQRTCNT) & 0x8000); //wait for coprocessor to finish - return ((s32)READREG32(HW_REG_SQRT_RESULT) + 1) >> 1; + return ((fx32)READREG32(HW_REG_SQRT_RESULT) + 1) >> 1; } void VEC_Normalize(struct Vecx32 *a, struct Vecx32 *dst){ - s64 l2 = (s64)a->x * a->x; - l2 += (s64)a->y * a->y; - l2 += (s64)a->z * a->z; + fx64 l2 = (fx64)a->x * a->x; + l2 += (fx64)a->y * a->y; + l2 += (fx64)a->z * a->z; //1/sqrt(l) is computed by calculating sqrt(l)*(1/l) SETREG16(HW_REG_DIVCNT, 0x2); SETREG64(HW_REG_DIV_NUMER, 0x0100000000000000); @@ -72,7 +72,7 @@ void VEC_Normalize(struct Vecx32 *a, struct Vecx32 *dst){ SETREG16(HW_REG_SQRTCNT, 0x1); SETREG64(HW_REG_SQRT_PARAM, l2 * 4); while (READREG16(HW_REG_SQRTCNT) & 0x8000); //wait for sqrt to finish - s32 sqrtresult = READREG32(HW_REG_SQRT_RESULT); + fx32 sqrtresult = READREG32(HW_REG_SQRT_RESULT); while (READREG16(HW_REG_DIVCNT) & 0x8000); //wait for division to finish l2 = READREG64(HW_REG_DIV_RESULT); l2 = sqrtresult * l2; @@ -82,7 +82,7 @@ void VEC_Normalize(struct Vecx32 *a, struct Vecx32 *dst){ } void VEC_Fx16Normalize(struct Vecx16 *a, struct Vecx16 *dst){ - s64 l2 = a->x * a->x; + fx64 l2 = a->x * a->x; l2 += a->y * a->y; l2 += a->z * a->z; //1/sqrt(l) is computed by calculating sqrt(l)*(1/l) @@ -92,7 +92,7 @@ void VEC_Fx16Normalize(struct Vecx16 *a, struct Vecx16 *dst){ SETREG16(HW_REG_SQRTCNT, 0x1); SETREG64(HW_REG_SQRT_PARAM, l2 * 4); while (READREG16(HW_REG_SQRTCNT) & 0x8000); //wait for sqrt to finish - s32 sqrtresult = READREG32(HW_REG_SQRT_RESULT); + fx32 sqrtresult = READREG32(HW_REG_SQRT_RESULT); while (READREG16(HW_REG_DIVCNT) & 0x8000); //wait for division to finish l2 = READREG64(HW_REG_DIV_RESULT); l2 = sqrtresult * l2; @@ -101,8 +101,8 @@ void VEC_Fx16Normalize(struct Vecx16 *a, struct Vecx16 *dst){ dst->z = (l2 * a->z + (1LL << (0x2D - 1))) >> 0x2D; } -void VEC_MultAdd(s32 factor, struct Vecx32 *a, struct Vecx32 *b, struct Vecx32 *dst){ - dst->x = (s32)(((s64)factor * a->x) >> FX32_INT_SHIFT) + b->x; - dst->y = (s32)(((s64)factor * a->y) >> FX32_INT_SHIFT) + b->y; - dst->z = (s32)(((s64)factor * a->z) >> FX32_INT_SHIFT) + b->z; +void VEC_MultAdd(fx32 factor, struct Vecx32 *a, struct Vecx32 *b, struct Vecx32 *dst){ + dst->x = (fx32)(((fx64)factor * a->x) >> FX32_INT_SHIFT) + b->x; + dst->y = (fx32)(((fx64)factor * a->y) >> FX32_INT_SHIFT) + b->y; + dst->z = (fx32)(((fx64)factor * a->z) >> FX32_INT_SHIFT) + b->z; } |