1#include "./common.h"
2
3#ifndef SIGNALSMITH_FFT_V5
4#define SIGNALSMITH_FFT_V5
5
6#include "./perf.h"
7
8#include <vector>
9#include <complex>
10#include <cmath>
11
12namespace signalsmith { namespace fft {
13 /** @defgroup FFT FFT (complex and real)
14 @brief Fourier transforms (complex and real)
15
16 @{
17 @file
18 */
19
20 namespace _fft_impl {
21
22 template <typename V>
23 SIGNALSMITH_INLINE V complexReal(const std::complex<V> &c) {
24 return ((V*)(&c))[0];
25 }
26 template <typename V>
27 SIGNALSMITH_INLINE V complexImag(const std::complex<V> &c) {
28 return ((V*)(&c))[1];
29 }
30
31 // Complex multiplication has edge-cases around Inf/NaN - handling those properly makes std::complex non-inlineable, so we use our own
32 template <bool conjugateSecond, typename V>
33 SIGNALSMITH_INLINE std::complex<V> complexMul(const std::complex<V> &a, const std::complex<V> &b) {
34 V aReal = complexReal(a), aImag = complexImag(a);
35 V bReal = complexReal(b), bImag = complexImag(b);
36 return conjugateSecond ? std::complex<V>{
37 bReal*aReal + bImag*aImag,
38 bReal*aImag - bImag*aReal
39 } : std::complex<V>{
40 aReal*bReal - aImag*bImag,
41 aReal*bImag + aImag*bReal
42 };
43 }
44
45 template<bool flipped, typename V>
46 SIGNALSMITH_INLINE std::complex<V> complexAddI(const std::complex<V> &a, const std::complex<V> &b) {
47 V aReal = complexReal(a), aImag = complexImag(a);
48 V bReal = complexReal(b), bImag = complexImag(b);
49 return flipped ? std::complex<V>{
50 aReal + bImag,
51 aImag - bReal
52 } : std::complex<V>{
53 aReal - bImag,
54 aImag + bReal
55 };
56 }
57
58 // Use SFINAE to get an iterator from std::begin(), if supported - otherwise assume the value itself is an iterator
59 template<typename T, typename=void>
60 struct GetIterator {
61 static T get(const T &t) {
62 return t;
63 }
64 };
65 template<typename T>
66 struct GetIterator<T, decltype((void)std::begin(std::declval<T>()))> {
67 static auto get(const T &t) -> decltype(std::begin(t)) {
68 return std::begin(t);
69 }
70 };
71 }
72
73 /** Floating-point FFT implementation.
74 It is fast for 2^a * 3^b.
75 Here are the peak and RMS errors for `float`/`double` computation:
76 \diagram{fft-errors.svg Simulated errors for pure-tone harmonic inputs\, compared to a theoretical upper bound from "Roundoff error analysis of the fast Fourier transform" (G. Ramos, 1971)}
77 */
78 template<typename V=double>
79 class FFT {
80 using complex = std::complex<V>;
81 size_t _size;
82 std::vector<complex> workingVector;
83
84 enum class StepType {
85 generic, step2, step3, step4
86 };
87 struct Step {
88 StepType type;
89 size_t factor;
90 size_t startIndex;
91 size_t innerRepeats;
92 size_t outerRepeats;
93 size_t twiddleIndex;
94 };
95 std::vector<size_t> factors;
96 std::vector<Step> plan;
97 std::vector<complex> twiddleVector;
98
99 struct PermutationPair {size_t from, to;};
100 std::vector<PermutationPair> permutation;
101
102 void addPlanSteps(size_t factorIndex, size_t start, size_t length, size_t repeats) {
103 if (factorIndex >= factors.size()) return;
104
105 size_t factor = factors[factorIndex];
106 if (factorIndex + 1 < factors.size()) {
107 if (factors[factorIndex] == 2 && factors[factorIndex + 1] == 2) {
108 ++factorIndex;
109 factor = 4;
110 }
111 }
112
113 size_t subLength = length/factor;
114 Step mainStep{StepType::generic, factor, start, subLength, repeats, twiddleVector.size()};
115
116 if (factor == 2) mainStep.type = StepType::step2;
117 if (factor == 3) mainStep.type = StepType::step3;
118 if (factor == 4) mainStep.type = StepType::step4;
119
120 // Twiddles
121 bool foundStep = false;
122 for (const Step &existingStep : plan) {
123 if (existingStep.factor == mainStep.factor && existingStep.innerRepeats == mainStep.innerRepeats) {
124 foundStep = true;
125 mainStep.twiddleIndex = existingStep.twiddleIndex;
126 break;
127 }
128 }
129 if (!foundStep) {
130 for (size_t i = 0; i < subLength; ++i) {
131 for (size_t f = 0; f < factor; ++f) {
132 double phase = 2*M_PI*i*f/length;
133 complex twiddle = {V(std::cos(x: phase)), V(-std::sin(x: phase))};
134 twiddleVector.push_back(twiddle);
135 }
136 }
137 }
138
139 if (repeats == 1 && sizeof(complex)*subLength > 65536) {
140 for (size_t i = 0; i < factor; ++i) {
141 addPlanSteps(factorIndex: factorIndex + 1, start: start + i*subLength, length: subLength, repeats: 1);
142 }
143 } else {
144 addPlanSteps(factorIndex: factorIndex + 1, start, length: subLength, repeats: repeats*factor);
145 }
146 plan.push_back(mainStep);
147 }
148 void setPlan() {
149 factors.resize(new_size: 0);
150 size_t size = _size, factor = 2;
151 while (size > 1) {
152 if (size%factor == 0) {
153 factors.push_back(x: factor);
154 size /= factor;
155 } else if (factor > sqrt(x: size)) {
156 factor = size;
157 } else {
158 ++factor;
159 }
160 }
161
162 plan.resize(0);
163 twiddleVector.resize(0);
164 addPlanSteps(factorIndex: 0, start: 0, length: _size, repeats: 1);
165
166 permutation.resize(0);
167 permutation.push_back(PermutationPair{0, 0});
168 size_t indexLow = 0, indexHigh = factors.size();
169 size_t inputStepLow = _size, outputStepLow = 1;
170 size_t inputStepHigh = 1, outputStepHigh = _size;
171 while (outputStepLow*inputStepHigh < _size) {
172 size_t f, inputStep, outputStep;
173 if (outputStepLow <= inputStepHigh) {
174 f = factors[indexLow++];
175 inputStep = (inputStepLow /= f);
176 outputStep = outputStepLow;
177 outputStepLow *= f;
178 } else {
179 f = factors[--indexHigh];
180 inputStep = inputStepHigh;
181 inputStepHigh *= f;
182 outputStep = (outputStepHigh /= f);
183 }
184 size_t oldSize = permutation.size();
185 for (size_t i = 1; i < f; ++i) {
186 for (size_t j = 0; j < oldSize; ++j) {
187 PermutationPair pair = permutation[j];
188 pair.from += i*inputStep;
189 pair.to += i*outputStep;
190 permutation.push_back(pair);
191 }
192 }
193 }
194 }
195
196 template<bool inverse, typename RandomAccessIterator>
197 void fftStepGeneric(RandomAccessIterator &&origData, const Step &step) {
198 complex *working = workingVector.data();
199 const size_t stride = step.innerRepeats;
200
201 for (size_t outerRepeat = 0; outerRepeat < step.outerRepeats; ++outerRepeat) {
202 RandomAccessIterator data = origData;
203
204 const complex *twiddles = twiddleVector.data() + step.twiddleIndex;
205 const size_t factor = step.factor;
206 for (size_t repeat = 0; repeat < step.innerRepeats; ++repeat) {
207 for (size_t i = 0; i < step.factor; ++i) {
208 working[i] = _fft_impl::complexMul<inverse>(data[i*stride], twiddles[i]);
209 }
210 for (size_t f = 0; f < factor; ++f) {
211 complex sum = working[0];
212 for (size_t i = 1; i < factor; ++i) {
213 double phase = 2*M_PI*f*i/factor;
214 complex twiddle = {V(std::cos(x: phase)), V(-std::sin(x: phase))};
215 sum += _fft_impl::complexMul<inverse>(working[i], twiddle);
216 }
217 data[f*stride] = sum;
218 }
219 ++data;
220 twiddles += factor;
221 }
222 origData += step.factor*step.innerRepeats;
223 }
224 }
225
226 template<bool inverse, typename RandomAccessIterator>
227 SIGNALSMITH_INLINE void fftStep2(RandomAccessIterator &&origData, const Step &step) {
228 const size_t stride = step.innerRepeats;
229 const complex *origTwiddles = twiddleVector.data() + step.twiddleIndex;
230 for (size_t outerRepeat = 0; outerRepeat < step.outerRepeats; ++outerRepeat) {
231 const complex* twiddles = origTwiddles;
232 for (RandomAccessIterator data = origData; data < origData + stride; ++data) {
233 complex A = data[0];
234 complex B = _fft_impl::complexMul<inverse>(data[stride], twiddles[1]);
235
236 data[0] = A + B;
237 data[stride] = A - B;
238 twiddles += 2;
239 }
240 origData += 2*stride;
241 }
242 }
243
244 template<bool inverse, typename RandomAccessIterator>
245 SIGNALSMITH_INLINE void fftStep3(RandomAccessIterator &&origData, const Step &step) {
246 constexpr complex factor3 = {-0.5, inverse ? 0.8660254037844386 : -0.8660254037844386};
247 const size_t stride = step.innerRepeats;
248 const complex *origTwiddles = twiddleVector.data() + step.twiddleIndex;
249
250 for (size_t outerRepeat = 0; outerRepeat < step.outerRepeats; ++outerRepeat) {
251 const complex* twiddles = origTwiddles;
252 for (RandomAccessIterator data = origData; data < origData + stride; ++data) {
253 complex A = data[0];
254 complex B = _fft_impl::complexMul<inverse>(data[stride], twiddles[1]);
255 complex C = _fft_impl::complexMul<inverse>(data[stride*2], twiddles[2]);
256
257 complex realSum = A + (B + C)*factor3.real();
258 complex imagSum = (B - C)*factor3.imag();
259
260 data[0] = A + B + C;
261 data[stride] = _fft_impl::complexAddI<false>(realSum, imagSum);
262 data[stride*2] = _fft_impl::complexAddI<true>(realSum, imagSum);
263
264 twiddles += 3;
265 }
266 origData += 3*stride;
267 }
268 }
269
270 template<bool inverse, typename RandomAccessIterator>
271 SIGNALSMITH_INLINE void fftStep4(RandomAccessIterator &&origData, const Step &step) {
272 const size_t stride = step.innerRepeats;
273 const complex *origTwiddles = twiddleVector.data() + step.twiddleIndex;
274
275 for (size_t outerRepeat = 0; outerRepeat < step.outerRepeats; ++outerRepeat) {
276 const complex* twiddles = origTwiddles;
277 for (RandomAccessIterator data = origData; data < origData + stride; ++data) {
278 complex A = data[0];
279 complex C = _fft_impl::complexMul<inverse>(data[stride], twiddles[2]);
280 complex B = _fft_impl::complexMul<inverse>(data[stride*2], twiddles[1]);
281 complex D = _fft_impl::complexMul<inverse>(data[stride*3], twiddles[3]);
282
283 complex sumAC = A + C, sumBD = B + D;
284 complex diffAC = A - C, diffBD = B - D;
285
286 data[0] = sumAC + sumBD;
287 data[stride] = _fft_impl::complexAddI<!inverse>(diffAC, diffBD);
288 data[stride*2] = sumAC - sumBD;
289 data[stride*3] = _fft_impl::complexAddI<inverse>(diffAC, diffBD);
290
291 twiddles += 4;
292 }
293 origData += 4*stride;
294 }
295 }
296
297 template<typename InputIterator, typename OutputIterator>
298 void permute(InputIterator input, OutputIterator data) {
299 for (auto pair : permutation) {
300 data[pair.from] = input[pair.to];
301 }
302 }
303
304 template<bool inverse, typename InputIterator, typename OutputIterator>
305 void run(InputIterator &&input, OutputIterator &&data) {
306 permute(input, data);
307
308 for (const Step &step : plan) {
309 switch (step.type) {
310 case StepType::generic:
311 fftStepGeneric<inverse>(data + step.startIndex, step);
312 break;
313 case StepType::step2:
314 fftStep2<inverse>(data + step.startIndex, step);
315 break;
316 case StepType::step3:
317 fftStep3<inverse>(data + step.startIndex, step);
318 break;
319 case StepType::step4:
320 fftStep4<inverse>(data + step.startIndex, step);
321 break;
322 }
323 }
324 }
325
326 static bool validSize(size_t size) {
327 constexpr static bool filter[32] = {
328 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, // 0-9
329 0, 0, 1, 0, 0, 0, 1, 0, 1, 0, // 10-19
330 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, // 20-29
331 0, 0
332 };
333 return filter[size];
334 }
335 public:
336 static size_t fastSizeAbove(size_t size) {
337 size_t power2 = 1;
338 while (size >= 32) {
339 size = (size - 1)/2 + 1;
340 power2 *= 2;
341 }
342 while (size < 32 && !validSize(size)) {
343 ++size;
344 }
345 return power2*size;
346 }
347 static size_t fastSizeBelow(size_t size) {
348 size_t power2 = 1;
349 while (size >= 32) {
350 size /= 2;
351 power2 *= 2;
352 }
353 while (size > 1 && !validSize(size)) {
354 --size;
355 }
356 return power2*size;
357 }
358
359 FFT(size_t size, int fastDirection=0) : _size(0) {
360 if (fastDirection > 0) size = fastSizeAbove(size);
361 if (fastDirection < 0) size = fastSizeBelow(size);
362 this->setSize(size);
363 }
364
365 size_t setSize(size_t size) {
366 if (size != _size) {
367 _size = size;
368 workingVector.resize(size);
369 setPlan();
370 }
371 return _size;
372 }
373 size_t setFastSizeAbove(size_t size) {
374 return setSize(fastSizeAbove(size));
375 }
376 size_t setFastSizeBelow(size_t size) {
377 return setSize(fastSizeBelow(size));
378 }
379 const size_t & size() const {
380 return _size;
381 }
382
383 template<typename InputIterator, typename OutputIterator>
384 void fft(InputIterator &&input, OutputIterator &&output) {
385 auto inputIter = _fft_impl::GetIterator<InputIterator>::get(input);
386 auto outputIter = _fft_impl::GetIterator<OutputIterator>::get(output);
387 return run<false>(inputIter, outputIter);
388 }
389
390 template<typename InputIterator, typename OutputIterator>
391 void ifft(InputIterator &&input, OutputIterator &&output) {
392 auto inputIter = _fft_impl::GetIterator<InputIterator>::get(input);
393 auto outputIter = _fft_impl::GetIterator<OutputIterator>::get(output);
394 return run<true>(inputIter, outputIter);
395 }
396 };
397
398 struct FFTOptions {
399 static constexpr int halfFreqShift = 1;
400 };
401
402 template<typename V, int optionFlags=0>
403 class RealFFT {
404 static constexpr bool modified = (optionFlags&FFTOptions::halfFreqShift);
405
406 using complex = std::complex<V>;
407 std::vector<complex> complexBuffer1, complexBuffer2;
408 std::vector<complex> twiddlesMinusI;
409 std::vector<complex> modifiedRotations;
410 FFT<V> complexFft;
411 public:
412 static size_t fastSizeAbove(size_t size) {
413 return FFT<V>::fastSizeAbove((size + 1)/2)*2;
414 }
415 static size_t fastSizeBelow(size_t size) {
416 return FFT<V>::fastSizeBelow(size/2)*2;
417 }
418
419 RealFFT(size_t size=0, int fastDirection=0) : complexFft(0) {
420 if (fastDirection > 0) size = fastSizeAbove(size);
421 if (fastDirection < 0) size = fastSizeBelow(size);
422 this->setSize(std::max<size_t>(a: size, b: 2));
423 }
424
425 size_t setSize(size_t size) {
426 complexBuffer1.resize(size/2);
427 complexBuffer2.resize(size/2);
428
429 size_t hhSize = size/4 + 1;
430 twiddlesMinusI.resize(hhSize);
431 for (size_t i = 0; i < hhSize; ++i) {
432 V rotPhase = -2*M_PI*(modified ? i + 0.5 : i)/size;
433 twiddlesMinusI[i] = {std::sin(rotPhase), -std::cos(rotPhase)};
434 }
435 if (modified) {
436 modifiedRotations.resize(size/2);
437 for (size_t i = 0; i < size/2; ++i) {
438 V rotPhase = -2*M_PI*i/size;
439 modifiedRotations[i] = {std::cos(rotPhase), std::sin(rotPhase)};
440 }
441 }
442
443 return complexFft.setSize(size/2);
444 }
445 size_t setFastSizeAbove(size_t size) {
446 return setSize(fastSizeAbove(size));
447 }
448 size_t setFastSizeBelow(size_t size) {
449 return setSize(fastSizeBelow(size));
450 }
451 size_t size() const {
452 return complexFft.size()*2;
453 }
454
455 template<typename InputIterator, typename OutputIterator>
456 void fft(InputIterator &&input, OutputIterator &&output) {
457 size_t hSize = complexFft.size();
458 for (size_t i = 0; i < hSize; ++i) {
459 if (modified) {
460 complexBuffer1[i] = _fft_impl::complexMul<false>({input[2*i], input[2*i + 1]}, modifiedRotations[i]);
461 } else {
462 complexBuffer1[i] = {input[2*i], input[2*i + 1]};
463 }
464 }
465
466 complexFft.fft(complexBuffer1.data(), complexBuffer2.data());
467
468 if (!modified) output[0] = {
469 complexBuffer2[0].real() + complexBuffer2[0].imag(),
470 complexBuffer2[0].real() - complexBuffer2[0].imag()
471 };
472 for (size_t i = modified ? 0 : 1; i <= hSize/2; ++i) {
473 size_t conjI = modified ? (hSize - 1 - i) : (hSize - i);
474
475 complex odd = (complexBuffer2[i] + conj(complexBuffer2[conjI]))*(V)0.5;
476 complex evenI = (complexBuffer2[i] - conj(complexBuffer2[conjI]))*(V)0.5;
477 complex evenRotMinusI = _fft_impl::complexMul<false>(evenI, twiddlesMinusI[i]);
478
479 output[i] = odd + evenRotMinusI;
480 output[conjI] = conj(odd - evenRotMinusI);
481 }
482 }
483
484 template<typename InputIterator, typename OutputIterator>
485 void ifft(InputIterator &&input, OutputIterator &&output) {
486 size_t hSize = complexFft.size();
487 if (!modified) complexBuffer1[0] = {
488 input[0].real() + input[0].imag(),
489 input[0].real() - input[0].imag()
490 };
491 for (size_t i = modified ? 0 : 1; i <= hSize/2; ++i) {
492 size_t conjI = modified ? (hSize - 1 - i) : (hSize - i);
493 complex v = input[i], v2 = input[conjI];
494
495 complex odd = v + conj(v2);
496 complex evenRotMinusI = v - conj(v2);
497 complex evenI = _fft_impl::complexMul<true>(evenRotMinusI, twiddlesMinusI[i]);
498
499 complexBuffer1[i] = odd + evenI;
500 complexBuffer1[conjI] = conj(odd - evenI);
501 }
502
503 complexFft.ifft(complexBuffer1.data(), complexBuffer2.data());
504
505 for (size_t i = 0; i < hSize; ++i) {
506 complex v = complexBuffer2[i];
507 if (modified) v = _fft_impl::complexMul<true>(v, modifiedRotations[i]);
508 output[2*i] = v.real();
509 output[2*i + 1] = v.imag();
510 }
511 }
512 };
513
514 template<typename V>
515 struct ModifiedRealFFT : public RealFFT<V, FFTOptions::halfFreqShift> {
516 using RealFFT<V, FFTOptions::halfFreqShift>::RealFFT;
517 };
518
519/// @}
520}} // namespace
521#endif // include guard
522

source code of qtmultimedia/src/3rdparty/signalsmith-stretch/dsp/fft.h