Main Page | Namespaces | Classes | Compounds | Files | Compound Members | Related
fft.c
00001 00002 00003 #define M_PI 3.14159265359 00004 #define M_PI_F 3.14159265359f 00005 00006 #include <math.h> 00007 #include "fft.h" 00008 00009 #define SWAP_F(x,y) {float tmp=x; x=y; y=tmp;} 00010 00011 void revbin_permute_f(float *fr, long n) 00012 { 00013 long m,j; 00014 for (m=1,j=0; m<n-1; m++) 00015 { 00016 long k; 00017 for(k=n>>1; (!((j^=k)&k)); k>>=1); 00018 00019 if (j>m) SWAP_F(fr[m], fr[j]); 00020 } 00021 } 00022 00023 00024 void fft_f(float *fr, float *fi, int ldn, int is) 00025 /* 00026 * radix 2 fft a la cooley tukey 00027 * 00028 */ 00029 { 00030 int n2,ldm,m,mh,j,r; 00031 int t1, t2; 00032 float pi,phi,c,s; 00033 float ur,vr, ui,vi; 00034 00035 n2=1<<ldn; 00036 00037 pi=is*M_PI_F; 00038 00039 revbin_permute_f(fr,n2); 00040 revbin_permute_f(fi,n2); 00041 00042 for (ldm=1; ldm<=ldn; ++ldm) 00043 { 00044 m=(1<<ldm); /* m=2^ldm */ 00045 mh=(m>>1); /* mh=m/2 */ 00046 00047 phi=pi/(float)(mh); 00048 00049 for (j=0; j<mh; ++j) 00050 { 00051 float w=phi*(float)j; 00052 00053 c=(float) cos(w); 00054 s=(float) sin(w); 00055 00056 for (r=0; r<n2; r+=m) 00057 { 00058 /* 00059 * u=f[t1] 00060 * v=f[t2]*exp(+-2*pi*i*j/m) 00061 * f[t1]= (u+v) 00062 * f[t2]= (u-v) 00063 */ 00064 00065 t1=r+j; 00066 t2=t1+mh; 00067 00068 vr=fr[t2]*c-fi[t2]*s; 00069 vi=fr[t2]*s+fi[t2]*c; 00070 00071 ur=fr[t1]; 00072 fr[t1]+=vr; 00073 fr[t2]=ur-vr; 00074 00075 ui=fi[t1]; 00076 fi[t1]+=vi; 00077 fi[t2]=ui-vi; 00078 } 00079 } 00080 } 00081 } 00082 00083 #define sumdiff(a,b,s,d) { s=a+b; d=a-b; } 00084 #define sumdiff_05(a,b,s,d) { s=0.5*(a+b); d=0.5*(a-b); } 00085 /* NEVER call like sumdiff(a,b,a,b) (i.e. input=output) */ 00086 00087 #define sumdiff2(s,d) { double t; t=s-d; s+=d; d=t; } 00088 00089 #define cmult(c,s,u,v) { double t; t=u*s+v*c; u*=c; u-=v*s; v=t; } 00090 00091 void revbin_permute(double *fr, long n); 00092 void revbin_permute_f(float *fr, long n); 00093 void interlace(double *f, long n); 00094 00095 00096 void real_fft(double *f, int ldn) 00097 /* 00098 * ordering on output: 00099 * f[0] = re[0] 00100 * f[1] = re[1] 00101 * ... 00102 * f[n/2-1] = re[n/2-1] 00103 * f[n/2] = re[n/2](==nyquist freq) 00104 * 00105 * f[n/2+1] = -im[1] (wrt. compl fft with is=-1) 00106 * f[n/2+2] = -im[2] 00107 * ... 00108 * f[n-1] = -im[n/2-1] 00109 * 00110 * corresponding real and imag parts (with the exception of 00111 * zero and nyquist freq) are found in f[i] and f[n/2+i] 00112 */ 00113 { 00114 long i,j; 00115 long n,n2,n4; 00116 double ph0; 00117 double *re,*im; 00118 00119 n=(1<<ldn); 00120 n2=(n>>1); 00121 n4=(n>>2); 00122 00123 re=f; 00124 im=f+n2; 00125 00126 /* re gets even indexed elements, im gets odd indexed: */ 00127 interlace(f,n); 00128 00129 fft(re,im,ldn-1,+1); 00130 00131 ph0=+1.0*M_PI/n2; 00132 for (i=1,j=n2-1; i<=n4; ++i,--j) 00133 { 00134 double evr,evi,odr,odi; 00135 double c,s,w; 00136 00137 sumdiff(re[i],re[j],evr,odr); 00138 sumdiff(im[i],im[j],odi,evi); 00139 00140 w=ph0*i; 00141 c=cos(w); 00142 s=sin(w); 00143 00144 cmult(s,-c,odr,odi); 00145 00146 sumdiff_05(evr,odr,re[i],re[j]); 00147 sumdiff_05(odi,evi,im[i],im[j]); 00148 } 00149 00150 sumdiff2(re[0],im[0]); 00151 } 00152 /* --------------------------- */ 00153 00154 00155 00156 void fft(double *fr, double *fi, int ldn, int is) 00157 /* 00158 * radix 2 fft a la cooley tukey 00159 * 00160 */ 00161 { 00162 int n2,ldm,m,mh,j,r; 00163 int t1, t2; 00164 double pi,phi,c,s; 00165 double ur,vr, ui,vi; 00166 00167 n2=1<<ldn; 00168 00169 pi=is*M_PI; 00170 00171 revbin_permute(fr,n2); 00172 revbin_permute(fi,n2); 00173 00174 for (ldm=1; ldm<=ldn; ++ldm) 00175 { 00176 m=(1<<ldm); /* m=2^ldm */ 00177 mh=(m>>1); /* mh=m/2 */ 00178 00179 phi=pi/(double)(mh); 00180 00181 for (j=0; j<mh; ++j) 00182 { 00183 double w=phi*(double)j; 00184 00185 c=cos(w); 00186 s=sin(w); 00187 00188 for (r=0; r<n2; r+=m) 00189 { 00190 /* 00191 * u=f[t1] 00192 * v=f[t2]*exp(+-2*pi*i*j/m) 00193 * f[t1]= (u+v) 00194 * f[t2]= (u-v) 00195 */ 00196 00197 t1=r+j; 00198 t2=t1+mh; 00199 00200 vr=fr[t2]*c-fi[t2]*s; 00201 vi=fr[t2]*s+fi[t2]*c; 00202 00203 ur=fr[t1]; 00204 fr[t1]+=vr; 00205 fr[t2]=ur-vr; 00206 00207 ui=fi[t1]; 00208 fi[t1]+=vi; 00209 fi[t2]=ui-vi; 00210 } 00211 } 00212 } 00213 } 00214 /* --------------------------- */ 00215 00216 00217 /* --------------------------- */ 00218 00219 #define SWAP(x,y) {double tmp=x; x=y; y=tmp;} 00220 00221 void revbin_permute(double *fr, long n) 00222 { 00223 long m,j; 00224 for (m=1,j=0; m<n-1; m++) 00225 { 00226 long k; 00227 for(k=n>>1; (!((j^=k)&k)); k>>=1); 00228 00229 if (j>m) SWAP(fr[m], fr[j]); 00230 } 00231 } 00232 /* --------------------------- */ 00233 00234 /* --------------------------- */ 00235 00236 00237 /* --------------------------- */ 00238 00239 00240 00241 void interlace(double *f, long n) 00242 /* 00243 * put part of data with even indices 00244 * sorted into the lower half, 00245 * odd part into the higher half 00246 */ 00247 { 00248 revbin_permute(f, n); 00249 00250 revbin_permute(f, n/2); 00251 revbin_permute(f+n/2, n/2); 00252 } 00253 /* --------------------------- */
Docs made by Doxygen. Email: Mikael Christensen