libclod
C library for interacting with NBTs, region files, LOD data and other things.
Loading...
Searching...
No Matches
fft.c
1#include <signal.h>
2#include <stdbool.h>
3
4#include "config.h"
5#include "debug.h"
6#include <clod/math/fft.h>
7
8#define PI 3.141592653589793238462643383279502884197169399375105820974944592307f
9
10#define R 0
11#define I 1
12
13#define INPUT_FORMAT(opt) (((opt) / CLOD_FFT_INPUT) & 0xFF)
14#define OUTPUT_FORMAT(opt) (((opt) / CLOD_FFT_OUTPUT) & 0xFF)
15
16static float fft_sin(float n) {
17 while (n > PI) n -= 2.0f * PI;
18 while (n < -PI) n += 2.0f * PI;
19
20 float res = n;
21 float s = n;
22 for (int i = 1; i <= 12; i += 2) {
23 res += s = -s * n * n / (float)((i + 1) * (i + 2));
24 }
25 return res;
26}
27static float fft_cos(float n) {
28 while (n > PI) n -= 2.0f * PI;
29 while (n < -PI) n += 2.0f * PI;
30
31 float res = 1.0f;
32 float s = 1.0f;
33 for (int i = 0; i <= 12; i += 2) {
34 res += s = -s * n * n / (float)((i + 1) * (i + 2));
35 }
36 return res;
37}
38static float fft_atan(const float x) {
39 if (x > 1.0f) return PI / 2.0f - fft_atan(1.0f / x);
40 if (x < -1.0f) return -PI / 2.0f - fft_atan(1.0f / x);
41
42 float res = x;
43 float s = x;
44 for (int i = 1; i <= 36; i++) {
45 res += s = -s * x * x * (float)(2 * i - 1) / (float)(2 * i + 1);
46 }
47 return res;
48}
49static float fft_sqrt(const float x) {
50 if (x <= 0.0f) return 0.0f;
51 float guess = x / 2.0f;
52 for (int i = 0; i < 8; i++) {
53 guess = (guess + x / guess) / 2.0f;
54 }
55 return guess;
56}
57static float fft_atan2(const float y, const float x) {
58 if (x > 0.0f) return fft_atan(y / x);
59 if (x == 0.0f) {
60 if (y > 0.0f) return PI / 2.0f;
61 if (y < 0.0f) return -PI / 2.0f;
62 return 0.0f;
63 }
64 if (y >= 0.0f) return fft_atan(y / x) + PI;
65 return fft_atan(y / x) - PI;
66}
67
68static void swap(float *restrict a, float *restrict b) {
69 float tmp[2];
70 tmp[R] = a[R];
71 tmp[I] = a[I];
72 a[R] = b[R];
73 a[I] = b[I];
74 b[R] = tmp[R];
75 b[I] = tmp[I];
76}
77static void mul(float a[2], const float b[2]) {
78 const float tmp = a[R] * b[R] - a[I] * b[I];
79 a[I] = a[R] * b[I] + a[I] * b[R];
80 a[R] = tmp;
81}
82static void complex_to_mag(float *restrict data, const size_t len) {
83 for (size_t i = 0; i < len; i++) {
84 const float real = data[i * 2 + R];
85 const float imag = data[i * 2 + I];
86 data[i * 2 + R] = fft_sqrt(real * real + imag * imag);
87 data[i * 2 + I] = fft_atan2(imag, real);
88 }
89}
90/*
91static void complex_inverted_to_mag(float *restrict data, const size_t len) {
92 for (size_t i = 0; i < len; i++) {
93 float n[2] = { data[i * 2 + R], data[i * 2 + I] };
94 n[I] *= -1.0f;
95 mul(n, (float[2]){ 1.0f / (float)len, 0.0f });
96 data[i * 2 + R] = fft_sqrt(n[R] * n[R] + n[I] * n[I]);
97 data[i * 2 + I] = fft_atan2(n[I], n[R]);
98 }
99}
100*/
101static void mag_to_complex(float *restrict data, const size_t len) {
102 for (size_t i = 0; i <= len / 2; i++) {
103 const float mag = data[i * 2 + R];
104 const float phase = data[i * 2 + I];
105 const float real = mag * fft_cos(phase);
106 const float imag = mag * fft_sin(phase);
107 data[i * 2 + R] = real;
108 data[i * 2 + I] = imag;
109
110 if (i > 0 && i < len / 2) {
111 const size_t conjugate = len - i;
112 data[conjugate * 2 + R] = real;
113 data[conjugate * 2 + I] = -imag;
114 }
115 }
116}
117/*
118static void mag_to_complex_inverted(float *restrict data, const size_t len) {
119 for (size_t i = 0; i < len; i++) {
120 float n[2] = { data[i * 2 + R], data[i * 2 + I] };
121 n[I] *= -1.0f;
122 data[i * 2 + R] = n[R] * fft_cos(n[I]);
123 data[i * 2 + I] = n[R] * fft_sin(n[I]);
124 }
125}
126*/
127static void pack(float *restrict data, const size_t len, const float padding) {
128 size_t i = 0;
129 while (i < len / 2) {
130 data[i] = data[i * 2];
131 i++;
132 }
133 while (i < len) {
134 data[i] = data[i * 2];
135 data[i * 2] = padding;
136 data[i * 2 + 1] = padding;
137 i++;
138 }
139}
140static void invert_imag(float *restrict data, const size_t len) {
141 for (size_t i = 0; i < len; i++) {
142 data[i * 2 + 1] *= -1.0f;
143 }
144}
145static void invert_scale(float *restrict data, const size_t len) {
146 for (size_t i = 0; i < len; i++) {
147 mul(&data[i * 2], (float[2]){ 1.0f / (float)len, 0.0f });
148 }
149}
150static void invert_scale_pack(float *restrict data, const size_t len, const float padding) {
151 size_t i = 0;
152 while (i < len / 2) {
153 float n[2] = { data[i * 2 + R], data[i * 2 + I] };
154 n[I] *= -1.0f;
155 mul(n, (float[2]){ 1.0f / (float)len, 0.0f });
156 data[i] = n[R];
157 i++;
158 }
159 while (i < len) {
160 float n[2] = { data[i * 2 + R], data[i * 2 + I] };
161 n[I] *= -1.0f;
162 mul(n, (float[2]){ 1.0f / (float)len, 0.0f });
163 data[i] = n[R];
164 data[i * 2 + R] = padding;
165 data[i * 2 + I] = padding;
166 i++;
167 }
168}
169
170static void fft(float *data, const size_t len) {
171 for (size_t i = 1, rev = 0; i < len; i++) {
172 size_t b = len >> 1;
173 while (rev & b) {
174 rev ^= b;
175 b >>= 1;
176 }
177 rev ^= b;
178 if (data[i * 2] != data[i * 2]) {
179 data[i * 2] = 0.0f;
180 debug(CLOD_DEBUG, "NaN detected in FFT input data.");
181 }
182 if (data[i * 2 + 1] != data[i * 2 + 1]) {
183 data[i * 2 + 1] = 0.0f;
184 debug(CLOD_DEBUG, "NaN detected in FFT input data.");
185 }
186 if (i < rev) {
187 swap(data + i * 2, data + rev * 2);
188 }
189 }
190
191 for (size_t s = 2; s <= len; s <<= 1) {
192 const float wn[2] = {
193 fft_cos(2.0f * PI / (float)s),
194 fft_sin(2.0f * PI / (float)s)
195 };
196
197 for (size_t i = 0; i < len; i += s) {
198 float w[2] = {1.0f, 0.0f};
199 for (size_t j = 0; j < s / 2; j++) {
200 const size_t index1 = 2 * (i + j);
201 const size_t index2 = 2 * (i + j + s / 2);
202
203 float u[2] = {data[index1], data[index1 + 1]};
204 float v[2] = {data[index2], data[index2 + 1]};
205 mul(v, w);
206
207 data[index1] = u[R] + v[R];
208 data[index1 + 1] = u[I] + v[I];
209
210 data[index2] = u[R] - v[R];
211 data[index2 + 1] = u[I] - v[I];
212 mul(w, wn);
213 }
214 }
215 }
216}
217
218void clod_fft(float *restrict data, const size_t len, const int opt) {
219 if (len == 0) goto error;
220
221 switch (INPUT_FORMAT(opt)) {
222
223 case CLOD_FFT_TIME_MAG_PACKED:
224 switch (OUTPUT_FORMAT(opt)) {
225
226 case CLOD_FFT_TIME_COMPLEX:
227 for (size_t i = len; i > 0; i--) {
228 data[(i - 1) * 2] = data[i - 1];
229 data[(i - 1) * 2 + 1] = 0.0f;
230 }
231 return;
232
233 case CLOD_FFT_TIME_MAG_PACKED:
234 for (size_t i = len; i < len * 2; i++)
235 data[i] = 0.0f;
236 return;
237
238 case CLOD_FFT_FREQ_COMPLEX:
239 for (size_t i = len; i > 0; i--) {
240 data[(i - 1) * 2] = data[i - 1];
241 data[(i - 1) * 2 + 1] = 0.0f;
242 }
243 fft(data, len);
244 return;
245
246 case CLOD_FFT_FREQ_MAG:
247 for (size_t i = len; i > 0; i--) {
248 data[(i - 1) * 2] = data[i - 1];
249 data[(i - 1) * 2 + 1] = 0.0f;
250 }
251 fft(data, len);
252 complex_to_mag(data, len);
253 return;
254
255 default: goto error;
256 }
257
258 case CLOD_FFT_TIME_COMPLEX:
259 switch (OUTPUT_FORMAT(opt)) {
260
261 case CLOD_FFT_TIME_COMPLEX:
262 return;
263
264 case CLOD_FFT_TIME_MAG_PACKED:
265 pack(data, len, 0.0f);
266 return;
267
268 case CLOD_FFT_FREQ_COMPLEX:
269 fft(data, len);
270 return;
271
272 case CLOD_FFT_FREQ_MAG:
273 fft(data, len);
274 complex_to_mag(data, len);
275 return;
276
277 default: goto error;
278 }
279
280 case CLOD_FFT_FREQ_COMPLEX:
281 switch (OUTPUT_FORMAT(opt)) {
282
283 case CLOD_FFT_TIME_COMPLEX:
284 invert_imag(data, len);
285 fft(data, len);
286 invert_scale(data, len);
287 return;
288
289 case CLOD_FFT_TIME_MAG_PACKED:
290 invert_imag(data, len);
291 fft(data, len);
292 invert_scale_pack(data, len, 0.0f);
293 return;
294
295 case CLOD_FFT_FREQ_COMPLEX:
296 return;
297
298 case CLOD_FFT_FREQ_MAG:
299 complex_to_mag(data, len);
300 return;
301
302 default: goto error;
303 }
304
305 case CLOD_FFT_FREQ_MAG:
306 switch (OUTPUT_FORMAT(opt)) {
307
308 case CLOD_FFT_TIME_COMPLEX:
309 mag_to_complex(data, len);
310 invert_imag(data, len);
311 fft(data, len);
312 invert_scale(data, len);
313 return;
314
315 case CLOD_FFT_TIME_MAG_PACKED:
316 mag_to_complex(data, len);
317 invert_imag(data, len);
318 fft(data, len);
319 invert_scale_pack(data, len, 0.0f);
320 return;
321
322 case CLOD_FFT_FREQ_COMPLEX:
323 mag_to_complex(data, len);
324 return;
325
326 case CLOD_FFT_FREQ_MAG:
327 return;
328
329 default: goto error;
330 }
331
332 default: goto error;
333 }
334
335error:
336 debug(CLOD_DEBUG, "Ignoring invalid FFT arguments (%ptr, %size, %bi).", (void*)data, len, opt);
337}