Implemented Cos() and Tan()

asik · Jan 17, 2013 · e62579c · e62579c
1 parent b0416b2
commit e62579c
Show file tree

Hide file tree

Showing 4 changed files with 25,916 additions and 12 deletions.
diff --git a/Fix64.cs b/Fix64.cs
@@ -29,7 +29,7 @@ public partial struct Fix64 : IEquatable<Fix64>, IComparable<Fix64> {
         public static readonly Fix64 PiInv = (Fix64)0.3183098861837906715377675267M;
         public static readonly Fix64 PiOver2Inv = (Fix64)0.6366197723675813430755350535M;
 
-        static readonly Fix64 SinInterval = (Fix64)(SIN_LUT_SIZE - 1) / PiOver2;
+        static readonly Fix64 LutInterval = (Fix64)(LUT_SIZE - 1) / PiOver2;
         const long MAX_VALUE = long.MaxValue;
         const long MIN_VALUE = long.MinValue;
         const int NUM_BITS = 64;
@@ -38,7 +38,7 @@ public partial struct Fix64 : IEquatable<Fix64>, IComparable<Fix64> {
         const long PI_TIMES_2 = 0x6487ED511;
         const long PI = 0x3243F6A88;
         const long PI_OVER_2 = 0x1921FB544;
-        const int SIN_LUT_SIZE = (int)(PI_OVER_2 >> 15);
+        const int LUT_SIZE = (int)(PI_OVER_2 >> 15);
 
         /// <summary>
         /// Returns a number indicating the sign of a Fix64 number.
@@ -441,7 +441,7 @@ public static Fix64 Sin(Fix64 x) {
 
             // Find the two closest values in the LUT and perform linear interpolation
             // This is what kills the performance of this function on x86 - x64 is fine though
-            var rawIndex = FastMul(clamped, SinInterval);
+            var rawIndex = FastMul(clamped, LutInterval);
             var roundedIndex = Round(rawIndex); 
             var indexError = FastSub(rawIndex, roundedIndex);
 
@@ -470,7 +470,7 @@ public static Fix64 FastSin(Fix64 x) {
             // Here we use the fact that the SinLut table has a number of entries
             // equal to (PI_OVER_2 >> 15) to use the angle to index directly into it
             var rawIndex = (uint)(clampedL >> 15);
-            if (rawIndex == SIN_LUT_SIZE) {
+            if (rawIndex == LUT_SIZE) {
                 --rawIndex;
             }
             var nearestValue = SinLut[flipHorizontal ?
@@ -504,6 +504,56 @@ static long ClampSinValue(long angle, out bool flipHorizontal, out bool flipVert
             return clampedPiOver2;
         }
 
+        /// <summary>
+        /// Returns the cosine of x.
+        /// See Sin() for more details.
+        /// </summary>
+        public static Fix64 Cos(Fix64 x) {
+            var xl = x.m_rawValue;
+            var rawAngle = xl + (xl > 0 ? -PI - PI_OVER_2 : PI_OVER_2);
+            return Sin(new Fix64(rawAngle));
+        }
+
+        /// <summary>
+        /// Returns a rough approximation of the cosine of x.
+        /// See FastSin for more details.
+        /// </summary>
+        public static Fix64 FastCos(Fix64 x) {
+            var xl = x.m_rawValue;
+            var rawAngle = xl + (xl > 0 ? -PI - PI_OVER_2 : PI_OVER_2);
+            return FastSin(new Fix64(rawAngle));
+        }
+
+        public static Fix64 Tan(Fix64 x) {
+            var clampedPi = x.m_rawValue % PI;
+            var flip = false;
+            if (clampedPi < 0) {
+                clampedPi = -clampedPi;
+                flip = true;
+            }
+            if (clampedPi > PI_OVER_2) {
+                flip = !flip;
+                clampedPi = PI_OVER_2 - (clampedPi - PI_OVER_2);
+            }
+
+            var clamped = new Fix64(clampedPi);
+
+            // Find the two closest values in the LUT and perform linear interpolation
+            var rawIndex = FastMul(clamped, LutInterval);
+            var roundedIndex = Round(rawIndex);
+            var indexError = FastSub(rawIndex, roundedIndex);
+
+            var nearestValue = new Fix64(TanLut[(int)roundedIndex]);
+            var secondNearestValue = new Fix64(TanLut[(int)roundedIndex + Sign(indexError)]);
+
+            var delta = FastMul(indexError, FastAbs(FastSub(nearestValue, secondNearestValue))).m_rawValue;
+            var interpolatedValue = nearestValue.m_rawValue + delta;
+            var finalValue = flip ? -interpolatedValue : interpolatedValue;
+            return new Fix64(finalValue);
+        }
+
+
+
         public static explicit operator Fix64(long value) {
             return new Fix64(value * ONE);
         }
@@ -560,8 +610,8 @@ internal static void GenerateSinLut() {
     partial struct Fix64 {
         public static readonly long[] SinLut = new[] {");
                 int lineCounter = 0;
-                for (int i = 0; i < SIN_LUT_SIZE; ++i) {
-                    var angle = i * Math.PI * 0.5 / (SIN_LUT_SIZE - 1);
+                for (int i = 0; i < LUT_SIZE; ++i) {
+                    var angle = i * Math.PI * 0.5 / (LUT_SIZE - 1);
                     if (lineCounter++ % 8 == 0) {
                         writer.WriteLine();
                         writer.Write("            ");
@@ -578,6 +628,34 @@ partial struct Fix64 {
             }
         }
 
+        internal static void GenerateTanLut() {
+            using (var writer = new StreamWriter("Fix64TanLut.cs")) {
+                writer.Write(
+@"namespace FixMath.NET {
+    partial struct Fix64 {
+        public static readonly long[] TanLut = new[] {");
+                int lineCounter = 0;
+                for (int i = 0; i < LUT_SIZE; ++i) {
+                    var angle = i * Math.PI * 0.5 / (LUT_SIZE - 1);
+                    if (lineCounter++ % 8 == 0) {
+                        writer.WriteLine();
+                        writer.Write("            ");
+                    }
+                    var tan = Math.Tan(angle);
+                    if (tan > (double)MaxValue || tan < 0.0) {
+                        tan = (double)MaxValue;
+                    }
+                    var rawValue = (((decimal)tan > (decimal)MaxValue || tan < 0.0) ? MaxValue : (Fix64)tan).m_rawValue;
+                    writer.Write(string.Format("0x{0:X}L, ", rawValue));
+                }
+                writer.Write(
+@"
+        };
+    }
+}");
+            }
+        }
+
         /// <summary>
         /// The underlying integer representation
         /// </summary>