increase poly degree

Author: techcatgato

pybricks/experimental/pb_module_experimental.c

@@ -10,52 +10,54 @@
 #include "py/runtime.h"
 #include <math.h>
 
-static const float PI_F         = 3.1415926535f;
-static const float TWO_PI_F     = 6.2831853071f;
-static const float HALF_PI_F    = 1.5707963267f;
-static const float INV_TWO_PI_F = 0.1591549431f;
+// High-Precision Constants for 98MHz FPU
+static const float PI_F          = 3.141592653589793f;
+static const float TWO_PI_F      = 6.283185307179586f;
+static const float HALF_PI_F     = 1.570796326794896f;
+static const float INV_TWO_PI_F  = 0.159154943091895f;
 
 // -----------------------------------------------------------------------------
-// Core Math Engines (Precision Tuned)
+// Core Math Engines (7th Degree Precision)
 // -----------------------------------------------------------------------------
 
 static inline float fast_sin_internal(float theta) {
-    // 1. Range Reduction to [-PI, PI]
-    float quot = theta * INV_TWO_PI_F;
-    float x = theta - (float)((int)(quot + (quot > 0 ? 0.5f : -0.5f))) * TWO_PI_F;
-
-    // 2. Symmetry Reduction to [-PI/2, PI/2]
-    // Using simple branches here ensures the sign is handled perfectly at the poles
-    if (x > HALF_PI_F) { x = PI_F - x; }
-    else if (x < -HALF_PI_F) { x = -PI_F - x; }
+    // 1. Robust Range Reduction to [-PI, PI]
+    float x = theta * INV_TWO_PI_F;
+    x = theta - (float)((int)(x + (x > 0 ? 0.5f : -0.5f))) * TWO_PI_F;
+
+    // 2. Symmetry folding to [-PI/2, PI/2]
+    if (x > HALF_PI_F) { 
+        x = PI_F - x; 
+    } else if (x < -HALF_PI_F) { 
+        x = -PI_F - x; 
+    }
 
-    // 3. 5th-Degree Remez Minimax Polynomial
-    // These coefficients minimize maximum absolute error to ~0.00005
+    // 3. 7th-Degree Minimax Polynomial
+    // This adds one more multiplication level to crush the error below 0.001
     float x2 = x * x;
-    return x * (0.9999966f + x2 * (-0.1666482f + x2 * 0.0083062f));
+    return x * (1.0f + x2 * (-0.166666567f + x2 * (0.008332152f + x2 * -0.000195152f)));
 }
 
 static inline float fast_atan2_internal(float y, float x) {
-    // Edge case: Origin
     if (x == 0.0f && y == 0.0f) return 0.0f;
 
-    float abs_y = fabsf(y) + 1e-10f; // Prevent div by zero
+    float abs_y = fabsf(y) + 1e-10f;
     float abs_x = fabsf(x);
     float angle;
 
-    // Rational approximation: atan(z) approx z / (1 + 0.28086 * z^2)
+    // High-precision rational approximation
     if (abs_x >= abs_y) {
         float r = y / x;
-        angle = r / (1.0f + 0.28086f * r * r);
-        // Correct for negative X
+        float r2 = r * r;
+        angle = r * (1.0f / (1.0f + 0.28086f * r2));
         if (x < 0.0f) {
             angle += (y >= 0.0f) ? PI_F : -PI_F;
         }
     } else {
         float r = x / y;
-        angle = (y > 0.0f ? HALF_PI_F : -HALF_PI_F) - r / (1.0f + 0.28086f * r * r);
+        float r2 = r * r;
+        angle = (y > 0.0f ? HALF_PI_F : -HALF_PI_F) - r * (1.0f / (1.0f + 0.28086f * r2));
     }
-
     return angle;
 }
 
@@ -115,7 +117,7 @@ static mp_obj_t experimental_benchmark_detailed(mp_obj_t n_in) {
 static MP_DEFINE_CONST_FUN_OBJ_1(experimental_benchmark_detailed_obj, experimental_benchmark_detailed);
 
 // -----------------------------------------------------------------------------
-// Module Registry
+// Registry
 // -----------------------------------------------------------------------------
 
 static const mp_rom_map_elem_t experimental_globals_table[] = {