FFT.c

   1 #include <math.h>
   2 #include <m68881.h>
   3
   4
   5
   6
   7 static void FFT (float data[], LONG nn, LONG isign);
   8 static void RealFFT (float data[], LONG n, LONG isign);
   9 static void MyFFT (double *real_vet, double *im_vet, long n, int m, int ind, double cf);
  10
  11
  12
  13 #ifdef __SASC
  14
  15 /* Disable SAS/C floating point error handler */
  16 void __stdargs _CXFERR(int code)
  17 {
  18 }
  19
  20 #endif /* __SASC */
  21
  22 #if 0
  23 GLOBALCALL void Filter (WORD *data, LONG len)
  24 {
  25         float *fftdata;
  26         LONG i, nn;
  27
  28
  29         /* Calculate nearest power of 2 */
  30         for (i = len-1, nn = 1; i != 0; i >>= 1)
  31                 nn <<= 1;
  32
  33         if (!(fftdata = AllocVec (sizeof (float) * nn * 2, MEMF_CLEAR)))
  34                 return;
  35
  36         /* Fill array of complex numbers (imaginary part is always set to 0) */
  37         for (i = 0; i < len; i++)
  38                 fftdata[i] = (float) data[i];
  39
  40
  41         /* Obtain frequency spectrum */
  42         RealFFT (fftdata, nn>>1, 1);
  43
  44         /* Low-pass Filter */
  45         for (i = 0; i < nn; i++)
  46                 fftdata[i*2] *= (i/nn) * 4;
  47
  48         /* Return to time domain */
  49         RealFFT (fftdata, nn>>1, -1);
  50         for (i = 0; i < len; i++)
  51                 fftdata[i] /= nn;
  52
  53         /* Put filtered data */
  54         for (i = 0; i < len; i++)
  55                 data[i] = (WORD) fftdata[i];
  56
  57         FreeVec (fftdata);
  58 }
  59 #endif
  60
  61
  62 GLOBALCALL void Filter (WORD *data, LONG len)
  63 {
  64         double *fftdata;
  65         LONG i, nn, mm;
  66
  67
  68         /* Calculate nearest power of 2 */
  69         for (i = len-1, nn = 1, mm = 0; i != 0; i >>= 1)
  70         {
  71                 nn <<= 1;
  72                 mm++;
  73         }
  74
  75         if (!(fftdata = AllocVec (sizeof (double) * nn * 2, MEMF_CLEAR)))
  76                 return;
  77
  78         /* Fill array of complex numbers (imaginary part is always set to 0) */
  79         for (i = 0; i < len; i++)
  80                 fftdata[i] = (float) data[i];
  81
  82
  83         /* Obtain frequency spectrum */
  84         MyFFT (fftdata, fftdata + nn, nn>>1, mm, +1, 1.0/32768.0);
  85
  86         /* Low-pass Filter */
  87 //      for (i = 0; i < nn; i++)
  88 //              fftdata[i] *= (i/nn) * 4;
  89
  90         /* Return to time domain */
  91         MyFFT (fftdata, fftdata + nn, nn>>1, mm, -1, 1.0);
  92
  93 //      for (i = 0; i < len; i++)
  94 //              fftdata[i] /= nn;
  95
  96         /* Put filtered data */
  97         for (i = 0; i < len; i++)
  98                 data[i] = (WORD) fftdata[i];
  99
 100         FreeVec (fftdata);
 101 }
 102
 103
 104 #if 0
 105 #define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr
 106
 107 static void FFT (float data[], LONG nn, LONG isign)
 108 {
 109         int n,mmax,m,j,istep,i;
 110         double wtemp,wr,wpr,wpi,wi,theta;
 111         float tempr,tempi;
 112
 113         n=nn << 1;
 114         j=1;
 115         for (i=1;i<n;i+=2)
 116         {
 117                 if (j > i)
 118                 {
 119                         SWAP(data[j],data[i]);
 120                         SWAP(data[j+1],data[i+1]);
 121                 }
 122                 m=n >> 1;
 123                 while (m >= 2 && j > m)
 124                 {
 125                         j -= m;
 126                         m >>= 1;
 127                 }
 128                 j += m;
 129         }
 130         mmax = 2;
 131         while (n > mmax)
 132         {
 133                 istep=2*mmax;
 134                 theta=6.28318530717959/(isign*mmax);
 135                 wtemp=sin(0.5*theta);
 136                 wpr = -2.0*wtemp*wtemp;
 137                 wpi=sin(theta);
 138                 wr=1.0;
 139                 wi=0.0;
 140                 for (m=1;m<mmax;m+=2)
 141                 {
 142                         for (i=m;i<=n;i+=istep)
 143                         {
 144                                 j = i+mmax;
 145                                 tempr=wr*data[j]-wi*data[j+1];
 146                                 tempi=wr*data[j+1]+wi*data[j];
 147                                 data[j]=data[i]-tempr;
 148                                 data[j+1]=data[i+1]-tempi;
 149                                 data[i] += tempr;
 150                                 data[i+1] += tempi;
 151                         }
 152                         wr=(wtemp=wr)*wpr-wi*wpi+wr;
 153                         wi=wi*wpr+wtemp*wpi+wi;
 154                 }
 155                 mmax=istep;
 156         }
 157 }
 158
 159
 160 static void RealFFT (float data[], LONG n, LONG isign)
 161 {
 162         int i,i1,i2,i3,i4,n2p3;
 163         float c1=0.5,c2,h1r,h1i,h2r,h2i;
 164         double wr,wi,wpr,wpi,wtemp,theta;
 165         void four1();
 166
 167         theta=3.141592653589793/(double) n;
 168         if (isign == 1)
 169         {
 170                 c2 = -0.5;
 171                 FFT (data,n,1);
 172         }
 173         else
 174         {
 175                 c2=0.5;
 176                 theta = -theta;
 177         }
 178
 179         wtemp=sin(0.5*theta);
 180         wpr = -2.0*wtemp*wtemp;
 181         wpi=sin(theta);
 182         wr=1.0+wpr;
 183         wi=wpi;
 184         n2p3=2*n+3;
 185         for (i=2;i<=n/2;i++)
 186         {
 187                 i4=1+(i3=n2p3-(i2=1+(i1=i+i-1)));
 188                 h1r=c1*(data[i1]+data[i3]);
 189                 h1i=c1*(data[i2]-data[i4]);
 190                 h2r = -c2*(data[i2]+data[i4]);
 191                 h2i=c2*(data[i1]-data[i3]);
 192                 data[i1]=h1r+wr*h2r-wi*h2i;
 193                 data[i2]=h1i+wr*h2i+wi*h2r;
 194                 data[i3]=h1r-wr*h2r+wi*h2i;
 195                 data[i4] = -h1i+wr*h2i+wi*h2r;
 196                 wr=(wtemp=wr)*wpr-wi*wpi+wr;
 197                 wi=wi*wpr+wtemp*wpi+wi;
 198         }
 199         if (isign == 1)
 200         {
 201                 data[1] = (h1r=data[1])+data[2];
 202                 data[2] = h1r-data[2];
 203         }
 204         else
 205         {
 206                 data[1]=c1*((h1r=data[1])+data[2]);
 207                 data[2]=c1*(h1r-data[2]);
 208                 FFT (data,n,-1);
 209         }
 210 }
 211
 212
 213 #undef SWAP
 214 #endif
 215
 216 static void MyFFT (double *real_vet, double *im_vet, long n, int m, int ind, double cf)
 217 {
 218         long l, i, j, nbut, ngrup, nc;
 219         long but, grup;
 220         long primo, secondo;
 221         double w_real, w_im;
 222         double t_real, t_im;
 223         double arg;
 224
 225         // Ordinamento binario del vettore
 226         j = 0;
 227         nc = n >> 1;
 228         for (i = 0 ; i< n-1; i++)
 229         {
 230                 if (i < j)
 231                 {
 232                         t_real = real_vet[j];
 233                         t_im = im_vet[j];
 234                         real_vet[j] = real_vet[i];
 235                         im_vet[j] = im_vet[i];
 236                         real_vet[i] = t_real;
 237                         im_vet[i] = t_im;
 238                 }
 239                 l = nc;
 240                 while(l <= j)
 241                 {
 242                         j -= l;
 243                         l >>= 1;
 244                 }
 245                 j+=l;
 246         }
 247
 248         //Calcolo FFT
 249         for(l=0; l<m; l++)                                              //Ciclo esterno Log2(N)
 250         {
 251                 nbut = (1L) << l;
 252                 ngrup = nc / nbut;
 253                 for(but = 0; but < nbut; but++)                         //Ciclo medio: numero di farfalle
 254                 {
 255                         arg = PI * but / nbut;                  // M_PI e' la costante PI Greco
 256                         w_real = cos(arg);
 257                         w_im = (-1)*ind*sin(arg);
 258                         for(grup = 0; grup < ngrup; grup++)             //Ciclo interno: numero di gruppi
 259                         {
 260                                 primo = 2*nbut*grup + but;
 261                                 secondo = primo + nbut;
 262                                 t_real = w_real * real_vet[secondo] - w_im * im_vet[secondo];
 263                                 t_im = w_real * im_vet[secondo] + w_im * real_vet[secondo];
 264                                 real_vet[secondo] = real_vet[primo] - t_real;
 265                                 im_vet[secondo] = im_vet[primo] - t_im;
 266                                 real_vet[primo] += t_real;
 267                                 im_vet[primo] += t_im;
 268                         }
 269                 }
 270         }
 271
 272         //Normalizzazione
 273         if (cf != 1.0)                          // Modifica di Bernardo
 274                 for(i = 0; i < n; i++)
 275                 {
 276                         real_vet[i] *= cf;
 277                         im_vet[i] *= cf;
 278                 }
 279 }