00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 namespace Eigen {
00026
00027 namespace internal {
00028
00029
00030
00031
00032 template <typename _Scalar>
00033 struct kiss_cpx_fft
00034 {
00035 typedef _Scalar Scalar;
00036 typedef std::complex<Scalar> Complex;
00037 std::vector<Complex> m_twiddles;
00038 std::vector<int> m_stageRadix;
00039 std::vector<int> m_stageRemainder;
00040 std::vector<Complex> m_scratchBuf;
00041 bool m_inverse;
00042
00043 inline
00044 void make_twiddles(int nfft,bool inverse)
00045 {
00046 m_inverse = inverse;
00047 m_twiddles.resize(nfft);
00048 Scalar phinc = (inverse?2:-2)* acos( (Scalar) -1) / nfft;
00049 for (int i=0;i<nfft;++i)
00050 m_twiddles[i] = exp( Complex(0,i*phinc) );
00051 }
00052
00053 void factorize(int nfft)
00054 {
00055
00056 int n= nfft;
00057 int p=4;
00058 do {
00059 while (n % p) {
00060 switch (p) {
00061 case 4: p = 2; break;
00062 case 2: p = 3; break;
00063 default: p += 2; break;
00064 }
00065 if (p*p>n)
00066 p=n;
00067 }
00068 n /= p;
00069 m_stageRadix.push_back(p);
00070 m_stageRemainder.push_back(n);
00071 if ( p > 5 )
00072 m_scratchBuf.resize(p);
00073 }while(n>1);
00074 }
00075
00076 template <typename _Src>
00077 inline
00078 void work( int stage,Complex * xout, const _Src * xin, size_t fstride,size_t in_stride)
00079 {
00080 int p = m_stageRadix[stage];
00081 int m = m_stageRemainder[stage];
00082 Complex * Fout_beg = xout;
00083 Complex * Fout_end = xout + p*m;
00084
00085 if (m>1) {
00086 do{
00087
00088
00089
00090
00091 work(stage+1, xout , xin, fstride*p,in_stride);
00092 xin += fstride*in_stride;
00093 }while( (xout += m) != Fout_end );
00094 }else{
00095 do{
00096 *xout = *xin;
00097 xin += fstride*in_stride;
00098 }while(++xout != Fout_end );
00099 }
00100 xout=Fout_beg;
00101
00102
00103 switch (p) {
00104 case 2: bfly2(xout,fstride,m); break;
00105 case 3: bfly3(xout,fstride,m); break;
00106 case 4: bfly4(xout,fstride,m); break;
00107 case 5: bfly5(xout,fstride,m); break;
00108 default: bfly_generic(xout,fstride,m,p); break;
00109 }
00110 }
00111
00112 inline
00113 void bfly2( Complex * Fout, const size_t fstride, int m)
00114 {
00115 for (int k=0;k<m;++k) {
00116 Complex t = Fout[m+k] * m_twiddles[k*fstride];
00117 Fout[m+k] = Fout[k] - t;
00118 Fout[k] += t;
00119 }
00120 }
00121
00122 inline
00123 void bfly4( Complex * Fout, const size_t fstride, const size_t m)
00124 {
00125 Complex scratch[6];
00126 int negative_if_inverse = m_inverse * -2 +1;
00127 for (size_t k=0;k<m;++k) {
00128 scratch[0] = Fout[k+m] * m_twiddles[k*fstride];
00129 scratch[1] = Fout[k+2*m] * m_twiddles[k*fstride*2];
00130 scratch[2] = Fout[k+3*m] * m_twiddles[k*fstride*3];
00131 scratch[5] = Fout[k] - scratch[1];
00132
00133 Fout[k] += scratch[1];
00134 scratch[3] = scratch[0] + scratch[2];
00135 scratch[4] = scratch[0] - scratch[2];
00136 scratch[4] = Complex( scratch[4].imag()*negative_if_inverse , -scratch[4].real()* negative_if_inverse );
00137
00138 Fout[k+2*m] = Fout[k] - scratch[3];
00139 Fout[k] += scratch[3];
00140 Fout[k+m] = scratch[5] + scratch[4];
00141 Fout[k+3*m] = scratch[5] - scratch[4];
00142 }
00143 }
00144
00145 inline
00146 void bfly3( Complex * Fout, const size_t fstride, const size_t m)
00147 {
00148 size_t k=m;
00149 const size_t m2 = 2*m;
00150 Complex *tw1,*tw2;
00151 Complex scratch[5];
00152 Complex epi3;
00153 epi3 = m_twiddles[fstride*m];
00154
00155 tw1=tw2=&m_twiddles[0];
00156
00157 do{
00158 scratch[1]=Fout[m] * *tw1;
00159 scratch[2]=Fout[m2] * *tw2;
00160
00161 scratch[3]=scratch[1]+scratch[2];
00162 scratch[0]=scratch[1]-scratch[2];
00163 tw1 += fstride;
00164 tw2 += fstride*2;
00165 Fout[m] = Complex( Fout->real() - Scalar(.5)*scratch[3].real() , Fout->imag() - Scalar(.5)*scratch[3].imag() );
00166 scratch[0] *= epi3.imag();
00167 *Fout += scratch[3];
00168 Fout[m2] = Complex( Fout[m].real() + scratch[0].imag() , Fout[m].imag() - scratch[0].real() );
00169 Fout[m] += Complex( -scratch[0].imag(),scratch[0].real() );
00170 ++Fout;
00171 }while(--k);
00172 }
00173
00174 inline
00175 void bfly5( Complex * Fout, const size_t fstride, const size_t m)
00176 {
00177 Complex *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
00178 size_t u;
00179 Complex scratch[13];
00180 Complex * twiddles = &m_twiddles[0];
00181 Complex *tw;
00182 Complex ya,yb;
00183 ya = twiddles[fstride*m];
00184 yb = twiddles[fstride*2*m];
00185
00186 Fout0=Fout;
00187 Fout1=Fout0+m;
00188 Fout2=Fout0+2*m;
00189 Fout3=Fout0+3*m;
00190 Fout4=Fout0+4*m;
00191
00192 tw=twiddles;
00193 for ( u=0; u<m; ++u ) {
00194 scratch[0] = *Fout0;
00195
00196 scratch[1] = *Fout1 * tw[u*fstride];
00197 scratch[2] = *Fout2 * tw[2*u*fstride];
00198 scratch[3] = *Fout3 * tw[3*u*fstride];
00199 scratch[4] = *Fout4 * tw[4*u*fstride];
00200
00201 scratch[7] = scratch[1] + scratch[4];
00202 scratch[10] = scratch[1] - scratch[4];
00203 scratch[8] = scratch[2] + scratch[3];
00204 scratch[9] = scratch[2] - scratch[3];
00205
00206 *Fout0 += scratch[7];
00207 *Fout0 += scratch[8];
00208
00209 scratch[5] = scratch[0] + Complex(
00210 (scratch[7].real()*ya.real() ) + (scratch[8].real() *yb.real() ),
00211 (scratch[7].imag()*ya.real()) + (scratch[8].imag()*yb.real())
00212 );
00213
00214 scratch[6] = Complex(
00215 (scratch[10].imag()*ya.imag()) + (scratch[9].imag()*yb.imag()),
00216 -(scratch[10].real()*ya.imag()) - (scratch[9].real()*yb.imag())
00217 );
00218
00219 *Fout1 = scratch[5] - scratch[6];
00220 *Fout4 = scratch[5] + scratch[6];
00221
00222 scratch[11] = scratch[0] +
00223 Complex(
00224 (scratch[7].real()*yb.real()) + (scratch[8].real()*ya.real()),
00225 (scratch[7].imag()*yb.real()) + (scratch[8].imag()*ya.real())
00226 );
00227
00228 scratch[12] = Complex(
00229 -(scratch[10].imag()*yb.imag()) + (scratch[9].imag()*ya.imag()),
00230 (scratch[10].real()*yb.imag()) - (scratch[9].real()*ya.imag())
00231 );
00232
00233 *Fout2=scratch[11]+scratch[12];
00234 *Fout3=scratch[11]-scratch[12];
00235
00236 ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
00237 }
00238 }
00239
00240
00241 inline
00242 void bfly_generic(
00243 Complex * Fout,
00244 const size_t fstride,
00245 int m,
00246 int p
00247 )
00248 {
00249 int u,k,q1,q;
00250 Complex * twiddles = &m_twiddles[0];
00251 Complex t;
00252 int Norig = static_cast<int>(m_twiddles.size());
00253 Complex * scratchbuf = &m_scratchBuf[0];
00254
00255 for ( u=0; u<m; ++u ) {
00256 k=u;
00257 for ( q1=0 ; q1<p ; ++q1 ) {
00258 scratchbuf[q1] = Fout[ k ];
00259 k += m;
00260 }
00261
00262 k=u;
00263 for ( q1=0 ; q1<p ; ++q1 ) {
00264 int twidx=0;
00265 Fout[ k ] = scratchbuf[0];
00266 for (q=1;q<p;++q ) {
00267 twidx += static_cast<int>(fstride) * k;
00268 if (twidx>=Norig) twidx-=Norig;
00269 t=scratchbuf[q] * twiddles[twidx];
00270 Fout[ k ] += t;
00271 }
00272 k += m;
00273 }
00274 }
00275 }
00276 };
00277
00278 template <typename _Scalar>
00279 struct kissfft_impl
00280 {
00281 typedef _Scalar Scalar;
00282 typedef std::complex<Scalar> Complex;
00283
00284 void clear()
00285 {
00286 m_plans.clear();
00287 m_realTwiddles.clear();
00288 }
00289
00290 inline
00291 void fwd( Complex * dst,const Complex *src,int nfft)
00292 {
00293 get_plan(nfft,false).work(0, dst, src, 1,1);
00294 }
00295
00296 inline
00297 void fwd2( Complex * dst,const Complex *src,int n0,int n1)
00298 {
00299 EIGEN_UNUSED_VARIABLE(dst);
00300 EIGEN_UNUSED_VARIABLE(src);
00301 EIGEN_UNUSED_VARIABLE(n0);
00302 EIGEN_UNUSED_VARIABLE(n1);
00303 }
00304
00305 inline
00306 void inv2( Complex * dst,const Complex *src,int n0,int n1)
00307 {
00308 EIGEN_UNUSED_VARIABLE(dst);
00309 EIGEN_UNUSED_VARIABLE(src);
00310 EIGEN_UNUSED_VARIABLE(n0);
00311 EIGEN_UNUSED_VARIABLE(n1);
00312 }
00313
00314
00315
00316
00317
00318 inline
00319 void fwd( Complex * dst,const Scalar * src,int nfft)
00320 {
00321 if ( nfft&3 ) {
00322
00323 m_tmpBuf1.resize(nfft);
00324 get_plan(nfft,false).work(0, &m_tmpBuf1[0], src, 1,1);
00325 std::copy(m_tmpBuf1.begin(),m_tmpBuf1.begin()+(nfft>>1)+1,dst );
00326 }else{
00327 int ncfft = nfft>>1;
00328 int ncfft2 = nfft>>2;
00329 Complex * rtw = real_twiddles(ncfft2);
00330
00331
00332 fwd( dst, reinterpret_cast<const Complex*> (src), ncfft);
00333 Complex dc = dst[0].real() + dst[0].imag();
00334 Complex nyquist = dst[0].real() - dst[0].imag();
00335 int k;
00336 for ( k=1;k <= ncfft2 ; ++k ) {
00337 Complex fpk = dst[k];
00338 Complex fpnk = conj(dst[ncfft-k]);
00339 Complex f1k = fpk + fpnk;
00340 Complex f2k = fpk - fpnk;
00341 Complex tw= f2k * rtw[k-1];
00342 dst[k] = (f1k + tw) * Scalar(.5);
00343 dst[ncfft-k] = conj(f1k -tw)*Scalar(.5);
00344 }
00345 dst[0] = dc;
00346 dst[ncfft] = nyquist;
00347 }
00348 }
00349
00350
00351 inline
00352 void inv(Complex * dst,const Complex *src,int nfft)
00353 {
00354 get_plan(nfft,true).work(0, dst, src, 1,1);
00355 }
00356
00357
00358 inline
00359 void inv( Scalar * dst,const Complex * src,int nfft)
00360 {
00361 if (nfft&3) {
00362 m_tmpBuf1.resize(nfft);
00363 m_tmpBuf2.resize(nfft);
00364 std::copy(src,src+(nfft>>1)+1,m_tmpBuf1.begin() );
00365 for (int k=1;k<(nfft>>1)+1;++k)
00366 m_tmpBuf1[nfft-k] = conj(m_tmpBuf1[k]);
00367 inv(&m_tmpBuf2[0],&m_tmpBuf1[0],nfft);
00368 for (int k=0;k<nfft;++k)
00369 dst[k] = m_tmpBuf2[k].real();
00370 }else{
00371
00372 int ncfft = nfft>>1;
00373 int ncfft2 = nfft>>2;
00374 Complex * rtw = real_twiddles(ncfft2);
00375 m_tmpBuf1.resize(ncfft);
00376 m_tmpBuf1[0] = Complex( src[0].real() + src[ncfft].real(), src[0].real() - src[ncfft].real() );
00377 for (int k = 1; k <= ncfft / 2; ++k) {
00378 Complex fk = src[k];
00379 Complex fnkc = conj(src[ncfft-k]);
00380 Complex fek = fk + fnkc;
00381 Complex tmp = fk - fnkc;
00382 Complex fok = tmp * conj(rtw[k-1]);
00383 m_tmpBuf1[k] = fek + fok;
00384 m_tmpBuf1[ncfft-k] = conj(fek - fok);
00385 }
00386 get_plan(ncfft,true).work(0, reinterpret_cast<Complex*>(dst), &m_tmpBuf1[0], 1,1);
00387 }
00388 }
00389
00390 protected:
00391 typedef kiss_cpx_fft<Scalar> PlanData;
00392 typedef std::map<int,PlanData> PlanMap;
00393
00394 PlanMap m_plans;
00395 std::map<int, std::vector<Complex> > m_realTwiddles;
00396 std::vector<Complex> m_tmpBuf1;
00397 std::vector<Complex> m_tmpBuf2;
00398
00399 inline
00400 int PlanKey(int nfft, bool isinverse) const { return (nfft<<1) | int(isinverse); }
00401
00402 inline
00403 PlanData & get_plan(int nfft, bool inverse)
00404 {
00405
00406 PlanData & pd = m_plans[ PlanKey(nfft,inverse) ];
00407 if ( pd.m_twiddles.size() == 0 ) {
00408 pd.make_twiddles(nfft,inverse);
00409 pd.factorize(nfft);
00410 }
00411 return pd;
00412 }
00413
00414 inline
00415 Complex * real_twiddles(int ncfft2)
00416 {
00417 std::vector<Complex> & twidref = m_realTwiddles[ncfft2];
00418 if ( (int)twidref.size() != ncfft2 ) {
00419 twidref.resize(ncfft2);
00420 int ncfft= ncfft2<<1;
00421 Scalar pi = acos( Scalar(-1) );
00422 for (int k=1;k<=ncfft2;++k)
00423 twidref[k-1] = exp( Complex(0,-pi * (Scalar(k) / ncfft + Scalar(.5)) ) );
00424 }
00425 return &twidref[0];
00426 }
00427 };
00428
00429 }
00430
00431 }
00432
00433