Ticket #2109: sincos_approx.patch

source/maths/Fixed.cpp

@@ -143 +143 @@
     return r.str();
 }
 
+//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(308830);
+static const CFixed_15_16 zero = CFixed_15_16();
+
 // 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)
 {
@@ -184 +192 @@
     return CFixed_15_16(205887); // = pi << 16
 }
 
+typedef enum
+{
+    QUAD1,
+    QUAD2,
+    QUAD3,
+    QUAD4
+}SINCOS_OUTPUT_STATE;
+
 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
+    i32 loop_var;
+    i32 previous_x_val;
+    i32 new_x_val;
+    SINCOS_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
+    else if(a < angle_1_0_pi)
+    {
+        a -= angle_0_5_pi;
+        output_state = (is_positive_angle? QUAD2: QUAD3);
+    }
+    else if(a < angle_1_5_pi)
+    {
+        a -= angle_1_0_pi;
+        output_state = (is_positive_angle? QUAD3: QUAD2);
+    }
+    else
+    {
+        a -= angle_1_5_pi;
+        output_state = (is_positive_angle? QUAD4: QUAD1);
+    }
 
-    // Map radians onto the range [0, 4)
-    fixed z = a.Multiply(c2_pi) % fixed::FromInt(4);
+    for(loop_var = 0; loop_var < 8; ++loop_var)
+    {
+        i32 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);
 
-    // 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)
-    {
-        sz = z - fixed::FromInt(4);
-        cz = z - fixed::FromInt(3);
-    }
-    else if (z >= fixed::FromInt(2)) // [2, 3)
-    {
-        sz = fixed::FromInt(2) - z;
-        cz = z - fixed::FromInt(3);
-    }
-    else if (z >= fixed::FromInt(1)) // [1, 2)
-    {
-        sz = fixed::FromInt(2) - z;
-        cz = fixed::FromInt(1) - z;
-    }
-    else // [0, 1)
-    {
-        sz = z;
-        cz = fixed::FromInt(1) - z;
-    }
+            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);
 
-    // Third-order (max absolute error ~0.02)
+            angle += tan_angle_table_rad[loop_var];
+        }
+    }
 
-//  sin_out = (sz / 2).Multiply(fixed::FromInt(3) - sz.Multiply(sz));
-//  cos_out = (cz / 2).Multiply(fixed::FromInt(3) - cz.Multiply(cz));
-
-    // 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

@@ -109 +109 @@
 private:
     T value;
 
-    explicit CFixed(T v) : value(v) { }
-
 public:
     enum { fract_bits = fract_bits_ };
 
     CFixed() : value(0) { }
+    explicit CFixed(T v) : value(v) { }
 
     static CFixed Zero() { return CFixed(0); }
     static CFixed Epsilon() { return CFixed(1); }