decompile FX_mtx44 and update fx header

2 files changed, 187 insertions, 19 deletions

diff --git a/arm9/lib/src/FX_cp.c b/arm9/lib/src/FX_cp.c
index a8da0a8f..a2d8307b 100644
--- a/arm9/lib/src/FX_cp.c
+++ b/arm9/lib/src/FX_cp.c
@@ -1,11 +1,7 @@
 #include "global.h"
 #include "main.h"
 #include "fx.h"
-
-s32 FX_GetDivResult();
-s32 FX_GetSqrtResult();
-void FX_DivAsync(s32 numerator, s32 denominator);
-void FX_InvAsync(s32 x);
+ 
 
 s32 FX_Div(s32 numerator, s32 denominator){
     FX_DivAsync(numerator, denominator);
@@ -57,18 +53,18 @@ void FX_DivAsync(s32 numerator, s32 denominator){
     SETREG64(HW_REG_DIV_DENOM, (u32)denominator);
 }
 
- s32 FX_DivS32(s32 numerator, s32 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);
- }
+s32 FX_DivS32(s32 numerator, s32 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);
+}
 
- s32 FX_ModS32(s32 num, s32 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);
- }
+s32 FX_ModS32(s32 num, s32 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_mtx44.c b/arm9/lib/src/FX_mtx44.c
new file mode 100644
index 00000000..3c91b4df
--- /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, s32 x, s32 y, s32 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);
+}
+
+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;
+
+    if (c == b)
+        dst = &temp;
+    else
+        dst = c;
+
+    a0 = a->_[0];
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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, s32 sinphi, s32 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, s32 sinphi, s32 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, s32 sinphi, s32 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