Bunny83

FFT / Complex

Feb 22nd, 2017
2,425
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #region License and Information
  2. /*****
  3. *
  4. * This is an implementation of a complex number class which supports most
  5. * common operations. Though, not all of them has been tested. Some are staight
  6. * forward implementation as you can find them on Wikipedia and other sources.
  7. *
  8. * In addition the FFT class contains a fast implementation of the Fast Fourier
  9. * Transformation. It's basically a port of the implementation of Paul Bourke
  10. * http://paulbourke.net/miscellaneous/dft/
  11. *
  12. * CalculateFFT is designed to perform both the FFT as well as the inverse FFT.
  13. * If you pass "true" to the "reverse" parameter it will calculate the inverse
  14. * FFT. The FFT is calculated in-place on an array of Complex values.
  15. *
  16. * For convenience i added a few helper functions to convert a float array as
  17. * well as a double array into a Complex array. The reverse also exists. The
  18. * Complex2Float and Complex2Double have also a "reverse" parameter which will
  19. * preserve the sign of the real part
  20. *
  21. * Keep in mind when using this as FFT filter you have to preserve the Complex
  22. * samples as the phase information might be required for the inverse FFT.
  23. *
  24. * Final note: I've written this mainly to better understand the FFT algorithm.
  25. * The original version of Paul Bourke is probably faster, but i wanted to use
  26. * actual Complex numbers so it's easier to follow the code.
  27. *
  28. *
  29. * Copyright (c) 2015 Bunny83
  30. *
  31. * Permission is hereby granted, free of charge, to any person obtaining a copy
  32. * of this software and associated documentation files (the "Software"), to
  33. * deal in the Software without restriction, including without limitation the
  34. * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
  35. * sell copies of the Software, and to permit persons to whom the Software is
  36. * furnished to do so, subject to the following conditions:
  37. *
  38. * The above copyright notice and this permission notice shall be included in
  39. * all copies or substantial portions of the Software.
  40. *
  41. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  42. * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  43. * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  44. * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  45. * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  46. * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  47. * IN THE SOFTWARE.
  48. *
  49. *****/
  50. #endregion License and Information
  51. #define UNITY
  52.  
  53.  
  54. namespace B83.MathHelpers
  55. {
  56.     using System;
  57.     public struct Complex
  58.     {
  59.         public double real;
  60.         public double img;
  61.         public Complex(double aReal, double aImg)
  62.         {
  63.             real = aReal;
  64.             img = aImg;
  65.         }
  66.         public static Complex FromAngle(double aAngle, double aMagnitude)
  67.         {
  68.             return new Complex(Math.Cos(aAngle) * aMagnitude, Math.Sin(aAngle) * aMagnitude);
  69.         }
  70.  
  71.         public Complex conjugate { get { return new Complex(real, -img); } }
  72.         public double magnitude { get { return Math.Sqrt(real * real + img * img); } }
  73.         public double sqrMagnitude { get { return real * real + img * img; } }
  74.         public double angle { get { return Math.Atan2(img, real); } }
  75.  
  76.         public float fReal { get { return (float)real; } set { real = value; } }
  77.         public float fImg { get { return (float)img; } set { img = value; } }
  78.         public float fMagnitude { get { return (float)Math.Sqrt(real * real + img * img); } }
  79.         public float fSqrMagnitude { get { return (float)(real * real + img * img); } }
  80.         public float fAngle { get { return (float)Math.Atan2(img, real); } }
  81.  
  82.         #region Basic operations + - * /
  83.         public static Complex operator +(Complex a, Complex b)
  84.         {
  85.             return new Complex(a.real + b.real, a.img + b.img);
  86.         }
  87.         public static Complex operator +(Complex a, double b)
  88.         {
  89.             return new Complex(a.real + b, a.img);
  90.         }
  91.         public static Complex operator +(double b, Complex a)
  92.         {
  93.             return new Complex(a.real + b, a.img);
  94.         }
  95.  
  96.         public static Complex operator -(Complex a)
  97.         {
  98.             return new Complex(-a.real , -a.img);
  99.         }
  100.  
  101.         public static Complex operator -(Complex a, Complex b)
  102.         {
  103.             return new Complex(a.real - b.real, a.img - b.img);
  104.         }
  105.         public static Complex operator -(Complex a, double b)
  106.         {
  107.             return new Complex(a.real - b, a.img);
  108.         }
  109.         public static Complex operator -(double b, Complex a)
  110.         {
  111.             return new Complex(b- a.real, -a.img);
  112.         }
  113.  
  114.         public static Complex operator *(Complex a, Complex b)
  115.         {
  116.             return new Complex(a.real * b.real - a.img * b.img, a.real * b.img + a.img * b.real);
  117.         }
  118.         public static Complex operator *(Complex a, double b)
  119.         {
  120.             return new Complex(a.real * b, a.img * b);
  121.         }
  122.         public static Complex operator *(double b, Complex a)
  123.         {
  124.             return new Complex(a.real * b, a.img * b);
  125.         }
  126.  
  127.         public static Complex operator /(Complex a, Complex b)
  128.         {
  129.             double d = 1d / (b.real * b.real + b.img * b.img);
  130.             return new Complex((a.real * b.real + a.img * b.img) * d, (-a.real * b.img + a.img * b.real) * d);
  131.         }
  132.         public static Complex operator /(Complex a, double b)
  133.         {
  134.             return new Complex(a.real / b, a.img / b);
  135.         }
  136.         public static Complex operator /(double a, Complex b)
  137.         {
  138.             double d = 1d / (b.real * b.real + b.img * b.img);
  139.             return new Complex(a * b.real*d, -a * b.img);
  140.         }
  141.  
  142.         #endregion Basic operations + - * /
  143.  
  144.         #region Trigonometic operations
  145.  
  146.         public static Complex Sin(Complex a)
  147.         {
  148.             return new Complex(Math.Sin(a.real)*Math.Cosh(a.img), Math.Cos(a.real) * Math.Sinh(a.img));
  149.         }
  150.         public static Complex Cos(Complex a)
  151.         {
  152.             return new Complex(Math.Cos(a.real) * Math.Cosh(a.img), -Math.Sin(a.real) * Math.Sinh(a.img));
  153.         }
  154.  
  155.         private static double arcosh(double a)
  156.         {
  157.             return Math.Log(a + Math.Sqrt(a*a-1));
  158.         }
  159.         private static double artanh(double a)
  160.         {
  161.             return 0.5 * Math.Log((1+a) / (1-a));
  162.  
  163.         }
  164.  
  165.         public static Complex ASin(Complex a)
  166.         {
  167.             double r2 = a.real * a.real;
  168.             double i2 = a.img * a.img;
  169.             double sMag = r2 + i2;
  170.             double c = Math.Sqrt((sMag-1)*(sMag-1) + 4 * i2);
  171.             double sr = a.real > 0 ? 0.5 : a.real < 0 ? -0.5 : 0;
  172.             double si = a.img > 0 ? 0.5 : a.img < 0 ? -0.5 : 0;
  173.  
  174.             return new Complex(sr*Math.Acos(c - sMag),si*arcosh(c+sMag));
  175.         }
  176.         public static Complex ACos(Complex a)
  177.         {
  178.             return Math.PI * 0.5 - ASin(a);
  179.         }
  180.  
  181.         public static Complex Sinh(Complex a)
  182.         {
  183.             return new Complex(Math.Sinh(a.real) * Math.Cos(a.img), Math.Cosh(a.real) * Math.Sin(a.img));
  184.         }
  185.         public static Complex Cosh(Complex a)
  186.         {
  187.             return new Complex(Math.Cosh(a.real) * Math.Cos(a.img), Math.Sinh(a.real) * Math.Sin(a.img));
  188.         }
  189.         public static Complex Tan(Complex a)
  190.         {
  191.             return new Complex(Math.Sin(2*a.real)/(Math.Cos(2*a.real)+Math.Cosh(2*a.img)), Math.Sinh(2*a.img)/(Math.Cos(2*a.real) + Math.Cosh(2*a.img)));
  192.         }
  193.         public static Complex ATan(Complex a)
  194.         {
  195.             double sMag = a.real * a.real + a.img * a.img;
  196.             double i = 0.5* artanh(2*a.img / (sMag + 1));
  197.             if (a.real == 0)
  198.                 return new Complex(a.img > 1 ? Math.PI * 0.5 : a.img < -1 ? -Math.PI * 0.5 : 0, i);
  199.             double sr = a.real > 0 ? 0.5 : a.real < 0 ? -0.5 : 0;
  200.             return new Complex(0.5 * (Math.Atan((sMag - 1) / (2 * a.real)) + Math.PI * sr), i);
  201.         }
  202.  
  203.         #endregion Trigonometic operations
  204.  
  205.         public static Complex Exp(Complex a)
  206.         {
  207.             double e = Math.Exp(a.real);
  208.             return new Complex(e * Math.Cos(a.img), e * Math.Sin(a.img));
  209.         }
  210.         public static Complex Log(Complex a)
  211.         {
  212.             return new Complex(Math.Log(Math.Sqrt(a.real*a.real + a.img*a.img)), Math.Atan2(a.img, a.real));
  213.         }
  214.         public Complex sqrt(Complex a)
  215.         {
  216.             double r = Math.Sqrt(Math.Sqrt(a.real * a.real + a.img * a.img));
  217.             double halfAngle = 0.5 * Math.Atan2(a.img, a.real);
  218.             return new Complex(r * Math.Cos(halfAngle), r * Math.Sin(halfAngle));
  219.         }
  220.  
  221. #if UNITY
  222.         public static explicit operator UnityEngine.Vector2(Complex a)
  223.         {
  224.             return new UnityEngine.Vector2((float)a.real, (float)a.img);
  225.         }
  226.         public static explicit operator UnityEngine.Vector3(Complex a)
  227.         {
  228.             return new UnityEngine.Vector3((float)a.real, (float)a.img);
  229.         }
  230.         public static explicit operator UnityEngine.Vector4(Complex a)
  231.         {
  232.             return new UnityEngine.Vector4((float)a.real, (float)a.img);
  233.         }
  234.         public static implicit operator Complex(UnityEngine.Vector2 a)
  235.         {
  236.             return new Complex(a.x, a.y);
  237.         }
  238.         public static implicit operator Complex(UnityEngine.Vector3 a)
  239.         {
  240.             return new Complex(a.x, a.y);
  241.         }
  242.         public static implicit operator Complex(UnityEngine.Vector4 a)
  243.         {
  244.             return new Complex(a.x, a.y);
  245.         }
  246. #endif
  247.     }
  248.  
  249.     public class FFT
  250.     {
  251.         // aSamples.Length need to be a power of two
  252.         public static Complex[] CalculateFFT(Complex[] aSamples, bool aReverse)
  253.         {
  254.             int power = (int)Math.Log(aSamples.Length, 2);
  255.             int count = 1;
  256.             for (int i = 0; i < power; i++)
  257.                 count <<= 1;
  258.  
  259.             int mid = count >> 1; // mid = count / 2;
  260.             int j = 0;
  261.             for (int i = 0; i < count - 1; i++)
  262.             {
  263.                 if (i < j)
  264.                 {
  265.                     var tmp = aSamples[i];
  266.                     aSamples[i] = aSamples[j];
  267.                     aSamples[j] = tmp;
  268.                 }
  269.                 int k = mid;
  270.                 while (k <= j)
  271.                 {
  272.                     j -= k;
  273.                     k >>= 1;
  274.                 }
  275.                 j += k;
  276.             }
  277.             Complex r = new Complex(-1, 0);
  278.             int l2 = 1;
  279.             for (int l = 0; l < power; l++)
  280.             {
  281.                 int l1 = l2;
  282.                 l2 <<= 1;
  283.                 Complex r2 = new Complex(1, 0);
  284.                 for (int n = 0; n < l1; n++)
  285.                 {
  286.                     for (int i = n; i < count; i += l2)
  287.                     {
  288.                         int i1 = i + l1;
  289.                         Complex tmp = r2 * aSamples[i1];
  290.                         aSamples[i1] = aSamples[i] - tmp;
  291.                         aSamples[i] += tmp;
  292.                     }
  293.                     r2 = r2 * r;
  294.                 }
  295.                 r.img = Math.Sqrt((1d - r.real) / 2d);
  296.                 if (!aReverse)
  297.                     r.img = -r.img;
  298.                 r.real = Math.Sqrt((1d + r.real) / 2d);
  299.             }
  300.             if (!aReverse)
  301.             {
  302.                 double scale = 1d / count;
  303.                 for (int i = 0; i < count; i++)
  304.                     aSamples[i] *= scale;
  305.             }
  306.             return aSamples;
  307.         }
  308.  
  309.  
  310.         #region float / double array conversion helpers
  311.         public static Complex[] Double2Complex(double[] aData)
  312.         {
  313.             Complex[] data = new Complex[aData.Length];
  314.             for (int i = 0; i < data.Length; i++)
  315.             {
  316.                 data[i] = new Complex(aData[i], 0);
  317.             }
  318.             return data;
  319.         }
  320.         public static double[] Complex2Double(Complex[] aData, bool aReverse)
  321.         {
  322.             double[] result = new double[aData.Length];
  323.             if (!aReverse)
  324.             {
  325.                 for (int i = 0; i < aData.Length; i++)
  326.                 {
  327.                     result[i] = aData[i].magnitude;
  328.                 }
  329.                 return result;
  330.             }
  331.             for (int i = 0; i < aData.Length; i++)
  332.             {
  333.                 result[i] = Math.Sign(aData[i].real) * aData[i].magnitude;
  334.             }
  335.             return result;
  336.         }
  337.  
  338.         public static Complex[] Float2Complex(float[] aData)
  339.         {
  340.             Complex[] data = new Complex[aData.Length];
  341.             for (int i = 0; i < data.Length; i++)
  342.             {
  343.                 data[i] = new Complex(aData[i], 0);
  344.             }
  345.             return data;
  346.         }
  347.         public static float[] Complex2Float(Complex[] aData, bool aReverse)
  348.         {
  349.             float[] result = new float[aData.Length];
  350.             if (!aReverse)
  351.             {
  352.                 for (int i = 0; i < aData.Length; i++)
  353.                 {
  354.                     result[i] = (float)aData[i].magnitude;
  355.                 }
  356.                 return result;
  357.             }
  358.             for (int i = 0; i < aData.Length; i++)
  359.             {
  360.                 result[i] = (float)(Math.Sign(aData[i].real) * aData[i].magnitude);
  361.             }
  362.             return result;
  363.         }
  364.         #endregion float / double array conversion helpers
  365.     }
  366. }
RAW Paste Data