FFT.cpp

00001 /*
00002   @file FFT.cpp
00003   @version: 1.0
00004   @author: Suky
00005   @web www.micros-designs.com.ar
00006   @date 10/02/11
00007 */
00008 #include "FFT.h"
00009 
00010 // Extracted from Numerical Recipes in C
00011 void vFFT(float data[], unsigned int nn){
00012 /*Replaces data[1..2*nn] by its discrete Fourier transform, if isign is input as 1; or replaces
00013 data[1..2*nn] by nn times its inverse discrete Fourier transform, if isign is input as -1.
00014 data is a complex array of length nn or, equivalently, a real array of length 2*nn. nn MUST
00015 be an integer power of 2 (this is not checked for!).*/
00016     unsigned int n,mmax,m,j,istep,i;
00017     double wtemp,wr,wpr,wpi,wi,theta; 
00018     float tempr,tempi;
00019     
00020     #define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr
00021     
00022     n=nn << 1;
00023     j=1;
00024     for (i=1;i<n;i+=2) { 
00025         if(j>i){
00026             SWAP(data[j],data[i]); 
00027             SWAP(data[j+1],data[i+1]);
00028         }
00029         m=n >> 1;
00030         while (m >= 2 &&j>m){
00031             j-=m;
00032             m >>= 1;
00033         }
00034         j+=m;
00035     }
00036     
00037     mmax=2;
00038     while (n > mmax) { 
00039         istep=mmax << 1;
00040         theta=(6.28318530717959/mmax); 
00041         wtemp=sin(0.5*theta);
00042         wpr = -2.0*wtemp*wtemp;
00043         wpi=sin(theta);
00044         wr=1.0;
00045         wi=0.0;
00046         for (m=1;m<mmax;m+=2) { 
00047             for (i=m;i<=n;i+=istep) {
00048                 j=i+mmax; 
00049                 tempr=wr*data[j]-wi*data[j+1];
00050                 tempi=wr*data[j+1]+wi*data[j];
00051                 data[j]=data[i]-tempr;
00052                 data[j+1]=data[i+1]-tempi;
00053                 data[i] += tempr;
00054                 data[i+1] += tempi;
00055             }
00056             wr=(wtemp=wr)*wpr-wi*wpi+wr;
00057             wi=wi*wpr+wtemp*wpi+wi;
00058         }
00059         mmax=istep;
00060     }
00061 }
00062 
00063 // Extracted from Numerical Recipes in C
00064 void vRealFFT(float data[], unsigned int n){
00065 /*Calculates the Fourier transform of a set of n real-valued data points. Replaces this data (which
00066 is stored in array data[1..n]) by the positive frequency half of its complex Fourier transform.
00067 The real-valued rst and last components of the complex transform are returned as elements
00068 data[1] and data[2], respectively. n must be a power of 2. This routine also calculates the
00069 inverse transform of a complex data array if it is the transform of real data. (Result in this case
00070 must be multiplied by 2/n.)*/
00071     unsigned long i,i1,i2,i3,i4,np3;
00072     float c1=0.5,c2,h1r,h1i,h2r,h2i;
00073     double wr,wi,wpr,wpi,wtemp,theta; 
00074     theta=3.141592653589793/(double) (n>>1); 
00075     
00076     c2 = -0.5;
00077     vFFT(data,n>>1); 
00078     wtemp=sin(0.5*theta);
00079     wpr = -2.0*wtemp*wtemp;
00080     wpi=sin(theta);
00081     wr=1.0+wpr;
00082     wi=wpi;
00083     np3=n+3;
00084     for (i=2;i<=(n>>2);i++) {         
00085         i4=1+(i3=np3-(i2=1+(i1=i+i-1)));
00086         h1r=c1*(data[i1]+data[i3]); 
00087         h1i=c1*(data[i2]-data[i4]);
00088         h2r = -c2*(data[i2]+data[i4]);
00089         h2i=c2*(data[i1]-data[i3]);
00090         data[i1]=h1r+wr*h2r-wi*h2i; 
00091         data[i2]=h1i+wr*h2i+wi*h2r;
00092         data[i3]=h1r-wr*h2r+wi*h2i;
00093         data[i4] = -h1i+wr*h2i+wi*h2r;
00094         wr=(wtemp=wr)*wpr-wi*wpi+wr; 
00095         wi=wi*wpr+wtemp*wpi+wi;
00096     }
00097     data[1] = (h1r=data[1])+data[2]; 
00098     data[2] = h1r-data[2];
00099 
00100 }
00101 
00102 
00103 void vCalPowerf(float Input[],float Power[], unsigned int n){
00104     unsigned char k,j;
00105     
00106     for(k=0,j=0;k<n;k++,j+=2){
00107         Power[k]=sqrt(Input[j]*Input[j]+Input[j+1]*Input[j+1]);
00108     }    
00109 }
00110 
00111 void vCalPowerInt(float Input[],unsigned char Power[], unsigned int n){
00112     unsigned char k,j;
00113     
00114     for(k=0,j=0;k<n;k++,j+=2){
00115         Power[k]=sqrt(Input[j]*Input[j]+Input[j+1]*Input[j+1]);
00116     }    
00117 }
00118 
00119 void vCalPowerLog(float Input[],unsigned char Power[], unsigned int n){
00120     unsigned char k,j;
00121     float Temp;
00122     
00123     for(k=0,j=0;k<n;k++,j+=2){
00124         if((Input[j]!=0) && (Input[j+1]!=0)){
00125             Temp=sqrt(Input[j]*Input[j]+Input[j+1]*Input[j+1]);
00126             Power[k]=10.0*log10(Temp);
00127         }else{
00128             Power[k]=0;
00129         }    
00130     }
00131 
00132 }