32 template <
typename _Scalar>
35 typedef _Scalar Scalar;
36 typedef std::complex<Scalar> Complex;
37 std::vector<Complex> m_twiddles;
38 std::vector<int> m_stageRadix;
39 std::vector<int> m_stageRemainder;
40 std::vector<Complex> m_scratchBuf;
44 void make_twiddles(
int nfft,
bool inverse)
47 m_twiddles.resize(nfft);
48 Scalar phinc = (inverse?2:-2)*
acos( (Scalar) -1) / nfft;
49 for (
int i=0;i<nfft;++i)
50 m_twiddles[i] =
exp( Complex(0,i*phinc) );
53 void factorize(
int nfft)
63 default: p += 2;
break;
69 m_stageRadix.push_back(p);
70 m_stageRemainder.push_back(n);
72 m_scratchBuf.resize(p);
76 template <
typename _Src>
78 void work(
int stage,Complex * xout,
const _Src * xin,
size_t fstride,
size_t in_stride)
80 int p = m_stageRadix[stage];
81 int m = m_stageRemainder[stage];
82 Complex * Fout_beg = xout;
83 Complex * Fout_end = xout + p*m;
91 work(stage+1, xout , xin, fstride*p,in_stride);
92 xin += fstride*in_stride;
93 }
while( (xout += m) != Fout_end );
97 xin += fstride*in_stride;
98 }
while(++xout != Fout_end );
104 case 2: bfly2(xout,fstride,m);
break;
105 case 3: bfly3(xout,fstride,m);
break;
106 case 4: bfly4(xout,fstride,m);
break;
107 case 5: bfly5(xout,fstride,m);
break;
108 default: bfly_generic(xout,fstride,m,p);
break;
113 void bfly2( Complex * Fout,
const size_t fstride,
int m)
115 for (
int k=0;k<m;++k) {
116 Complex t = Fout[m+k] * m_twiddles[k*fstride];
117 Fout[m+k] = Fout[k] - t;
123 void bfly4( Complex * Fout,
const size_t fstride,
const size_t m)
126 int negative_if_inverse = m_inverse * -2 +1;
127 for (
size_t k=0;k<m;++k) {
128 scratch[0] = Fout[k+m] * m_twiddles[k*fstride];
129 scratch[1] = Fout[k+2*m] * m_twiddles[k*fstride*2];
130 scratch[2] = Fout[k+3*m] * m_twiddles[k*fstride*3];
131 scratch[5] = Fout[k] - scratch[1];
133 Fout[k] += scratch[1];
134 scratch[3] = scratch[0] + scratch[2];
135 scratch[4] = scratch[0] - scratch[2];
136 scratch[4] = Complex( scratch[4].
imag()*negative_if_inverse , -scratch[4].
real()* negative_if_inverse );
138 Fout[k+2*m] = Fout[k] - scratch[3];
139 Fout[k] += scratch[3];
140 Fout[k+m] = scratch[5] + scratch[4];
141 Fout[k+3*m] = scratch[5] - scratch[4];
146 void bfly3( Complex * Fout,
const size_t fstride,
const size_t m)
149 const size_t m2 = 2*m;
153 epi3 = m_twiddles[fstride*m];
155 tw1=tw2=&m_twiddles[0];
158 scratch[1]=Fout[m] * *tw1;
159 scratch[2]=Fout[m2] * *tw2;
161 scratch[3]=scratch[1]+scratch[2];
162 scratch[0]=scratch[1]-scratch[2];
165 Fout[m] = Complex( Fout->real() - Scalar(.5)*scratch[3].real() , Fout->imag() - Scalar(.5)*scratch[3].imag() );
166 scratch[0] *= epi3.imag();
168 Fout[m2] = Complex( Fout[m].
real() + scratch[0].
imag() , Fout[m].
imag() - scratch[0].
real() );
169 Fout[m] += Complex( -scratch[0].
imag(),scratch[0].
real() );
175 void bfly5( Complex * Fout,
const size_t fstride,
const size_t m)
177 Complex *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
180 Complex * twiddles = &m_twiddles[0];
183 ya = twiddles[fstride*m];
184 yb = twiddles[fstride*2*m];
193 for ( u=0; u<m; ++u ) {
196 scratch[1] = *Fout1 * tw[u*fstride];
197 scratch[2] = *Fout2 * tw[2*u*fstride];
198 scratch[3] = *Fout3 * tw[3*u*fstride];
199 scratch[4] = *Fout4 * tw[4*u*fstride];
201 scratch[7] = scratch[1] + scratch[4];
202 scratch[10] = scratch[1] - scratch[4];
203 scratch[8] = scratch[2] + scratch[3];
204 scratch[9] = scratch[2] - scratch[3];
206 *Fout0 += scratch[7];
207 *Fout0 += scratch[8];
209 scratch[5] = scratch[0] + Complex(
210 (scratch[7].
real()*ya.real() ) + (scratch[8].
real() *yb.real() ),
211 (scratch[7].
imag()*ya.real()) + (scratch[8].
imag()*yb.real())
214 scratch[6] = Complex(
215 (scratch[10].
imag()*ya.imag()) + (scratch[9].
imag()*yb.imag()),
216 -(scratch[10].
real()*ya.imag()) - (scratch[9].
real()*yb.imag())
219 *Fout1 = scratch[5] - scratch[6];
220 *Fout4 = scratch[5] + scratch[6];
222 scratch[11] = scratch[0] +
224 (scratch[7].
real()*yb.real()) + (scratch[8].
real()*ya.real()),
225 (scratch[7].
imag()*yb.real()) + (scratch[8].
imag()*ya.real())
228 scratch[12] = Complex(
229 -(scratch[10].
imag()*yb.imag()) + (scratch[9].
imag()*ya.imag()),
230 (scratch[10].
real()*yb.imag()) - (scratch[9].
real()*ya.imag())
233 *Fout2=scratch[11]+scratch[12];
234 *Fout3=scratch[11]-scratch[12];
236 ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
244 const size_t fstride,
250 Complex * twiddles = &m_twiddles[0];
252 int Norig =
static_cast<int>(m_twiddles.size());
253 Complex * scratchbuf = &m_scratchBuf[0];
255 for ( u=0; u<m; ++u ) {
257 for ( q1=0 ; q1<p ; ++q1 ) {
258 scratchbuf[q1] = Fout[ k ];
263 for ( q1=0 ; q1<p ; ++q1 ) {
265 Fout[ k ] = scratchbuf[0];
267 twidx +=
static_cast<int>(fstride) * k;
268 if (twidx>=Norig) twidx-=Norig;
269 t=scratchbuf[q] * twiddles[twidx];
278 template <
typename _Scalar>
281 typedef _Scalar Scalar;
282 typedef std::complex<Scalar> Complex;
287 m_realTwiddles.clear();
291 void fwd( Complex * dst,
const Complex *src,
int nfft)
293 get_plan(nfft,
false).work(0, dst, src, 1,1);
297 void fwd2( Complex * dst,
const Complex *src,
int n0,
int n1)
299 EIGEN_UNUSED_VARIABLE(dst);
300 EIGEN_UNUSED_VARIABLE(src);
301 EIGEN_UNUSED_VARIABLE(n0);
302 EIGEN_UNUSED_VARIABLE(n1);
306 void inv2( Complex * dst,
const Complex *src,
int n0,
int n1)
308 EIGEN_UNUSED_VARIABLE(dst);
309 EIGEN_UNUSED_VARIABLE(src);
310 EIGEN_UNUSED_VARIABLE(n0);
311 EIGEN_UNUSED_VARIABLE(n1);
319 void fwd( Complex * dst,
const Scalar * src,
int nfft)
323 m_tmpBuf1.resize(nfft);
324 get_plan(nfft,
false).work(0, &m_tmpBuf1[0], src, 1,1);
325 std::copy(m_tmpBuf1.begin(),m_tmpBuf1.begin()+(nfft>>1)+1,dst );
328 int ncfft2 = nfft>>2;
329 Complex * rtw = real_twiddles(ncfft2);
332 fwd( dst, reinterpret_cast<const Complex*> (src), ncfft);
333 Complex dc = dst[0].real() + dst[0].imag();
334 Complex nyquist = dst[0].real() - dst[0].imag();
336 for ( k=1;k <= ncfft2 ; ++k ) {
337 Complex fpk = dst[k];
338 Complex fpnk = conj(dst[ncfft-k]);
339 Complex f1k = fpk + fpnk;
340 Complex f2k = fpk - fpnk;
341 Complex tw= f2k * rtw[k-1];
342 dst[k] = (f1k + tw) * Scalar(.5);
343 dst[ncfft-k] = conj(f1k -tw)*Scalar(.5);
346 dst[ncfft] = nyquist;
352 void inv(Complex * dst,
const Complex *src,
int nfft)
354 get_plan(nfft,
true).work(0, dst, src, 1,1);
359 void inv( Scalar * dst,
const Complex * src,
int nfft)
362 m_tmpBuf1.resize(nfft);
363 m_tmpBuf2.resize(nfft);
364 std::copy(src,src+(nfft>>1)+1,m_tmpBuf1.begin() );
365 for (
int k=1;k<(nfft>>1)+1;++k)
366 m_tmpBuf1[nfft-k] = conj(m_tmpBuf1[k]);
367 inv(&m_tmpBuf2[0],&m_tmpBuf1[0],nfft);
368 for (
int k=0;k<nfft;++k)
369 dst[k] = m_tmpBuf2[k].
real();
373 int ncfft2 = nfft>>2;
374 Complex * rtw = real_twiddles(ncfft2);
375 m_tmpBuf1.resize(ncfft);
376 m_tmpBuf1[0] = Complex( src[0].
real() + src[ncfft].
real(), src[0].
real() - src[ncfft].
real() );
377 for (
int k = 1; k <= ncfft / 2; ++k) {
379 Complex fnkc = conj(src[ncfft-k]);
380 Complex fek = fk + fnkc;
381 Complex tmp = fk - fnkc;
382 Complex fok = tmp * conj(rtw[k-1]);
383 m_tmpBuf1[k] = fek + fok;
384 m_tmpBuf1[ncfft-k] = conj(fek - fok);
386 get_plan(ncfft,
true).work(0, reinterpret_cast<Complex*>(dst), &m_tmpBuf1[0], 1,1);
391 typedef kiss_cpx_fft<Scalar> PlanData;
392 typedef std::map<int,PlanData> PlanMap;
395 std::map<int, std::vector<Complex> > m_realTwiddles;
396 std::vector<Complex> m_tmpBuf1;
397 std::vector<Complex> m_tmpBuf2;
400 int PlanKey(
int nfft,
bool isinverse)
const {
return (nfft<<1) | int(isinverse); }
403 PlanData & get_plan(
int nfft,
bool inverse)
406 PlanData & pd = m_plans[ PlanKey(nfft,inverse) ];
407 if ( pd.m_twiddles.size() == 0 ) {
408 pd.make_twiddles(nfft,inverse);
415 Complex * real_twiddles(
int ncfft2)
417 std::vector<Complex> & twidref = m_realTwiddles[ncfft2];
418 if ( (
int)twidref.size() != ncfft2 ) {
419 twidref.resize(ncfft2);
420 int ncfft= ncfft2<<1;
421 Scalar pi =
acos( Scalar(-1) );
422 for (
int k=1;k<=ncfft2;++k)
423 twidref[k-1] =
exp( Complex(0,-pi * (Scalar(k) / ncfft + Scalar(.5)) ) );