Decompile FX_mtx33.s

4 files changed, 147 insertions, 263 deletions

diff --git a/arm9/asm/FX_mtx33.s b/arm9/asm/FX_mtx33.s
deleted file mode 100644
index 2b31fd6c..00000000
--- a/arm9/asm/FX_mtx33.s
+++ /dev/null
@@ -1,259 +0,0 @@
-	.include "asm/macros.inc"
-	.include "global.inc"
-
-	.text
-
-	arm_func_start MTX_Identity33_
-MTX_Identity33_: ; 0x020C3F14
-	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
-
-	thumb_func_start MTX_RotX33_
-MTX_RotX33_: ; 0x020C3F38
-	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
-
-	thumb_func_start MTX_RotY33_
-MTX_RotY33_: ; 0x020C3F54
-	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
-
-	thumb_func_start MTX_RotZ33_
-MTX_RotZ33_: ; 0x020C3F70
-	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
-
-	arm_func_start MTX_MultVec33
-MTX_MultVec33: ; 0x020C3F88
-	stmdb sp!, {r4-r6,lr}
-	ldr r12, [r0, #0x4]
-	ldr r4, [r1, #0xc]
-	ldr r3, [r0, #0x0]
-	smull r6, r5, r12, r4
-	ldr r4, [r1, #0x0]
-	ldr r0, [r0, #0x8]
-	smlal r6, r5, r3, r4
-	ldr r4, [r1, #0x18]
-	smlal r6, r5, r0, r4
-	mov r4, r6, lsr #0xc
-	orr r4, r4, r5, lsl #0x14
-	str r4, [r2, #0x0]
-	ldr r4, [r1, #0x10]
-	ldr r5, [r1, #0x4]
-	smull r6, lr, r12, r4
-	smlal r6, lr, r3, r5
-	ldr r4, [r1, #0x1c]
-	smlal r6, lr, r0, r4
-	mov r4, r6, lsr #0xc
-	orr r4, r4, lr, lsl #0x14
-	str r4, [r2, #0x4]
-	ldr lr, [r1, #0x14]
-	ldr r4, [r1, #0x8]
-	smull r5, lr, r12, lr
-	smlal r5, lr, r3, r4
-	ldr r1, [r1, #0x20]
-	smlal r5, lr, r0, r1
-	mov r0, r5, lsr #0xc
-	orr r0, r0, lr, lsl #0x14
-	str r0, [r2, #0x8]
-	ldmia sp!, {r4-r6,lr}
-	bx lr
-
-	arm_func_start MTX_Concat33
-MTX_Concat33: ; 0x020C400C
-	stmdb sp!, {r4-r9,lr}
-	sub sp, sp, #0x24
-	ldr r5, [r0, #0x4]
-	ldr r3, [r1, #0xc]
-	mov r12, r2
-	smull r8, r7, r5, r3
-	ldr r6, [r0, #0x0]
-	ldr r3, [r1, #0x0]
-	cmp r12, r1
-	smlal r8, r7, r6, r3
-	ldr r4, [r0, #0x8]
-	ldr r3, [r1, #0x18]
-	addeq r2, sp, #0x0
-	smlal r8, r7, r4, r3
-	mov r3, r8, lsr #0xc
-	orr r3, r3, r7, lsl #0x14
-	str r3, [r2, #0x0]
-	ldr r3, [r1, #0x10]
-	ldr r7, [r1, #0x4]
-	smull r9, r8, r5, r3
-	smlal r9, r8, r6, r7
-	ldr r3, [r1, #0x1c]
-	add lr, sp, #0x0
-	smlal r9, r8, r4, r3
-	mov r3, r9, lsr #0xc
-	orr r3, r3, r8, lsl #0x14
-	str r3, [r2, #0x4]
-	ldr r3, [r1, #0x14]
-	ldr r7, [r1, #0x8]
-	smull r9, r8, r5, r3
-	smlal r9, r8, r6, r7
-	ldr r6, [r1, #0x20]
-	cmp r2, lr
-	smlal r9, r8, r4, r6
-	mov r4, r9, lsr #0xc
-	orr r4, r4, r8, lsl #0x14
-	str r4, [r2, #0x8]
-	ldr r4, [r0, #0x10]
-	ldr r5, [r0, #0xc]
-	smull r9, r8, r4, r3
-	smlal r9, r8, r5, r7
-	ldr r3, [r0, #0x14]
-	addne sp, sp, #0x24
-	smlal r9, r8, r3, r6
-	mov r6, r9, lsr #0xc
-	orr r6, r6, r8, lsl #0x14
-	str r6, [r2, #0x14]
-	ldr r6, [r1, #0x10]
-	ldr r7, [r1, #0x4]
-	smull r9, r8, r4, r6
-	smlal r9, r8, r5, r7
-	ldr r6, [r1, #0x1c]
-	smlal r9, r8, r3, r6
-	mov r6, r9, lsr #0xc
-	orr r6, r6, r8, lsl #0x14
-	str r6, [r2, #0x10]
-	ldr r7, [r1, #0xc]
-	ldr r6, [r1, #0x0]
-	smull r9, r8, r4, r7
-	smlal r9, r8, r5, r6
-	ldr r5, [r1, #0x18]
-	smlal r9, r8, r3, r5
-	mov r3, r9, lsr #0xc
-	orr r3, r3, r8, lsl #0x14
-	str r3, [r2, #0xc]
-	ldr r4, [r0, #0x1c]
-	ldr r3, [r0, #0x18]
-	smull r8, r7, r4, r7
-	smlal r8, r7, r3, r6
-	ldr r0, [r0, #0x20]
-	smlal r8, r7, r0, r5
-	mov r5, r8, lsr #0xc
-	orr r5, r5, r7, lsl #0x14
-	str r5, [r2, #0x18]
-	ldr r5, [r1, #0x10]
-	ldr r6, [r1, #0x4]
-	smull r8, r5, r4, r5
-	smlal r8, r5, r3, r6
-	ldr r7, [r1, #0x1c]
-	smlal r8, r5, r0, r7
-	mov r6, r8, lsr #0xc
-	orr r6, r6, r5, lsl #0x14
-	str r6, [r2, #0x1c]
-	ldr r6, [r1, #0x20]
-	ldr r5, [r1, #0x8]
-	ldr r1, [r1, #0x14]
-	smull r7, r1, r4, r1
-	smlal r7, r1, r3, r5
-	smlal r7, r1, r0, r6
-	mov r0, r7, lsr #0xc
-	orr r0, r0, r1, lsl #0x14
-	str r0, [r2, #0x20]
-	ldmneia sp!, {r4-r9,lr}
-	bxne lr
-	ldmia lr!, {r0-r3}
-	stmia r12!, {r0-r3}
-	ldmia lr!, {r0-r3}
-	stmia r12!, {r0-r3}
-	ldr r0, [lr, #0x0]
-	str r0, [r12, #0x0]
-	add sp, sp, #0x24
-	ldmia sp!, {r4-r9,lr}
-	bx lr
-
-	arm_func_start MTX_ScaleApply33
-MTX_ScaleApply33: ; 0x020C41A8
-	stmdb sp!, {r4,lr}
-	ldr r4, [r0, #0x0]
-	ldr r12, [sp, #0x8]
-	smull lr, r4, r2, r4
-	mov lr, lr, lsr #0xc
-	orr lr, lr, r4, lsl #0x14
-	str lr, [r1, #0x0]
-	ldr r4, [r0, #0x4]
-	smull lr, r4, r2, r4
-	mov lr, lr, lsr #0xc
-	orr lr, lr, r4, lsl #0x14
-	str lr, [r1, #0x4]
-	ldr lr, [r0, #0x8]
-	smull r4, lr, r2, lr
-	mov r2, r4, lsr #0xc
-	orr r2, r2, lr, lsl #0x14
-	str r2, [r1, #0x8]
-	ldr r2, [r0, #0xc]
-	smull lr, r2, r3, r2
-	mov lr, lr, lsr #0xc
-	orr lr, lr, r2, lsl #0x14
-	str lr, [r1, #0xc]
-	ldr r2, [r0, #0x10]
-	smull lr, r2, r3, r2
-	mov lr, lr, lsr #0xc
-	orr lr, lr, r2, lsl #0x14
-	str lr, [r1, #0x10]
-	ldr r2, [r0, #0x14]
-	smull lr, r2, r3, r2
-	mov r3, lr, lsr #0xc
-	orr r3, r3, r2, lsl #0x14
-	str r3, [r1, #0x14]
-	ldr r2, [r0, #0x18]
-	smull r3, r2, r12, r2
-	mov r3, r3, lsr #0xc
-	orr r3, r3, r2, lsl #0x14
-	str r3, [r1, #0x18]
-	ldr r2, [r0, #0x1c]
-	smull r3, r2, r12, r2
-	mov r3, r3, lsr #0xc
-	orr r3, r3, r2, lsl #0x14
-	str r3, [r1, #0x1c]
-	ldr r0, [r0, #0x20]
-	smull r2, r0, r12, r0
-	mov r2, r2, lsr #0xc
-	orr r2, r2, r0, lsl #0x14
-	str r2, [r1, #0x20]
-	ldmia sp!, {r4,lr}
-	bx lr
diff --git a/arm9/lib/src/FX_mtx33.c b/arm9/lib/src/FX_mtx33.c
new file mode 100644
index 00000000..32b1ad2e
--- /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, 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_Concat33(struct Mtx33 *a, struct Mtx33 *b, struct Mtx33 *c){
+    struct Mtx33 temp;
+    struct Mtx33 *dst;
+    s32 a0, a1, a2;
+    s32 b0, b1, b2;
+
+    if (c == b)
+        dst = &temp;
+    else
+        dst = 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);
+    b0 = b->_[2];
+    b1 = b->_[5];
+    b2 = b->_[8];
+    dst->_[2] =  (((s64)a0  * b0 + (s64)a1  * b1 + (s64)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);
+    b0 = b->_[0];
+    b1 = b->_[3];
+    b2 = b->_[6];
+    dst->_[3] =  (((s64)a0  * b0 + (s64)a1  * b1 + (s64)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);
+    b0 = b->_[2];
+    b1 = b->_[5];
+    b2 = b->_[8];
+    dst->_[8] = (((s64)a0  * b0 + (s64)a1  * b1 + (s64)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;
+    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;
+}
+
+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, s32 sinphi, s32 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, s32 sinphi, s32 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, s32 sinphi, s32 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
index c5d227b8..9cb8aa6e 100644
--- a/arm9/lib/src/FX_mtx43.c
+++ b/arm9/lib/src/FX_mtx43.c
@@ -3,9 +3,6 @@
 #include "fx.h"
 
 
-
-
-
 void MTX_ScaleApply43(struct Mtx43 *mtx, struct Mtx43 *dst, s32 x, s32 y, s32 z){
     //this works because matrices are indexed columns first
     MTX_ScaleApply33((struct Mtx33 *)mtx, (struct Mtx33 *)dst, x, y, z);
diff --git a/include/fx.h b/include/fx.h
index ee436883..319c823d 100644
--- a/include/fx.h
+++ b/include/fx.h
@@ -59,6 +59,9 @@
 #define READREG32(x)               (*(vu32 *)x)
 #define READREG64(x)               (*(vu64 *)x)
 
+//TODO: add muladd aund mulsub macros
+//need the extra term on mulsub to round up?
+
 struct Vecx32
 {
     s32 x;
@@ -129,7 +132,6 @@ s32 FX_ModS32(s32 num, s32 mod);
 
 //Mtx
 //The functions ending in underscores seem to be in assembly originally
-//at least I see no way to recreate the stmia copies and some of them are actually in thumb
 //Mtx44
 void MTX_TransApply44(struct Mtx44 *mtx, struct Mtx44 *dst, s32 x, s32 y, s32 z);
 void MTX_Concat44(struct Mtx44 *a, struct Mtx44 *b, struct Mtx44 *c);
@@ -150,5 +152,14 @@ void MTX_Scale43_(struct Mtx43 *dst, s32 x, s32 y, s32 z);
 void MTX_RotX43_(struct Mtx43 *mtx, s32 sinphi, s32 cosphi);
 void MTX_RotY43_(struct Mtx43 *mtx, s32 sinphi, s32 cosphi);
 
+//Mtx33
+void MTX_ScaleApply33(struct Mtx33 *mtx, struct Mtx33 *dst, s32 x, s32 y, s32 z);
+void MTX_Concat33(struct Mtx33 *a, struct Mtx33 *b, struct Mtx33 *c);
+void MTX_MultVec33(struct Vecx32 *vec, struct Mtx33 *mtx, struct Vecx32 *dst);
+void MTX_Identity33_(struct Mtx33 *mtx);
+void MTX_RotX33_(struct Mtx33 *mtx, s32 sinphi, s32 cosphi);
+void MTX_RotY33_(struct Mtx33 *mtx, s32 sinphi, s32 cosphi);
+void MTX_RotZ33_(struct Mtx33 *mtx, s32 sinphi, s32 cosphi);
+
 
 #endif //GUARD_FX_H