source: ntrip/trunk/BNC/newmat/fft.cpp@ 3805

Last change on this file since 3805 was 2013, checked in by mervart, 15 years ago

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