diff --git a/unsupported/Eigen/src/FFT/ei_kissfft_impl.h b/unsupported/Eigen/src/FFT/ei_kissfft_impl.h index ce2c9f16e..3580e6c61 100644 --- a/unsupported/Eigen/src/FFT/ei_kissfft_impl.h +++ b/unsupported/Eigen/src/FFT/ei_kissfft_impl.h @@ -24,21 +24,279 @@ #include #include +#include namespace Eigen { + template + struct ei_kiss_cpx_fft + { + typedef _Scalar Scalar; + typedef std::complex Complex; + std::vector m_twiddles; + std::vector m_stageRadix; + std::vector m_stageRemainder; + bool m_inverse; + + ei_kiss_cpx_fft() { } + + void make_twiddles(int nfft,bool inverse) + { + m_inverse = inverse; + m_twiddles.resize(nfft); + Scalar phinc = (inverse?2:-2)* acos( (Scalar) -1) / nfft; + for (int i=0;in) + p=n;// impossible to have a factor > sqrt(n) + } + n /= p; + m_stageRadix.push_back(p); + m_stageRemainder.push_back(n); + }while(n>1); + } + } + + template + void work( int stage,Complex * xout, const _Src * xin, size_t fstride,size_t in_stride) + { + int p = m_stageRadix[stage]; + int m = m_stageRemainder[stage]; + Complex * Fout_beg = xout; + Complex * Fout_end = xout + p*m; + + if (m>1) { + do{ + // recursive call: + // DFT of size m*p performed by doing + // p instances of smaller DFTs of size m, + // each one takes a decimated version of the input + work(stage+1, xout , xin, fstride*p,in_stride); + xin += fstride*in_stride; + }while( (xout += m) != Fout_end ); + }else{ + do{ + *xout = *xin; + xin += fstride*in_stride; + }while(++xout != Fout_end ); + } + xout=Fout_beg; + + // recombine the p smaller DFTs + switch (p) { + case 2: bfly2(xout,fstride,m); break; + case 3: bfly3(xout,fstride,m); break; + case 4: bfly4(xout,fstride,m); break; + case 5: bfly5(xout,fstride,m); break; + default: bfly_generic(xout,fstride,m,p); break; + } + } + + void bfly2( Complex * Fout, const size_t fstride, int m) + { + for (int k=0;kreal() - .5*scratch[3].real() , Fout->imag() - .5*scratch[3].imag() ); + scratch[0] *= epi3.imag(); + *Fout += scratch[3]; + Fout[m2] = Complex( Fout[m].real() + scratch[0].imag() , Fout[m].imag() - scratch[0].real() ); + Fout[m] += Complex( -scratch[0].imag(),scratch[0].real() ); + ++Fout; + }while(--k); + } + + void bfly5( Complex * Fout, const size_t fstride, const size_t m) + { + Complex *Fout0,*Fout1,*Fout2,*Fout3,*Fout4; + size_t u; + Complex scratch[13]; + Complex * twiddles = &m_twiddles[0]; + Complex *tw; + Complex ya,yb; + ya = twiddles[fstride*m]; + yb = twiddles[fstride*2*m]; + + Fout0=Fout; + Fout1=Fout0+m; + Fout2=Fout0+2*m; + Fout3=Fout0+3*m; + Fout4=Fout0+4*m; + + tw=twiddles; + for ( u=0; u=Norig) twidx-=Norig; + t=scratchbuf[q] * twiddles[twidx]; + Fout[ k ] += t; + } + k += m; + } + } + } + }; + + template struct ei_kissfft_impl { typedef _Scalar Scalar; typedef std::complex Complex; - ei_kissfft_impl() : m_nfft(0) {} + ei_kissfft_impl() {} + + void clear() + { + m_plans.clear(); + m_realTwiddles.clear(); + } template void fwd( Complex * dst,const _Src *src,int nfft) { - prepare(nfft,false); - work(0, dst, src, 1,1); + get_plan(nfft,false).work(0, dst, src, 1,1); } // real-to-complex forward FFT @@ -47,16 +305,16 @@ namespace Eigen { // then fill in the conjugate symmetric half void fwd( Complex * dst,const Scalar * src,int nfft) { - if ( nfft&1 ) { + if ( nfft&3 ) { // use generic mode for odd - prepare(nfft,false); - work(0, dst, src, 1,1); + get_plan(nfft,false).work(0, dst, src, 1,1); }else{ int ncfft = nfft>>1; int ncfft2 = nfft>>2; + Complex * rtw = real_twiddles(ncfft2); + // use optimized mode for even real - fwd( dst, reinterpret_cast (src),ncfft); - make_real_twiddles(nfft); + fwd( dst, reinterpret_cast (src), ncfft); Complex dc = dst[0].real() + dst[0].imag(); Complex nyquist = dst[0].real() - dst[0].imag(); int k; @@ -65,8 +323,7 @@ namespace Eigen { Complex fpnk = conj(dst[ncfft-k]); Complex f1k = fpk + fpnk; Complex f2k = fpk - fpnk; - //Complex tw = f2k * exp( Complex(0,-3.14159265358979323846264338327 * ((double) (k) / ncfft + .5) ) ); - Complex tw= f2k * m_realTwiddles[k-1]; + Complex tw= f2k * rtw[k-1]; dst[k] = (f1k + tw) * Scalar(.5); dst[ncfft-k] = conj(f1k -tw)*Scalar(.5); @@ -94,304 +351,67 @@ namespace Eigen { void inv(Complex * dst,const Complex *src,int nfft) { - prepare(nfft,true); - work(0, dst, src, 1,1); - scale(dst, Scalar(1)/m_nfft ); - } - - void prepare(int nfft,bool inverse) - { - make_twiddles(nfft,inverse); - factorize(nfft); - } - - void make_real_twiddles(int nfft) - { - int ncfft2 = nfft>>2; - if ( m_realTwiddles.size() != ncfft2) { - m_realTwiddles.resize(ncfft2); - int ncfft= nfft>>1; - for (int k=1;k<=ncfft2;++k) - m_realTwiddles[k-1] = exp( Complex(0,-3.14159265358979323846264338327 * ((double) (k) / ncfft + .5) ) ); - } - } - - void make_twiddles(int nfft,bool inverse) - { - if ( m_twiddles.size() == nfft) { - // reuse the twiddles, conjugate if necessary - if (inverse != m_inverse) - for (int i=0;in) - p=n;// impossible to have a factor > sqrt(n) - } - n /= p; - m_stageRadix.push_back(p); - m_stageRemainder.push_back(n); - }while(n>1); - } - m_nfft = nfft; - } - - void scale(Complex *dst,Scalar s) - { - for (int k=0;k - void work( int stage,Complex * xout, const _Src * xin, size_t fstride,size_t in_stride) + typedef ei_kiss_cpx_fft PlanData; + + typedef std::map PlanMap; + PlanMap m_plans; + std::map > m_realTwiddles; + + int PlanKey(int nfft,bool isinverse) const { return (nfft<<1) | isinverse; } + + PlanData & get_plan(int nfft,bool inverse) { - int p = m_stageRadix[stage]; - int m = m_stageRemainder[stage]; - Complex * Fout_beg = xout; - Complex * Fout_end = xout + p*m; - - if (m>1) { - do{ - // recursive call: - // DFT of size m*p performed by doing - // p instances of smaller DFTs of size m, - // each one takes a decimated version of the input - work(stage+1, xout , xin, fstride*p,in_stride); - xin += fstride*in_stride; - }while( (xout += m) != Fout_end ); - }else{ - do{ - *xout = *xin; - xin += fstride*in_stride; - }while(++xout != Fout_end ); - } - xout=Fout_beg; - - // recombine the p smaller DFTs - switch (p) { - case 2: bfly2(xout,fstride,m); break; - case 3: bfly3(xout,fstride,m); break; - case 4: bfly4(xout,fstride,m); break; - case 5: bfly5(xout,fstride,m); break; - default: bfly_generic(xout,fstride,m,p); break; - } - } - - void bfly2( Complex * Fout, const size_t fstride, int m) - { - for (int k=0;kreal() - .5*scratch[3].real() , Fout->imag() - .5*scratch[3].imag() ); - scratch[0] *= epi3.imag(); - *Fout += scratch[3]; - Fout[m2] = Complex( Fout[m].real() + scratch[0].imag() , Fout[m].imag() - scratch[0].real() ); - Fout[m] += Complex( -scratch[0].imag(),scratch[0].real() ); - ++Fout; - }while(--k); - } - - void bfly5( Complex * Fout, const size_t fstride, const size_t m) - { - Complex *Fout0,*Fout1,*Fout2,*Fout3,*Fout4; - size_t u; - Complex scratch[13]; - Complex * twiddles = &m_twiddles[0]; - Complex *tw; - Complex ya,yb; - ya = twiddles[fstride*m]; - yb = twiddles[fstride*2*m]; - - Fout0=Fout; - Fout1=Fout0+m; - Fout2=Fout0+2*m; - Fout3=Fout0+3*m; - Fout4=Fout0+4*m; - - tw=twiddles; - for ( u=0; u::iterator MapIt; + MapIt it; + it = m_plans.find( PlanKey(nfft,inverse) ); + if (it == m_plans.end() ) { + // create new entry + it = m_plans.insert( make_pair( PlanKey(nfft,inverse) , PlanData() ) ); + MapIt it2 = m_plans.find( PlanKey(nfft,!inverse) ); + if (it2 != m_plans.end() ) { + it->second = it2.second; + it->second.invert(); + }else{ + it->second.make_twiddles(nfft,inverse); + it->second.factorize(nfft); + } } - - k=u; - for ( q1=0 ; q1

=Norig) twidx-=Norig; - t=scratchbuf[q] * twiddles[twidx]; - Fout[ k ] += t; - } - k += m; + return it->second; + */ + PlanData & pd = m_plans[ PlanKey(nfft,inverse) ]; + if ( pd.m_twiddles.size() == 0 ) { + pd.make_twiddles(nfft,inverse); + pd.factorize(nfft); } - } + return pd; } - int m_nfft; - bool m_inverse; - std::vector m_twiddles; - std::vector m_realTwiddles; - std::vector m_stageRadix; - std::vector m_stageRemainder; -/* - enum {FORWARD,INVERSE,REAL,COMPLEX}; - - struct PlanKey + Complex * real_twiddles(int ncfft2) { - PlanKey(int nfft,bool isinverse,bool iscomplex) - { - _key = (nfft<<2) | (isinverse<<1) | iscomplex; + std::vector & twidref = m_realTwiddles[ncfft2];// creates new if not there + if ( (int)twidref.size() != ncfft2 ) { + twidref.resize(ncfft2); + int ncfft= ncfft2<<1; + Scalar pi = acos( Scalar(-1) ); + for (int k=1;k<=ncfft2;++k) + twidref[k-1] = exp( Complex(0,-pi * ((double) (k) / ncfft + .5) ) ); } + return &twidref[0]; + } - bool operator<(const PlanKey & other) const - { - return this->_key < other._key; - } - int _key; - }; - - struct PlanData + void scale(Complex *dst,int n,Scalar s) { - std::vector m_twiddles; - }; - - std::map promote(long double x) { return complex( x); { long double totalpower=0; long double difpower=0; - cerr <<"idx\ttruth\t\tvalue\n"; + cerr <<"idx\ttruth\t\tvalue\t|dif|=\n"; for (size_t k0=0;k0 acc = 0; long double phinc = -2.*k0* M_PIl / timebuf.size(); @@ -55,7 +55,7 @@ complex promote(long double x) { return complex( x); complex x = promote(fftbuf[k0]); complex dif = acc - x; difpower += norm(dif); - cerr << k0 << "\t" << acc << "\t" << x << endl; + cerr << k0 << "\t" << acc << "\t" << x << "\t" << sqrt(norm(dif)) << endl; } cerr << "rmse:" << sqrt(difpower/totalpower) << endl; return sqrt(difpower/totalpower); @@ -127,8 +127,9 @@ void test_FFT() #endif #if 1 - CALL_SUBTEST( test_scalar(45) ); CALL_SUBTEST( test_scalar(45) ); CALL_SUBTEST( test_scalar(45) ); CALL_SUBTEST( test_scalar(32) ); CALL_SUBTEST( test_scalar(32) ); CALL_SUBTEST( test_scalar(32) ); + CALL_SUBTEST( test_scalar(45) ); CALL_SUBTEST( test_scalar(45) ); CALL_SUBTEST( test_scalar(45) ); + CALL_SUBTEST( test_scalar(50) ); CALL_SUBTEST( test_scalar(50) ); CALL_SUBTEST( test_scalar(50) ); CALL_SUBTEST( test_scalar(256) ); CALL_SUBTEST( test_scalar(256) ); CALL_SUBTEST( test_scalar(256) ); CALL_SUBTEST( test_scalar(2*3*4*5*7) ); CALL_SUBTEST( test_scalar(2*3*4*5*7) ); CALL_SUBTEST( test_scalar(2*3*4*5*7) ); #endif