From 87914c6cc6c38460a07452cc23d666975f786450 Mon Sep 17 00:00:00 2001
From: cadmic <cadmic24@gmail.com>
Date: Tue, 3 Sep 2024 22:12:44 -0700
Subject: [PATCH] Match fp_math.c (#2116)

---
 include/libc/math.h                 |  9 ++++
 src/code/fp.s                       |  2 +
 src/code/fp_math.c                  | 83 ++++++++++++++++++++++++++---
 tools/disasm/ntsc-1.2/functions.txt |  4 +-
 4 files changed, 90 insertions(+), 8 deletions(-)

diff --git a/include/libc/math.h b/include/libc/math.h
index 5e5ecc4ceb..b893cce72e 100644
--- a/include/libc/math.h
+++ b/include/libc/math.h
@@ -41,6 +41,13 @@ f32 fabsf(f32 f);
 #define fabsf(f) __builtin_fabsf((f32)(f))
 #endif
 
+f64 fabs(f64 f);
+#ifdef __sgi
+#pragma intrinsic(fabs)
+#else
+#define fabs(f) __builtin_fabs((f64)(f))
+#endif
+
 f32 sqrtf(f32 f);
 // IDO has a sqrtf intrinsic, but in N64 versions it's not used for some files.
 // For these files we define NO_SQRTF_INTRINSIC to use the sqrtf function instead.
@@ -53,8 +60,10 @@ f64 sqrt(f64 f);
 #pragma intrinsic(sqrt)
 #endif
 
+#if PLATFORM_GC
 extern float qNaN0x3FFFFF;
 extern float qNaN0x10000;
 extern float sNaN0x3FFFFF;
+#endif
 
 #endif
diff --git a/src/code/fp.s b/src/code/fp.s
index 431348d897..f7a00e2136 100644
--- a/src/code/fp.s
+++ b/src/code/fp.s
@@ -6,6 +6,7 @@
 
 .balign 16
 
+#if PLATFORM_GC
 DATA(qNaN0x3FFFFF)
     .word 0x7FBFFFFF
 ENDDATA(qNaN0x3FFFFF)
@@ -17,6 +18,7 @@ ENDDATA(qNaN0x10000)
 DATA(sNaN0x3FFFFF)
     .word 0x7FFFFFFF
 ENDDATA(sNaN0x3FFFFF)
+#endif
 
 .section .text
 
diff --git a/src/code/fp_math.c b/src/code/fp_math.c
index c429b83e23..fb73b58ef6 100644
--- a/src/code/fp_math.c
+++ b/src/code/fp_math.c
@@ -1,7 +1,9 @@
 #include "z64math.h"
 #include "macros.h"
 
+#if PLATFORM_GC
 s32 gUseAtanContFrac;
+#endif
 
 /**
  * @param angle radians
@@ -22,6 +24,16 @@ f32 Math_FCeilF(f32 x) {
     return ceilf(x);
 }
 
+#if PLATFORM_N64
+f64 Math_FAbs(f64 x) {
+    return x < 0.0 ? -x : x;
+}
+
+f32 Math_FAbsF(f32 x) {
+    return x < 0.0f ? -x : x;
+}
+#endif
+
 f32 Math_FRoundF(f32 x) {
     return roundf(x);
 }
@@ -34,6 +46,7 @@ f32 Math_FNearbyIntF(f32 x) {
     return nearbyintf(x);
 }
 
+#if PLATFORM_GC
 /* Arctangent approximation using a Taylor series (one quadrant) */
 f32 Math_FAtanTaylorQF(f32 x) {
     static const f32 coeffs[] = {
@@ -89,6 +102,7 @@ f32 Math_FAtanTaylorF(f32 x) {
         return -q;
     }
 }
+#endif
 
 /* Arctangent approximation using a continued fraction */
 f32 Math_FAtanContFracF(f32 x) {
@@ -98,6 +112,17 @@ f32 Math_FAtanContFracF(f32 x) {
     f32 sq;
     s32 i;
 
+#if PLATFORM_N64
+    if (x > 1.0f) {
+        sector = 1;
+        x = 1.0f / x;
+    } else if (x < -1.0f) {
+        sector = -1;
+        x = 1.0f / x;
+    } else {
+        sector = 0;
+    }
+#else
     if (x >= -1.0f && x <= 1.0f) {
         sector = 0;
     } else if (x > 1.0f) {
@@ -109,16 +134,35 @@ f32 Math_FAtanContFracF(f32 x) {
     } else {
         return qNaN0x10000;
     }
+#endif
 
     sq = SQ(x);
     conv = 0.0f;
+
+#if PLATFORM_N64
+    z = 24.0f;
+    i = 24;
+#else
     z = 8.0f;
-    for (i = 8; i != 0; i--) {
+    i = 8;
+#endif
+
+    while (i != 0) {
         conv = SQ(z) * sq / (2.0f * z + 1.0f + conv);
         z -= 1.0f;
+        i--;
     }
-    conv = x / (1.0f + conv);
 
+#if PLATFORM_N64
+    if (sector > 0) {
+        return M_PI / 2 - (x / (1.0f + conv));
+    } else if (sector < 0) {
+        return -M_PI / 2 - (x / (1.0f + conv));
+    } else {
+        return x / (1.0f + conv);
+    }
+#else
+    conv = x / (1.0f + conv);
     if (sector == 0) {
         return conv;
     } else if (sector > 0) {
@@ -126,8 +170,10 @@ f32 Math_FAtanContFracF(f32 x) {
     } else {
         return -M_PI / 2 - conv;
     }
+#endif
 }
 
+#if PLATFORM_GC
 /**
  * @return arctan(x) in radians, in (-pi/2,pi/2) range
  */
@@ -138,11 +184,33 @@ f32 Math_FAtanF(f32 x) {
         return Math_FAtanContFracF(x);
     }
 }
+#endif
 
 /**
  * @return angle to (x,y) from vector (1,0) around (0,0) in radians, in (-pi,pi] range
  */
 f32 Math_FAtan2F(f32 y, f32 x) {
+#if PLATFORM_N64
+    if (y == 0.0f && x == 0.0f) {
+        return 0.0f;
+    }
+
+    if (x == 0.0f) {
+        if (y < 0.0f) {
+            return -M_PI / 2;
+        } else {
+            return M_PI / 2;
+        }
+    } else if (x < 0.0f) {
+        if (y < 0.0f) {
+            return -(M_PI - Math_FAtanContFracF(fabs(y / x)));
+        } else {
+            return M_PI - Math_FAtanContFracF(fabs(y / x));
+        }
+    } else { // x > 0.0f
+        return Math_FAtanContFracF(y / x);
+    }
+#else
     if (x == 0.0f) {
         if (y == 0.0f) {
             return 0.0f;
@@ -155,11 +223,14 @@ f32 Math_FAtan2F(f32 y, f32 x) {
         }
     } else if (x >= 0.0f) {
         return Math_FAtanF(y / x);
-    } else if (y < 0.0f) {
-        return Math_FAtanF(y / x) - M_PI;
-    } else {
-        return M_PI - Math_FAtanF(-(y / x));
+    } else { // x < 0.0f
+        if (y < 0.0f) {
+            return Math_FAtanF(y / x) - M_PI;
+        } else {
+            return M_PI - Math_FAtanF(-(y / x));
+        }
     }
+#endif
 }
 
 /**
diff --git a/tools/disasm/ntsc-1.2/functions.txt b/tools/disasm/ntsc-1.2/functions.txt
index bdbd2052ee..60bc1a729f 100644
--- a/tools/disasm/ntsc-1.2/functions.txt
+++ b/tools/disasm/ntsc-1.2/functions.txt
@@ -2873,8 +2873,8 @@ PadSetup_Init = 0x800CDC10; // type:func
 Math_FTanF = 0x800CDD50; // type:func
 Math_FFloorF = 0x800CDD84; // type:func
 Math_FCeilF = 0x800CDDA4; // type:func
-func_800CDDC4_unknown = 0x800CDDC4; // type:func
-func_800CDDF8_unknown = 0x800CDDF8; // type:func
+Math_FAbs = 0x800CDDC4; // type:func
+Math_FAbsF = 0x800CDDF8; // type:func
 Math_FRoundF = 0x800CDE28; // type:func
 Math_FTruncF = 0x800CDE48; // type:func
 Math_FNearbyIntF = 0x800CDE68; // type:func