summaryrefslogtreecommitdiff
path: root/arm9/lib/src
diff options
context:
space:
mode:
Diffstat (limited to 'arm9/lib/src')
-rw-r--r--arm9/lib/src/FX.c20
-rw-r--r--arm9/lib/src/FX_atan.c161
-rw-r--r--arm9/lib/src/FX_cp.c70
-rw-r--r--arm9/lib/src/FX_mtx22.c30
-rw-r--r--arm9/lib/src/FX_mtx33.c135
-rw-r--r--arm9/lib/src/FX_mtx43.c204
-rw-r--r--arm9/lib/src/FX_mtx44.c172
-rw-r--r--arm9/lib/src/FX_vec.c108
8 files changed, 900 insertions, 0 deletions
diff --git a/arm9/lib/src/FX.c b/arm9/lib/src/FX.c
new file mode 100644
index 00000000..f39b1995
--- /dev/null
+++ b/arm9/lib/src/FX.c
@@ -0,0 +1,20 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+void FX_Init(){
+ return;
+}
+
+fx32 FX_Modf(fx32 x, fx32 *iptr){
+ if (x >= 0)
+ {
+ *iptr = x & 0x7FFFF000;
+ return x & 0xFFF;
+ }
+ else
+ {
+ *iptr = -(-x & 0x7FFFF000);
+ return -(-x & 0xFFF);
+ }
+}
diff --git a/arm9/lib/src/FX_atan.c b/arm9/lib/src/FX_atan.c
new file mode 100644
index 00000000..085763f8
--- /dev/null
+++ b/arm9/lib/src/FX_atan.c
@@ -0,0 +1,161 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+extern fx16 FX_AtanTable_[];
+
+u16 FX_Atan(fx32 x){
+ if (x >= 0)
+ {
+ if (x > 0x1000)
+ {
+ x = FX_Inv(x);
+ fx16 y = FX_AtanTable_[x >> 5];
+ return 0x4000 - y;
+ }
+ else if (x < 0x1000)
+ {
+ return FX_AtanTable_[x >> 5];
+ }
+ else
+ {
+ return 0x2000;
+ }
+ }
+ else
+ {
+ if (x < -0x1000)
+ {
+ x = FX_Inv(-x);
+ fx16 y = FX_AtanTable_[x >> 5];
+ return y - 0x4000;
+ }
+ else if (x > -0x1000)
+ {
+ return -FX_AtanTable_[-x >> 5];
+ }
+ else
+ {
+ return -0x2000;
+ }
+ }
+}
+
+u16 FX_Atan2(fx32 x, fx32 y){
+ fx32 result;
+ u32 positive, bias, denominator, numerator;
+ if (x > 0)
+ {
+ if (y > 0)
+ {
+ if (y > x)
+ {
+ numerator = x;
+ denominator = y;
+ bias = 0;
+ positive = TRUE;
+ }
+ else if (y < x)
+ {
+ numerator = y;
+ denominator = x;
+ bias = 0x4000;
+ positive = FALSE;
+ }
+ else
+ {
+ return 0x2000;
+ }
+ }
+ else if (y < 0)
+ {
+ y = -y;
+ if (y < x)
+ {
+ numerator = y;
+ denominator = x;
+ bias = 0x4000;
+ positive = TRUE;
+ }
+ else if (y > x)
+ {
+ numerator = x;
+ denominator = y;
+ bias = 0x8000;
+ positive = FALSE;
+ }
+ else
+ {
+ return 0x6000;
+ }
+ }
+ else
+ {
+ return 0x4000;
+ }
+ }
+ else if (x < 0)
+ {
+ x = -x;
+ if (y < 0)
+ {
+ y = -y;
+ if (y > x)
+ {
+ numerator = x;
+ denominator = y;
+ bias = -0x8000;
+ positive = TRUE;
+ }
+ else if (y < x)
+ {
+ numerator = y;
+ denominator = x;
+ bias = -0x4000;
+ positive = FALSE;
+ }
+ else
+ {
+ return 0xA000;
+ }
+ }
+ else if (y > 0)
+ {
+ if (y < x)
+ {
+ numerator = y;
+ denominator = x;
+ bias = -0x4000;
+ positive = TRUE;
+ }
+ else if (y > x)
+ {
+ numerator = x;
+ denominator = y;
+ bias = 0x0;
+ positive = FALSE;
+ }
+ else
+ {
+ return 0xE000;
+ }
+ }
+ else
+ {
+ return 0xC000;
+ }
+ }
+ else
+ {
+ if (y >= 0)
+ return 0x0;
+ else
+ return 0x8000;
+ }
+ if (denominator == 0x0)
+ return 0x0;
+ if (positive)
+ return bias + FX_AtanTable_[FX_Div(numerator, denominator) >> 5];
+ else
+ return bias - FX_AtanTable_[FX_Div(numerator, denominator) >> 5];
+}
diff --git a/arm9/lib/src/FX_cp.c b/arm9/lib/src/FX_cp.c
new file mode 100644
index 00000000..76dea4b4
--- /dev/null
+++ b/arm9/lib/src/FX_cp.c
@@ -0,0 +1,70 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+
+fx32 FX_Div(fx32 numerator, fx32 denominator){
+ FX_DivAsync(numerator, denominator);
+ return FX_GetDivResult();
+}
+
+fx32 FX_Inv(fx32 x){
+ FX_InvAsync(x);
+ return FX_GetDivResult();
+}
+
+fx32 FX_Sqrt(fx32 x){
+ if (x > 0)
+ {
+ SETREG16(HW_REG_SQRTCNT, 0x1);
+ SETREG64(HW_REG_SQRT_PARAM, (fx64)x << 32);
+ return FX_GetSqrtResult();
+ }
+ else
+ {
+ return 0;
+ }
+}
+
+fx64c FX_GetDivResultFx64c(){
+ while (READREG16(HW_REG_DIVCNT) & 0x8000);
+ return READREG64(HW_REG_DIV_RESULT);
+}
+
+fx32 FX_GetDivResult(){
+ while (READREG16(HW_REG_DIVCNT) & 0x8000);
+ return (READREG64(HW_REG_DIV_RESULT) + (1 << (0x14 - 1))) >> 0x14;
+}
+
+void FX_InvAsync(fx32 x){
+ SETREG16(HW_REG_DIVCNT, 0x1);
+ SETREG64(HW_REG_DIV_NUMER, (fx64)0x00001000 << 32);
+ SETREG64(HW_REG_DIV_DENOM, (u32)x);
+}
+
+fx32 FX_GetSqrtResult(){
+ while (READREG16(HW_REG_SQRTCNT) & 0x8000);
+ return (READREG32(HW_REG_SQRT_RESULT) + (1 << (0xA - 1))) >> 0xA;
+}
+
+void FX_DivAsync(fx32 numerator, fx32 denominator){
+ SETREG16(HW_REG_DIVCNT, 0x1);
+ SETREG64(HW_REG_DIV_NUMER, (fx64)numerator << 32);
+ SETREG64(HW_REG_DIV_DENOM, (u32)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);
+ while (READREG16(HW_REG_DIVCNT) & 0x8000);
+ return READREG32(HW_REG_DIV_RESULT);
+}
+
+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);
+ while (READREG16(HW_REG_DIVCNT) & 0x8000);
+ return READREG32(HW_REG_DIVREM_RESULT);
+}
diff --git a/arm9/lib/src/FX_mtx22.c b/arm9/lib/src/FX_mtx22.c
new file mode 100644
index 00000000..19504574
--- /dev/null
+++ b/arm9/lib/src/FX_mtx22.c
@@ -0,0 +1,30 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+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){
+ mov r1, #0x0
+ mov r2, #0x1000
+ mov r3, #0x0
+ stmia r0!, {r2-r3}
+ stmia r0!, {r1-r2}
+ bx lr
+}
+
+#pragma thumb on
+asm void MTX_Rot22_(struct Mtx22 *mtx, fx32 sinphi, fx32 cosphi){
+ str r2, [r0, #0x0]
+ str r1, [r0, #0x4]
+ neg r1, r1
+ str r1, [r0, #0x8]
+ str r2, [r0, #0xc]
+ bx lr
+}
+#pragma thumb off
diff --git a/arm9/lib/src/FX_mtx33.c b/arm9/lib/src/FX_mtx33.c
new file mode 100644
index 00000000..391a6104
--- /dev/null
+++ b/arm9/lib/src/FX_mtx33.c
@@ -0,0 +1,135 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+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;
+ fx32 a0, a1, a2;
+ fx32 b0, b1, b2;
+
+ if (c == b)
+ dst = &temp;
+ else
+ dst = c;
+
+ a0 = a->_[0];
+ a1 = a->_[1];
+ a2 = a->_[2];
+ 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] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT);
+ a0 = a->_[3];
+ a1 = a->_[4];
+ a2 = a->_[5];
+ 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] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT);
+ a0 = a->_[6];
+ a1 = a->_[7];
+ a2 = a->_[8];
+ 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] = (((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){
+ fx32 x, y, z;
+ x = vec->x;
+ y = vec->y;
+ z = vec->z;
+ 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){
+ mov r2, #0x1000
+ str r2, [r0, #0x20]
+ mov r3, #0x0
+ stmia r0!, {r2-r3}
+ mov r1, #0x0
+ stmia r0!, {r1,r3}
+ stmia r0!, {r2-r3}
+ stmia r0!, {r1,r3}
+ bx lr
+}
+
+#pragma thumb on
+asm void MTX_RotX33_(struct Mtx33 *mtx, fx32 sinphi, fx32 cosphi){
+ mov r3, #0x1
+ lsl r3, r3, #0xc
+ str r3, [r0, #0x0]
+ mov r3, #0x0
+ str r3, [r0, #0x4]
+ str r3, [r0, #0x8]
+ str r3, [r0, #0xc]
+ str r2, [r0, #0x10]
+ str r1, [r0, #0x14]
+ str r3, [r0, #0x18]
+ neg r1, r1
+ str r1, [r0, #0x1c]
+ str r2, [r0, #0x20]
+ bx lr
+}
+#pragma thumb off
+
+#pragma thumb on
+asm void MTX_RotY33_(struct Mtx33 *mtx, fx32 sinphi, fx32 cosphi){
+ str r2, [r0, #0x0]
+ str r2, [r0, #0x20]
+ mov r3, #0x0
+ str r3, [r0, #0x4]
+ str r3, [r0, #0xc]
+ str r3, [r0, #0x14]
+ str r3, [r0, #0x1c]
+ neg r2, r1
+ mov r3, #0x1
+ lsl r3, r3, #0xc
+ str r1, [r0, #0x18]
+ str r2, [r0, #0x8]
+ str r3, [r0, #0x10]
+ bx lr
+}
+#pragma thumb off
+
+#pragma thumb on
+asm void MTX_RotZ33_(struct Mtx33 *mtx, fx32 sinphi, fx32 cosphi){
+ stmia r0!, {r2}
+ mov r3, #0x0
+ stmia r0!, {r1,r3}
+ neg r1, r1
+ stmia r0!, {r1-r2}
+ mov r1, #0x1
+ lsl r1, r1, #0xc
+ str r3, [r0, #0x0]
+ str r3, [r0, #0x4]
+ str r3, [r0, #0x8]
+ str r1, [r0, #0xc]
+ bx lr
+}
+#pragma thumb off
diff --git a/arm9/lib/src/FX_mtx43.c b/arm9/lib/src/FX_mtx43.c
new file mode 100644
index 00000000..862bd6f2
--- /dev/null
+++ b/arm9/lib/src/FX_mtx43.c
@@ -0,0 +1,204 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+
+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];
+ dst->_[10] = mtx->_[10];
+ dst->_[11] = mtx->_[11];
+}
+
+fx32 MTX_Inverse43(struct Mtx43 *mtx, struct Mtx43 *inv){
+ struct Mtx43 tempmat;
+ struct Mtx43 *dst;
+ fx32 det0, det1, det2, det;
+ fx32 var0, var1, var2, var3;
+ if (mtx == inv)
+ dst = &tempmat;
+ else
+ dst = inv;
+ //subdeterminants
+ 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 = ((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 = ((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);
+ return 0;
+}
+
+void MTX_Concat43(struct Mtx43 *a, struct Mtx43 *b, struct Mtx43 *c){
+ struct Mtx43 temp;
+ struct Mtx43 *dst;
+ fx32 a0, a1, a2;
+ fx32 b0, b1, b2;
+
+ if (c == b)
+ dst = &temp;
+ else
+ dst = c;
+
+ a0 = a->_[0];
+ a1 = a->_[1];
+ a2 = a->_[2];
+ 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] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT);
+ a0 = a->_[3];
+ a1 = a->_[4];
+ a2 = a->_[5];
+ 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] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT);
+ a0 = a->_[6];
+ a1 = a->_[7];
+ a2 = a->_[8];
+ 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] = (((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2) >> FX32_INT_SHIFT);
+ a0 = a->_[9];
+ a1 = a->_[10];
+ a2 = a->_[11];
+ 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){
+ fx32 x, y, z;
+ x = vec->x;
+ y = vec->y;
+ z = vec->z;
+ dst->x = ((fx64)x * mtx->_[0] + (fx64)y * mtx->_[3] + (fx64)z * mtx->_[6]) >> FX32_INT_SHIFT;
+ dst->x += mtx->_[9];
+ dst->y = ((fx64)x * mtx->_[1] + (fx64)y * mtx->_[4] + (fx64)z * mtx->_[7]) >> FX32_INT_SHIFT;
+ dst->y += mtx->_[10];
+ dst->z = ((fx64)x * mtx->_[2] + (fx64)y * mtx->_[5] + (fx64)z * mtx->_[8]) >> FX32_INT_SHIFT;
+ dst->z += mtx->_[11];
+}
+
+asm void MTX_Identity43_(struct Mtx43 *mtx){
+ mov r2, #0x1000
+ mov r3, #0x0
+ stmia r0!, {r2-r3}
+ mov r1, #0x0
+ stmia r0!, {r1,r3}
+ stmia r0!, {r2-r3}
+ stmia r0!, {r1,r3}
+ stmia r0!, {r2-r3}
+ stmia r0!, {r1,r3}
+ bx lr
+}
+
+asm void MTX_Copy43To44_(struct Mtx43 *src, struct Mtx44 *dst){
+ stmdb sp!, {r4}
+ mov r12, #0x0
+ ldmia r0!, {r2-r4}
+ stmia r1!, {r2-r4,r12}
+ ldmia r0!, {r2-r4}
+ stmia r1!, {r2-r4,r12}
+ ldmia r0!, {r2-r4}
+ stmia r1!, {r2-r4,r12}
+ mov r12, #0x1000
+ ldmia r0!, {r2-r4}
+ stmia r1!, {r2-r4,r12}
+ ldmia sp!, {r4}
+ bx lr
+}
+
+#pragma thumb on
+asm void MTX_Scale43_(struct Mtx43 *dst, fx32 x, fx32 y, fx32 z){
+ stmia r0!, {r1}
+ mov r1, #0x0
+ str r3, [r0, #0x1c]
+ mov r3, #0x0
+ stmia r0!, {r1,r3}
+ stmia r0!, {r1-r3}
+ mov r2, #0x0
+ stmia r0!, {r1,r3}
+ add r0, #0x4
+ stmia r0!, {r1-r3}
+ bx lr
+}
+#pragma thumb off
+
+#pragma thumb on
+asm void MTX_RotX43_(struct Mtx43 *mtx, fx32 sinphi, fx32 cosphi){
+ str r1, [r0, #0x14]
+ neg r1, r1
+ str r1, [r0, #0x1c]
+ mov r1, #0x1
+ lsl r1, r1, #0xc
+ stmia r0!, {r1}
+ mov r3, #0x0
+ mov r1, #0x0
+ stmia r0!, {r1,r3}
+ stmia r0!, {r1-r2}
+ str r1, [r0, #0x4]
+ add r0, #0xc
+ stmia r0!, {r2-r3}
+ stmia r0!, {r1,r3}
+ bx lr
+}
+#pragma thumb off
+
+#pragma thumb on
+asm void MTX_RotY43_(struct Mtx43 *mtx, fx32 sinphi, fx32 cosphi){
+ str r1, [r0, #0x18]
+ mov r3, #0x0
+ stmia r0!, {r2-r3}
+ neg r1, r1
+ stmia r0!, {r1,r3}
+ mov r1, #0x1
+ lsl r1, r1, #0xc
+ stmia r0!, {r1,r3}
+ add r0, #0x4
+ mov r1, #0x0
+ stmia r0!, {r1-r3}
+ stmia r0!, {r1,r3}
+ bx lr
+}
+#pragma thumb off
diff --git a/arm9/lib/src/FX_mtx44.c b/arm9/lib/src/FX_mtx44.c
new file mode 100644
index 00000000..8d158602
--- /dev/null
+++ b/arm9/lib/src/FX_mtx44.c
@@ -0,0 +1,172 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+void MI_Copy48B(void *src, void *dst);
+
+
+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] + (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;
+ fx32 a0, a1, a2, a3;
+ fx32 b0, b1, b2, b3;
+
+ if (c == b)
+ dst = &temp;
+ else
+ dst = c;
+
+ a0 = a->_[0];
+ a1 = a->_[1];
+ a2 = a->_[2];
+ a3 = a->_[3];
+ 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] = (((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] = (((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] = (((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] = (((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] = (((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] = (((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;
+}
+
+asm void MTX_Identity44_(struct Mtx44 *dst){
+ mov r2, #0x1000
+ mov r3, #0x0
+ stmia r0!, {r2-r3}
+ mov r1, #0x0
+ stmia r0!, {r1,r3}
+ stmia r0!, {r1-r3}
+ stmia r0!, {r1,r3}
+ stmia r0!, {r1-r3}
+ stmia r0!, {r1,r3}
+ stmia r0!, {r1-r2}
+ bx lr
+}
+
+asm void MTX_Copy44To43_(struct Mtx44 *src, struct Mtx43 *dst){
+ ldmia r0!, {r2-r3,r12}
+ add r0, r0, #0x4
+ stmia r1!, {r2-r3,r12}
+ ldmia r0!, {r2-r3,r12}
+ add r0, r0, #0x4
+ stmia r1!, {r2-r3,r12}
+ ldmia r0!, {r2-r3,r12}
+ add r0, r0, #0x4
+ stmia r1!, {r2-r3,r12}
+ ldmia r0!, {r2-r3,r12}
+ add r0, r0, #0x4
+ stmia r1!, {r2-r3,r12}
+ bx lr
+}
+
+#pragma thumb on
+asm void MTX_RotX44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){
+ str r2, [r0, #0x14]
+ str r2, [r0, #0x28]
+ str r1, [r0, #0x18]
+ neg r1, r1
+ str r1, [r0, #0x24]
+ mov r1, #0x1
+ mov r2, #0x0
+ lsl r1, r1, #0xc
+ mov r3, #0x0
+ stmia r0!, {r1-r3}
+ stmia r0!, {r2-r3}
+ add r0, #0x8
+ stmia r0!, {r2-r3}
+ add r0, #0x8
+ stmia r0!, {r2-r3}
+ stmia r0!, {r2-r3}
+ str r1, [r0, #0x0]
+ bx lr
+}
+#pragma thumb off
+
+#pragma thumb on
+asm void MTX_RotY44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){
+ str r2, [r0, #0x0]
+ str r2, [r0, #0x28]
+ str r1, [r0, #0x20]
+ neg r1, r1
+ str r1, [r0, #0x8]
+ mov r3, #0x1
+ mov r1, #0x0
+ lsl r3, r3, #0xc
+ mov r2, #0x0
+ str r2, [r0, #0x4]
+ add r0, #0xc
+ stmia r0!, {r1-r3}
+ stmia r0!, {r1-r2}
+ str r2, [r0, #0x4]
+ add r0, #0xc
+ stmia r0!, {r1-r2}
+ stmia r0!, {r1-r3}
+ bx lr
+}
+#pragma thumb off
+
+#pragma thumb on
+asm void MTX_RotZ44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){
+ str r2, [r0, #0x0]
+ str r2, [r0, #0x14]
+ str r1, [r0, #0x4]
+ neg r1, r1
+ str r1, [r0, #0x10]
+ mov r3, #0x1
+ mov r1, #0x0
+ lsl r3, r3, #0xc
+ mov r2, #0x0
+ add r0, #0x8
+ stmia r0!, {r1-r2}
+ add r0, #0x8
+ stmia r0!, {r1-r2}
+ stmia r0!, {r1-r3}
+ stmia r0!, {r1-r2}
+ stmia r0!, {r1-r3}
+ bx lr
+}
+#pragma thumb off
diff --git a/arm9/lib/src/FX_vec.c b/arm9/lib/src/FX_vec.c
new file mode 100644
index 00000000..10a792be
--- /dev/null
+++ b/arm9/lib/src/FX_vec.c
@@ -0,0 +1,108 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+void VEC_Add(struct Vecx32 *a, struct Vecx32 *b, struct Vecx32 *dst){
+ dst->x = a->x + b->x;
+ dst->y = a->y + b->y;
+ dst->z = a->z + b->z;
+}
+
+void VEC_Subtract(struct Vecx32 *a, struct Vecx32 *b, struct Vecx32 *dst){
+ dst->x = a->x - b->x;
+ dst->y = a->y - b->y;
+ dst->z = a->z - b->z;
+}
+
+void VEC_Fx16Add(struct Vecx16 *a, struct Vecx16 *b, struct Vecx16 *dst){
+ dst->x = a->x + b->x;
+ dst->y = a->y + b->y;
+ dst->z = a->z + b->z;
+}
+
+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;
+}
+
+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 (fx32)(((fx64)temp1 + temp2) >> FX64_INT_SHIFT);
+}
+
+void VEC_CrossProduct(struct Vecx32 *a, struct Vecx32 *b, struct Vecx32 *dst){
+ 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){
+ 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);
+ dst->x = x;
+ dst->y = y;
+ dst->z = 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 ((fx32)READREG32(HW_REG_SQRT_RESULT) + 1) >> 1;
+}
+
+void VEC_Normalize(struct Vecx32 *a, struct Vecx32 *dst){
+ 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);
+ SETREG64(HW_REG_DIV_DENOM, l2);
+ SETREG16(HW_REG_SQRTCNT, 0x1);
+ SETREG64(HW_REG_SQRT_PARAM, l2 * 4);
+ while (READREG16(HW_REG_SQRTCNT) & 0x8000); //wait for sqrt to finish
+ 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;
+ dst->x = (l2 * a->x + (1LL << (0x2D - 1))) >> 0x2D;
+ dst->y = (l2 * a->y + (1LL << (0x2D - 1))) >> 0x2D;
+ dst->z = (l2 * a->z + (1LL << (0x2D - 1))) >> 0x2D;
+}
+
+void VEC_Fx16Normalize(struct Vecx16 *a, struct Vecx16 *dst){
+ 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)
+ SETREG16(HW_REG_DIVCNT, 0x2);
+ SETREG64(HW_REG_DIV_NUMER, 0x0100000000000000);
+ SETREG64(HW_REG_DIV_DENOM, l2);
+ SETREG16(HW_REG_SQRTCNT, 0x1);
+ SETREG64(HW_REG_SQRT_PARAM, l2 * 4);
+ while (READREG16(HW_REG_SQRTCNT) & 0x8000); //wait for sqrt to finish
+ 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;
+ dst->x = (l2 * a->x + (1LL << (0x2D - 1))) >> 0x2D;
+ dst->y = (l2 * a->y + (1LL << (0x2D - 1))) >> 0x2D;
+ dst->z = (l2 * a->z + (1LL << (0x2D - 1))) >> 0x2D;
+}
+
+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;
+}
generated by cgit v1.2.3 (git 2.25.1) at 2025-05-12 07:59:36 +0000