home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 7 / 07.iso / c / c129 / 1.ddi / FFT.C < prev    next >
Encoding:
C/C++ Source or Header  |  1990-02-26  |  7.9 KB  |  362 lines
  1. # include <stdio.h>
  2. # include <stdlib.h>
  3. # include <math.h>
  4. # include "miscio.h"
  5.  
  6.  
  7. float Parzen(float n,float j)
  8. {
  9.    float fret;
  10.  
  11.    fret = 1.0 - fabs((j - 0.5 * (n - 1.0)) / (0.5 * (n - 1.0)));
  12.    return(fret);
  13. }
  14.  
  15.  
  16. float Hanning(float n,float j)
  17. {
  18.    float fret;
  19.  
  20.    fret = 0.5 * (1.0 - cos(2.0 * M_PI * j / (n-1.0)));
  21.    return(fret);
  22. }
  23.  
  24. float Hamming(float n,float j)
  25. {
  26.    float fret;
  27.  
  28.    fret = 0.54 - 0.46 * cos(2.0 * M_PI * j / (n-1.0));
  29.    return(fret);
  30. }
  31.  
  32. float ExactBlackman(float n,float j)
  33. {
  34.    float fret;
  35.  
  36.    fret = 0.42 - 0.50 * cos(2.0 * M_PI * j /(n-1.0)) + 0.08 * cos(4.0 * M_PI * j / (n-1.0));
  37.    return(fret);
  38. }
  39.  
  40. float Welch(float n,float j)
  41. {
  42.    float fret;
  43.     fret = (j - 0.5 * (n - 1.0)) / (0.5 * (n + 1.0));
  44.    fret = 1 - fret*fret;
  45.    return(fret);
  46. }
  47.  
  48. void WindowData(float *x,int numdat,int window)
  49. {
  50.    int i;
  51.    float multiplier;
  52.  
  53.    for ( i = 0; i <= numdat - 1; ++i ) {
  54.       switch ( window ) {
  55.          case 0:
  56.             multiplier = 1.0;
  57.             break;
  58.          case 1:
  59.             multiplier = Parzen(numdat,i);
  60.             break;
  61.          case 2:
  62.             multiplier = Hanning(numdat,i);
  63.             break;
  64.          case 3:
  65.             multiplier = Welch(numdat,i);
  66.             break;
  67.          case 4:
  68.             multiplier = Hamming(numdat,i);
  69.             break;
  70.          case 5:
  71.             multiplier = ExactBlackman(numdat,i);
  72.             break;
  73.          default:
  74.             multiplier = 1.0;
  75.             break;
  76.       }
  77.       x[i] = multiplier * x[i];
  78.    }
  79. }
  80.  
  81. void WindowFFTData(float *xdata, float *ydata, int numdat, int window)
  82. {
  83.    WindowData(xdata,numdat, window);
  84.    WindowData(xdata,numdat, window );
  85. }
  86.  
  87.  
  88. void FFT_swap(float *s1, float *s2)
  89. {
  90.    float temp;
  91.  
  92.    temp = (*s1);
  93.    (*s1) = (*s2);
  94.    (*s2) = temp;
  95. }
  96.  
  97. void fft(float *xreal, float *yimag, int numdat, int flag)
  98. {
  99.    int maxpower, arg, q, cntr, pnt0, pnt1, i;
  100.    int j, a, b, k, m;
  101.    float sign, prodreal, prodimag, harm, x, y;
  102.    float *cosary;
  103.    float *sinary;
  104.  
  105.    cosary = (float *) calloc(numdat,sizeof(float));
  106.    if (cosary == NULL)
  107.      exit(1);
  108.    sinary = (float *) calloc(numdat,sizeof(float));
  109.    if (sinary == NULL)
  110.      exit(1);
  111.    j = 0;
  112.    if ( flag != 0 ) {
  113.       sign = 1.0;
  114.       for ( i = 0; i <= numdat - 1; ++i ) {
  115.          xreal[i] = xreal[i] / numdat;
  116.          yimag[i] = yimag[i] / numdat;
  117.       }
  118.    }
  119.    else {
  120.       sign = -1.0;
  121.    }
  122.    for ( i = 0; i <= numdat - 2; ++i ) {
  123.       if ( i < j ) {
  124.          FFT_swap(&xreal[i],&xreal[j]);
  125.          FFT_swap(&yimag[i],&yimag[j]);
  126.       }
  127.       k = numdat / 2;
  128.       while ( k <= j ) {
  129.          j = j - k;
  130.          k = k / 2;
  131.       }
  132.       j = j + k;
  133.    }
  134.    maxpower = 0;
  135.    i = numdat;
  136.    while ( i != 1 ) {
  137.       maxpower = maxpower + 1;
  138.       i = i / 2;
  139.    }
  140.    harm = 2 * M_PI / numdat;
  141.    for ( i = 0; i <= numdat - 1; ++i ) {
  142.       sinary[i] = sign * sin(harm * i);
  143.       cosary[i] = cos(harm * i);
  144.    }
  145.    a = 2;
  146.    b = 1;
  147.    for ( cntr = 1; cntr <= maxpower; ++cntr ) {
  148.       pnt0 = numdat / a;
  149.       pnt1 = 0;
  150.       for ( k = 0; k <= b - 1; ++k ) {
  151.          i = k;
  152.          while ( i < numdat ) {
  153.             arg = i + b;
  154.             if ( k == 0 ) {
  155.                prodreal = xreal[arg];
  156.                prodimag = yimag[arg];
  157.             }
  158.             else {
  159.                prodreal = xreal[arg] * cosary[pnt1] - yimag[arg] * sinary[pnt1];
  160.                prodimag = xreal[arg] * sinary[pnt1] + yimag[arg] * cosary[pnt1];
  161.             }
  162.             xreal[arg] = xreal[i] - prodreal;
  163.             yimag[arg] = yimag[i] - prodimag;
  164.             xreal[i] = xreal[i] + prodreal;
  165.             yimag[i] = yimag[i] + prodimag;
  166.             i = i + a;
  167.          }
  168.          pnt1 = pnt1 + pnt0;
  169.       }
  170.       a = 2 * a;
  171.       b = b * 2;
  172.    }
  173.    free(cosary);
  174.    free(sinary);
  175. }
  176.  
  177. void FFTCalc(float *xreal,float *yimag,int numdat)
  178. {
  179.  
  180.    fft(xreal,yimag,numdat,0);
  181. }
  182.  
  183. void FFTInvCalc(float *xreal,float *yimag, int numdat)
  184. {
  185.  
  186.    fft(xreal,yimag,numdat,1);
  187. }
  188.  
  189. float SquareAndSum(float a,float b)
  190. {
  191.    float fret;
  192.  
  193.    fret = a*a+b*b;
  194.    return(fret);
  195. }
  196.  
  197. void PowerSpectrumCalc(float *xreal, float *yimag,int numdat,float delta)
  198. {
  199.    float timespan;
  200.    float normal;
  201.    int i;
  202.    float n;
  203.    char c;
  204.  
  205.    n = numdat;
  206.    normal = 1.0 / (n*n);
  207.    fft(xreal,yimag,numdat,0);
  208.    xreal[0] = SquareAndSum(xreal[0],yimag[0]) * normal;
  209.  
  210.    for ( i = 1; i <= (numdat / 2) - 1; ++i ) {
  211.       xreal[i] = normal * (SquareAndSum(xreal[i],yimag[i]) +
  212.         SquareAndSum(xreal[numdat - i],yimag[numdat - i]));
  213.  
  214.    }
  215.    i = numdat / 2;
  216.    xreal[i] = SquareAndSum(xreal[i],yimag[i]) * normal;
  217.    timespan = (1.0*numdat) * delta;
  218.    for ( i = 0; i <= (numdat/2); ++i ) {
  219.       yimag[i] = (1.0*i) / timespan;
  220.    }
  221. }
  222.  
  223. void FFT2DCalc(float *xreal,float *yimag,
  224.                int c,int r,int flag)
  225. {
  226.    int a, b, i, j, jc;
  227.    float *yVector;
  228.    float *xVector;
  229.  
  230.    xVector = (float *) calloc(r,sizeof(float));
  231.    if (xVector == NULL)
  232.     exit(1);
  233.    yVector = (float *) calloc(r,sizeof(float));
  234.    if (yVector == NULL)
  235.     exit(1);
  236.    for ( j = 0; j <= r - 1; ++j ) {
  237.       jc = j * c;
  238.       for ( a = 0; a <= c - 1; ++a ) {
  239.      xVector[a] = xreal[jc+a];
  240.      yVector[a] = yimag[jc+a];
  241.       }
  242.       if ( flag == 0 )
  243.          FFTCalc(xVector,yVector,c);
  244.       else
  245.          FFTInvCalc(xVector,yVector,c);
  246.       for ( a = 0; a <= c - 1; ++a ) {
  247.      xreal[jc+a] = xVector[a];
  248.      yimag[jc+a] = yVector[a];
  249.       }
  250.    }
  251.    for ( i = 0; i <= c - 1; ++i ) {
  252.       for ( a = 0; a <= r - 1; ++a ) {
  253.          xVector[a] = xreal[a*c+i];
  254.          yVector[a] = yimag[a*c+i];
  255.       }
  256.       if ( flag == 0 )
  257.          FFTCalc(xVector,yVector,r);
  258.       else
  259.          FFTInvCalc(xVector,yVector,r);
  260.       for ( a = 0; a <= r - 1; ++a ) {
  261.          xreal[a*c+i] = xVector[a];
  262.          yimag[a*c+i] = yVector[a];
  263.       }
  264.    }
  265.    free(xVector); free(yVector);
  266. }
  267.  
  268. void FreqResponse(float *filtcoef, float *a, int n,int k )
  269. {
  270. int i,j,m,n2;
  271. float att,am,q;
  272.  
  273.   q = M_PI/k;
  274.   am = (n+1)/2.0;
  275.   m = (n+1) / 2;
  276.   n2 = n / 2;
  277.   for (j = 1; j <= k + 1; j++)
  278.   {
  279.     att = 0.0;
  280.     if (am == m)  att = filtcoef[m-1]/2.0;
  281.     for (i = 1; i <= n2; i++)
  282.       att = att + filtcoef[i-1] * cos(q*(am-i)*(j-1));
  283.     a[j-1] = 2.0 * att;
  284.   }
  285. }
  286.  
  287. void FIRFreqSample(float *filtcoef,float fp,int n,
  288.         int dc,int filtType, int win)
  289.  
  290. {
  291.   int np, i, j, m, m1, n2;
  292.   float lowbin, highbin, xt, am, q;
  293.   float *a;
  294.  
  295.     a = (float *) calloc(n,sizeof(float));
  296.     if (a == NULL)
  297.       exit(1);
  298.     m = (n+1) / 2;
  299.     am = (n+1)/2.0;
  300.     m1 = n / 2 + 1;
  301.     q = 2.0*M_PI/n;
  302.     n2 = n / 2;
  303.     if (dc == 1)
  304.       np = floor(n * fp + 0.5);
  305.     else
  306.       np = floor(n * fp + 1.0);
  307.  
  308.     if (filtType == 1){
  309.       lowbin = 0.0;
  310.       highbin = 1.0;
  311.     }
  312.     else {
  313.       lowbin = 1.0;
  314.       highbin = 0.0;
  315.     }
  316.     for (j = 0; j <= np; j++)
  317.       a[j] = lowbin;
  318.  
  319.     for (j = np+1; j <= m1; j++)
  320.       a[j] = highbin;
  321.  
  322.     if (dc == 0) {
  323.       for (j = 1; j <= m; j++){
  324.     xt = a[1] / 2.0;
  325.     for (i = 2; i <= m; i++)
  326.       xt = xt + a[i]* cos(q * (am-j) * (i-1));
  327.     filtcoef[j-1] = 2.0 * xt / n;
  328.     filtcoef[n-j] = filtcoef[j-1];
  329.       }
  330.     }
  331.     else{
  332.       for (j = 1; j <= m; j++){
  333.     xt = 0.0;
  334.     for (i = 1; i <= n2; i++)
  335.       xt = xt + a[i]* cos(q * (am-j) * (i-0.5));
  336.     if (am != m)  xt = xt + a[m]*cos(M_PI * (am-j))/2.0;
  337.     filtcoef[j-1] = 2.0 * xt / n;
  338.     filtcoef[n-j] = filtcoef[j-1];
  339.       }
  340.     }
  341.   /* Modify filter by Hanning Window if win = 1 */
  342.    if (win > 0 ) WindowData( filtcoef, n, win );
  343.   free(a);
  344. }
  345.  
  346.  
  347. void Convolve(float *filtcoef,float *x,float *y, int filtLen,int aLen)
  348. {
  349.   int n,m;
  350.   float summ;
  351.  
  352.   for (n = 0; n <= aLen-1; n++){
  353.     summ = 0.0;
  354.     if (n >= filtLen-1) {
  355.       for (m = 0; m <= filtLen-1; m++)
  356.         summ = summ + filtcoef[m] * x[n-m];
  357.     }
  358.     y[n] = summ;
  359.   }
  360. }
  361.  
  362.