Chris@337
|
1
|
Chris@337
|
2 /* Public domain FFT implementation from Don Cross. */
|
Chris@337
|
3
|
Chris@337
|
4 static void
|
Chris@337
|
5 fft(unsigned int n, bool inverse,
|
Chris@337
|
6 const double *ri, const double *ii,
|
Chris@337
|
7 double *ro, double *io)
|
Chris@337
|
8 {
|
Chris@337
|
9 if (!ri || !ro || !io) return;
|
Chris@337
|
10
|
Chris@337
|
11 unsigned int bits;
|
Chris@337
|
12 unsigned int i, j, k, m;
|
Chris@337
|
13 unsigned int blockSize, blockEnd;
|
Chris@337
|
14
|
Chris@337
|
15 double tr, ti;
|
Chris@337
|
16
|
Chris@337
|
17 if (n < 2) return;
|
Chris@337
|
18 if (n & (n-1)) return;
|
Chris@337
|
19
|
Chris@337
|
20 double angle = 2.0 * M_PI;
|
Chris@337
|
21 if (inverse) angle = -angle;
|
Chris@337
|
22
|
Chris@337
|
23 for (i = 0; ; ++i) {
|
Chris@337
|
24 if (n & (1 << i)) {
|
Chris@337
|
25 bits = i;
|
Chris@337
|
26 break;
|
Chris@337
|
27 }
|
Chris@337
|
28 }
|
Chris@337
|
29
|
Chris@337
|
30 int table[n];
|
Chris@337
|
31
|
Chris@337
|
32 for (i = 0; i < n; ++i) {
|
Chris@337
|
33 m = i;
|
Chris@337
|
34 for (j = k = 0; j < bits; ++j) {
|
Chris@337
|
35 k = (k << 1) | (m & 1);
|
Chris@337
|
36 m >>= 1;
|
Chris@337
|
37 }
|
Chris@337
|
38 table[i] = k;
|
Chris@337
|
39 }
|
Chris@337
|
40
|
Chris@337
|
41 if (ii) {
|
Chris@337
|
42 for (i = 0; i < n; ++i) {
|
Chris@337
|
43 ro[table[i]] = ri[i];
|
Chris@337
|
44 io[table[i]] = ii[i];
|
Chris@337
|
45 }
|
Chris@337
|
46 } else {
|
Chris@337
|
47 for (i = 0; i < n; ++i) {
|
Chris@337
|
48 ro[table[i]] = ri[i];
|
Chris@337
|
49 io[table[i]] = 0.0;
|
Chris@337
|
50 }
|
Chris@337
|
51 }
|
Chris@337
|
52
|
Chris@337
|
53 blockEnd = 1;
|
Chris@337
|
54
|
Chris@337
|
55 for (blockSize = 2; blockSize <= n; blockSize <<= 1) {
|
Chris@337
|
56
|
Chris@337
|
57 double delta = angle / (double)blockSize;
|
Chris@337
|
58 double sm2 = -sin(-2 * delta);
|
Chris@337
|
59 double sm1 = -sin(-delta);
|
Chris@337
|
60 double cm2 = cos(-2 * delta);
|
Chris@337
|
61 double cm1 = cos(-delta);
|
Chris@337
|
62 double w = 2 * cm1;
|
Chris@337
|
63 double ar[3], ai[3];
|
Chris@337
|
64
|
Chris@337
|
65 for (i = 0; i < n; i += blockSize) {
|
Chris@337
|
66
|
Chris@337
|
67 ar[2] = cm2;
|
Chris@337
|
68 ar[1] = cm1;
|
Chris@337
|
69
|
Chris@337
|
70 ai[2] = sm2;
|
Chris@337
|
71 ai[1] = sm1;
|
Chris@337
|
72
|
Chris@337
|
73 for (j = i, m = 0; m < blockEnd; j++, m++) {
|
Chris@337
|
74
|
Chris@337
|
75 ar[0] = w * ar[1] - ar[2];
|
Chris@337
|
76 ar[2] = ar[1];
|
Chris@337
|
77 ar[1] = ar[0];
|
Chris@337
|
78
|
Chris@337
|
79 ai[0] = w * ai[1] - ai[2];
|
Chris@337
|
80 ai[2] = ai[1];
|
Chris@337
|
81 ai[1] = ai[0];
|
Chris@337
|
82
|
Chris@337
|
83 k = j + blockEnd;
|
Chris@337
|
84 tr = ar[0] * ro[k] - ai[0] * io[k];
|
Chris@337
|
85 ti = ar[0] * io[k] + ai[0] * ro[k];
|
Chris@337
|
86
|
Chris@337
|
87 ro[k] = ro[j] - tr;
|
Chris@337
|
88 io[k] = io[j] - ti;
|
Chris@337
|
89
|
Chris@337
|
90 ro[j] += tr;
|
Chris@337
|
91 io[j] += ti;
|
Chris@337
|
92 }
|
Chris@337
|
93 }
|
Chris@337
|
94
|
Chris@337
|
95 blockEnd = blockSize;
|
Chris@337
|
96 }
|
Chris@337
|
97
|
Chris@337
|
98 if (inverse) {
|
Chris@337
|
99
|
Chris@337
|
100 double denom = (double)n;
|
Chris@337
|
101
|
Chris@337
|
102 for (i = 0; i < n; i++) {
|
Chris@337
|
103 ro[i] /= denom;
|
Chris@337
|
104 io[i] /= denom;
|
Chris@337
|
105 }
|
Chris@337
|
106 }
|
Chris@337
|
107 }
|
Chris@337
|
108
|