fft.zip_it

#include "stdafx.h" #include "stdlib.h" #include <math.h> #include <windows.h> #include "FFT.h" #define N 8 #define N_2 4 #define V 3 #define PI (double)3.14159265358979323846 #define M_PI PI #define DOUBLE_PI 6.283185307179586476925286766559 #define BITREV_F(j, nu, fp) \ { int j2, j1 = j, k = 0; \ for (int i = 1; i <= nu; i++) \ { j2 = j1/2; k = ((2*k) + j1) - (2*j2); j1 = j2; } \ *fp = (float) k; \ } #define BITREV_I(j, nu, ip) \ { int j2, j1 = j, k = 0; \ for (int i = 1; i <= nu; i++) \ { j2 = j1/2; k = ((2*k) + j1) - (2*j2); j1 = j2; } \ *ip = k; \ } int bitrev[N]; int power_2; double hanning (int i, int nn) { double factor; factor = 8.0*atan(1.0)/(nn-1); //factor = 2*PI/(nn-1); return ( 0.5 * (1 - cos (factor*i)) ); } void FFT(double * data, int n, bool isInverse) { int mmax, m=0, j, step, i; double temp; double theta, sin_htheta, sin_theta, pwr, wr, wi, tempr, tempi; n = 2 * (1 << n); int nn = n >> 1; // 長度為1的傅里葉轉換, 位置交換過程 j = 1; for(i = 1; i < n; i += 2) { if(j > i) { temp = data[j - 1]; data[j - 1] = data[i - 1]; data[i - 1] = temp; data[j] = temp; data[j] = data[i]; data[i] = temp; } // 相反的二進制加法 m = nn; while(m >= 2 && j > m) { j -= m; m >>= 1; } j += m; } // Danielson - Lanczos 引理應用 mmax = 2; while(n > mmax) { step = mmax << 1; theta = DOUBLE_PI / mmax; if(isInverse) { theta = -theta; } sin_htheta = sin(0.5 * theta); sin_theta = sin(theta); pwr = -2.0 * sin_htheta * sin_htheta; wr = 1.0; wi = 0.0; for(m = 1; m < mmax; m += 2) { for(i = m; i <= n; i += step) { j = i + mmax; tempr = wr * data[j - 1] - wi * data[j]; tempi = wr * data[j] + wi * data[j - 1]; data[j - 1] = data[i - 1] - tempr; data[j] = data[i] - tempi; data[i - 1] += tempr; data[i] += tempi; } sin_htheta = wr; wr = sin_htheta * pwr - wi * sin_theta + wr; wi = wi * pwr + sin_htheta * sin_theta + wi; } mmax = step; } } void ComplexFFT(COMPLEX *x, int m) { static COMPLEX *w; /* used to store the w complex array */ static int mstore = 0; /* stores m for future reference */ static int n = 1; /* length of fft stored for future */ COMPLEX u,temp,tm; COMPLEX *xi,*xip,*xj,*wptr; int i,j,k,l,le,windex; double arg,w_real,w_imag,wrecur_real,wrecur_imag,wtemp_real; if(m != mstore) { /* free previously allocated storage and set new m */ if(mstore != 0) free(w); mstore = m; if(m == 0) return; /* if m=0 then done */ /* n = 2**m = fft length */ n = 1 << m; le = n/2; /* allocate the storage for w */ w = (COMPLEX *) calloc(le-1,sizeof(COMPLEX)); if(!w) { //printf("\nUnable to allocate complex W array\n"); exit(1); } /* calculate the w values recursively */ //arg = 4.0*atan(1.0)/le; /* PI/le calculation */ arg = 4.0 * atan(1.0)/le; wrecur_real = w_real = cos(arg); wrecur_imag = w_imag = -sin(arg); xj = w; for (j = 1 ; j < le ; j++) { xj->real = (float)wrecur_real; xj->imag = (float)wrecur_imag; xj++; wtemp_real = wrecur_real*w_real - wrecur_imag*w_imag; wrecur_imag = wrecur_real*w_imag + wrecur_imag*w_real; wrecur_real = wtemp_real; } } /* start fft */ le = n; windex = 1; for (l = 0 ; l < m ; l++) { le = le/2; /* first iteration with no multiplies */ for(i = 0 ; i < n ; i = i + 2*le) { xi = x + i; xip = xi + le; temp.real = xi->real + xip->real; temp.imag = xi->imag + xip->imag; xip->real = xi->real - xip->real; xip->imag = xi->imag - xip->imag; *xi = temp; } /* remaining iterations use stored w */ wptr = w + windex - 1; for (j = 1 ; j < le ; j++) { u = *wptr; for (i = j ; i < n ; i = i + 2*le) { xi = x + i; xip = xi + le; temp.real = xi->real + xip->real; temp.imag = xi->imag + xip->imag; tm.real = xi->real - xip->real; tm.imag = xi->imag - xip->imag; xip->real = tm.real*u.real - tm.imag*u.imag; xip->imag = tm.real*u.imag + tm.imag*u.real; *xi = temp; } wptr = wptr + windex; } windex = 2*windex; } /* rearrange data by bit reversing */ j = 0; for (i = 1 ; i < (n-1) ; i++) { k = n/2; while(k <= j) { j = j - k; k = k/2; } j = j + k; if (i < j) { xi = x + i; xj = x + j; temp = *xj; *xj = *xi; *xi = temp; } } } biquad *biqundfilter(int type, double dbGain, double freq, double srate, double bandwidth) { biquad b; double A, omega, sn, cs, alpha, beta; double a0, a1, a2, b0, b1, b2; zeroIt(b); A = pow(10, dbGain / 40); omega = 2 * M_PI * freq / srate; sn = sin(omega); cs = cos(omega); alpha = sn * sinh(DOUBLE_PI / 2 * bandwidth * omega / sn); beta = sqrt(A + A); switch (type) { case LPF: b0 = (1 - cs) / 2; b1 = 1 - cs; b2 = (1 - cs) / 2; a0 = 1 + alpha; a1 = -2 * cs; a2 = 1 - alpha; break; case HPF: /* double wa = (double)2 * tan(0.1*PI); double B0 = pow(wa, 3); double B1 = (double)3 * pow(wa, 3); double B2 = (double)3 * pow(wa, 3); double B3 = pow(wa, 3); double A0 = (double)8 + (double)8 * wa + (double)4 * pow(wa, 2) + pow(wa, 3); double A1 = ((double)-24 - (double)8 * wa) + (double)4 * pow(wa, 2) + (double)3 * pow(wa, 3); double A2 = ((double)24 - (double)8 * wa - (double)4 * pow(wa, 2)) + (double)3 * pow(wa, 3); double A3 = (((double)-8 + (double)8 * wa) - (double)4 * pow(wa, 2)) + pow(wa, 3); B0 /= A0; B1 /= A0; B2 /= A0; B3 /= A0; A1 /= A0; A2 /= A0; A3 /= A0; wa = (double)2 * tan(0.1*M_PI); b0 = pow(wa, 3); b1 = (double)3 * pow(wa, 3); b2 = (double)3 * pow(wa, 3); a0 = (double)8 + (double)8 * wa + (double)4 * pow(wa, 2) + pow(wa, 3); a1 = ((double)-24 - (double)8 * wa) + (double)4 * pow(wa, 2) + (double)3 * pow(wa, 3); a2 = ((double)24 - (double)8 * wa - (double)4 * pow(wa, 2)) + (double)3 * pow(wa, 3); */ b0 = (1 + cs) / 2; b1 = -(1 + cs); b2 = (1 + cs) / 2; a0 = 1 + alpha; a1 = -2 * cs; a2 = 1 - alpha; break; case BPF: b0 = alpha; b1 = 0; b2 = -alpha; a0 = 1 + alpha; a1 = -2 * cs; a2 = 1 - alpha; break; case NOTCH: b0 = 1; b1 = -2 * cs; b2 = 1; a0 = 1 + alpha;