17 template <
typename _Scalar>
20 typedef _Scalar Scalar;
21 typedef std::complex<Scalar> Complex;
22 std::vector<Complex> m_twiddles;
23 std::vector<int> m_stageRadix;
24 std::vector<int> m_stageRemainder;
25 std::vector<Complex> m_scratchBuf;
29 void make_twiddles(
int nfft,
bool inverse)
32 m_twiddles.resize(nfft);
33 Scalar phinc = (inverse?2:-2)* acos( (Scalar) -1) / nfft;
34 for (
int i=0;i<nfft;++i)
35 m_twiddles[i] = exp( Complex(0,i*phinc) );
38 void factorize(
int nfft)
48 default: p += 2;
break;
54 m_stageRadix.push_back(p);
55 m_stageRemainder.push_back(n);
57 m_scratchBuf.resize(p);
61 template <
typename _Src>
63 void work(
int stage,Complex * xout,
const _Src * xin,
size_t fstride,
size_t in_stride)
65 int p = m_stageRadix[stage];
66 int m = m_stageRemainder[stage];
67 Complex * Fout_beg = xout;
68 Complex * Fout_end = xout + p*m;
76 work(stage+1, xout , xin, fstride*p,in_stride);
77 xin += fstride*in_stride;
78 }
while( (xout += m) != Fout_end );
82 xin += fstride*in_stride;
83 }
while(++xout != Fout_end );
89 case 2: bfly2(xout,fstride,m);
break;
90 case 3: bfly3(xout,fstride,m);
break;
91 case 4: bfly4(xout,fstride,m);
break;
92 case 5: bfly5(xout,fstride,m);
break;
93 default: bfly_generic(xout,fstride,m,p);
break;
98 void bfly2( Complex * Fout,
const size_t fstride,
int m)
100 for (
int k=0;k<m;++k) {
101 Complex t = Fout[m+k] * m_twiddles[k*fstride];
102 Fout[m+k] = Fout[k] - t;
108 void bfly4( Complex * Fout,
const size_t fstride,
const size_t m)
111 int negative_if_inverse = m_inverse * -2 +1;
112 for (
size_t k=0;k<m;++k) {
113 scratch[0] = Fout[k+m] * m_twiddles[k*fstride];
114 scratch[1] = Fout[k+2*m] * m_twiddles[k*fstride*2];
115 scratch[2] = Fout[k+3*m] * m_twiddles[k*fstride*3];
116 scratch[5] = Fout[k] - scratch[1];
118 Fout[k] += scratch[1];
119 scratch[3] = scratch[0] + scratch[2];
120 scratch[4] = scratch[0] - scratch[2];
121 scratch[4] = Complex( scratch[4].imag()*negative_if_inverse , -scratch[4].real()* negative_if_inverse );
123 Fout[k+2*m] = Fout[k] - scratch[3];
124 Fout[k] += scratch[3];
125 Fout[k+m] = scratch[5] + scratch[4];
126 Fout[k+3*m] = scratch[5] - scratch[4];
131 void bfly3( Complex * Fout,
const size_t fstride,
const size_t m)
134 const size_t m2 = 2*m;
138 epi3 = m_twiddles[fstride*m];
140 tw1=tw2=&m_twiddles[0];
143 scratch[1]=Fout[m] * *tw1;
144 scratch[2]=Fout[m2] * *tw2;
146 scratch[3]=scratch[1]+scratch[2];
147 scratch[0]=scratch[1]-scratch[2];
150 Fout[m] = Complex( Fout->real() - Scalar(.5)*scratch[3].real() , Fout->imag() - Scalar(.5)*scratch[3].imag() );
151 scratch[0] *= epi3.imag();
153 Fout[m2] = Complex( Fout[m].real() + scratch[0].imag() , Fout[m].imag() - scratch[0].real() );
154 Fout[m] += Complex( -scratch[0].imag(),scratch[0].real() );
160 void bfly5( Complex * Fout,
const size_t fstride,
const size_t m)
162 Complex *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
165 Complex * twiddles = &m_twiddles[0];
168 ya = twiddles[fstride*m];
169 yb = twiddles[fstride*2*m];
178 for ( u=0; u<m; ++u ) {
181 scratch[1] = *Fout1 * tw[u*fstride];
182 scratch[2] = *Fout2 * tw[2*u*fstride];
183 scratch[3] = *Fout3 * tw[3*u*fstride];
184 scratch[4] = *Fout4 * tw[4*u*fstride];
186 scratch[7] = scratch[1] + scratch[4];
187 scratch[10] = scratch[1] - scratch[4];
188 scratch[8] = scratch[2] + scratch[3];
189 scratch[9] = scratch[2] - scratch[3];
191 *Fout0 += scratch[7];
192 *Fout0 += scratch[8];
194 scratch[5] = scratch[0] + Complex(
195 (scratch[7].real()*ya.real() ) + (scratch[8].real() *yb.real() ),
196 (scratch[7].imag()*ya.real()) + (scratch[8].imag()*yb.real())
199 scratch[6] = Complex(
200 (scratch[10].imag()*ya.imag()) + (scratch[9].imag()*yb.imag()),
201 -(scratch[10].real()*ya.imag()) - (scratch[9].real()*yb.imag())
204 *Fout1 = scratch[5] - scratch[6];
205 *Fout4 = scratch[5] + scratch[6];
207 scratch[11] = scratch[0] +
209 (scratch[7].real()*yb.real()) + (scratch[8].real()*ya.real()),
210 (scratch[7].imag()*yb.real()) + (scratch[8].imag()*ya.real())
213 scratch[12] = Complex(
214 -(scratch[10].imag()*yb.imag()) + (scratch[9].imag()*ya.imag()),
215 (scratch[10].real()*yb.imag()) - (scratch[9].real()*ya.imag())
218 *Fout2=scratch[11]+scratch[12];
219 *Fout3=scratch[11]-scratch[12];
221 ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
229 const size_t fstride,
235 Complex * twiddles = &m_twiddles[0];
237 int Norig =
static_cast<int>(m_twiddles.size());
238 Complex * scratchbuf = &m_scratchBuf[0];
240 for ( u=0; u<m; ++u ) {
242 for ( q1=0 ; q1<p ; ++q1 ) {
243 scratchbuf[q1] = Fout[ k ];
248 for ( q1=0 ; q1<p ; ++q1 ) {
250 Fout[ k ] = scratchbuf[0];
252 twidx +=
static_cast<int>(fstride) * k;
253 if (twidx>=Norig) twidx-=Norig;
254 t=scratchbuf[q] * twiddles[twidx];
263 template <
typename _Scalar>
266 typedef _Scalar Scalar;
267 typedef std::complex<Scalar> Complex;
272 m_realTwiddles.clear();
276 void fwd( Complex * dst,
const Complex *src,
int nfft)
278 get_plan(nfft,
false).work(0, dst, src, 1,1);
282 void fwd2( Complex * dst,
const Complex *src,
int n0,
int n1)
284 EIGEN_UNUSED_VARIABLE(dst);
285 EIGEN_UNUSED_VARIABLE(src);
286 EIGEN_UNUSED_VARIABLE(n0);
287 EIGEN_UNUSED_VARIABLE(n1);
291 void inv2( Complex * dst,
const Complex *src,
int n0,
int n1)
293 EIGEN_UNUSED_VARIABLE(dst);
294 EIGEN_UNUSED_VARIABLE(src);
295 EIGEN_UNUSED_VARIABLE(n0);
296 EIGEN_UNUSED_VARIABLE(n1);
304 void fwd( Complex * dst,
const Scalar * src,
int nfft)
308 m_tmpBuf1.resize(nfft);
309 get_plan(nfft,
false).work(0, &m_tmpBuf1[0], src, 1,1);
310 std::copy(m_tmpBuf1.begin(),m_tmpBuf1.begin()+(nfft>>1)+1,dst );
313 int ncfft2 = nfft>>2;
314 Complex * rtw = real_twiddles(ncfft2);
317 fwd( dst, reinterpret_cast<const Complex*> (src), ncfft);
318 Complex dc = dst[0].real() + dst[0].imag();
319 Complex nyquist = dst[0].real() - dst[0].imag();
321 for ( k=1;k <= ncfft2 ; ++k ) {
322 Complex fpk = dst[k];
323 Complex fpnk = conj(dst[ncfft-k]);
324 Complex f1k = fpk + fpnk;
325 Complex f2k = fpk - fpnk;
326 Complex tw= f2k * rtw[k-1];
327 dst[k] = (f1k + tw) * Scalar(.5);
328 dst[ncfft-k] = conj(f1k -tw)*Scalar(.5);
331 dst[ncfft] = nyquist;
337 void inv(Complex * dst,
const Complex *src,
int nfft)
339 get_plan(nfft,
true).work(0, dst, src, 1,1);
344 void inv( Scalar * dst,
const Complex * src,
int nfft)
347 m_tmpBuf1.resize(nfft);
348 m_tmpBuf2.resize(nfft);
349 std::copy(src,src+(nfft>>1)+1,m_tmpBuf1.begin() );
350 for (
int k=1;k<(nfft>>1)+1;++k)
351 m_tmpBuf1[nfft-k] = conj(m_tmpBuf1[k]);
352 inv(&m_tmpBuf2[0],&m_tmpBuf1[0],nfft);
353 for (
int k=0;k<nfft;++k)
354 dst[k] = m_tmpBuf2[k].real();
358 int ncfft2 = nfft>>2;
359 Complex * rtw = real_twiddles(ncfft2);
360 m_tmpBuf1.resize(ncfft);
361 m_tmpBuf1[0] = Complex( src[0].real() + src[ncfft].real(), src[0].real() - src[ncfft].real() );
362 for (
int k = 1; k <= ncfft / 2; ++k) {
364 Complex fnkc = conj(src[ncfft-k]);
365 Complex fek = fk + fnkc;
366 Complex tmp = fk - fnkc;
367 Complex fok = tmp * conj(rtw[k-1]);
368 m_tmpBuf1[k] = fek + fok;
369 m_tmpBuf1[ncfft-k] = conj(fek - fok);
371 get_plan(ncfft,
true).work(0, reinterpret_cast<Complex*>(dst), &m_tmpBuf1[0], 1,1);
376 typedef kiss_cpx_fft<Scalar> PlanData;
377 typedef std::map<int,PlanData> PlanMap;
380 std::map<int, std::vector<Complex> > m_realTwiddles;
381 std::vector<Complex> m_tmpBuf1;
382 std::vector<Complex> m_tmpBuf2;
385 int PlanKey(
int nfft,
bool isinverse)
const {
return (nfft<<1) | int(isinverse); }
388 PlanData & get_plan(
int nfft,
bool inverse)
391 PlanData & pd = m_plans[ PlanKey(nfft,inverse) ];
392 if ( pd.m_twiddles.size() == 0 ) {
393 pd.make_twiddles(nfft,inverse);
400 Complex * real_twiddles(
int ncfft2)
402 std::vector<Complex> & twidref = m_realTwiddles[ncfft2];
403 if ( (
int)twidref.size() != ncfft2 ) {
404 twidref.resize(ncfft2);
405 int ncfft= ncfft2<<1;
406 Scalar pi = acos( Scalar(-1) );
407 for (
int k=1;k<=ncfft2;++k)
408 twidref[k-1] = exp( Complex(0,-pi * (Scalar(k) / ncfft + Scalar(.5)) ) );