Eigen-Contrib  5.0.1-dev+master
 
Loading...
Searching...
No Matches
kissfft_impl.h
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2009 Mark Borgerding mark a borgerding net
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9// SPDX-License-Identifier: MPL-2.0
10
11#ifndef EIGEN_FFT_KISSFFT_IMPL_H
12#define EIGEN_FFT_KISSFFT_IMPL_H
13
14// IWYU pragma: private
15#include "./InternalHeaderCheck.h"
16
17namespace Eigen {
18
19namespace internal {
20
21// This FFT implementation was derived from kissfft http://sourceforge.net/projects/kissfft
22// Copyright 2003-2009 Mark Borgerding
23
24template <typename Scalar_>
25struct kiss_cpx_fft {
26 using Scalar = Scalar_;
27 using Complex = std::complex<Scalar>;
28 std::vector<Complex> m_twiddles;
29 std::vector<int> m_stageRadix;
30 std::vector<int> m_stageRemainder;
31 std::vector<Complex> m_scratchBuf;
32 bool m_inverse;
33
34 static const Scalar m_pi4; // constant pi / 4
35
36 inline void make_twiddles(int nfft, bool inverse) {
37 using numext::cos;
38 using numext::sin;
39 m_inverse = inverse;
40 m_twiddles.resize(nfft);
41 Scalar phinc = m_pi4 / nfft;
42 Scalar flip = inverse ? Scalar(1) : Scalar(-1);
43 m_twiddles[0] = Complex(Scalar(1), Scalar(0));
44 if ((nfft & 1) == 0) m_twiddles[nfft / 2] = Complex(Scalar(-1), Scalar(0));
45 int i = 1;
46 for (; i * 8 < nfft; ++i) {
47 Scalar c = Scalar(cos(i * 8 * phinc));
48 Scalar s = Scalar(sin(i * 8 * phinc));
49 m_twiddles[i] = Complex(c, s * flip);
50 m_twiddles[nfft - i] = Complex(c, -s * flip);
51 }
52 for (; i * 4 < nfft; ++i) {
53 Scalar c = Scalar(cos((2 * nfft - 8 * i) * phinc));
54 Scalar s = Scalar(sin((2 * nfft - 8 * i) * phinc));
55 m_twiddles[i] = Complex(s, c * flip);
56 m_twiddles[nfft - i] = Complex(s, -c * flip);
57 }
58 for (; i * 8 < 3 * nfft; ++i) {
59 Scalar c = Scalar(cos((8 * i - 2 * nfft) * phinc));
60 Scalar s = Scalar(sin((8 * i - 2 * nfft) * phinc));
61 m_twiddles[i] = Complex(-s, c * flip);
62 m_twiddles[nfft - i] = Complex(-s, -c * flip);
63 }
64 for (; i * 2 < nfft; ++i) {
65 Scalar c = Scalar(cos((4 * nfft - 8 * i) * phinc));
66 Scalar s = Scalar(sin((4 * nfft - 8 * i) * phinc));
67 m_twiddles[i] = Complex(-c, s * flip);
68 m_twiddles[nfft - i] = Complex(-c, -s * flip);
69 }
70 }
71
72 void factorize(int nfft) {
73 // start factoring out 4's, then 2's, then 3,5,7,9,...
74 int n = nfft;
75 int p = 4;
76 do {
77 while (n % p) {
78 switch (p) {
79 case 4:
80 p = 2;
81 break;
82 case 2:
83 p = 3;
84 break;
85 default:
86 p += 2;
87 break;
88 }
89 if (p * p > n) p = n; // impossible to have a factor > sqrt(n)
90 }
91 n /= p;
92 m_stageRadix.push_back(p);
93 m_stageRemainder.push_back(n);
94 if (p > 5) m_scratchBuf.resize(p); // scratchbuf will be needed in bfly_generic
95 } while (n > 1);
96 }
97
98 template <typename Src_>
99 inline void work(int stage, Complex *xout, const Src_ *xin, size_t fstride, size_t in_stride) {
100 int p = m_stageRadix[stage];
101 int m = m_stageRemainder[stage];
102 Complex *Fout_beg = xout;
103 Complex *Fout_end = xout + p * m;
104
105 if (m > 1) {
106 do {
107 // recursive call:
108 // DFT of size m*p performed by doing
109 // p instances of smaller DFTs of size m,
110 // each one takes a decimated version of the input
111 work(stage + 1, xout, xin, fstride * p, in_stride);
112 xin += fstride * in_stride;
113 } while ((xout += m) != Fout_end);
114 } else {
115 do {
116 *xout = *xin;
117 xin += fstride * in_stride;
118 } while (++xout != Fout_end);
119 }
120 xout = Fout_beg;
121
122 // recombine the p smaller DFTs
123 switch (p) {
124 case 1: // nfft == 1: the copy above is the whole transform
125 break;
126 case 2:
127 bfly2(xout, fstride, m);
128 break;
129 case 3:
130 bfly3(xout, fstride, m);
131 break;
132 case 4:
133 bfly4(xout, fstride, m);
134 break;
135 case 5:
136 bfly5(xout, fstride, m);
137 break;
138 default:
139 bfly_generic(xout, fstride, m, p);
140 break;
141 }
142 }
143
144 inline void bfly2(Complex *Fout, const size_t fstride, int m) {
145 for (int k = 0; k < m; ++k) {
146 Complex t = Fout[m + k] * m_twiddles[k * fstride];
147 Fout[m + k] = Fout[k] - t;
148 Fout[k] += t;
149 }
150 }
151
152 inline void bfly4(Complex *Fout, const size_t fstride, const size_t m) {
153 Complex scratch[6];
154 int negative_if_inverse = m_inverse * -2 + 1;
155 for (size_t k = 0; k < m; ++k) {
156 scratch[0] = Fout[k + m] * m_twiddles[k * fstride];
157 scratch[1] = Fout[k + 2 * m] * m_twiddles[k * fstride * 2];
158 scratch[2] = Fout[k + 3 * m] * m_twiddles[k * fstride * 3];
159 scratch[5] = Fout[k] - scratch[1];
160
161 Fout[k] += scratch[1];
162 scratch[3] = scratch[0] + scratch[2];
163 scratch[4] = scratch[0] - scratch[2];
164 scratch[4] = Complex(scratch[4].imag() * negative_if_inverse, -scratch[4].real() * negative_if_inverse);
165
166 Fout[k + 2 * m] = Fout[k] - scratch[3];
167 Fout[k] += scratch[3];
168 Fout[k + m] = scratch[5] + scratch[4];
169 Fout[k + 3 * m] = scratch[5] - scratch[4];
170 }
171 }
172
173 inline void bfly3(Complex *Fout, const size_t fstride, const size_t m) {
174 size_t k = m;
175 const size_t m2 = 2 * m;
176 Complex *tw1, *tw2;
177 Complex scratch[5];
178 Complex epi3;
179 epi3 = m_twiddles[fstride * m];
180
181 tw1 = tw2 = &m_twiddles[0];
182
183 do {
184 scratch[1] = Fout[m] * *tw1;
185 scratch[2] = Fout[m2] * *tw2;
186
187 scratch[3] = scratch[1] + scratch[2];
188 scratch[0] = scratch[1] - scratch[2];
189 tw1 += fstride;
190 tw2 += fstride * 2;
191 Fout[m] = Complex(Fout->real() - Scalar(.5) * scratch[3].real(), Fout->imag() - Scalar(.5) * scratch[3].imag());
192 scratch[0] *= epi3.imag();
193 *Fout += scratch[3];
194 Fout[m2] = Complex(Fout[m].real() + scratch[0].imag(), Fout[m].imag() - scratch[0].real());
195 Fout[m] += Complex(-scratch[0].imag(), scratch[0].real());
196 ++Fout;
197 } while (--k);
198 }
199
200 inline void bfly5(Complex *Fout, const size_t fstride, const size_t m) {
201 Complex *Fout0, *Fout1, *Fout2, *Fout3, *Fout4;
202 size_t u;
203 Complex scratch[13];
204 Complex *twiddles = &m_twiddles[0];
205 Complex *tw;
206 Complex ya, yb;
207 ya = twiddles[fstride * m];
208 yb = twiddles[fstride * 2 * m];
209
210 Fout0 = Fout;
211 Fout1 = Fout0 + m;
212 Fout2 = Fout0 + 2 * m;
213 Fout3 = Fout0 + 3 * m;
214 Fout4 = Fout0 + 4 * m;
215
216 tw = twiddles;
217 for (u = 0; u < m; ++u) {
218 scratch[0] = *Fout0;
219
220 scratch[1] = *Fout1 * tw[u * fstride];
221 scratch[2] = *Fout2 * tw[2 * u * fstride];
222 scratch[3] = *Fout3 * tw[3 * u * fstride];
223 scratch[4] = *Fout4 * tw[4 * u * fstride];
224
225 scratch[7] = scratch[1] + scratch[4];
226 scratch[10] = scratch[1] - scratch[4];
227 scratch[8] = scratch[2] + scratch[3];
228 scratch[9] = scratch[2] - scratch[3];
229
230 *Fout0 += scratch[7];
231 *Fout0 += scratch[8];
232
233 scratch[5] = scratch[0] + Complex((scratch[7].real() * ya.real()) + (scratch[8].real() * yb.real()),
234 (scratch[7].imag() * ya.real()) + (scratch[8].imag() * yb.real()));
235
236 scratch[6] = Complex((scratch[10].imag() * ya.imag()) + (scratch[9].imag() * yb.imag()),
237 -(scratch[10].real() * ya.imag()) - (scratch[9].real() * yb.imag()));
238
239 *Fout1 = scratch[5] - scratch[6];
240 *Fout4 = scratch[5] + scratch[6];
241
242 scratch[11] = scratch[0] + Complex((scratch[7].real() * yb.real()) + (scratch[8].real() * ya.real()),
243 (scratch[7].imag() * yb.real()) + (scratch[8].imag() * ya.real()));
244
245 scratch[12] = Complex(-(scratch[10].imag() * yb.imag()) + (scratch[9].imag() * ya.imag()),
246 (scratch[10].real() * yb.imag()) - (scratch[9].real() * ya.imag()));
247
248 *Fout2 = scratch[11] + scratch[12];
249 *Fout3 = scratch[11] - scratch[12];
250
251 ++Fout0;
252 ++Fout1;
253 ++Fout2;
254 ++Fout3;
255 ++Fout4;
256 }
257 }
258
259 /* perform the butterfly for one stage of a mixed radix FFT */
260 inline void bfly_generic(Complex *Fout, const size_t fstride, int m, int p) {
261 int u, k, q1, q;
262 Complex *twiddles = &m_twiddles[0];
263 Complex t;
264 int Norig = static_cast<int>(m_twiddles.size());
265 Complex *scratchbuf = &m_scratchBuf[0];
266
267 for (u = 0; u < m; ++u) {
268 k = u;
269 for (q1 = 0; q1 < p; ++q1) {
270 scratchbuf[q1] = Fout[k];
271 k += m;
272 }
273
274 k = u;
275 for (q1 = 0; q1 < p; ++q1) {
276 int twidx = 0;
277 Fout[k] = scratchbuf[0];
278 for (q = 1; q < p; ++q) {
279 twidx += static_cast<int>(fstride) * k;
280 if (twidx >= Norig) twidx -= Norig;
281 t = scratchbuf[q] * twiddles[twidx];
282 Fout[k] += t;
283 }
284 k += m;
285 }
286 }
287 }
288};
289
290template <typename _Scalar>
291const typename kiss_cpx_fft<_Scalar>::Scalar kiss_cpx_fft<_Scalar>::m_pi4 =
292 numext::atan(kiss_cpx_fft<_Scalar>::Scalar(1));
293
294template <typename Scalar_>
295struct kissfft_impl {
296 using Scalar = Scalar_;
297 using Complex = std::complex<Scalar>;
298
299 void clear() {
300 m_plans.clear();
301 m_realTwiddles.clear();
302 }
303
304 inline void fwd(Complex *dst, const Complex *src, int nfft) { run_c2c(dst, src, nfft, /*inverse=*/false); }
305
306 inline void fwd2(Complex *dst, const Complex *src, int n0, int n1) {
307 EIGEN_UNUSED_VARIABLE(dst);
308 EIGEN_UNUSED_VARIABLE(src);
309 EIGEN_UNUSED_VARIABLE(n0);
310 EIGEN_UNUSED_VARIABLE(n1);
311 }
312
313 inline void inv2(Complex *dst, const Complex *src, int n0, int n1) {
314 EIGEN_UNUSED_VARIABLE(dst);
315 EIGEN_UNUSED_VARIABLE(src);
316 EIGEN_UNUSED_VARIABLE(n0);
317 EIGEN_UNUSED_VARIABLE(n1);
318 }
319
320 // real-to-complex forward FFT
321 // perform two FFTs of src even and src odd
322 // then twiddle to recombine them into the half-spectrum format
323 // then fill in the conjugate symmetric half
324 inline void fwd(Complex *dst, const Scalar *src, int nfft) {
325 if (nfft & 3) {
326 // use generic mode for odd
327 m_tmpBuf1.resize(nfft);
328 get_plan(nfft, false).work(0, &m_tmpBuf1[0], src, 1, 1);
329 std::copy(m_tmpBuf1.begin(), m_tmpBuf1.begin() + (nfft >> 1) + 1, dst);
330 } else {
331 int ncfft = nfft >> 1;
332 int ncfft2 = nfft >> 2;
333 Complex *rtw = real_twiddles(ncfft2);
334
335 // use optimized mode for even real
336 fwd(dst, reinterpret_cast<const Complex *>(src), ncfft);
337 Complex dc(dst[0].real() + dst[0].imag());
338 Complex nyquist(dst[0].real() - dst[0].imag());
339 int k;
340 for (k = 1; k <= ncfft2; ++k) {
341 Complex fpk = dst[k];
342 Complex fpnk = conj(dst[ncfft - k]);
343 Complex f1k = fpk + fpnk;
344 Complex f2k = fpk - fpnk;
345 Complex tw = f2k * rtw[k - 1];
346 dst[k] = (f1k + tw) * Scalar(.5);
347 dst[ncfft - k] = conj(f1k - tw) * Scalar(.5);
348 }
349 dst[0] = dc;
350 dst[ncfft] = nyquist;
351 }
352 }
353
354 // inverse complex-to-complex
355 inline void inv(Complex *dst, const Complex *src, int nfft) { run_c2c(dst, src, nfft, /*inverse=*/true); }
356
357 // half-complex to scalar
358 inline void inv(Scalar *dst, const Complex *src, int nfft) {
359 if (nfft & 3) {
360 m_tmpBuf1.resize(nfft);
361 m_tmpBuf2.resize(nfft);
362 std::copy(src, src + (nfft >> 1) + 1, m_tmpBuf1.begin());
363 for (int k = 1; k < (nfft >> 1) + 1; ++k) m_tmpBuf1[nfft - k] = conj(m_tmpBuf1[k]);
364 inv(&m_tmpBuf2[0], &m_tmpBuf1[0], nfft);
365 for (int k = 0; k < nfft; ++k) dst[k] = m_tmpBuf2[k].real();
366 } else {
367 // optimized version for multiple of 4
368 int ncfft = nfft >> 1;
369 int ncfft2 = nfft >> 2;
370 Complex *rtw = real_twiddles(ncfft2);
371 m_tmpBuf1.resize(ncfft);
372 m_tmpBuf1[0] = Complex(src[0].real() + src[ncfft].real(), src[0].real() - src[ncfft].real());
373 for (int k = 1; k <= ncfft / 2; ++k) {
374 Complex fk = src[k];
375 Complex fnkc = conj(src[ncfft - k]);
376 Complex fek = fk + fnkc;
377 Complex tmp = fk - fnkc;
378 Complex fok = tmp * conj(rtw[k - 1]);
379 m_tmpBuf1[k] = fek + fok;
380 m_tmpBuf1[ncfft - k] = conj(fek - fok);
381 }
382 get_plan(ncfft, true).work(0, reinterpret_cast<Complex *>(dst), &m_tmpBuf1[0], 1, 1);
383 }
384 }
385
386 protected:
387 using PlanData = kiss_cpx_fft<Scalar>;
388 using PlanMap = std::map<int, PlanData>;
389
390 PlanMap m_plans;
391 std::map<int, std::vector<Complex> > m_realTwiddles;
392 std::vector<Complex> m_tmpBuf1;
393 std::vector<Complex> m_tmpBuf2;
394
395 inline int PlanKey(int nfft, bool isinverse) const { return (nfft << 1) | int(isinverse); }
396
397 inline PlanData &get_plan(int nfft, bool inverse) {
398 // TODO: look for PlanKey(nfft, ! inverse) and conjugate the twiddles
399 PlanData &pd = m_plans[PlanKey(nfft, inverse)];
400 if (pd.m_twiddles.size() == 0) {
401 pd.make_twiddles(nfft, inverse);
402 pd.factorize(nfft);
403 }
404 return pd;
405 }
406
407 // work() writes dst while reading src, so an in-place call must stage src
408 // through a scratch buffer first.
409 inline void run_c2c(Complex *dst, const Complex *src, int nfft, bool inverse) {
410 if (dst == src) {
411 ei_declare_aligned_stack_constructed_variable(Complex, scratch, nfft, 0);
412 std::copy(src, src + nfft, scratch);
413 get_plan(nfft, inverse).work(0, dst, scratch, 1, 1);
414 return;
415 }
416 get_plan(nfft, inverse).work(0, dst, src, 1, 1);
417 }
418
419 inline Complex *real_twiddles(int ncfft2) {
420 using std::acos;
421 std::vector<Complex> &twidref = m_realTwiddles[ncfft2]; // creates new if not there
422 if ((int)twidref.size() != ncfft2) {
423 twidref.resize(ncfft2);
424 int ncfft = ncfft2 << 1;
425 Scalar pi = acos(Scalar(-1));
426 for (int k = 1; k <= ncfft2; ++k) twidref[k - 1] = exp(Complex(0, -pi * (Scalar(k) / ncfft + Scalar(.5))));
427 }
428 return &twidref[0];
429 }
430};
431
432} // end namespace internal
433
434} // end namespace Eigen
435
436#endif // EIGEN_FFT_KISSFFT_IMPL_H
Namespace containing all symbols from the Eigen library.