decompile FX.s, FX_atan.s and partially decompile FX_ves.s

6 files changed, 348 insertions, 90 deletions

diff --git a/arm9/arm9.lcf b/arm9/arm9.lcf
index 42b0a8ee..046ce41d 100644
--- a/arm9/arm9.lcf
+++ b/arm9/arm9.lcf
@@ -127,6 +127,7 @@ SECTIONS {
 		FX_mtx44.o (.text)
 		FX_cp.o (.text)
 		FX_vec.o (.text)
+        FX_vec_c.o (.text)
 		FX_atan.o (.text)
 		FX.o (.text)
 		GX.o (.text)
diff --git a/arm9/asm/FX_vec.s b/arm9/asm/FX_vec.s
index e6270162..08068eaf 100644
--- a/arm9/asm/FX_vec.s
+++ b/arm9/asm/FX_vec.s
@@ -310,93 +310,94 @@ VEC_CrossProduct:
 	ldmia sp!, {r4-r11,lr}
 	bx lr
 
-	arm_func_start VEC_Fx16DotProduct
-VEC_Fx16DotProduct: ; 0x020C5538
-	stmdb sp!, {r4-r5,lr}
-	sub sp, sp, #0x4
-	ldrsh lr, [r0, #0x2]
-	ldrsh r12, [r1, #0x2]
-	ldrsh r3, [r0, #0x4]
-	ldrsh r2, [r1, #0x4]
-	ldrsh r5, [r0, #0x0]
-	ldrsh r4, [r1, #0x0]
-	smulbb r1, lr, r12
-	smulbb r0, r3, r2
-	smlabb r1, r5, r4, r1
-	add r0, r0, #0x800
-	adds r2, r1, r0
-	mov r1, r1, asr #0x1f
-	adc r1, r1, r0, asr #0x1f
-	mov r0, r2, lsr #0xc
-	orr r0, r0, r1, lsl #0x14
-	add sp, sp, #0x4
-	ldmia sp!, {r4-r5,lr}
-	bx lr
-
-	arm_func_start VEC_DotProduct
-VEC_DotProduct:
-	stmdb sp!, {r4,lr}
-	ldr r3, [r0, #0x4]
-	ldr r2, [r1, #0x4]
-	ldr r12, [r0, #0x0]
-	smull r4, lr, r3, r2
-	ldr r2, [r1, #0x0]
-	ldr r3, [r0, #0x8]
-	smlal r4, lr, r12, r2
-	ldr r1, [r1, #0x8]
-	mov r0, #0x800
-	smlal r4, lr, r3, r1
-	adds r0, r4, r0
-	adc r1, lr, #0x0
-	mov r0, r0, lsr #0xc
-	orr r0, r0, r1, lsl #0x14
-	ldmia sp!, {r4,lr}
-	bx lr
-
-	arm_func_start VEC_Fx16Add
-VEC_Fx16Add: ; 0x020C55CC
-	ldrsh r12, [r0, #0x0]
-	ldrsh r3, [r1, #0x0]
-	add r3, r12, r3
-	strh r3, [r2, #0x0]
-	ldrsh r12, [r0, #0x2]
-	ldrsh r3, [r1, #0x2]
-	add r3, r12, r3
-	strh r3, [r2, #0x2]
-	ldrsh r3, [r0, #0x4]
-	ldrsh r0, [r1, #0x4]
-	add r0, r3, r0
-	strh r0, [r2, #0x4]
-	bx lr
-
-	arm_func_start VEC_Subtract
-VEC_Subtract: ; 0x020C5600
-	ldr r12, [r0, #0x0]
-	ldr r3, [r1, #0x0]
-	sub r3, r12, r3
-	str r3, [r2, #0x0]
-	ldr r12, [r0, #0x4]
-	ldr r3, [r1, #0x4]
-	sub r3, r12, r3
-	str r3, [r2, #0x4]
-	ldr r3, [r0, #0x8]
-	ldr r0, [r1, #0x8]
-	sub r0, r3, r0
-	str r0, [r2, #0x8]
-	bx lr
-
-	arm_func_start VEC_Add
-VEC_Add: ; 0x020C5634
-	ldr r12, [r0, #0x0]
-	ldr r3, [r1, #0x0]
-	add r3, r12, r3
-	str r3, [r2, #0x0]
-	ldr r12, [r0, #0x4]
-	ldr r3, [r1, #0x4]
-	add r3, r12, r3
-	str r3, [r2, #0x4]
-	ldr r3, [r0, #0x8]
-	ldr r0, [r1, #0x8]
-	add r0, r3, r0
-	str r0, [r2, #0x8]
-	bx lr
+;	arm_func_start VEC_Fx16DotProduct
+;VEC_Fx16DotProduct: ; 0x020C5538
+;	stmdb sp!, {r4-r5,lr}
+;	sub sp, sp, #0x4
+;	ldrsh lr, [r0, #0x2]
+;	ldrsh r12, [r1, #0x2]
+;	ldrsh r3, [r0, #0x4]
+;	ldrsh r2, [r1, #0x4]
+;	ldrsh r5, [r0, #0x0]
+;	ldrsh r4, [r1, #0x0]
+;	smulbb r1, lr, r12
+;	smulbb r0, r3, r2
+;	smlabb r1, r5, r4, r1
+;	add r0, r0, #0x800
+;	adds r2, r1, r0
+;	mov r1, r1, asr #0x1f
+;	adc r1, r1, r0, asr #0x1f
+;	mov r0, r2, lsr #0xc
+;	orr r0, r0, r1, lsl #0x14
+;	add sp, sp, #0x4
+;	ldmia sp!, {r4-r5,lr}
+;	bx lr
+;
+;	arm_func_start VEC_DotProduct
+;VEC_DotProduct:
+;	stmdb sp!, {r4,lr}
+;	ldr r3, [r0, #0x4]
+;	ldr r2, [r1, #0x4]
+;	ldr r12, [r0, #0x0]
+;	smull r4, lr, r3, r2
+;	ldr r2, [r1, #0x0]
+;	ldr r3, [r0, #0x8]
+;	smlal r4, lr, r12, r2
+;	ldr r1, [r1, #0x8]
+;	mov r0, #0x800
+;	smlal r4, lr, r3, r1
+;	adds r0, r4, r0
+;	adc r1, lr, #0x0
+;	mov r0, r0, lsr #0xc
+;	orr r0, r0, r1, lsl #0x14
+;	ldmia sp!, {r4,lr}
+;	bx lr
+;
+;	arm_func_start VEC_Fx16Add
+;VEC_Fx16Add: ; 0x020C55CC
+;	ldrsh r12, [r0, #0x0]
+;	ldrsh r3, [r1, #0x0]
+;	add r3, r12, r3
+;	strh r3, [r2, #0x0]
+;	ldrsh r12, [r0, #0x2]
+;	ldrsh r3, [r1, #0x2]
+;	add r3, r12, r3
+;	strh r3, [r2, #0x2]
+;	ldrsh r3, [r0, #0x4]
+;	ldrsh r0, [r1, #0x4]
+;	add r0, r3, r0
+;	strh r0, [r2, #0x4]
+;	bx lr
+;
+;	arm_func_start VEC_Subtract
+;VEC_Subtract: ; 0x020C5600
+;	ldr r12, [r0, #0x0]
+;	ldr r3, [r1, #0x0]
+;	sub r3, r12, r3
+;	str r3, [r2, #0x0]
+;	ldr r12, [r0, #0x4]
+;	ldr r3, [r1, #0x4]
+;	sub r3, r12, r3
+;	str r3, [r2, #0x4]
+;	ldr r3, [r0, #0x8]
+;	ldr r0, [r1, #0x8]
+;	sub r0, r3, r0
+;	str r0, [r2, #0x8]
+;	bx lr
+;
+;	arm_func_start VEC_Add
+;VEC_Add: ; 0x020C5634
+;	ldr r12, [r0, #0x0]
+;	ldr r3, [r1, #0x0]
+;	add r3, r12, r3
+;	str r3, [r2, #0x0]
+;	ldr r12, [r0, #0x4]
+;	ldr r3, [r1, #0x4]
+;	add r3, r12, r3
+;	str r3, [r2, #0x4]
+;	ldr r3, [r0, #0x8]
+;	ldr r0, [r1, #0x8]
+;	add r0, r3, r0
+;	str r0, [r2, #0x8]
+;	bx lr
+;
diff --git a/arm9/lib/src/FX.c b/arm9/lib/src/FX.c
new file mode 100644
index 00000000..fa6fa6ba
--- /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;
+}
+
+s32 FX_Modf(s32 x, s32 *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..684db81e
--- /dev/null
+++ b/arm9/lib/src/FX_atan.c
@@ -0,0 +1,161 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+extern s16 FX_AtanTable_[];
+
+u16 FX_Atan(s32 x){
+    if (x >= 0)
+    {
+        if (x > 0x1000)
+        {
+            x = FX_Inv(x);
+            s16 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);
+            s16 y = FX_AtanTable_[x >> 5];
+            return y - 0x4000;
+        }
+        else if (x > -0x1000)
+        {
+            return -FX_AtanTable_[-x >> 5];
+        }
+        else
+        {
+            return -0x2000;
+        }
+    }
+}
+
+u16 FX_Atan2(s32 x, s32 y){
+    s32 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_vec_c.c b/arm9/lib/src/FX_vec_c.c
new file mode 100644
index 00000000..a2e0ffe4
--- /dev/null
+++ b/arm9/lib/src/FX_vec_c.c
@@ -0,0 +1,34 @@
+#include "global.h"
+#include "main.h"
+#include "fx.h"
+
+void VEC_Add(struct Vecx32 *x, struct Vecx32 *y, struct Vecx32 *dst){
+    dst->x = x->x + y->x;
+    dst->y = x->y + y->y;
+    dst->z = x->z + y->z;
+}
+
+void VEC_Subtract(struct Vecx32 *x, struct Vecx32 *y, struct Vecx32 *dst){
+    dst->x = x->x - y->x;
+    dst->y = x->y - y->y;
+    dst->z = x->z - y->z;
+}
+
+void VEC_Fx16Add(struct Vecx16 *x, struct Vecx16 *y, struct Vecx16 *dst){
+    dst->x = x->x + y->x;
+    dst->y = x->y + y->y;
+    dst->z = x->z + y->z;
+}
+
+s32 VEC_DotProduct(struct Vecx32 *x, struct Vecx32 *y){
+    return ((s64)x->x * y->x + (s64)x->y * y->y + (s64)x->z * y->z + (1 << (FX64_INT_SHIFT - 1))) >> FX64_INT_SHIFT;
+}
+
+s32 VEC_Fx16DotProduct(struct Vecx16 *x, struct Vecx16 *y){
+    s32 temp1, temp2;
+    temp1 = (x->x * y->x) + (x->y * y->y);
+    temp2 = (x->z * y->z) + (1 << (FX64_INT_SHIFT - 1));
+    return (s32)(((s64)temp1 + temp2) >> FX64_INT_SHIFT);
+}
+
+void VEC_CrossProduct(struct Vecx32 *x, struct Vecx32 *y, struct Vecx32 *);
diff --git a/include/fx.h b/include/fx.h
new file mode 100644
index 00000000..82938219
--- /dev/null
+++ b/include/fx.h
@@ -0,0 +1,41 @@
+#ifndef GUARD_FX_H
+#define GUARD_FX_H
+
+#define FX32_INT_MASK     0xFFFFF000
+#define FX32_INT_ABS_MASK 0x7FFFF000
+#define FX32_FRAC_MASK    0x00000FFF
+
+#define FX32_INT_SHIFT    0xC
+#define FX64_INT_SHIFT    0xC
+
+#define FX32_INT(x)       (((x) & FX32_INT_MASK) >> FX32_INT_SHIFT)
+#define FX32_INT_ABS(x)   (((x) & FX32_INT_ABS_MASK) >> FX32_INT_SHIFT)
+#define FX32_FRAC(x)      ((x) & FX32_FRAC_MASK)
+
+struct Vecx32
+{
+    s32 x;
+    s32 y;
+    s32 z;
+};
+
+struct Vecx16
+{
+    s16 x;
+    s16 y;
+    s16 z;
+};
+
+void FX_Init();
+s32 FX_Modf(s32 x, s32 *iptr);
+s32 FX_Inv(s32 x);
+u16 FX_Atan(s32 x);
+u16 FX_Atan2(s32 x, s32 y);
+
+//Vec
+void VEC_Add(struct Vecx32 *x, struct Vecx32 *y, struct Vecx32 *dst);
+void VEC_Subtract(struct Vecx32 *x, struct Vecx32 *y, struct Vecx32 *dst);
+void VEC_Fx16Add(struct Vecx16 *x, struct Vecx16 *y, struct Vecx16 *dst);
+s32 VEC_DotProduct(struct Vecx32 *x, struct Vecx32 *y);
+
+#endif //GUARD_FX_H