1 //$$ fft.cpp Fast fourier transform
2
3 // Copyright (C) 1991,2,3,4,8: R B Davies
4
5
6 #define WANT_MATH
7 // #define WANT_STREAM
8
9 #include "include.h"
10
11 #include "newmatap.h"
12
13 // #include "newmatio.h"
14
15 #ifdef use_namespace
16 namespace NEWMAT {
17 #endif
18
19 #ifdef DO_REPORT
20 #define REPORT { static ExeCounter ExeCount(__LINE__,19); ++ExeCount; }
21 #else
22 #define REPORT {}
23 #endif
24
cossin(int n,int d,Real & c,Real & s)25 static void cossin(int n, int d, Real& c, Real& s)
26 // calculate cos(twopi*n/d) and sin(twopi*n/d)
27 // minimise roundoff error
28 {
29 REPORT
30 long n4 = n * 4; int sector = (int)floor( (Real)n4 / (Real)d + 0.5 );
31 n4 -= sector * d;
32 if (sector < 0) { REPORT sector = 3 - (3 - sector) % 4; }
33 else { REPORT sector %= 4; }
34 Real ratio = 1.5707963267948966192 * (Real)n4 / (Real)d;
35
36 switch (sector)
37 {
38 case 0: REPORT c = cos(ratio); s = sin(ratio); break;
39 case 1: REPORT c = -sin(ratio); s = cos(ratio); break;
40 case 2: REPORT c = -cos(ratio); s = -sin(ratio); break;
41 case 3: REPORT c = sin(ratio); s = -cos(ratio); break;
42 }
43 }
44
fftstep(ColumnVector & A,ColumnVector & B,ColumnVector & X,ColumnVector & Y,int after,int now,int before)45 static void fftstep(ColumnVector& A, ColumnVector& B, ColumnVector& X,
46 ColumnVector& Y, int after, int now, int before)
47 {
48 REPORT
49 Tracer trace("FFT(step)");
50 // const Real twopi = 6.2831853071795864769;
51 const int gamma = after * before; const int delta = now * after;
52 // const Real angle = twopi / delta; Real temp;
53 // Real r_omega = cos(angle); Real i_omega = -sin(angle);
54 Real r_arg = 1.0; Real i_arg = 0.0;
55 Real* x = X.Store(); Real* y = Y.Store(); // pointers to array storage
56 const int m = A.Nrows() - gamma;
57
58 for (int j = 0; j < now; j++)
59 {
60 Real* a = A.Store(); Real* b = B.Store(); // pointers to array storage
61 Real* x1 = x; Real* y1 = y; x += after; y += after;
62 for (int ia = 0; ia < after; ia++)
63 {
64 // generate sins & cosines explicitly rather than iteratively
65 // for more accuracy; but slower
66 cossin(-(j*after+ia), delta, r_arg, i_arg);
67
68 Real* a1 = a++; Real* b1 = b++; Real* x2 = x1++; Real* y2 = y1++;
69 if (now==2)
70 {
71 REPORT int ib = before;
72 if (ib) for (;;)
73 {
74 REPORT
75 Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
76 Real r_value = *a2; Real i_value = *b2;
77 *x2 = r_value * r_arg - i_value * i_arg + *(a2-gamma);
78 *y2 = r_value * i_arg + i_value * r_arg + *(b2-gamma);
79 if (!(--ib)) break;
80 x2 += delta; y2 += delta;
81 }
82 }
83 else
84 {
85 REPORT int ib = before;
86 if (ib) for (;;)
87 {
88 REPORT
89 Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
90 Real r_value = *a2; Real i_value = *b2;
91 int in = now-1; while (in--)
92 {
93 // it should be possible to make this faster
94 // hand code for now = 2,3,4,5,8
95 // use symmetry to halve number of operations
96 a2 -= gamma; b2 -= gamma; Real temp = r_value;
97 r_value = r_value * r_arg - i_value * i_arg + *a2;
98 i_value = temp * i_arg + i_value * r_arg + *b2;
99 }
100 *x2 = r_value; *y2 = i_value;
101 if (!(--ib)) break;
102 x2 += delta; y2 += delta;
103 }
104 }
105
106 // temp = r_arg;
107 // r_arg = r_arg * r_omega - i_arg * i_omega;
108 // i_arg = temp * i_omega + i_arg * r_omega;
109
110 }
111 }
112 }
113
114
FFTI(const ColumnVector & U,const ColumnVector & V,ColumnVector & X,ColumnVector & Y)115 void FFTI(const ColumnVector& U, const ColumnVector& V,
116 ColumnVector& X, ColumnVector& Y)
117 {
118 // Inverse transform
119 Tracer trace("FFTI");
120 REPORT
121 FFT(U,-V,X,Y);
122 const Real n = X.Nrows(); X /= n; Y /= (-n);
123 }
124
RealFFT(const ColumnVector & U,ColumnVector & X,ColumnVector & Y)125 void RealFFT(const ColumnVector& U, ColumnVector& X, ColumnVector& Y)
126 {
127 // Fourier transform of a real series
128 Tracer trace("RealFFT");
129 REPORT
130 const int n = U.Nrows(); // length of arrays
131 const int n2 = n / 2;
132 if (n != 2 * n2)
133 Throw(ProgramException("Vector length not multiple of 2", U));
134 ColumnVector A(n2), B(n2);
135 Real* a = A.Store(); Real* b = B.Store(); Real* u = U.Store(); int i = n2;
136 while (i--) { *a++ = *u++; *b++ = *u++; }
137 FFT(A,B,A,B);
138 int n21 = n2 + 1;
139 X.ReSize(n21); Y.ReSize(n21);
140 i = n2 - 1;
141 a = A.Store(); b = B.Store(); // first els of A and B
142 Real* an = a + i; Real* bn = b + i; // last els of A and B
143 Real* x = X.Store(); Real* y = Y.Store(); // first els of X and Y
144 Real* xn = x + n2; Real* yn = y + n2; // last els of X and Y
145
146 *x++ = *a + *b; *y++ = 0.0; // first complex element
147 *xn-- = *a++ - *b++; *yn-- = 0.0; // last complex element
148
149 int j = -1; i = n2/2;
150 while (i--)
151 {
152 Real c,s; cossin(j--,n,c,s);
153 Real am = *a - *an; Real ap = *a++ + *an--;
154 Real bm = *b - *bn; Real bp = *b++ + *bn--;
155 Real samcbp = s * am + c * bp; Real sbpcam = s * bp - c * am;
156 *x++ = 0.5 * ( ap + samcbp); *y++ = 0.5 * ( bm + sbpcam);
157 *xn-- = 0.5 * ( ap - samcbp); *yn-- = 0.5 * (-bm + sbpcam);
158 }
159 }
160
RealFFTI(const ColumnVector & A,const ColumnVector & B,ColumnVector & U)161 void RealFFTI(const ColumnVector& A, const ColumnVector& B, ColumnVector& U)
162 {
163 // inverse of a Fourier transform of a real series
164 Tracer trace("RealFFTI");
165 REPORT
166 const int n21 = A.Nrows(); // length of arrays
167 if (n21 != B.Nrows() || n21 == 0)
168 Throw(ProgramException("Vector lengths unequal or zero", A, B));
169 const int n2 = n21 - 1; const int n = 2 * n2; int i = n2 - 1;
170
171 ColumnVector X(n2), Y(n2);
172 Real* a = A.Store(); Real* b = B.Store(); // first els of A and B
173 Real* an = a + n2; Real* bn = b + n2; // last els of A and B
174 Real* x = X.Store(); Real* y = Y.Store(); // first els of X and Y
175 Real* xn = x + i; Real* yn = y + i; // last els of X and Y
176
177 Real hn = 0.5 / n2;
178 *x++ = hn * (*a + *an); *y++ = - hn * (*a - *an);
179 a++; an--; b++; bn--;
180 int j = -1; i = n2/2;
181 while (i--)
182 {
183 Real c,s; cossin(j--,n,c,s);
184 Real am = *a - *an; Real ap = *a++ + *an--;
185 Real bm = *b - *bn; Real bp = *b++ + *bn--;
186 Real samcbp = s * am - c * bp; Real sbpcam = s * bp + c * am;
187 *x++ = hn * ( ap + samcbp); *y++ = - hn * ( bm + sbpcam);
188 *xn-- = hn * ( ap - samcbp); *yn-- = - hn * (-bm + sbpcam);
189 }
190 FFT(X,Y,X,Y); // have done inverting elsewhere
191 U.ReSize(n); i = n2;
192 x = X.Store(); y = Y.Store(); Real* u = U.Store();
193 while (i--) { *u++ = *x++; *u++ = - *y++; }
194 }
195
FFT(const ColumnVector & U,const ColumnVector & V,ColumnVector & X,ColumnVector & Y)196 void FFT(const ColumnVector& U, const ColumnVector& V,
197 ColumnVector& X, ColumnVector& Y)
198 {
199 // from Carl de Boor (1980), Siam J Sci Stat Comput, 1 173-8
200 // but first try Sande and Gentleman
201 Tracer trace("FFT");
202 REPORT
203 const int n = U.Nrows(); // length of arrays
204 if (n != V.Nrows() || n == 0)
205 Throw(ProgramException("Vector lengths unequal or zero", U, V));
206 if (n == 1) { REPORT X = U; Y = V; return; }
207
208 // see if we can use the newfft routine
209 if (!FFT_Controller::OnlyOldFFT && FFT_Controller::CanFactor(n))
210 {
211 REPORT
212 X = U; Y = V;
213 if ( FFT_Controller::ar_1d_ft(n,X.Store(),Y.Store()) ) return;
214 }
215
216 ColumnVector B = V;
217 ColumnVector A = U;
218 X.ReSize(n); Y.ReSize(n);
219 const int nextmx = 8;
220 int prime[8] = { 2,3,5,7,11,13,17,19 };
221 int after = 1; int before = n; int next = 0; bool inzee = true;
222 int now = 0; int b1; // initialised to keep gnu happy
223
224 do
225 {
226 for (;;)
227 {
228 if (next < nextmx) { REPORT now = prime[next]; }
229 b1 = before / now; if (b1 * now == before) { REPORT break; }
230 next++; now += 2;
231 }
232 before = b1;
233
234 if (inzee) { REPORT fftstep(A, B, X, Y, after, now, before); }
235 else { REPORT fftstep(X, Y, A, B, after, now, before); }
236
237 inzee = !inzee; after *= now;
238 }
239 while (before != 1);
240
241 if (inzee) { REPORT A.Release(); X = A; B.Release(); Y = B; }
242 }
243
244 // Trigonometric transforms
245 // see Charles Van Loan (1992) "Computational frameworks for the fast
246 // Fourier transform" published by SIAM; section 4.4.
247
DCT_II(const ColumnVector & U,ColumnVector & V)248 void DCT_II(const ColumnVector& U, ColumnVector& V)
249 {
250 // Discrete cosine transform, type II, of a real series
251 Tracer trace("DCT_II");
252 REPORT
253 const int n = U.Nrows(); // length of arrays
254 const int n2 = n / 2; const int n4 = n * 4;
255 if (n != 2 * n2)
256 Throw(ProgramException("Vector length not multiple of 2", U));
257 ColumnVector A(n);
258 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
259 int i = n2;
260 while (i--) { *a++ = *u++; *(--b) = *u++; }
261 ColumnVector X, Y;
262 RealFFT(A, X, Y); A.CleanUp();
263 V.ReSize(n);
264 Real* x = X.Store(); Real* y = Y.Store();
265 Real* v = V.Store(); Real* w = v + n;
266 *v = *x;
267 int k = 0; i = n2;
268 while (i--)
269 {
270 Real c, s; cossin(++k, n4, c, s);
271 Real xi = *(++x); Real yi = *(++y);
272 *(++v) = xi * c + yi * s; *(--w) = xi * s - yi * c;
273 }
274 }
275
DCT_II_inverse(const ColumnVector & V,ColumnVector & U)276 void DCT_II_inverse(const ColumnVector& V, ColumnVector& U)
277 {
278 // Inverse of discrete cosine transform, type II
279 Tracer trace("DCT_II_inverse");
280 REPORT
281 const int n = V.Nrows(); // length of array
282 const int n2 = n / 2; const int n4 = n * 4; const int n21 = n2 + 1;
283 if (n != 2 * n2)
284 Throw(ProgramException("Vector length not multiple of 2", V));
285 ColumnVector X(n21), Y(n21);
286 Real* x = X.Store(); Real* y = Y.Store();
287 Real* v = V.Store(); Real* w = v + n;
288 *x = *v; *y = 0.0;
289 int i = n2; int k = 0;
290 while (i--)
291 {
292 Real c, s; cossin(++k, n4, c, s);
293 Real vi = *(++v); Real wi = *(--w);
294 *(++x) = vi * c + wi * s; *(++y) = vi * s - wi * c;
295 }
296 ColumnVector A; RealFFTI(X, Y, A);
297 X.CleanUp(); Y.CleanUp(); U.ReSize(n);
298 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
299 i = n2;
300 while (i--) { *u++ = *a++; *u++ = *(--b); }
301 }
302
DST_II(const ColumnVector & U,ColumnVector & V)303 void DST_II(const ColumnVector& U, ColumnVector& V)
304 {
305 // Discrete sine transform, type II, of a real series
306 Tracer trace("DST_II");
307 REPORT
308 const int n = U.Nrows(); // length of arrays
309 const int n2 = n / 2; const int n4 = n * 4;
310 if (n != 2 * n2)
311 Throw(ProgramException("Vector length not multiple of 2", U));
312 ColumnVector A(n);
313 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
314 int i = n2;
315 while (i--) { *a++ = *u++; *(--b) = -(*u++); }
316 ColumnVector X, Y;
317 RealFFT(A, X, Y); A.CleanUp();
318 V.ReSize(n);
319 Real* x = X.Store(); Real* y = Y.Store();
320 Real* v = V.Store(); Real* w = v + n;
321 *(--w) = *x;
322 int k = 0; i = n2;
323 while (i--)
324 {
325 Real c, s; cossin(++k, n4, c, s);
326 Real xi = *(++x); Real yi = *(++y);
327 *v++ = xi * s - yi * c; *(--w) = xi * c + yi * s;
328 }
329 }
330
DST_II_inverse(const ColumnVector & V,ColumnVector & U)331 void DST_II_inverse(const ColumnVector& V, ColumnVector& U)
332 {
333 // Inverse of discrete sine transform, type II
334 Tracer trace("DST_II_inverse");
335 REPORT
336 const int n = V.Nrows(); // length of array
337 const int n2 = n / 2; const int n4 = n * 4; const int n21 = n2 + 1;
338 if (n != 2 * n2)
339 Throw(ProgramException("Vector length not multiple of 2", V));
340 ColumnVector X(n21), Y(n21);
341 Real* x = X.Store(); Real* y = Y.Store();
342 Real* v = V.Store(); Real* w = v + n;
343 *x = *(--w); *y = 0.0;
344 int i = n2; int k = 0;
345 while (i--)
346 {
347 Real c, s; cossin(++k, n4, c, s);
348 Real vi = *v++; Real wi = *(--w);
349 *(++x) = vi * s + wi * c; *(++y) = - vi * c + wi * s;
350 }
351 ColumnVector A; RealFFTI(X, Y, A);
352 X.CleanUp(); Y.CleanUp(); U.ReSize(n);
353 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store();
354 i = n2;
355 while (i--) { *u++ = *a++; *u++ = -(*(--b)); }
356 }
357
DCT_inverse(const ColumnVector & V,ColumnVector & U)358 void DCT_inverse(const ColumnVector& V, ColumnVector& U)
359 {
360 // Inverse of discrete cosine transform, type I
361 Tracer trace("DCT_inverse");
362 REPORT
363 const int n = V.Nrows()-1; // length of transform
364 const int n2 = n / 2; const int n21 = n2 + 1;
365 if (n != 2 * n2)
366 Throw(ProgramException("Vector length not multiple of 2", V));
367 ColumnVector X(n21), Y(n21);
368 Real* x = X.Store(); Real* y = Y.Store(); Real* v = V.Store();
369 Real vi = *v++; *x++ = vi; *y++ = 0.0;
370 Real sum1 = vi / 2.0; Real sum2 = sum1; vi = *v++;
371 int i = n2-1;
372 while (i--)
373 {
374 Real vi2 = *v++; sum1 += vi2 + vi; sum2 += vi2 - vi;
375 *x++ = vi2; vi2 = *v++; *y++ = vi - vi2; vi = vi2;
376 }
377 sum1 += vi; sum2 -= vi;
378 vi = *v; *x = vi; *y = 0.0; vi /= 2.0; sum1 += vi; sum2 += vi;
379 ColumnVector A; RealFFTI(X, Y, A);
380 X.CleanUp(); Y.CleanUp(); U.ReSize(n+1);
381 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store(); v = u + n;
382 i = n2; int k = 0; *u++ = sum1 / n2; *v-- = sum2 / n2;
383 while (i--)
384 {
385 Real s = sin(1.5707963267948966192 * (++k) / n2);
386 Real ai = *(++a); Real bi = *(--b);
387 Real bz = (ai - bi) / 4 / s; Real az = (ai + bi) / 2;
388 *u++ = az - bz; *v-- = az + bz;
389 }
390 }
391
DCT(const ColumnVector & U,ColumnVector & V)392 void DCT(const ColumnVector& U, ColumnVector& V)
393 {
394 // Discrete cosine transform, type I
395 Tracer trace("DCT");
396 REPORT
397 DCT_inverse(U, V);
398 V *= (V.Nrows()-1)/2;
399 }
400
DST_inverse(const ColumnVector & V,ColumnVector & U)401 void DST_inverse(const ColumnVector& V, ColumnVector& U)
402 {
403 // Inverse of discrete sine transform, type I
404 Tracer trace("DST_inverse");
405 REPORT
406 const int n = V.Nrows()-1; // length of transform
407 const int n2 = n / 2; const int n21 = n2 + 1;
408 if (n != 2 * n2)
409 Throw(ProgramException("Vector length not multiple of 2", V));
410 ColumnVector X(n21), Y(n21);
411 Real* x = X.Store(); Real* y = Y.Store(); Real* v = V.Store();
412 Real vi = *(++v); *x++ = 2 * vi; *y++ = 0.0;
413 int i = n2-1;
414 while (i--) { *y++ = *(++v); Real vi2 = *(++v); *x++ = vi2 - vi; vi = vi2; }
415 *x = -2 * vi; *y = 0.0;
416 ColumnVector A; RealFFTI(X, Y, A);
417 X.CleanUp(); Y.CleanUp(); U.ReSize(n+1);
418 Real* a = A.Store(); Real* b = a + n; Real* u = U.Store(); v = u + n;
419 i = n2; int k = 0; *u++ = 0.0; *v-- = 0.0;
420 while (i--)
421 {
422 Real s = sin(1.5707963267948966192 * (++k) / n2);
423 Real ai = *(++a); Real bi = *(--b);
424 Real az = (ai + bi) / 4 / s; Real bz = (ai - bi) / 2;
425 *u++ = az - bz; *v-- = az + bz;
426 }
427 }
428
DST(const ColumnVector & U,ColumnVector & V)429 void DST(const ColumnVector& U, ColumnVector& V)
430 {
431 // Discrete sine transform, type I
432 Tracer trace("DST");
433 REPORT
434 DST_inverse(U, V);
435 V *= (V.Nrows()-1)/2;
436 }
437
438 // Two dimensional FFT
FFT2(const Matrix & U,const Matrix & V,Matrix & X,Matrix & Y)439 void FFT2(const Matrix& U, const Matrix& V, Matrix& X, Matrix& Y)
440 {
441 Tracer trace("FFT2");
442 REPORT
443 int m = U.Nrows(); int n = U.Ncols();
444 if (m != V.Nrows() || n != V.Ncols() || m == 0 || n == 0)
445 Throw(ProgramException("Matrix dimensions unequal or zero", U, V));
446 X = U; Y = V;
447 int i; ColumnVector CVR; ColumnVector CVI;
448 for (i = 1; i <= m; ++i)
449 {
450 FFT(X.Row(i).t(), Y.Row(i).t(), CVR, CVI);
451 X.Row(i) = CVR.t(); Y.Row(i) = CVI.t();
452 }
453 for (i = 1; i <= n; ++i)
454 {
455 FFT(X.Column(i), Y.Column(i), CVR, CVI);
456 X.Column(i) = CVR; Y.Column(i) = CVI;
457 }
458 }
459
FFT2I(const Matrix & U,const Matrix & V,Matrix & X,Matrix & Y)460 void FFT2I(const Matrix& U, const Matrix& V, Matrix& X, Matrix& Y)
461 {
462 // Inverse transform
463 Tracer trace("FFT2I");
464 REPORT
465 FFT2(U,-V,X,Y);
466 const Real n = X.Nrows() * X.Ncols(); X /= n; Y /= (-n);
467 }
468
469
470 #ifdef use_namespace
471 }
472 #endif
473
474
475