Calculate 5th order polynomial in twin_sincos

Author: ndsl7109256

include/twin.h

@@ -736,8 +736,6 @@ void twin_path_rmove(twin_path_t *path, twin_fixed_t x, twin_fixed_t y);
 
 void twin_path_draw(twin_path_t *path, twin_fixed_t x, twin_fixed_t y);
 
-void twin_path_draw_polar(twin_path_t *path, twin_angle_t deg);
-
 void twin_path_rdraw(twin_path_t *path, twin_fixed_t x, twin_fixed_t y);
 
 void twin_path_circle(twin_path_t *path,

src/path.c

@@ -134,7 +134,7 @@ void twin_path_draw(twin_path_t *path, twin_fixed_t x, twin_fixed_t y)
                      _twin_matrix_y(&path->state.matrix, x, y));
 }
 
-void twin_path_draw_polar(twin_path_t *path, twin_angle_t deg)
+static void twin_path_draw_polar(twin_path_t *path, twin_angle_t deg)
 {
     twin_fixed_t s, c;
     twin_sincos(deg, &s, &c);

src/trig.c

@@ -12,39 +12,16 @@
 
 twin_fixed_t twin_sin(twin_angle_t a)
 {
-    twin_fixed_t sin;
-
-    /* limit to [0..360) */
-    a = a & (TWIN_ANGLE_360 - 1);
-    /* special case for 90 degrees - no room in table */
-    if ((a & ~(TWIN_ANGLE_180)) == TWIN_ANGLE_90)
-        sin = TWIN_FIXED_ONE;
-    else {
-        /* mirror second and third quadrant values across y axis */
-        if (a & TWIN_ANGLE_90)
-            a = TWIN_ANGLE_180 - a;
-        /* 5th order polynomial approximation */
-        const uint64_t A1 = 3370945099, B1 = 2746362156, C1 = 2339369;
-        const uint64_t A = 16, n = 10, p = 32, q = 31, r = 3;
-        uint64_t x = a & (TWIN_ANGLE_90 - 1);
-        uint64_t y = (C1 * x) >> n;
-        y = B1 - ((x * y) >> r);
-        y = x * (y >> n);
-        y = x * (y >> n);
-        y = A1 - (y >> (p - q));
-        y = x * (y >> n);
-        y = (y + (1UL << (q - A - 1))) >> (q - A);  // Rounding
-        sin = y;
-    }
-    /* mirror third and fourth quadrant values across x axis */
-    if (a & TWIN_ANGLE_180)
-        sin = -sin;
-    return sin;
+    twin_fixed_t sin_val = 0;
+    twin_sincos(a, &sin_val, NULL);
+    return sin_val;
 }
 
 twin_fixed_t twin_cos(twin_angle_t a)
 {
-    return twin_sin(a + TWIN_ANGLE_90);
+    twin_fixed_t cos_val = 0;
+    twin_sincos(a, NULL, &cos_val);
+    return cos_val;
 }
 
 twin_fixed_t twin_tan(twin_angle_t a)
@@ -63,9 +40,63 @@ twin_fixed_t twin_tan(twin_angle_t a)
     return ((s << 15) / c) << 1;
 }
 
-void twin_sincos(twin_angle_t a, twin_fixed_t *sin, twin_fixed_t *cos)
+static inline twin_fixed_t sin_poly(twin_angle_t x)
 {
-    *sin = twin_sin(a);
-    *cos = twin_cos(a);
+    /* S(x) = x * 2^(-n) * (A1 - 2 ^ (q-p) * x * (2^-n) * x * 2^(-n) * (B1 - 2 ^
+     * (-r) * x * 2 ^ (-n) * C1 * x)) * 2 ^ (a-q)
+     * @n: the angle scale
+     * @A: the amplitude
+     * @p,q,r: the scaling factor
+     *
+     * A1 = 2^q * a5, B1 = 2 ^ p * b5, C1 = 2 ^ (r+p-n) * c5
+     * where a5, b5, c5 are the coefficients for 5th-order polynomial
+     * a5 = 4 * (3 / pi - 9 / 16)
+     * b5 = 2 * a5 - 5 / 2
+     * c5 = a5 - 3 / 2
+     */
+    const uint64_t A = 16, n = 10, p = 32, q = 31, r = 3;
+    const uint64_t A1 = 3370945099, B1 = 2746362156, C1 = 2339369;
+    uint64_t y = (C1 * x) >> n;
+    y = B1 - ((x * y) >> r);
+    y = x * (y >> n);
+    y = x * (y >> n);
+    y = A1 - (y >> (p - q));
+    y = x * (y >> n);
+    y = (y + (1UL << (q - A - 1))) >> (q - A);  // Rounding
+    return y;
 }
 
+void twin_sincos(twin_angle_t a, twin_fixed_t *sin, twin_fixed_t *cos)
+{
+    twin_fixed_t sin_val = 0, cos_val = 0;
+
+    /* limit to [0..360) */
+    a = a & (TWIN_ANGLE_360 - 1);
+    int c = a > TWIN_ANGLE_90 && a < TWIN_ANGLE_270;
+    /* special case for 90 degrees - no room in table */
+    if ((a & ~(TWIN_ANGLE_180)) == TWIN_ANGLE_90) {
+        sin_val = TWIN_FIXED_ONE;
+        cos_val = 0;
+    } else {
+        /* mirror second and third quadrant values across y axis */
+        if (a & TWIN_ANGLE_90)
+            a = TWIN_ANGLE_180 - a;
+        twin_angle_t x = a & (TWIN_ANGLE_90 - 1);
+        if (sin)
+            sin_val = sin_poly(x);
+        if (cos)
+            cos_val = sin_poly(TWIN_ANGLE_90 - x);
+    }
+    if (sin) {
+        /* mirror third and fourth quadrant values across x axis */
+        if (a & TWIN_ANGLE_180)
+            sin_val = -sin_val;
+        *sin = sin_val;
+    }
+    if (cos) {
+        /* mirror first and fourth quadrant values across y axis */
+        if (c)
+            cos_val = -cos_val;
+        *cos = cos_val;
+    }
+}