Ticket #2109: cordic_patch.patch

source/maths/Fixed.cpp

@@ -1 @@
-/* Copyright (C) 2010 Wildfire Games.
+/* Copyright (C) 2015 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -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)
+typedef enum
 {
-    CFixed_15_16 zero;
+    QUAD1,
+    QUAD2,
+    QUAD3,
+    QUAD4
+}CORDIC_OUTPUT_STATE;
 
-    // Special case to avoid division-by-zero
-    if (x.IsZero() && y.IsZero())
-        return zero;
+//angles: 45, 26.565, 14.036, 7.125, 3.576, 1.790, 0.895, 0.448
+static const CFixed_15_16 tan_angle_table_rad[8] = { CFixed_15_16(51472), CFixed_15_16(30386), CFixed_15_16(16055), CFixed_15_16(8150), CFixed_15_16(4091), CFixed_15_16(2047), CFixed_15_16(1042), CFixed_15_16(512) };
+//angles 90 and 180
+static const CFixed_15_16 angle_0_5_pi = CFixed_15_16(102944);
+static const CFixed_15_16 angle_1_0_pi = CFixed_15_16(205887);
+static const CFixed_15_16 angle_1_5_pi = CFixed_15_16(308831);
+static const CFixed_15_16 angle_2_0_pi = CFixed_15_16(411775);
+static const CFixed_15_16 zero = CFixed_15_16();
 
-    CFixed_15_16 c1;
-    c1.SetInternalValue(51472); // pi/4 << 16
+CFixed_15_16 atan2_approx(CFixed_15_16 y, CFixed_15_16 x)
+{
+    CFixed_15_16 angle;
+    int loop_var;
+    int previous_x_val;
+    int new_x_val;
+    CORDIC_OUTPUT_STATE output_state = QUAD1;
 
-    CFixed_15_16 c2;
-    c2.SetInternalValue(154415); // 3*pi/4 << 16
+    if (y == zero)
+    {
+        if (x >= zero)
+            return zero;
+        else
+            return angle_1_0_pi;
+    }
 
-    CFixed_15_16 abs_y = y.Absolute();
+    //quadrant 2
+    else if (x < zero && y > zero)
+    {
+        x.SetInternalValue(x.GetInternalValue()* -1);
+        output_state = QUAD2;
+    }
 
-    CFixed_15_16 angle;
-    if (x >= zero)
+    //quadrant 4
+    else if (x > zero && y < zero)
     {
-        CFixed_15_16 r = (x - abs_y) / (x + abs_y);
-        angle = c1 - c1.Multiply(r);
+        y.SetInternalValue(y.GetInternalValue()* -1);
+        output_state = QUAD4;
     }
-    else
+
+    //quadrant 3
+    else if (x < zero && y < zero)
     {
-        CFixed_15_16 r = (x + abs_y) / (abs_y - x);
-        angle = c2 - c1.Multiply(r);
+        x.SetInternalValue(x.GetInternalValue()* -1);
+        y.SetInternalValue(y.GetInternalValue()* -1);
+        output_state = QUAD3;
     }
 
-    if (y < zero)
-        return -angle;
-    else
+
+    for (loop_var = 0; loop_var < 5; ++loop_var)
+    {
+        int factor;
+        //these divisions should be optimised by the compiler to shifts because the operations are always division by a power of two
+        //this makes the cost of each division 1 cycle.
+        if (y < zero)
+        {
+            factor = (1 << loop_var);
+            previous_x_val = x.GetInternalValue();
+            x -= (y / factor);
+            new_x_val = x.GetInternalValue();
+            x.SetInternalValue(previous_x_val);
+            y += (x / factor);
+            x.SetInternalValue(new_x_val);
+
+            angle -= tan_angle_table_rad[loop_var];
+        }
+        else
+        {
+            factor = (1 << loop_var);
+            previous_x_val = x.GetInternalValue();
+            x += (y / factor);
+            new_x_val = x.GetInternalValue();
+            x.SetInternalValue(previous_x_val);
+            y -= (x / factor);
+            x.SetInternalValue(new_x_val);
+
+            angle += tan_angle_table_rad[loop_var];
+        }
+    }
+
+    //this should be optimised to a jump table
+    switch (output_state)
+    {
+    case QUAD1:
         return angle;
+    case QUAD2:
+        return (angle_1_0_pi - angle);
+    case QUAD3:
+        return (angle_1_0_pi - angle) * -1;
+    case QUAD4:
+        return angle * -1;
+    default:
+        return angle;
+    }
 }
 
 template<>
@@ -186 +254 @@
 
 void sincos_approx(CFixed_15_16 a, CFixed_15_16& sin_out, CFixed_15_16& cos_out)
 {
-    // Based on http://www.coranac.com/2009/07/sines/
+    CFixed_15_16 angle = CFixed_15_16();
+    CFixed_15_16 y = CFixed_15_16();
+    CFixed_15_16 x = CFixed_15_16(39797); //cordic gain for 8 angles
+    int loop_var;
+    int previous_x_val;
+    int new_x_val;
+    CORDIC_OUTPUT_STATE output_state;
+    bool is_positive_angle = true;
 
-    // TODO: this could be made a bit more precise by being careful about scaling
+    if (a < zero)
+    {
+        is_positive_angle = false;
+        a.SetInternalValue(a.GetInternalValue() * -1);
+    }
 
-    typedef CFixed_15_16 fixed;
+    if (a < angle_0_5_pi)
+        output_state = (is_positive_angle ? QUAD1 : QUAD4);
 
-    fixed c2_pi;
-    c2_pi.SetInternalValue(41721); // = 2/pi << 16
-
-    // Map radians onto the range [0, 4)
-    fixed z = a.Multiply(c2_pi) % fixed::FromInt(4);
-
-    // Map z onto the range [-1, +1] for sin, and the same with z+1 to compute cos
-    fixed sz, cz;
-    if (z >= fixed::FromInt(3)) // [3, 4)
+    else if (a < angle_1_0_pi)
     {
-        sz = z - fixed::FromInt(4);
-        cz = z - fixed::FromInt(3);
+        a -= angle_0_5_pi;
+        output_state = (is_positive_angle ? QUAD2 : QUAD3);
     }
-    else if (z >= fixed::FromInt(2)) // [2, 3)
+    else if (a < angle_1_5_pi)
     {
-        sz = fixed::FromInt(2) - z;
-        cz = z - fixed::FromInt(3);
+        a -= angle_1_0_pi;
+        output_state = (is_positive_angle ? QUAD3 : QUAD2);
     }
-    else if (z >= fixed::FromInt(1)) // [1, 2)
+    else
     {
-        sz = fixed::FromInt(2) - z;
-        cz = fixed::FromInt(1) - z;
+        a -= angle_1_5_pi;
+        output_state = (is_positive_angle ? QUAD4 : QUAD1);
     }
-    else // [0, 1)
+
+    for (loop_var = 0; loop_var < 8; ++loop_var)
     {
-        sz = z;
-        cz = fixed::FromInt(1) - z;
-    }
+        int factor;
+        //these divisions should be optimised by the compiler to shifts because the operations are always division by a power of two
+        //this makes the cost of each division 1 cycle.
+        if (angle > a)
+        {
+            factor = (1 << loop_var);
+            previous_x_val = x.GetInternalValue();
+            x -= (y / factor);
+            new_x_val = x.GetInternalValue();
+            x.SetInternalValue(previous_x_val);
+            y += (x / factor);
+            x.SetInternalValue(new_x_val);
 
-    // Third-order (max absolute error ~0.02)
+            angle -= tan_angle_table_rad[loop_var];
+        }
+        else
+        {
+            factor = (1 << loop_var);
+            previous_x_val = x.GetInternalValue();
+            x += (y / factor);
+            new_x_val = x.GetInternalValue();
+            x.SetInternalValue(previous_x_val);
+            y -= (x / factor);
+            x.SetInternalValue(new_x_val);
 
-//  sin_out = (sz / 2).Multiply(fixed::FromInt(3) - sz.Multiply(sz));
-//  cos_out = (cz / 2).Multiply(fixed::FromInt(3) - cz.Multiply(cz));
+            angle += tan_angle_table_rad[loop_var];
+        }
+    }
 
-    // Fifth-order (max absolute error ~0.0005)
-
-    fixed sz2 = sz.Multiply(sz);
-    sin_out = sz.Multiply(fixed::Pi() - sz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - sz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2;
-
-    fixed cz2 = cz.Multiply(cz);
-    cos_out = cz.Multiply(fixed::Pi() - cz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - cz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2;
+    //the cases are contiguous therefore this should be optimised to a jump table
+    switch (output_state)
+    {
+    case QUAD1:
+        sin_out.SetInternalValue(y.GetInternalValue() * -1);
+        cos_out.SetInternalValue(x.GetInternalValue());
+        return;
+    case QUAD2:
+        sin_out.SetInternalValue(y.GetInternalValue() * -1);
+        cos_out.SetInternalValue(x.GetInternalValue() * -1);
+        return;
+    case QUAD3:
+        sin_out.SetInternalValue(y.GetInternalValue());
+        cos_out.SetInternalValue(x.GetInternalValue() * -1);
+        return;
+    case QUAD4:
+        sin_out.SetInternalValue(y.GetInternalValue());
+        cos_out.SetInternalValue(x.GetInternalValue());
+        return;
+    default:
+        return;
+    }
 }

source/maths/Fixed.h

@@ -1 @@
-/* Copyright (C) 2013 Wildfire Games.
+/* Copyright (C) 2015 Wildfire Games.
  * This file is part of 0 A.D.
  *
  * 0 A.D. is free software: you can redistribute it and/or modify
@@ -117 +117 @@
 private:
     T value;
 
+public:
+
     explicit CFixed(T v) : value(v) { }
-
-public:
     enum { fract_bits = fract_bits_ };
 
     CFixed() : value(0) { }