fft_lib.c

精简的FFT程序库,可实现任意点FFT与CZT算法

#include "FFT_lib.h"
#include "FFT_debug.h"

int LOG2(int n){
	int i=0;
	FOR_PC_DEBUG(ASSERT(n>=1));
	while (n>1) {
		i++;
		n=n>>1;
	}
	return i;
}

void COMPLEX_PRINT(const COMPLEX * pcplx){
	if (pcplx->imag>=0) {
		printf("%g + %gj",pcplx->real,pcplx->imag);
	}
	else{
		printf("%g - %gj",pcplx->real,fabs(pcplx->imag));	
	}
	printf("\n");
}

void COMPLEX_ARR_PRINT(const COMPLEX  ArrCplx[],int size){
	int i;
	for (i=0;i<size;i++){
		COMPLEX_PRINT(&ArrCplx[i]);
	}
}

void COMPLEX_POW(COMPLEX * destCplx,const COMPLEX *baseCplx,double exp){
	double magnitude,angle;
	magnitude=sqrt(baseCplx->real*baseCplx->real+
		baseCplx->imag*baseCplx->imag);
	angle=atan2(baseCplx->imag,baseCplx->real);
	magnitude=pow(magnitude,exp);
	angle*=exp;
	destCplx->real=magnitude*cos(angle);
	destCplx->imag=magnitude*sin(angle);
}

void FFT_Power2(COMPLEX destArrCplx[],
				const COMPLEX srcArrCplx[],
				int size){
	int i,j,k,l,m,n,tmp;
	COMPLEX tmpArrCplx1[MAXN],tmpArrCplx2[MAXN];
	COMPLEX tmpCplx1,W;
	FOR_PC_DEBUG(ASSERT(size>=1));
	for (i=0;i<size;i++){
		tmp=0;
		for (j=0;j<LOG2(size);j++){
			tmp=(tmp<<1)+((i>>j)&1);
		}
		if (i<=tmp){
			tmpArrCplx1[i]=srcArrCplx[tmp];
			tmpArrCplx1[tmp]=srcArrCplx[i];
		}
	}
	

	k=size/2;
	for (i=0;i<LOG2(size);i++){
		for (j=0;j<k;j++){
			n=size/k;
			for (l=0;l<n/2;l++){
				m=l+j*n;
				COMPLEX_INIT(&W,cos(2*PI*m/n),-sin(2*PI*m/n));
				COMPLEX_MUL(&tmpCplx1,&tmpArrCplx1[m+n/2],&W);
				COMPLEX_ADD(&tmpArrCplx2[m],
					&tmpArrCplx1[m],
					&tmpCplx1);
				COMPLEX_SUB(&tmpArrCplx2[m+n/2],
					&tmpArrCplx1[m],
					&tmpCplx1);
			}
		}
		memcpy(tmpArrCplx1,tmpArrCplx2,size*sizeof(COMPLEX));
		k/=2;
	}
	memcpy(destArrCplx,tmpArrCplx1,size*sizeof(COMPLEX));
}

void IFFT_Power2(COMPLEX destArrCplx[],
				const COMPLEX srcArrCplx[],
				int size){
	int i;
	COMPLEX tmpArrCplx1[MAXN];
	COMPLEX tmpCplx;
	for (i=0;i<size;i++){
		COMPLEX_CONJ(&tmpArrCplx1[i],&srcArrCplx[i]);
	}
	FFT_Power2(destArrCplx,tmpArrCplx1,size);
	for (i=0;i<size;i++){
		COMPLEX_CONJ(&tmpArrCplx1[i],&destArrCplx[i]);
	}
	COMPLEX_INIT(&tmpCplx,1.0/size,0);
	for (i=0;i<size;i++){
		COMPLEX_MUL(&destArrCplx[i],&tmpArrCplx1[i],&tmpCplx);
	}
}

void CZT(COMPLEX destArrCplx[],
		 int N,
		 const COMPLEX srcArrCplx[],
		 int M,
		 const COMPLEX * pA,
		 const COMPLEX * pW
		 ){
	int i,L;
	COMPLEX f[MAXN],h[MAXN];
	COMPLEX F[MAXN],H[MAXN];
	COMPLEX tmpCplx1,tmpCplx2,tmpCplx3;
	COMPLEX tmpArrCplx1[MAXN],tmpArrCplx2[MAXN];
	L=1;
	while (L<N+M-1) L=L<<1;
	
	for (i=0;i<N;i++){
		COMPLEX_POW(&tmpCplx1,pA,-1.0*i);
		COMPLEX_POW(&tmpCplx2,pW,1.0*i*i/2);
		//f[i]=srcArrCplx[i]*tmpCplx1*tmpCplx2;
		COMPLEX_MUL(&tmpCplx3,&tmpCplx2,&tmpCplx1);
		COMPLEX_MUL(&tmpCplx1,&tmpCplx3,&srcArrCplx[i]);
		f[i]=tmpCplx1;
	}
	for (i=N;i<L;i++){
		COMPLEX_INIT(&f[i],0.0,0.0);
	}
	for (i=0;i<M;i++){
		COMPLEX_POW(&tmpCplx1,pW,-1.0*i*i/2);
		h[i]=tmpCplx1;
	}
	for (i=L-N+1;i<L;i++){
		COMPLEX_POW(&tmpCplx1,pW,-1.0*(L-i)*(L-i)/2);
		h[i]=tmpCplx1;
	}
	for (i=M;i<=L-N;i++){
		COMPLEX_INIT(&h[i],0.0,0.0);
	}
	FFT_Power2(F,f,L);
	FFT_Power2(H,h,L);
	for (i=0;i<L;i++){
		//tmpArrCplx1[i]=F[i]*H[i];
		COMPLEX_MUL(&tmpArrCplx1[i],&F[i],&H[i]);
	}
	IFFT_Power2(tmpArrCplx2,tmpArrCplx1,L);
	for (i=0;i<M;i++){
		COMPLEX_POW(&tmpCplx1,pW,1.0*i*i/2);
		//destArrCplx[i]=tmpCplx1*tmpArrCplx2[i];
		COMPLEX_MUL(&destArrCplx[i],&tmpCplx1,&tmpArrCplx2[i]);
	}
	/*
	for (i=M;i<L;i++){
		COMPLEX_INIT(&destArrCplx[i],0.0,0.0);
	}
	*/
}

void FFT(COMPLEX destArrCplx[],
		 const COMPLEX srcArrCplx[],
		 int size){
	COMPLEX A,W;
	COMPLEX_INIT(&A,1.0,0.0);
	COMPLEX_INIT(&W,cos(2*PI/size),-1.0*sin(2*PI/size));
	CZT(destArrCplx,size,srcArrCplx,size,&A,&W);
}

void IFFT(COMPLEX destArrCplx[],
		const COMPLEX srcArrCplx[],
		int size){
	int i;
	COMPLEX tmpArrCplx1[MAXN];
	COMPLEX tmpCplx;
	for (i=0;i<size;i++){
		COMPLEX_CONJ(&tmpArrCplx1[i],&srcArrCplx[i]);
	}
	FFT(destArrCplx,tmpArrCplx1,size);
	for (i=0;i<size;i++){
		COMPLEX_CONJ(&tmpArrCplx1[i],&destArrCplx[i]);
	}
	COMPLEX_INIT(&tmpCplx,1.0/size,0);
	for (i=0;i<size;i++){
		COMPLEX_MUL(&destArrCplx[i],&tmpArrCplx1[i],&tmpCplx);
	}
}