1
2 /*
3 * fourier.c
4 *
5 * Routines from numerical recipes for fourier transforms
6 *
7 */
8
9 char *fourier_cvsid = "$Id: fourier.c,v 1.2 2005-07-11 22:03:36 girod Exp $";
10
11 #include "nr.h"
12 #include <math.h>
13
14
15 #define SWAP(a,b) tempr=(a);(a)=(b);(b)=tempr
16
17 /*
18 Replaces data[1..2*nn] by its discrete Fourier transform, if isign is input as 1; or
19 replaces data[1..2*nn] by nn times its inverse discrete Fourier transform, if isign is
20 input as - 1. data is a complex array of length nn or, equivalently, a real array of
21 length 2*nn. nn MUST be an integer power of 2 (this is not checked for!).
22 */
23
24 void four1(float data[], unsigned long nn, int isign)
25 {
26 unsigned long n,mmax,m,j,istep,i;
27 // Double precision for the trigonometric recurrences.
28 double wtemp,wr,wpr,wpi,wi,theta;
29
30 float tempr,tempi;
31 n=nn << 1;
32 j=1;
33
34 // This is the bit-reversal section of the routine.
35 for (i=1;i<n;i+=2) {
36 if (j > i) {
37 // Exchange the two complex numbers.
38 SWAP(data[j],data[i]);
39 SWAP(data[j+1],data[i+1]);
40 }
41 m=n >> 1;
42 while (m >= 2 && j > m) {
43 j -=m;
44 m >>= 1;
45 }
46 j += m;
47 }
48
49 // Here begins the Danielson-Lanczos section of the routine.
50
51 mmax=2;
52 // Outer loop executed log 2 nn times.
53 while (n > mmax) {
54 istep=mmax << 1;
55
56 // Initialize the trigonometric recurrence.
57 theta=isign*(6.28318530717959/mmax);
58 wtemp=sin(0.5*theta);
59 wpr = -2.0*wtemp*wtemp;
60 wpi=sin(theta);
61 wr=1.0;
62 wi=0.0;
63
64 // Here are the two nested inner loops.
65 for (m=1;m<mmax;m+=2) {
66 for (i=m;i<=n;i+=istep) {
67 // This is the Danielson-Lanczos formula:
68 j=i+mmax;
69 tempr=wr*data[j]-wi*data[j+1];
70 tempi=wr*data[j+1]+wi*data[j];
71 data[j]=data[i]-tempr;
72 data[j+1]=data[i+1]-tempi;
73 data[i] += tempr;
74 data[i+1] += tempi;
75 }
76
77 // Trigonometric recurrence.
78 wr=(wtemp=wr)*wpr-wi*wpi+wr;
79 wi=wi*wpr+wtemp*wpi+wi;
80 }
81 mmax=istep;
82 }
83 }
84
85
86 /*
87 Calculates the Fourier transform of a set of n real-valued data points. Replaces this
88 data (which is stored in array data[1..n]) by the positive frequency half of its complex
89 Fourier transform. The real-valued first and last components of the complex transform
90 are returned as elements data[1] and data[2], respectively. n must be a power of 2. This
91 routine also calculates the inverse transform of a complex data array if it is the
92 transform of real data. (Result in this case must be multiplied by 2/n.)
93 */
94
95 void realft(float data[], unsigned long n, int isign)
96 {
97 unsigned long i,i1,i2,i3,i4,np3;
98 float c1=0.5,c2,h1r,h1i,h2r,h2i;
99
100 // Double precision for the trigonometric recurrences.
101 double wr,wi,wpr,wpi,wtemp,theta;
102
103 // Initialize the recurrence.
104 theta=M_PI/(double) (n>>1);
105
106 if (isign == 1) {
107 c2 = -0.5;
108 // The forward transform is here.
109 four1(data,n>>1,1);
110 }
111
112 else {
113 // Otherwise set up for an inverse transform.
114 c2=0.5;
115 theta = -theta;
116 }
117
118 wtemp=sin(0.5*theta);
119 wpr = -2.0*wtemp*wtemp;
120 wpi=sin(theta);
121 wr=1.0+wpr;
122 wi=wpi;
123 np3=n+3;
124 for (i=2;i<=(n>>2);i++) {
125 // Case i=1 done separately below.
126 i4=1+(i3=np3-(i2=1+(i1=i+i-1)));
127
128 // The two separate transforms are separated out of data.
129 h1r=c1*(data[i1]+data[i3]);
130 h1i=c1*(data[i2]-data[i4]);
131 h2r = -c2*(data[i2]+data[i4]);
132 h2i=c2*(data[i1]-data[i3]);
133
134 // Here they are recombined to form the true transform of the original real data.
135 data[i1]=h1r+wr*h2r-wi*h2i;
136 data[i2]=h1i+wr*h2i+wi*h2r;
137 data[i3]=h1r-wr*h2r+wi*h2i;
138 data[i4] = -h1i+wr*h2i+wi*h2r;
139
140 // The recurrence.
141 wr=(wtemp=wr)*wpr-wi*wpi+wr;
142 wi=wi*wpr+wtemp*wpi+wi;
143 }
144 if (isign == 1) {
145 // Squeeze the first and last data together to get them all within
146 // the original array.
147
148 data[1] = (h1r=data[1])+data[2];
149 data[2] = h1r-data[2];
150 } else {
151
152 // This is the inverse transform for the case isign=-1.
153
154 data[1]=c1*((h1r=data[1])+data[2]);
155 data[2]=c1*(h1r-data[2]);
156 four1(data,n>>1,-1);
157
158 }
159 }
160
161
162
163
164 /*
165 Given two real input arrays data1[1..n] and data2[1..n], this routine calls four1 and
166 returns two complex output arrays, fft1[1..2n] and fft2[1..2n], each of complex length
167 n (i.e., real length 2*n), which contain the discrete Fourier transforms of the
168 respective data arrays. n MUST be an integer power of 2.
169 */
170
171 void twofft(float data1[], float data2[], float fft1[], float fft2[], unsigned long n)
172 {
173 unsigned long nn3,nn2,jj,j;
174 float rep,rem,aip,aim;
175
176 nn3=1+(nn2=2+n+n);
177
178 // Pack the two real arrays into one complex array
179 for (j=1,jj=2;j<=n;j++,jj+=2) {
180 fft1[jj-1]=data1[j];
181 fft1[jj]=data2[j];
182 }
183
184 // Transform the complex array.
185 four1(fft1,n,1);
186 fft2[1]=fft1[2];
187 fft1[2]=fft2[2]=0.0;
188 for (j=3;j<=n+1;j+=2) {
189 // Use symmetries to separate the two transforms.
190 rep=0.5*(fft1[j]+fft1[nn2-j]);
191 rem=0.5*(fft1[j]-fft1[nn2-j]);
192 aip=0.5*(fft1[j+1]+fft1[nn3-j]);
193 aim=0.5*(fft1[j+1]-fft1[nn3-j]);
194
195 // Ship them out in two complex arrays.
196 fft1[j]=rep;
197 fft1[j+1]=aim;
198 fft1[nn2-j]=rep;
199 fft1[nn3-j] = -aim;
200 fft2[j]=aip;
201 fft2[j+1] = -rem;
202 fft2[nn2-j]=aip;
203 fft2[nn3-j]=rem;
204 }
205 }
206
207
208
209 /*
210 Computes the correlation of two real data sets data1[1..n] and data2[1..n] (including any
211 user-supplied zero padding). n MUST be an integer power of two. The answer is returned as
212 the first n points in ans[1..2*n] stored in wrap-around order, i.e., correlations at
213 increasingly negative lags are in ans[n] on down to ans[n/2+1], while correlations at
214 increasingly positive lags are in ans[1] (zerolag) on up to ans[n/2]. Note that ans must be
215 supplied in the calling program with length at least 2*n, since it is also used as
216 working space. Sign convention of this routine: if data1 lags data2, i.e., is shifted to
217 the right of it, then ans will show a peak at positive lags.
218
219 NOTE:
220 this description is reversed; positive lags are from n-1 (0 lag) down to n/2
221 increasingly negative lags are from 0 to n/2-1
222 -LDG
223 */
224
225 #include <stdio.h>
226
227 void correl(float data1[], float data2[], unsigned long n, float ans[])
228 {
229 unsigned long no2,i;
230 float dum,*fft;
231
232 fft=vector(1,n<<1);
233 if (!fft) return;
234
235 // Transform both data vectors at once.
236 twofft(data1,data2,fft,ans,n);
237
238
239 // ---------------------------------------
240 // - DeBuG - - DeBuG - - DeBuG - - DeBuG -
241 {
242 float sampling_rate = 48.0 ; // in kHz
243 char filename[32];
244 FILE *fp;
245 static int filecounter=0;
246 //#define write_all_fields
247 int f;
248 /* 'n' is the number of elements in data1[] and in data2[]
249 * because fft[] and ans[] represent complex arrays, they
250 * have n*2 elements ( that is, n complexes) .
251 * But because the fft creates a mirror, only the first
252 * half of the array is relevant, that is, the first 'n'
253 * elements (that is, n/2 complexes). Therefore, we'll
254 * only save the first 'n' elements to file.
255 */
256 snprintf(filename,32,"/tmp/spectrums%i.txt",filecounter++);
257 fprintf(stderr,"Creating %s\n",filename);
258 if((fp=fopen(filename,"w"))!=NULL){
259 fprintf(fp,
260 "# %s\n# %lu samples. Sampling rate: %4.1f kHz.\n"
261 "# from correl(data1[],data2[],n,ans[]) function in fourier.c\n"
262 "# in POST/putil/putil : \n#\n"
263 #ifdef write_all_fields
264 "# (x,y) values are in the arrays,\n"
265 "# (R,T) values are calculated here for convenience:\n"
266 "# x+i*y = R*(cos(T)+i*sin(T)) ==> R=sqrtf(x^2+y^2), T=acos(x/R)\n"
267 "frequency(kHz),Xref,Yref,Rref,Tref,Xdata,Ydata,Rdata,Tdata\n",
268 #else
269 "frequency(kHz),Rref=sqrtf(Xref^2+Yref^2),Rdata=sqrtf(Xdata^2+Ydata^2)\n",
270 #endif
271 filename, n, sampling_rate);
272 #ifdef write_all_fields
273 inline void rec2polar(float *x,float *y, double* r, double* t) {
274 *r = sqrtf(pow(*x,2)+pow(*y,2));
275 if ( *r == 0.0 )
276 *t = 0.0 ;
277 else {
278 double ac,as, pi=3.1415926535897932 ;
279 ac = acos((*x)/(*r)) ;
280 as = asin((*y)/(*r)) ;
281 *t = (as>=0.0)? ac : 2*pi - ac ;
282 *t = 180.0 * (*t) / pi ;
283 }
284 }
285 #endif
286 for(f= 1 ; f < n ; f+=2 ) {
287 #ifdef write_all_fields
288 double r1,t1,r2,t2;
289 rec2polar(fft+f,fft+f+1,&r1,&t1);
290 rec2polar(ans+f,ans+f+1,&r2,&t2);
291 fprintf(fp,"%7.3f,%9.2f,%9.2f,%9.2f,%9.2f,%9.2f,%9.2f,%9.2f,%9.2f\n",
292 ((float)f)*sampling_rate/(2*((float)n)),
293 fft[f],fft[f+1],r1,t1,ans[f],ans[f+1],r2,t2);
294 #else
295 fprintf(fp,"%7.3f,%9.2f,%9.2f\n",
296 ((float)f)*sampling_rate/(2*((float)n)),
297 sqrtf(pow(fft[f],2)+pow(fft[f+1],2)),
298 sqrtf(pow(ans[f],2)+pow(ans[f+1],2)));
299 #endif
300 }
301
302 fclose(fp);
303 fprintf(stderr,"file written.\n");
304 } else { fprintf(stderr,"Could not create file\n"); }
305 }
306 // - EnD - - EnD - - EnD - - EnD - - EnD -
307 // ---------------------------------------
308
309
310 // Normalization for inverse FFT.
311 no2=n>>1;
312 for (i=2;i<=n+2;i+=2) { // (a+b.i).(c+d.i) = (ac-bd)+(ad+bc)i
313 // Multiply to find FFT of their correlation.
314 ans[i-1]=(fft[i-1]*(dum=ans[i-1])+fft[i]*ans[i])/no2;
315 // a * c + b * d
316 ans[i]=(fft[i]*dum-fft[i-1]*ans[i])/no2;
317 // b * c - a * d
318 }
319
320 // Pack first and last into one element.
321 ans[2]=ans[n+1];
322
323 // Inverse transform gives correlation.
324 realft(ans,n,-1);
325
326 free_vector(fft,1,n<<1);
327 }
328
329
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.