Thursday, 14 May 2015

Neat and tidy FFT functions

Enjoy :


/* fft.c */

#include <math.h>


void fft_init (double *pFFTCoeffs,
    int FFTSize)

{
    register int i;

                /* Generate Sine and Cos tables */
    for (i = 0; i < ((3 * FFTSize) >> 2); i++)
    {
        *pFFTCoeffs++ = sin ((((double)i) * 6.283185307179586476925286766559) / ((double)FFTSize));
    }

}

void rfft (double RealData[],
    double ImagData[],
    const double *pFFTCoeffs,
    int FFTSize,
    int Log2Size)

{
    register const double   *pFFTSineCoeffs;
    register const double   *pFFTCosineCoeffs;
    register int            i, j, k, Stride, BflyCounter, g, h, HalfFFTSize;
    int                     Angle, AngleInc;    /* Angle step thro sin & cos tables */
    register double         RealTemp, ImagTemp, Cos, Sin;

    pFFTSineCoeffs = pFFTCoeffs;
    pFFTCosineCoeffs = pFFTCoeffs + (FFTSize >> 2);

    HalfFFTSize = FFTSize >> 1;

    Stride = j = HalfFFTSize;
    Angle = h = 0;

                        /* First stage */
    for (BflyCounter = 0; BflyCounter < Stride; BflyCounter++)
    {
        RealTemp = RealData[h] - RealData[j];
        RealData[h] = RealData[h] + RealData[j];
        ImagData[h] = 0.0;      /* Clear imaginary part */
        RealData[j] = pFFTCosineCoeffs[Angle] * RealTemp;
        ImagData[j] = -pFFTSineCoeffs[Angle] * RealTemp;
        Angle++;
        h++;
        j++;
    }

    AngleInc = 2;

    for (i = 1; i < (Log2Size - 1); i++)    /* Middle stages */
    {
        k = Stride;
        Stride >>= 1;
        Angle = 0;
        for (BflyCounter = 0; BflyCounter < Stride; BflyCounter++)
        {
            Cos = pFFTCosineCoeffs[Angle];
            Sin = pFFTSineCoeffs[Angle];
            Angle += AngleInc;

            h = BflyCounter;
            j = h + Stride;

            for (g = k; g <= FFTSize; g += k, h += k, j += k)
            {
                RealTemp = RealData[h] - RealData[j];
                ImagTemp = ImagData[h] - ImagData[j];
                RealData[h] = RealData[h] + RealData[j];
                ImagData[h] = ImagData[h] + ImagData[j];
                RealData[j] = Cos * RealTemp + Sin * ImagTemp;
                ImagData[j] = Cos * ImagTemp - Sin * RealTemp;
            }
        }
        AngleInc <<= 1;
    }

                        /* Final stage */
    for (h = 0, j = 1; h < FFTSize; h += 2, j += 2)
    {
        RealTemp = RealData[h] - RealData[j];
        ImagTemp = ImagData[h] - ImagData[j];
        RealData[h] = RealData[h] + RealData[j];
        ImagData[h] = ImagData[h] + ImagData[j];
        RealData[j] = RealTemp;     /* Cos = 1, sin = 0 */
        ImagData[j] = ImagTemp;
    }

                        /* Reorder scrambled data - slow mode uses less memory */
    for (j = 0, i = 0; j < FFTSize; j++)
    {
        if (j < i)
        {
            RealTemp = RealData[i]; RealData[i] = RealData[j]; RealData[j] = RealTemp;
            ImagTemp = ImagData[i]; ImagData[i] = ImagData[j]; ImagData[j] = ImagTemp;
        }

        k = HalfFFTSize;
        while ((k <= i) && (k >= 1))
        {
            i -= k;
            k >>= 1;
        }
        i += k;
    }

}   /* End of SDA_Rfft() */ 



void cfft (double RealData[],
    double ImagData[],
    const double *pFFTCoeffs,
    int FFTSize,
    int Log2Size)

{
    register int          i, j, k, Stride, BflyCounter, g, h;
    int                   Angle, AngleInc;    /* Angle step thro sin & cos tables */
    register double       ImagTemp, RealTemp, Cos, Sin;
    register const double *pFFTSineCoeffs = pFFTCoeffs;
    register const double *pFFTCosineCoeffs = pFFTCoeffs + (FFTSize >> 2);

    for (i = 0, Stride = FFTSize, AngleInc = 1; i < Log2Size; i++)
    {
        k = Stride;
        Stride >>= 1;
        Angle = 0;
        for (BflyCounter = 0; BflyCounter < Stride; BflyCounter++)
        {
            Cos = pFFTCosineCoeffs[Angle];
            Sin = pFFTSineCoeffs[Angle];
            Angle += AngleInc;

            h = BflyCounter;
            j = h + Stride;

            for (g = k; g <= FFTSize; g += k, h += k, j += k)
            {
                RealTemp = RealData[h] - RealData[j];
                ImagTemp = ImagData[h] - ImagData[j];
                RealData[h] = RealData[h] + RealData[j];
                ImagData[h] = ImagData[h] + ImagData[j];
                RealData[j] = Cos * RealTemp + Sin * ImagTemp;
                ImagData[j] = Cos * ImagTemp - Sin * RealTemp;
            }
        }
        AngleInc <<= 1;
    }
    
                        /* Reorder scrambled data - slow mode uses less memory */
    for (j = 0, i = 0; j < FFTSize; j++)
    {
        if (j < i)
        {
            RealTemp = RealData[i]; RealData[i] = RealData[j]; RealData[j] = RealTemp;
            ImagTemp = ImagData[i]; ImagData[i] = ImagData[j]; ImagData[j] = ImagTemp;
        }

        k = FFTSize >> 1;
        while ((k <= i) && (k >= 1))
        {
            i -= k;
            k >>= 1;
        }
        i += k;
    }
}

These functions, and more, are available in the SigLib DSP Library.

No comments:

Post a Comment