Add precise Atan and Acos functions

Author: Bartl

src/Fix64.cs

@@ -704,6 +704,77 @@ public static Fix64 Tan(Fix64 x) {
             return new Fix64(finalValue);
         }
 
+        /// <summary>
+        /// Returns the arccos of of the specified number, calculated using Atan and Sqrt
+        /// This function has at least 7 decimals of accuracy.
+        /// </summary>
+        public static Fix64 Acos(Fix64 x)
+        {
+            if (x.RawValue == 0)
+                return Fix64.PiOver2;
+
+            Fix64 result = Fix64.Atan(Fix64.Sqrt(One - x * x) / x);
+            if (x.RawValue < 0)
+                return result + Fix64.Pi;
+            else
+                return result;
+        }
+
+        /// <summary>
+        /// Returns the arctan of of the specified number, calculated using Euler series
+        /// This function has at least 7 decimals of accuracy.
+        /// </summary>
+        public static Fix64 Atan(Fix64 z)
+        {
+            if (z.RawValue == 0)
+                return Zero;
+
+            // Force positive values for argument
+            // Atan(-z) = -Atan(z).
+            bool neg = (z.RawValue < 0);
+            if (neg) z = -z;
+
+            Fix64 result;
+
+            if (z == One)
+                result = Pi/(Fix64)4;
+            else
+            {
+                bool invert = z > One;
+                if (invert) z = One / z;
+
+                result = One;
+                Fix64 term = One;
+
+                Fix64 zSq = z * z;
+                Fix64 zSq2 = zSq * (Fix64)2;
+                Fix64 zSqPlusOne = zSq + One;
+                Fix64 zSq12 = zSqPlusOne * (Fix64)2;
+                Fix64 dividend = zSq2;
+                Fix64 divisor = zSqPlusOne * (Fix64)3;
+
+                for (int i = 2; i < 30; i++)
+                {
+                    term *= dividend / divisor;
+                    result += term;
+
+                    dividend += zSq2;
+                    divisor += zSq12;
+
+                    if (term.RawValue == 0)
+                        break;
+                }
+
+                result = result * z / zSqPlusOne;
+
+                if (invert)
+                    result = PiOver2 - result;
+            }
+
+            if (neg) result = -result;
+            return result;
+        }
+
         public static Fix64 Atan2(Fix64 y, Fix64 x) {
             var yl = y.m_rawValue;
             var xl = x.m_rawValue;

tests/Fix64Tests.cs

@@ -579,6 +579,42 @@ public void FastSin()
             }
         }
 
+        [Fact]
+        public void Acos()
+        {
+            var maxDelta = 0.00000001m;
+            var deltas = new List<decimal>();
+
+            // Precision
+            for (var x = -1.0; x < 1.0; x += 0.001)
+            {
+                var xf = (Fix64)x;
+                var actual = (decimal)Fix64.Acos(xf);
+                var expected = (decimal)Math.Acos((double)xf);
+                var delta = Math.Abs(actual - expected);
+                deltas.Add(delta);
+                Assert.True(delta <= maxDelta, string.Format("Precision: Acos({0}): expected {1} but got {2}", xf, expected, actual));
+            }
+
+            for (int i = 0; i < m_testCases.Length; ++i)
+            {
+                var b = Fix64.FromRaw(m_testCases[i]);
+
+                if (b < -Fix64.One)
+                    continue;
+                if (b > Fix64.One)
+                    continue;
+
+                var expected = (decimal)Math.Acos((double)b);
+                var actual = (decimal)Fix64.Acos(b);
+                var delta = Math.Abs(expected - actual);
+                deltas.Add(delta);
+                Assert.True(delta <= maxDelta, string.Format("Acos({0}) = expected {1} but got {2}", b, expected, actual));
+            }
+            Console.WriteLine("Max error: {0} ({1} times precision)", deltas.Max(), deltas.Max() / Fix64.Precision);
+            Console.WriteLine("Average precision: {0} ({1} times precision)", deltas.Average(), deltas.Average() / Fix64.Precision);
+        }
+
         [Fact]
         public void Cos()
         {
@@ -666,6 +702,64 @@ public void Tan()
             //}
         }
 
+        [Fact]
+        public void Atan()
+        {
+            var maxDelta = 0.00000001m;
+            var deltas = new List<decimal>();
+
+            // Precision
+            for (var x = -1.0; x < 1.0; x += 0.0001)
+            {
+                var xf = (Fix64)x;
+                var actual = (decimal)Fix64.Atan(xf);
+                var expected = (decimal)Math.Atan((double)xf);
+                var delta = Math.Abs(actual - expected);
+                deltas.Add(delta);
+                Assert.True(delta <= maxDelta, string.Format("Precision: Atan({0}): expected {1} but got {2}", xf, expected, actual));
+            }
+
+            // Scalability and edge cases
+            foreach (var x in m_testCases)
+            {
+                var xf = (Fix64)x;
+                var actual = (decimal)Fix64.Atan(xf);
+                var expected = (decimal)Math.Atan((double)xf);
+                var delta = Math.Abs(actual - expected);
+                deltas.Add(delta);
+                Assert.True(delta <= maxDelta, string.Format("Scalability: Atan({0}): expected {1} but got {2}", xf, expected, actual));
+            }
+            Console.WriteLine("Max error: {0} ({1} times precision)", deltas.Max(), deltas.Max() / Fix64.Precision);
+            Console.WriteLine("Average precision: {0} ({1} times precision)", deltas.Average(), deltas.Average() / Fix64.Precision);
+        }
+
+        //[Fact]
+        public void AtanBenchmark()
+        {
+            var deltas = new List<decimal>();
+
+            var swf = new Stopwatch();
+            var swd = new Stopwatch();
+
+            for (var x = -1.0; x < 1.0; x += 0.001)
+            {
+                for (int k = 0; k < 1000; ++k)
+                {
+                    var xf = (Fix64)x;
+                    swf.Start();
+                    var actualF = Fix64.Atan(xf);
+                    swf.Stop();
+                    swd.Start();
+                    var expected = Math.Atan((double)xf);
+                    swd.Stop();
+                    deltas.Add(Math.Abs((decimal)actualF - (decimal)expected));
+                }
+            }
+            Console.WriteLine("Max error: {0} ({1} times precision)", deltas.Max(), deltas.Max() / Fix64.Precision);
+            Console.WriteLine("Average precision: {0} ({1} times precision)", deltas.Average(), deltas.Average() / Fix64.Precision);
+            Console.WriteLine("Fix64.Atan time = {0}ms, Math.Atan time = {1}ms", swf.ElapsedMilliseconds, swd.ElapsedMilliseconds);
+        }
+
         [Fact]
         public void Atan2()
         {