00001 #include "mcwfft.h"
00002 
00003 /*--------------------------------------------------------------------
00004    Complex-to-complex 2D FFT in place:
00005      modex = -1 or +1
00006      modey = -1 or +1
00007      idx   = dimension in x (power of 2)
00008      idy   = dimension in y (power of 2)
00009      zc    = input/output array
00010 ----------------------------------------------------------------------*/
00011 
00012 void FFT_2dcx( int modex , int modey , int idx , int idy , complex * zc )
00013 {
00014    int ii , jj ;
00015    static complex * tc = NULL ;
00016    static int ntc_old = -1 ;
00017 
00018    /*** make workspace, if needed ***/
00019 
00020    if( idy > ntc_old ){
00021       if( tc != NULL ) free(tc) ;
00022       tc = (complex *) malloc( sizeof(complex) * idy ) ;
00023       if( tc == NULL ){
00024          fprintf(stderr,"\n***FFT_2dcx cannot malloc workspace!\n") ;
00025          exit(-1) ;
00026       }
00027       ntc_old = idy ;
00028    }
00029 
00030    /*** x FFTs ***/
00031 
00032    for( jj=0 ; jj < idy ; jj++ )
00033       FFT_1dcx( modex , idx , zc + (jj*idx) ) ;
00034 
00035    /*** y FFTs ***/
00036 
00037    for( ii=0 ; ii < idx ; ii++ ){
00038       for( jj=0 ; jj < idy ; jj++ ) tc[jj] = zc[ii+jj*idx] ;
00039       FFT_1dcx( modey , idy , tc ) ;
00040       for( jj=0 ; jj < idy ; jj++ ) zc[ii+jj*idx] = tc[jj] ;
00041    }
00042 
00043    return ;
00044 }
00045 
00046 /*--------------------------------------------------------------------
00047    Complex-to-complex 1D FFT in place:
00048      mode = -1 or +1 [phase factor in exponent: NO SCALING ON INVERSE!]
00049      idim = dimension (power of 2)
00050      xc   = input/output array
00051    Automagically re-initializes itself when idim changes from
00052    previous call.  By AJJesmanowicz, modified by RWCox.
00053 ----------------------------------------------------------------------*/
00054 
00055 void FFT_1dcx( int mode , int idim , complex * xc )
00056 {
00057    register unsigned int  m, n, i0, i1, i2, i3, k;
00058    register complex       *r0, *r1, *csp;
00059    register float         co, si, f0, f1, f2, f3, f4;
00060    double                 al;
00061 
00062    static complex * csplus = NULL , * csminus = NULL ;  /* trig consts */
00063    static int nold = -666 ;
00064 
00065    /** perhaps initialize **/
00066 
00067    if( nold != idim ){
00068 
00069       if( idim > nold ){
00070          if( csplus != NULL ){ free(csplus) ; free(csminus) ; }
00071          csplus  = (complex *) malloc( sizeof(complex) * idim ) ;
00072          csminus = (complex *) malloc( sizeof(complex) * idim ) ;
00073          if( csplus == NULL || csminus == NULL ){
00074             fprintf(stderr,"\n*** FFT_1dcx cannot malloc workspace! ***\n");
00075             exit(-1) ;
00076          }
00077       }
00078       nold = n = idim ;
00079 
00080       f1 = 1.0 ;  /* csplus init */
00081       m  = 1;
00082       k  = 0;
00083       while (n > m) {
00084          i3 = m << 1;
00085          f2 = m;
00086          al = f1*PI/f2;
00087          co = cos(al);
00088          si = sin(al);
00089          (csplus + k)->r = 1.;
00090          (csplus + k)->i = 0.;
00091          for (i0=0; i0 < m; i0++) {
00092             k++;
00093             csp = csplus + k;
00094             r0 = csp - 1;
00095             csp->r = r0->r * co - r0->i * si;
00096             csp->i = r0->i * co + r0->r * si;
00097          }
00098          m = i3;
00099       }
00100 
00101       f1 = -1.0 ;  /* csminus init */
00102       m  = 1;
00103       k  = 0;
00104       while (n > m) {
00105          i3 = m << 1;
00106          f2 = m;
00107          al = f1*PI/f2;
00108          co = cos(al);
00109          si = sin(al);
00110          (csminus + k)->r = 1.;
00111          (csminus + k)->i = 0.;
00112          for (i0=0; i0 < m; i0++) {
00113             k++;
00114             csp = csminus + k;
00115             r0  = csp - 1;
00116             csp->r = r0->r * co - r0->i * si;
00117             csp->i = r0->i * co + r0->r * si;
00118          }
00119          m = i3;
00120       }
00121    }
00122 
00123    /** Main loop starts here **/
00124 
00125    n   = idim;
00126    i2  = idim >> 1;
00127    i1  = 0;
00128    csp = (mode > 0) ? csplus : csminus ;  /* choose const array */
00129 
00130    for (i0=0; i0 < n; i0 ++) {
00131       if ( i1 > i0 ) {
00132          r0    = xc + i0;
00133          r1    = xc + i1;
00134          f1    = r0->r;
00135          f2    = r0->i;
00136          r0->r = r1->r;
00137          r0->i = r1->i;
00138          r1->r = f1;
00139          r1->i = f2;
00140       }
00141       m = i2;
00142       while ( m && !(i1 < m) ) {
00143          i1 -= m;
00144          m >>= 1;
00145      }
00146      i1 += m;
00147    }
00148 
00149    m = 1;
00150    k = 0;
00151    while (n > m) {
00152       i3 = m << 1;
00153       for (i0=0; i0 < m; i0 ++) {
00154          co = (csp + k)->r;
00155          si = (csp + k)->i;
00156          for (i1=i0; i1 < n; i1 += i3) {
00157             r0    = xc + i1;
00158             r1    = r0 + m;
00159             f1    = r1->r * co;
00160             f2    = r1->i * si;
00161             f3    = r1->r * si;
00162             f4    = r1->i * co;
00163             f1   -= f2;
00164             f3   += f4;
00165             f2    = r0->r;
00166             f4    = r0->i;
00167             r1->r = f2 - f1;
00168             r1->i = f4 - f3;
00169             r0->r = f2 + f1;
00170             r0->i = f4 + f3;
00171          }
00172          k++;
00173       }
00174       m = i3;
00175    }
00176 
00177 #ifdef SCALE_INVERSE
00178    if (mode > 0) {
00179       f0 = 1.0 / idim ;
00180       i0 = 0;
00181       i1 = 1;
00182       while (i0 < n) {
00183          r0 = xc + i0;
00184          r1 = xc + i1;
00185          f1 = r0->r;
00186          f2 = r0->i;
00187          f3 = r1->r;
00188          f4 = r1->i;
00189          f1 *= f0;
00190          f2 *= f0;
00191          f3 *= f0;
00192          f4 *= f0;
00193          r0->r = f1;
00194          r0->i = f2;
00195          r1->r = f3;
00196          r1->i = f4;
00197          i0 += 2;
00198          i1 += 2;
00199       }
00200    }
00201 #endif
00202 
00203    return ;
00204 }
00205 
00206 /*----------------------------------------------------------------------
00207     SUM  SUM  x   exp( -2*Pi*I * (lp+mq)*alpha - 2*Pi*I*(mp-lq)*beta )
00208      l=0..N-1  lm
00209      m=0..N-1
00210 
00211   where N = idim and x = xc ; result overwrites array xc.
00212   alpha and beta are arbitrary floats;  N is arbitrary integer > 3.
00213 ------------------------------------------------------------------------*/
00214 
00215 void FFT_2dchirpz( float alpha , float beta , int idim , complex * xc )
00216 {
00217    int nn , ll , ll2 , pp,qq , ll2q , use_old , qloff , qnoff ;
00218 
00219    static complex * yc = NULL , * zc = NULL , * zhatc = NULL ;
00220    static int nn_old = -666 , ll_old = -666 ;
00221    static float alpha_old = -987.654 , beta_old = 314.1592 ;
00222 
00223    /*** if needed, create new workspace arrays ***/
00224 
00225    nn = idim ;
00226    if( nn == nn_old ){  /* same dimensions as before */
00227       ll   = ll_old ;
00228       ll2  = 2*ll ;
00229       ll2q = ll2 * ll2 ;
00230 
00231       use_old = ( alpha == alpha_old && beta == beta_old ) ;
00232 
00233    } else {             /* new dimensions */
00234 
00235       nn_old = nn ;
00236       for( ll=4 ; ll < nn ; ll *= 2 ) ;  /* until power of 2 >= nn */
00237       ll2  = 2*ll ;
00238       ll2q = ll2 * ll2 ;
00239 
00240       use_old = 0 ;
00241 
00242       if( ll != ll_old ){
00243          ll_old = ll ;
00244          if( yc != NULL ){ free(yc) ; free(zc) ; free(zhatc) ; }
00245 
00246          yc    = (complex *) malloc( sizeof(complex) * ll2q ) ;
00247          zc    = (complex *) malloc( sizeof(complex) * ll2q ) ;
00248          zhatc = (complex *) malloc( sizeof(complex) * ll2q ) ;
00249 
00250          if( yc == NULL || zc == NULL || zhatc == NULL ){
00251             fprintf(stderr,"\n*** FFT_2dchirpz cannot malloc workspace!\n") ;
00252             exit(-1) ;
00253          }
00254       }
00255    }
00256 
00257    /*** if needed, fill "z" workspace arrays ***/
00258 
00259    if( ! use_old ){
00260       float aq , bq , t , api ;
00261 
00262       for( pp=0 ; pp < ll2q ; pp++ ) zc[pp].r = zc[pp].i = 0.0 ;
00263 
00264       api = alpha * PI ;
00265 
00266       for( qq=-(nn-1) ; qq <= (nn-1) ; qq++ ){
00267 
00268          qloff = (qq < 0) ? ((qq+ll2)*ll2) : (qq*ll2) ;
00269          aq    = alpha * PI * qq * qq ;
00270          bq    = 2.0 * beta * PI * qq ;
00271 
00272          for( pp=0 ; pp < 2*nn ; pp++ ){
00273             t = aq - (api * pp + bq ) * pp ;
00274             zc[pp+qloff].r = cos(t) ;
00275             zc[pp+qloff].i = sin(t) ;
00276          }
00277       }
00278 
00279       t = 1.0 / ll2q ;
00280       for( pp=0 ; pp < ll2q ; pp++ ){
00281          zhatc[pp].r = t * zc[pp].r ;
00282          zhatc[pp].i = t * zc[pp].i ;
00283       }
00284       FFT_2dcx( 1 , 1 , ll2 , ll2 , zhatc ) ;
00285    }
00286 
00287    /*** fill "yc" workspace ***/
00288 
00289    for( pp=0 ; pp < ll2q ; pp++ ) yc[pp].r = yc[pp].i = 0.0 ;
00290 
00291    for( qq=0 ; qq < nn ; qq++ ){
00292       qloff = qq*ll2 ;
00293       qnoff = qq*nn ;
00294       for( pp=0 ; pp < nn ; pp++ )
00295          yc[pp] = CJMULT( xc[pp+qnoff] , zc[pp+qloff] ) ;
00296    }
00297 
00298    /*** 2D FFT, multiply, inverse 2D FFT ***/
00299 
00300    FFT_2dcx( -1 , -1 , ll2 , ll2 , yc ) ;
00301    for( pp=0 ; pp < ll2q ; pp++ ) yc[pp] = CMULT( yc[pp] , zhatc[pp] ) ;
00302    FFT_2dcx( -1 , 1 , ll2 , ll2 , yc ) ;
00303 
00304    /*** produce output ***/
00305 
00306    for( qq=0 ; qq < nn ; qq++ ){
00307       qloff = qq*ll2 ;
00308       qnoff = qq*nn ;
00309       for( pp=0 ; pp < nn ; pp++ )
00310          xc[pp+qnoff] = CMULT( zc[pp+qloff] , yc[pp+qloff] ) ;
00311    }
00312 
00313    return ;
00314 }