fft.cpp

fft变换算法的c++源代码

//---------------------------------------------------------------------------

#include <vcl.h>
#pragma hdrstop

#include "FFT.h"
#pragma package(smart_init)
//---------------------------------------------------------------------------
void __fastcall TFFT::Factorize(int Count,int &tFactorCount,TIdx1FactorArray &tFact)
{
  int i;
  TIdx1FactorArray Factors;

  // Define specific FFT lengths (radices) that we can process with optimised routines
  const int cRadixCount = 6;
  const int cRadices[6] = {2, 3, 4, 5, 8, 10};
  int FactorCount;
  int Fact[cMaxFactorCount];
  if (Count == 1)
  {
    FactorCount = 1;
    Factors[1]  = 1;
  }
  else
  {
    FactorCount = 0;
  }

  // Factorise the original series length Count into known factors and rest value
  i = cRadixCount-1;
  while ((Count > 1) && (i > 0))
  {
    if(Count % cRadices[i] == 0)
    {
      Count = Count / cRadices[i];
      FactorCount++;
      Factors[FactorCount] = cRadices[i];
    }
    else
      i--;
   }

  // substitute factors 2*8 with more optimal 4*4
  if (Factors[FactorCount] == 2)
  {
    i = FactorCount - 1;
    while ((i > 0) && (Factors[i] != 8))
      i--;
    if(i > 0)
    {
      Factors[FactorCount] = 4;
      Factors[i] = 4;
    }
    }


  // Analyse the rest value and see if it can be factored in primes
  if(Count > 1)
  {
    for(i= 2;i<=(int)(sqrt(Count));i++)
    {
      while(Count%i == 0)
      {
        Count = Count / i;
        FactorCount++;
        Factors[FactorCount] = i;
      }
     }

    if (Count > 1)
    {
      FactorCount++;
      Factors[FactorCount]= Count;
    }
  }
  // Reverse factors so that primes are first
  tFactorCount=FactorCount;
  //tFact=new int[cMaxFactorCount];
  for(i= 1;i<=FactorCount;i++)
    tFact[i] = Factors[FactorCount - i +1]; //
}
void __fastcall TFFT::ReorderSeries(int Count,int Factors[cMaxFactorCount],int Remain[cMaxFactorCount],const complex<double> *X, complex<double> *Y)
{
 int i, j, k;
 int Counts[cMaxFactorCount];

 for(i=0;i<cMaxFactorCount;i++)
  Counts[i]=0;

  k = 0;
  for(i= 0;i<=Count - 2;i++)
  {
    Y[i] = X[k];
    j = 1;
    k = k + Remain[j];
    Counts[1] = Counts[1] + 1;
    while(Counts[j] >= Factors[j])
    {
      Counts[j] = 0;
      k = k - Remain[j - 1] + Remain[j + 1];
      j++;
      Counts[j]++;
      }
    }
  Y[Count - 1] = X[Count - 1];
}
void __fastcall TFFT::FFT_2(complex<double>*Z)
{
complex <double>T1;
  T1   = Z[0]+Z[1];
  Z[1] = Z[0]-Z[1];
  Z[0] = T1;
}
void __fastcall TFFT::FFT_3(complex<double>*Z)
{
  complex <double>T1, M1, M2, S1;
  T1   = Z[1]+Z[2];
  Z[0] = Z[0]+T1;
  M1   = c31*T1;
  M2=complex<double>(c32 * (Z[1].imag () - Z[2].imag ()),c32 * (Z[2].real() - Z[1].real()));
  //M2.real = ;
  //M2.Im = ;
  S1   = Z[0]+M1;
  Z[1] = S1+ M2;
  Z[2] = S1-M2;
}
void __fastcall TFFT::FFT_4(complex<double>*Z)
{
  complex <double>T1, T2, M2, M3;

  T1 = (Z[0]+Z[2]);
  T2 = (Z[1]+Z[3]);

  M2 = (Z[0]-Z[2]);
  M3=complex<double>(c32 * (Z[1].imag () - Z[3].imag ()),c32 * (Z[3].real() - Z[1].real()));
  //M3.Re := Z[1].Im - Z[3].Im;
  //M3.Im := Z[3].Re - Z[1].Re;

  Z[0] = (T1+T2);
  Z[2] = (T1-T2);
  Z[1] = (M2+M3);
  Z[3] = (M2-M3);
}
void __fastcall TFFT::FFT_5(complex<double>*Z)
{
  complex <double>T1, T2, T3, T4, T5;
  complex <double>M1, M2, M3, M4, M5;
  complex <double>S1, S2, S3, S4, S5;

  T1 = (Z[1]+Z[4]);
  T2 = (Z[2]+ Z[3]);
  T3 = (Z[1]- Z[4]);
  T4 = (Z[3]- Z[2]);

  T5   = (T1+ T2);
  Z[0] = (Z[0]+ T5);
  M1   = (c51* T5);
  M2   = (c52* (T1- T2));
  M3=complex<double>(-c53 * (T3.imag () - T4.imag ()),c53 * (T3.real() - T4.real()));
  M4=complex<double>(-c54 * T4.imag (),c54 * T4.real());
  M5=complex<double>(-c55 * T3.imag (),c55 * T3.real());
  //M3.Re := -c53 * (T3.Im + T4.Im);
  //M3.Im :=  c53 * (T3.Re + T4.Re);
  //M4.Re := -c54 * T4.Im;
  //M4.Im :=  c54 * T4.Re;
  //M5.Re := -c55 * T3.Im;
  //M5.Im :=  c55 * T3.Re;

  S3 = (M3- M4);
  S5 = (M3+ M5);;
  S1 = (Z[0]+ M1);
  S2 = (S1+ M2);
  S4 = (S1- M2);

  Z[1] = (S2+ S3);
  Z[2] = (S4+ S5);
  Z[3] = (S4- S5);
  Z[4] = (S2- S3);
}
void __fastcall TFFT::FFT_8(complex<double>*Z)
{
complex<double> A[4], B[4];
  float Gem;

  A[0] = Z[0]; B[0] = Z[1];
  A[1] = Z[2]; B[1] = Z[3];
  A[2] = Z[4]; B[2] = Z[5];
  A[3] = Z[6]; B[3] = Z[7];

  FFT_4(A);
  FFT_4(B);

  Gem     = c8 * (B[1].real() + B[1].imag ());
   B[1]=complex<double>(Gem,c8 * (B[1].imag () - B[1].real()));
  //B[1].Im := ;
  //B[1].Re := ;
  Gem     = B[2].imag ();
  B[2]=complex<double>(Gem,-B[2].real());
  //B[2].Im :=-B[2].real();
  //B[2].Re := Gem;
  Gem     = c8 * (B[3].imag () - B[3].real());
   B[3]=complex<double>(Gem,-c8 * (B[3].real() + B[3].imag ()));
  //B[3].Im :=-c8 * (B[3].real() + B[3].imag ());
  //B[3].Re := Gem;

  Z[0] = (A[0]+ B[0]); Z[4] = (A[0]- B[0]);
  Z[1] = (A[1]+ B[1]); Z[5] = (A[1]- B[1]);
  Z[2] = (A[2]+ B[2]); Z[6] = (A[2]- B[2]);
  Z[3] = (A[3]+ B[3]); Z[7] = (A[3]- B[3]);
}
void __fastcall TFFT::FFT_10(complex<double>*Z)
{
complex <double>A[4], B[4];
   A[0] = Z[0];  B[0] = Z[5];
   A[1] = Z[2];  B[1] = Z[7];
   A[2] = Z[4];  B[2] = Z[9];
   A[3] = Z[6];  B[3] = Z[1];
   A[4] = Z[8];  B[4] = Z[3];

   FFT_5(A);
   FFT_5(B);

   Z[0] = (A[0]+B[0]); Z[5] = (A[0]- B[0]);
   Z[6] = (A[1]+B[1]); Z[1] = (A[1]- B[1]);
   Z[2] = (A[2]+B[2]); Z[7] = (A[2]- B[2]);
   Z[8] = (A[3]+B[3]); Z[3] = (A[3]- B[3]);
   Z[4] = (A[4]+B[4]); Z[9] = (A[4]- B[4]);
}
//complex <double>Synth[cMaxPrimeFactor], Trig[cMaxPrimeFactor], Z[cMaxPrimeFactor];
void __fastcall TFFT::SynthesizeFFT(int Sofar, int Radix, int Remain,complex<double> *Y)
{
int GroupOffset, DataOffset, Position;
int GroupNo, DataNo, BlockNo, SynthNo;
double  Omega;
  complex <double>S, CosSin;
ComplexDoubleArray Synth, Trig, Z;
//  complex <double>Synth[cMaxPrimeFactor], *Trig, Z[cMaxPrimeFactor];
// Trig=new complex <double>[cMaxPrimeFactor];
 InitializeTrigonomials(Radix,Trig);

  Omega       = 2 * pi / (Sofar * Radix);
  CosSin      = complex <double>(cos(Omega), -sin(Omega));
  S           = complex <double>(1.0, 0.0);
  DataOffset  = 0;
  GroupOffset = 0;
  Position    = 0;

  for(DataNo= 0;DataNo<Sofar;DataNo++)
  {
    if(Sofar>1)
    {
      Synth[0] = complex <double>(1.0, 0.0);
      Synth[1] = S;
      for (SynthNo = 2; SynthNo<Radix;SynthNo++)
        Synth[SynthNo] = (S* Synth[SynthNo - 1]);
      S = CosSin* S;
      }
    for(GroupNo= 0;GroupNo< Remain;GroupNo++)
    {

      if ((Sofar > 1) && (DataNo > 0))
      {

        Z[0]    = Y[Position];
        BlockNo = 1;
        do{
          Position+=Sofar;
          Z[BlockNo] = (Synth[BlockNo]*Y[Position]);
          BlockNo++;
          }
        while(BlockNo < Radix);

      }else
      {
        for(BlockNo= 0;BlockNo<Radix;BlockNo++)
        {
          Z[BlockNo] = Y[Position];
          Position+=Sofar;
          }
          }
      switch(Radix)
      {
      case 2:  FFT_2(Z);break;
      case 3:  FFT_3(Z);break;
      case 4:  FFT_4(Z);break;
      case 5:  FFT_5(Z);break;
      case 8:  FFT_8(Z);break;
      case 10: FFT_10(Z);break;
      default: // Any larger prime number than 5 (so 7, 11, 13, etc, up to cMaxPrimeFactor)
        FFT_Prime(Radix,Trig,Z); break;
      } //case

      Position = GroupOffset;
      for (BlockNo = 0;BlockNo<Radix;BlockNo++)
      {
        Y[Position] = Z[BlockNo];
        Position+=Sofar;
        }

      GroupOffset = GroupOffset + Sofar * Radix;
      Position    = GroupOffset;
    }
    DataOffset++;
    GroupOffset = DataOffset;
    Position    = DataOffset;
  }
}
void __fastcall TFFT::InitializeTrigonomials(int Radix,ComplexDoubleArray&Trig)
{
    int i;
    double W;
    complex <double>X;

    W = 2 * pi / Radix;
    Trig[0] = complex <double>(1.0, 0.0);
    X = complex <double>(cos(W), -sin(W));
    Trig[1] = X;
    for (i = 2;i<Radix;i++)
      Trig[i] = (X*Trig[i - 1]);
}
void __fastcall TFFT::FFT_Prime(int Radix,ComplexDoubleArray&Trig,ComplexDoubleArray&Z)
{
    int i, j, k, N, AMax;
    complex <double>Re, Im;
    complex <double>V[cMaxPrimeFactorDiv2], W[cMaxPrimeFactorDiv2];

    N = Radix;
    AMax = (N + 1) / 2;
    for(j = 1;j<AMax;j++)
    {
      V[j]=complex <double>(Z[j].real() + Z[N-j].real(),Z[j].imag() - Z[N-j].imag());
      //V[j].Re := Z[j].Re + Z[n-j].Re;
      //V[j].Im := Z[j].Im - Z[n-j].Im;
      W[j]=complex <double>(Z[j].real() - Z[N-j].real(),Z[j].imag() + Z[N-j].imag());
      //W[j].Re := Z[j].Re - Z[n-j].Re;
      //W[j].Im := Z[j].Im + Z[n-j].Im;
    }

    for(j= 1;j<AMax;j++)
    {
      Z[j]   = Z[0];
      Z[N-j] = Z[0];
      k = j;
      for(i= 1;i<AMax;i++)
      {
        Re=complex<double>(Trig[k].real()*V[i].real(),Trig[k].real()*W[i].imag());
        Im=complex<double>(Trig[k].imag()*W[i].real(),Trig[k].imag()*V[i].imag());
        //Re.Re := Trig[k].Re * V[i].Re;
        //Im.Im := Trig[k].Im * V[i].Im;
        //Re.im := Trig[k].Re * W[i].Im;
        //Im.Re := Trig[k].Im * W[i].Re;
        Z[N-j]=complex<double>(Z[N-j].real() + Re.real() + Im.imag(),Z[N-j].imag() + Re.imag() - Im.real());
        //Z[N-j].Re := Z[N-j].Re + Re.Re + Im.Im;
        //Z[N-j].Im := Z[N-j].Im + Re.Im - Im.Re;
         Z[j]=complex<double>(Z[j].real() + Re.real() - Im.imag(),Z[j].imag() + Re.imag() + Im.real());
        //Z[j].Re   := Z[j].Re   + Re.Re - Im.Im;
        //Z[j].Im   := Z[j].Im   + Re.Im + Im.Re;

        k = k + j;
        if(k >= N)
          k = k - N;
      }
      }
    for(j= 1;j<AMax;j++)
    {
      Z[0]=complex<double>(Z[0].real() + V[j].real(),Z[0].imag() + W[j].imag());
      }
}
void __fastcall TFFT::ForwardFFT(const complex<double> *Source, complex<double> *Dest,int Count)
{
//  PComplexArray = ^TComplexArray;
//  TComplexArray = array[0..0] of TComplex;
 int i;
 int FactorCount;
 TIdx1FactorArray SofarRadix;
 TIdx1FactorArray ActualRadix;
 TIdx0FactorArray RemainRadix;
 complex<double> *TmpDest;

  if(Count == 0)return;
  //ActualRadix=new int[cMaxFactorCount+1];
  // Decompose the series with length Count into FactorCount factors in ActualRadix
  Factorize(Count, FactorCount, ActualRadix);

  // Check if our biggest prime factor is not too large
//  if (ActualRadix[1] > cMaxPrimeFactor)
//    raise EMathError.Create(sErrPrimeTooLarge);

  // Setup Sofar and Remain tables
  RemainRadix[0] = Count;
  SofarRadix[1]  = 1;
   SofarRadix[1]  = 1;
  RemainRadix[1] = Count / ActualRadix[1];
  for(i= 2;i<=FactorCount;i++)
  {
    SofarRadix[i]  = SofarRadix[i-1] * ActualRadix[i-1];
    RemainRadix[i] = RemainRadix[i-1] / ActualRadix[i];
  }

  // Make temp copy if dest = source (otherwise the permute procedure will completely
  // ruin the structure
//  if @Dest = @Source then begin
//    GetMem(TmpDest, SizeOf(TComplex) * Count);;
//    Move(Dest, TmpDest^, SizeOf(TComplex) * Count);
//  end else begin
//    TmpDest := @Dest;
//  end;
    if(&Source==&Dest)
    {
      TmpDest=new complex<double>[Count];
      for(i==0;i<Count;i++)
        *(TmpDest+i)=*(Dest+i);
      }
    else
      TmpDest=Dest;

  // Reorder the series so that the elements are already in the right place for
  // synthesis    {, FactorCount}
  ReorderSeries(Count, ActualRadix, RemainRadix, Source, TmpDest);

  // Free the temporary copy (if any)
//  if @Dest = @Source then begin
//    Move(TmpDest^, Dest, SizeOf(TComplex) * Count);
//    FreeMem(TmpDest);
//  end;
  if(&Dest==&Source)
  {
  for(i=0;i<Count;i++)
    *(Dest+i)=*(TmpDest+i);
  delete [] TmpDest;
  }
  // Synthesize each of the FFT factored series
  for(i= 1;i<=FactorCount;i++)
  {
    SynthesizeFFT(SofarRadix[i], ActualRadix[i], RemainRadix[i], Dest);
   }
}
void __fastcall TFFT::InverseFFT(const complex<double> *Source, complex<double> *Dest,int Count)
{
   int i;
  float S;
  complex<double> *TmpSource;

  if(Count == 0) return;

  // Since TmpSource is local, we do not have to free it again,
  // it will be freed automatically when out of scope
  //SetLength(TmpSource, Count);
  TmpSource=new complex<double>[Count];
  // Create a copy with inverted imaginary part.
  for(i= 0;i<Count;i++)
      TmpSource[i]= complex<double>(Source[i].real(), -Source[i].imag());
  ForwardFFT(TmpSource, Dest, Count);

  // Scale by 1/Count, and inverse the imaginary part
  S = 1.0 / Count;
  for(i= 0;i<Count;i++)
  {
    Dest[i]= complex<double>(S * Dest[i].real(),- S * Dest[i].imag());
    //Dest[i].Re :=   S * Dest[i].Re;
    //Dest[i].Im := - S * Dest[i].Im;
  }
}