libs/qm-dsp/ext/kissfft/kissfft.hh

   1 #ifndef KISSFFT_CLASS_HH
   2 #define KISSFFT_CLASS_HH
   3 #include <complex>
   4 #include <vector>
   5
   6 namespace kissfft_utils {
   7
   8 template <typename T_scalar>
   9 struct traits
  10 {
  11     typedef T_scalar scalar_type;
  12     typedef std::complex<scalar_type> cpx_type;
  13     void fill_twiddles( std::complex<T_scalar> * dst ,int nfft,bool inverse)
  14     {
  15         T_scalar phinc =  (inverse?2:-2)* acos( (T_scalar) -1)  / nfft;
  16         for (int i=0;i<nfft;++i)
  17             dst[i] = exp( std::complex<T_scalar>(0,i*phinc) );
  18     }
  19
  20     void prepare(
  21             std::vector< std::complex<T_scalar> > & dst,
  22             int nfft,bool inverse,
  23             std::vector<int> & stageRadix,
  24             std::vector<int> & stageRemainder )
  25     {
  26         _twiddles.resize(nfft);
  27         fill_twiddles( &_twiddles[0],nfft,inverse);
  28         dst = _twiddles;
  29
  30         //factorize
  31         //start factoring out 4's, then 2's, then 3,5,7,9,...
  32         int n= nfft;
  33         int p=4;
  34         do {
  35             while (n % p) {
  36                 switch (p) {
  37                     case 4: p = 2; break;
  38                     case 2: p = 3; break;
  39                     default: p += 2; break;
  40                 }
  41                 if (p*p>n)
  42                     p=n;// no more factors
  43             }
  44             n /= p;
  45             stageRadix.push_back(p);
  46             stageRemainder.push_back(n);
  47         }while(n>1);
  48     }
  49     std::vector<cpx_type> _twiddles;
  50
  51
  52     const cpx_type twiddle(int i) { return _twiddles[i]; }
  53 };
  54
  55 }
  56
  57 template <typename T_Scalar,
  58          typename T_traits=kissfft_utils::traits<T_Scalar>
  59          >
  60 class kissfft
  61 {
  62     public:
  63         typedef T_traits traits_type;
  64         typedef typename traits_type::scalar_type scalar_type;
  65         typedef typename traits_type::cpx_type cpx_type;
  66
  67         kissfft(int nfft,bool inverse,const traits_type & traits=traits_type() )
  68             :_nfft(nfft),_inverse(inverse),_traits(traits)
  69         {
  70             _traits.prepare(_twiddles, _nfft,_inverse ,_stageRadix, _stageRemainder);
  71         }
  72
  73         void transform(const cpx_type * src , cpx_type * dst)
  74         {
  75             kf_work(0, dst, src, 1,1);
  76         }
  77
  78     private:
  79         void kf_work( int stage,cpx_type * Fout, const cpx_type * f, size_t fstride,size_t in_stride)
  80         {
  81             int p = _stageRadix[stage];
  82             int m = _stageRemainder[stage];
  83             cpx_type * Fout_beg = Fout;
  84             cpx_type * Fout_end = Fout + p*m;
  85
  86             if (m==1) {
  87                 do{
  88                     *Fout = *f;
  89                     f += fstride*in_stride;
  90                 }while(++Fout != Fout_end );
  91             }else{
  92                 do{
  93                     // recursive call:
  94                     // DFT of size m*p performed by doing
  95                     // p instances of smaller DFTs of size m,
  96                     // each one takes a decimated version of the input
  97                     kf_work(stage+1, Fout , f, fstride*p,in_stride);
  98                     f += fstride*in_stride;
  99                 }while( (Fout += m) != Fout_end );
 100             }
 101
 102             Fout=Fout_beg;
 103
 104             // recombine the p smaller DFTs
 105             switch (p) {
 106                 case 2: kf_bfly2(Fout,fstride,m); break;
 107                 case 3: kf_bfly3(Fout,fstride,m); break;
 108                 case 4: kf_bfly4(Fout,fstride,m); break;
 109                 case 5: kf_bfly5(Fout,fstride,m); break;
 110                 default: kf_bfly_generic(Fout,fstride,m,p); break;
 111             }
 112         }
 113
 114         // these were #define macros in the original kiss_fft
 115         void C_ADD( cpx_type & c,const cpx_type & a,const cpx_type & b) { c=a+b;}
 116         void C_MUL( cpx_type & c,const cpx_type & a,const cpx_type & b) { c=a*b;}
 117         void C_SUB( cpx_type & c,const cpx_type & a,const cpx_type & b) { c=a-b;}
 118         void C_ADDTO( cpx_type & c,const cpx_type & a) { c+=a;}
 119         void C_FIXDIV( cpx_type & ,int ) {} // NO-OP for float types
 120         scalar_type S_MUL( const scalar_type & a,const scalar_type & b) { return a*b;}
 121         scalar_type HALF_OF( const scalar_type & a) { return a*.5;}
 122         void C_MULBYSCALAR(cpx_type & c,const scalar_type & a) {c*=a;}
 123
 124         void kf_bfly2( cpx_type * Fout, const size_t fstride, int m)
 125         {
 126             for (int k=0;k<m;++k) {
 127                 cpx_type t = Fout[m+k] * _traits.twiddle(k*fstride);
 128                 Fout[m+k] = Fout[k] - t;
 129                 Fout[k] += t;
 130             }
 131         }
 132
 133         void kf_bfly4( cpx_type * Fout, const size_t fstride, const size_t m)
 134         {
 135             cpx_type scratch[7];
 136             int negative_if_inverse = _inverse * -2 +1;
 137             for (size_t k=0;k<m;++k) {
 138                 scratch[0] = Fout[k+m] * _traits.twiddle(k*fstride);
 139                 scratch[1] = Fout[k+2*m] * _traits.twiddle(k*fstride*2);
 140                 scratch[2] = Fout[k+3*m] * _traits.twiddle(k*fstride*3);
 141                 scratch[5] = Fout[k] - scratch[1];
 142
 143                 Fout[k] += scratch[1];
 144                 scratch[3] = scratch[0] + scratch[2];
 145                 scratch[4] = scratch[0] - scratch[2];
 146                 scratch[4] = cpx_type( scratch[4].imag()*negative_if_inverse , -scratch[4].real()* negative_if_inverse );
 147
 148                 Fout[k+2*m]  = Fout[k] - scratch[3];
 149                 Fout[k] += scratch[3];
 150                 Fout[k+m] = scratch[5] + scratch[4];
 151                 Fout[k+3*m] = scratch[5] - scratch[4];
 152             }
 153         }
 154
 155         void kf_bfly3( cpx_type * Fout, const size_t fstride, const size_t m)
 156         {
 157             size_t k=m;
 158             const size_t m2 = 2*m;
 159             cpx_type *tw1,*tw2;
 160             cpx_type scratch[5];
 161             cpx_type epi3;
 162             epi3 = _twiddles[fstride*m];
 163
 164             tw1=tw2=&_twiddles[0];
 165
 166             do{
 167                 C_FIXDIV(*Fout,3); C_FIXDIV(Fout[m],3); C_FIXDIV(Fout[m2],3);
 168
 169                 C_MUL(scratch[1],Fout[m] , *tw1);
 170                 C_MUL(scratch[2],Fout[m2] , *tw2);
 171
 172                 C_ADD(scratch[3],scratch[1],scratch[2]);
 173                 C_SUB(scratch[0],scratch[1],scratch[2]);
 174                 tw1 += fstride;
 175                 tw2 += fstride*2;
 176
 177                 Fout[m] = cpx_type( Fout->real() - HALF_OF(scratch[3].real() ) , Fout->imag() - HALF_OF(scratch[3].imag() ) );
 178
 179                 C_MULBYSCALAR( scratch[0] , epi3.imag() );
 180
 181                 C_ADDTO(*Fout,scratch[3]);
 182
 183                 Fout[m2] = cpx_type(  Fout[m].real() + scratch[0].imag() , Fout[m].imag() - scratch[0].real() );
 184
 185                 C_ADDTO( Fout[m] , cpx_type( -scratch[0].imag(),scratch[0].real() ) );
 186                 ++Fout;
 187             }while(--k);
 188         }
 189
 190         void kf_bfly5( cpx_type * Fout, const size_t fstride, const size_t m)
 191         {
 192             cpx_type *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
 193             size_t u;
 194             cpx_type scratch[13];
 195             cpx_type * twiddles = &_twiddles[0];
 196             cpx_type *tw;
 197             cpx_type ya,yb;
 198             ya = twiddles[fstride*m];
 199             yb = twiddles[fstride*2*m];
 200
 201             Fout0=Fout;
 202             Fout1=Fout0+m;
 203             Fout2=Fout0+2*m;
 204             Fout3=Fout0+3*m;
 205             Fout4=Fout0+4*m;
 206
 207             tw=twiddles;
 208             for ( u=0; u<m; ++u ) {
 209                 C_FIXDIV( *Fout0,5); C_FIXDIV( *Fout1,5); C_FIXDIV( *Fout2,5); C_FIXDIV( *Fout3,5); C_FIXDIV( *Fout4,5);
 210                 scratch[0] = *Fout0;
 211
 212                 C_MUL(scratch[1] ,*Fout1, tw[u*fstride]);
 213                 C_MUL(scratch[2] ,*Fout2, tw[2*u*fstride]);
 214                 C_MUL(scratch[3] ,*Fout3, tw[3*u*fstride]);
 215                 C_MUL(scratch[4] ,*Fout4, tw[4*u*fstride]);
 216
 217                 C_ADD( scratch[7],scratch[1],scratch[4]);
 218                 C_SUB( scratch[10],scratch[1],scratch[4]);
 219                 C_ADD( scratch[8],scratch[2],scratch[3]);
 220                 C_SUB( scratch[9],scratch[2],scratch[3]);
 221
 222                 C_ADDTO( *Fout0, scratch[7]);
 223                 C_ADDTO( *Fout0, scratch[8]);
 224
 225                 scratch[5] = scratch[0] + cpx_type(
 226                         S_MUL(scratch[7].real(),ya.real() ) + S_MUL(scratch[8].real() ,yb.real() ),
 227                         S_MUL(scratch[7].imag(),ya.real()) + S_MUL(scratch[8].imag(),yb.real())
 228                         );
 229
 230                 scratch[6] =  cpx_type(
 231                         S_MUL(scratch[10].imag(),ya.imag()) + S_MUL(scratch[9].imag(),yb.imag()),
 232                         -S_MUL(scratch[10].real(),ya.imag()) - S_MUL(scratch[9].real(),yb.imag())
 233                         );
 234
 235                 C_SUB(*Fout1,scratch[5],scratch[6]);
 236                 C_ADD(*Fout4,scratch[5],scratch[6]);
 237
 238                 scratch[11] = scratch[0] +
 239                     cpx_type(
 240                             S_MUL(scratch[7].real(),yb.real()) + S_MUL(scratch[8].real(),ya.real()),
 241                             S_MUL(scratch[7].imag(),yb.real()) + S_MUL(scratch[8].imag(),ya.real())
 242                             );
 243
 244                 scratch[12] = cpx_type(
 245                         -S_MUL(scratch[10].imag(),yb.imag()) + S_MUL(scratch[9].imag(),ya.imag()),
 246                         S_MUL(scratch[10].real(),yb.imag()) - S_MUL(scratch[9].real(),ya.imag())
 247                         );
 248
 249                 C_ADD(*Fout2,scratch[11],scratch[12]);
 250                 C_SUB(*Fout3,scratch[11],scratch[12]);
 251
 252                 ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
 253             }
 254         }
 255
 256         /* perform the butterfly for one stage of a mixed radix FFT */
 257         void kf_bfly_generic(
 258                 cpx_type * Fout,
 259                 const size_t fstride,
 260                 int m,
 261                 int p
 262                 )
 263         {
 264             int u,k,q1,q;
 265             cpx_type * twiddles = &_twiddles[0];
 266             cpx_type t;
 267             int Norig = _nfft;
 268             cpx_type scratchbuf[p];
 269
 270             for ( u=0; u<m; ++u ) {
 271                 k=u;
 272                 for ( q1=0 ; q1<p ; ++q1 ) {
 273                     scratchbuf[q1] = Fout[ k  ];
 274                     C_FIXDIV(scratchbuf[q1],p);
 275                     k += m;
 276                 }
 277
 278                 k=u;
 279                 for ( q1=0 ; q1<p ; ++q1 ) {
 280                     int twidx=0;
 281                     Fout[ k ] = scratchbuf[0];
 282                     for (q=1;q<p;++q ) {
 283                         twidx += fstride * k;
 284                         if (twidx>=Norig) twidx-=Norig;
 285                         C_MUL(t,scratchbuf[q] , twiddles[twidx] );
 286                         C_ADDTO( Fout[ k ] ,t);
 287                     }
 288                     k += m;
 289                 }
 290             }
 291         }
 292
 293         int _nfft;
 294         bool _inverse;
 295         std::vector<cpx_type> _twiddles;
 296         std::vector<int> _stageRadix;
 297         std::vector<int> _stageRemainder;
 298         traits_type _traits;
 299 };
 300 #endif