Ticket #2109: atan2cordic.patch

source/maths/Fixed.cpp

@@ -143 +143 @@
     return r.str();
 }
 
-// Based on http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
 CFixed_15_16 atan2_approx(CFixed_15_16 y, CFixed_15_16 x)
 {
-    CFixed_15_16 zero;
+    // Error <= arctan(1/16)/2 ~ 1.788 degrees.
 
-    // Special case to avoid division-by-zero
-    if (x.IsZero() && y.IsZero())
-        return zero;
+    // Binary search using the following angles:
+    // 45, 26.565, 14.036, 7.125, 3.576 degrees (= arctan(1/2^n))
+    // Idea: [1, 2^(-n); -2^(-n), 1] are multiples of the rotation matrices for these angles.
+    i32 tan_angle_table_rad[5] = {51472, 30386, 16055, 8150, 4091};
 
-    CFixed_15_16 c1;
-    c1.SetInternalValue(51472); // pi/4 << 16
+    i32 angleint = 2045; // = 1/2*arctan(1/16) << 16
+    i32 xint = x.GetInternalValue(), yint = y.GetInternalValue(), tmpint;
 
-    CFixed_15_16 c2;
-    c2.SetInternalValue(154415); // 3*pi/4 << 16
+    // Handle the case (0,0).
+    if (xint == 0 && yint == 0)
+        return CFixed_15_16::FromInt(0);
 
-    CFixed_15_16 abs_y = y.Absolute();
+    // Reduce the problem to the case x,y >= 0, i.e. 0 <= angle <= pi/2
+    if (yint >= 0) // (0, pi)
+    {
+        if (xint < 0) // (pi/2, pi)
+        {
+            // (x, y) -> (y, -x)
+            tmpint = xint;
+            xint = yint;
+            yint = -tmpint;
+            angleint = 102944+2045; // pi/2 << 16
+        }
+    }
+    else // (-pi, 0)
+    {
+        if (xint < 0) // (-pi, -pi/2)
+        {
+            // (x, y) -> (-x, -y)
+            xint *= -1;
+            yint *= -1;
+            angleint = -205887+2045; // -pi << 16
+        }
+        else // (-pi/2, 0)
+        {
+            // (x, y) -> (-y, x)
+            tmpint = xint;
+            xint = -yint;
+            yint = tmpint;
+            angleint = -102944+2045; // -pi/2 << 16
+        }
+    }
 
-    CFixed_15_16 angle;
-    if (x >= zero)
+    // Scale up / down to prevent massive rounding errors / overflows.
+    if (xint < 64 && yint < 64)
+    {
+        xint <<= 6;
+        yint <<= 6;
+    }
+    else if (xint >= 8192 || yint >= 8192)
     {
-        CFixed_15_16 r = (x - abs_y) / (x + abs_y);
-        angle = c1 - c1.Multiply(r);
+        xint >>= 4;
+        yint >>= 4;
     }
-    else
+    for (u8 loop_var = 0; loop_var < 5; ++loop_var)
     {
-        CFixed_15_16 r = (x + abs_y) / (abs_y - x);
-        angle = c2 - c1.Multiply(r);
+        tmpint = xint >> loop_var;
+        // If remaining angle > arctan(1/2^n) "rotate" counterclockwise.
+        if (yint >= tmpint)
+        {
+            // (x,y) -> (x,y) + 1/2^n * (-y,x)
+            xint += yint >> loop_var;
+            yint -= tmpint;
+            angleint += tan_angle_table_rad[loop_var];
+        }
     }
+    // Remaining angle is in [0,arctan(1/16)], arctan(1/16)/2 is the best possible approximation.
 
-    if (y < zero)
-        return -angle;
-    else
-        return angle;
+    // If needed, get the value into the desired interval of length 2*pi.
+
+    CFixed_15_16 angle;
+    angle.SetInternalValue(angleint);
+    return angle;
 }
 
 template<>