1 files changed, 98 insertions, 45 deletions

diff --git a/arm9/source/devices/math.c b/arm9/source/devices/math.c
index bb592e0..33aeb63 100644
--- a/arm9/source/devices/math.c
+++ b/arm9/source/devices/math.c
@@ -1,66 +1,119 @@
-#include <nds.h>
-#include <math.h>
 #include "math.h"
-#include "../bang.h"
 
-#define CLEAR \
-    math->sqrt_rc = FALSE;\
-    math->atan_rc = FALSE;\
-    math->prod_rc = FALSE;\
-    math->quot_rc = FALSE;\
-    math->rem_rc  = FALSE;
 
-void set_op1_high(MathDevice *math, u8 high) { SET_HIGH(math->op1, high); CLEAR; }
-void set_op1_low( MathDevice *math, u8  low) {  SET_LOW(math->op1,  low); CLEAR; }
-void set_op2_high(MathDevice *math, u8 high) { SET_HIGH(math->op2, high); CLEAR; }
-void set_op2_low( MathDevice *math, u8  low) {  SET_LOW(math->op2,  low); CLEAR; }
+// Angle conversion constants.
+const double fromRadians = 10430.378350470453;     // 65536 / 2π
+const double toRadians   = 0.00009587379924285257; // 2π / 65536
 
-u16 get_sqrt(MathDevice *math) {
-    if (!math->sqrt_rc) {
-        u32 input = math->op1 << 16 | math->op2;
-        math->sqrt = sqrt32(input);
-        math->sqrt_rc = TRUE;
+
+// Reset the math device.
+void math_reset(MathDevice *math) {
+    math->x = 0;
+    math->y = 0;
+    math->r = 0;
+    math->t = 0;
+    math_clear_cartesian(math);
+    math_clear_polar(math);
+}
+
+// Clear all values calculated from cartesian coordinates.
+void math_clear_cartesian(MathDevice *math) {
+    math->r_cached    = false;
+    math->t_cached    = false;
+    math->prod_cached = false;
+    math->quot_cached = false;
+    math->rem_cached  = false;
+}
+
+// Clear all values calculated from polar coordinates.
+void math_clear_polar(MathDevice *math) {
+    math->x_cached    = false;
+    math->y_cached    = false;
+}
+
+// Calculate the x coordinate of the polar point.
+s16 math_get_x(MathDevice *math) {
+    if (!math->x_cached) {
+        double x = cos(math->t * toRadians) * math->r;
+        if (-32768.0 <= x && x <= 32768.0) {
+            math->x_read = x;
+        } else {
+            math->x_read = 0;
+        }
+        math->x_cached = true;
+    }
+    return math->x_read;
+}
+
+// Calculate the y coordinate of the polar point.
+s16 math_get_y(MathDevice *math) {
+    if (!math->y_cached) {
+        double y = sin(math->t * toRadians) * math->r;
+        if (-32768.0 <= y && y <= 32767.0) {
+            math->y_read = y;
+        } else {
+            math->y_read = 0;
+        }
+        math->y_cached = true;
+    }
+    return math->y_read;
+}
+
+// Calculate the r coordinate of the cartesian point.
+u16 math_get_r(MathDevice *math) {
+    if (!math->r_cached) {
+        u32 x2 = math->x * math->x;
+        u32 y2 = math->y * math->y;
+        math->r_read = sqrt32(x2 + y2);
+        math->r_cached = true;
     }
-    return math->sqrt;
+    return math->r_read;
 }
 
-u16 get_atan(MathDevice *math) {
-    if (!math->atan_rc) {
-        if (math->op1 || math->op2) {
-            double op1 = (s16) math->op1;
-            double op2 = (s16) math->op2;
-            const double scale = 10430.378350470453; // PI * 32768
-            math->atan = (s32) (atan2(op1, op2) * scale);
+// Calculate the t coordinate of the cartesian point.
+u16 math_get_t(MathDevice *math) {
+    if (!math->t_cached) {
+        if (math->x || math->y) {
+            math->t_read = atan2(math->y, math->x) * fromRadians;
+        } else {
+            math->t_read = 0;
         }
-        math->atan_rc = TRUE;
+        math->t_cached = true;
     }
-    return math->atan;
+    return math->t_read;
 }
 
-u32 get_prod(MathDevice *math) {
-    if (!math->prod_rc) {
-        math->prod = math->op1 * math->op2;
-        math->prod_rc = TRUE;
+// Calculate the product of x by y.
+u32 math_get_prod(MathDevice *math) {
+    if (!math->prod_cached) {
+        math->prod_read = (u16)math->x * (u16)math->y;
+        math->prod_cached = true;
     }
-    return math->prod;
+    return math->prod_read;
 }
 
-u16 get_quot(MathDevice *math) {
-    if (!math->quot_rc) {
-        if (math->op2) {
-            math->quot = div32(math->op1, math->op2);
+// Calculate the quotient of x by y.
+u16 math_get_quot(MathDevice *math) {
+    if (!math->quot_cached) {
+        if (math->y) {
+            math->quot_read = div32((u16)math->x, (u16)math->y);
+        } else {
+            math->quot_read = 0;
         }
-        math->quot_rc = TRUE;
+        math->quot_cached = true;
     }
-    return math->quot;
+    return math->quot_read;
 }
 
-u16 get_rem(MathDevice *math) {
-    if (!math->rem_rc) {
-        if (math->op2) {
-            math->rem = mod32(math->op1, math->op2);
+// Calculate the remainder of x by y.
+u16 math_get_rem(MathDevice *math) {
+    if (!math->rem_cached) {
+        if (math->y) {
+            math->rem_read = mod32((u16)math->x, (u16)math->y);
+        } else {
+            math->rem_read = 0;
         }
-        math->rem_rc = TRUE;
+        math->rem_cached = true;
     }
-    return math->rem;
+    return math->rem_read;
 }