fftpack.c

00001 /*
00002  *  This file is part of libfftpack.
00003  *
00004  *  libfftpack is free software; you can redistribute it and/or modify
00005  *  it under the terms of the GNU General Public License as published by
00006  *  the Free Software Foundation; either version 2 of the License, or
00007  *  (at your option) any later version.
00008  *
00009  *  libfftpack is distributed in the hope that it will be useful,
00010  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *  GNU General Public License for more details.
00013  *
00014  *  You should have received a copy of the GNU General Public License
00015  *  along with libfftpack; if not, write to the Free Software
00016  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00017  */
00018 
00019 /*
00020  *  libfftpack is being developed at the Max-Planck-Institut fuer Astrophysik
00021  *  and financially supported by the Deutsches Zentrum fuer Luft- und Raumfahrt
00022  *  (DLR).
00023  */
00024 
00025 /*
00026   fftpack.c : A set of FFT routines in C.
00027   Algorithmically based on Fortran-77 FFTPACK by Paul N. Swarztrauber
00028   (Version 4, 1985).
00029 
00030   C port by Martin Reinecke (2010)
00031  */
00032 
00033 #include <math.h>
00034 #include <stdlib.h>
00035 #include <string.h>
00036 #include "mytype.h"
00037 #include "fftpack.h"
00038 
00039 #define WA(x,i) wa[(i)+(x)*ido]
00040 #define CH(a,b,c) ch[(a)+ido*((b)+l1*(c))]
00041 #define CC(a,b,c) cc[(a)+ido*((b)+cdim*(c))]
00042 #define PM(a,b,c,d) { a=c+d; b=c-d; }
00043 #define PMC(a,b,c,d) { a.r=c.r+d.r; a.i=c.i+d.i; b.r=c.r-d.r; b.i=c.i-d.i; }
00044 #define ADDC(a,b,c) { a.r=b.r+c.r; a.i=b.i+c.i; }
00045 #define SCALEC(a,b) { a.r*=b; a.i*=b; }
00046 #define CONJFLIPC(a) { FLOAT tmp_=a.r; a.r=-a.i; a.i=tmp_; }
00047 /* (a+ib) = conj(c+id) * (e+if) */
00048 #define MULPM(a,b,c,d,e,f) { a=c*e+d*f; b=c*f-d*e; }
00049 
00050 typedef struct {
00051   FLOAT r,i;
00052 } cmplx;
00053 
00054 #define CONCAT(a,b) a ## b
00055 
00056 #define X(arg) CONCAT(passb,arg)
00057 #define BACKWARD
00058 #include "fftpack_inc.inc"
00059 #undef BACKWARD
00060 #undef X
00061 
00062 #define X(arg) CONCAT(passf,arg)
00063 #include "fftpack_inc.inc"
00064 #undef X
00065 
00066 #undef CC
00067 #undef CH
00068 #define CC(a,b,c) cc[(a)+ido*((b)+l1*(c))]
00069 #define CH(a,b,c) ch[(a)+ido*((b)+cdim*(c))]
00070 
00071 static void radf2 (size_t ido, size_t l1, const FLOAT *cc, FLOAT *ch,
00072   const FLOAT *wa)
00073   {
00074   const size_t cdim=2;
00075   size_t i, k, ic;
00076   FLOAT ti2, tr2;
00077 
00078   for (k=0; k<l1; k++)
00079     PM (CH(0,0,k),CH(ido-1,1,k),CC(0,k,0),CC(0,k,1))
00080   if ((ido&1)==0)
00081     for (k=0; k<l1; k++)
00082       {
00083       CH(    0,1,k) = -CC(ido-1,k,1);
00084       CH(ido-1,0,k) =  CC(ido-1,k,0);
00085       }
00086   if (ido<=2) return;
00087   for (k=0; k<l1; k++)
00088     for (i=2; i<ido; i+=2)
00089       {
00090       ic=ido-i;
00091       MULPM (tr2,ti2,WA(0,i-2),WA(0,i-1),CC(i-1,k,1),CC(i,k,1))
00092       PM (CH(i-1,0,k),CH(ic-1,1,k),CC(i-1,k,0),tr2)
00093       PM (CH(i  ,0,k),CH(ic  ,1,k),ti2,CC(i  ,k,0))
00094       }
00095   }
00096 
00097 static void radf3(size_t ido, size_t l1, const FLOAT *cc, FLOAT *ch,
00098   const FLOAT *wa)
00099   {
00100   const size_t cdim=3;
00101   static const FLOAT taur=-0.5, taui=0.86602540378443864676;
00102   size_t i, k, ic;
00103   FLOAT ci2, di2, di3, cr2, dr2, dr3, ti2, ti3, tr2, tr3;
00104 
00105   for (k=0; k<l1; k++)
00106     {
00107     cr2=CC(0,k,1)+CC(0,k,2);
00108     CH(0,0,k) = CC(0,k,0)+cr2;
00109     CH(0,2,k) = taui*(CC(0,k,2)-CC(0,k,1));
00110     CH(ido-1,1,k) = CC(0,k,0)+taur*cr2;
00111     }
00112   if (ido==1) return;
00113   for (k=0; k<l1; k++)
00114     for (i=2; i<ido; i+=2)
00115       {
00116       ic=ido-i;
00117       MULPM (dr2,di2,WA(0,i-2),WA(0,i-1),CC(i-1,k,1),CC(i,k,1))
00118       MULPM (dr3,di3,WA(1,i-2),WA(1,i-1),CC(i-1,k,2),CC(i,k,2))
00119       cr2=dr2+dr3;
00120       ci2=di2+di3;
00121       CH(i-1,0,k) = CC(i-1,k,0)+cr2;
00122       CH(i  ,0,k) = CC(i  ,k,0)+ci2;
00123       tr2 = CC(i-1,k,0)+taur*cr2;
00124       ti2 = CC(i  ,k,0)+taur*ci2;
00125       tr3 = taui*(di2-di3);
00126       ti3 = taui*(dr3-dr2);
00127       PM(CH(i-1,2,k),CH(ic-1,1,k),tr2,tr3)
00128       PM(CH(i  ,2,k),CH(ic  ,1,k),ti3,ti2)
00129       }
00130   }
00131 
00132 static void radf4(size_t ido, size_t l1, const FLOAT *cc, FLOAT *ch,
00133   const FLOAT *wa)
00134   {
00135   const size_t cdim=4;
00136   static const FLOAT hsqt2=0.70710678118654752440;
00137   size_t i, k, ic;
00138   FLOAT ci2, ci3, ci4, cr2, cr3, cr4, ti1, ti2, ti3, ti4, tr1, tr2, tr3, tr4;
00139 
00140   for (k=0; k<l1; k++)
00141     {
00142     PM (tr1,CH(0,2,k),CC(0,k,3),CC(0,k,1))
00143     PM (tr2,CH(ido-1,1,k),CC(0,k,0),CC(0,k,2))
00144     PM (CH(0,0,k),CH(ido-1,3,k),tr2,tr1)
00145     }
00146   if ((ido&1)==0)
00147     for (k=0; k<l1; k++)
00148       {
00149       ti1=-hsqt2*(CC(ido-1,k,1)+CC(ido-1,k,3));
00150       tr1= hsqt2*(CC(ido-1,k,1)-CC(ido-1,k,3));
00151       PM (CH(ido-1,0,k),CH(ido-1,2,k),CC(ido-1,k,0),tr1)
00152       PM (CH(    0,3,k),CH(    0,1,k),ti1,CC(ido-1,k,2))
00153       }
00154   if (ido<=2) return;
00155   for (k=0; k<l1; k++)
00156     for (i=2; i<ido; i+=2)
00157       {
00158       ic=ido-i;
00159       MULPM(cr2,ci2,WA(0,i-2),WA(0,i-1),CC(i-1,k,1),CC(i,k,1))
00160       MULPM(cr3,ci3,WA(1,i-2),WA(1,i-1),CC(i-1,k,2),CC(i,k,2))
00161       MULPM(cr4,ci4,WA(2,i-2),WA(2,i-1),CC(i-1,k,3),CC(i,k,3))
00162       PM(tr1,tr4,cr4,cr2)
00163       PM(ti1,ti4,ci2,ci4)
00164       PM(tr2,tr3,CC(i-1,k,0),cr3)
00165       PM(ti2,ti3,CC(i  ,k,0),ci3)
00166       PM(CH(i-1,0,k),CH(ic-1,3,k),tr2,tr1)
00167       PM(CH(i  ,0,k),CH(ic  ,3,k),ti1,ti2)
00168       PM(CH(i-1,2,k),CH(ic-1,1,k),tr3,ti4)
00169       PM(CH(i  ,2,k),CH(ic  ,1,k),tr4,ti3)
00170       }
00171   }
00172 
00173 static void radf5(size_t ido, size_t l1, const FLOAT *cc, FLOAT *ch,
00174   const FLOAT *wa)
00175   {
00176   const size_t cdim=5;
00177   static const FLOAT tr11= 0.3090169943749474241, ti11=0.95105651629515357212,
00178                       tr12=-0.8090169943749474241, ti12=0.58778525229247312917;
00179   size_t i, k, ic;
00180   FLOAT ci2, di2, ci4, ci5, di3, di4, di5, ci3, cr2, cr3, dr2, dr3,
00181          dr4, dr5, cr5, cr4, ti2, ti3, ti5, ti4, tr2, tr3, tr4, tr5;
00182 
00183   for (k=0; k<l1; k++)
00184     {
00185     PM (cr2,ci5,CC(0,k,4),CC(0,k,1))
00186     PM (cr3,ci4,CC(0,k,3),CC(0,k,2))
00187     CH(0,0,k)=CC(0,k,0)+cr2+cr3;
00188     CH(ido-1,1,k)=CC(0,k,0)+tr11*cr2+tr12*cr3;
00189     CH(0,2,k)=ti11*ci5+ti12*ci4;
00190     CH(ido-1,3,k)=CC(0,k,0)+tr12*cr2+tr11*cr3;
00191     CH(0,4,k)=ti12*ci5-ti11*ci4;
00192     }
00193   if (ido==1) return;
00194   for (k=0; k<l1;++k)
00195     for (i=2; i<ido; i+=2)
00196       {
00197       ic=ido-i;
00198       MULPM (dr2,di2,WA(0,i-2),WA(0,i-1),CC(i-1,k,1),CC(i,k,1))
00199       MULPM (dr3,di3,WA(1,i-2),WA(1,i-1),CC(i-1,k,2),CC(i,k,2))
00200       MULPM (dr4,di4,WA(2,i-2),WA(2,i-1),CC(i-1,k,3),CC(i,k,3))
00201       MULPM (dr5,di5,WA(3,i-2),WA(3,i-1),CC(i-1,k,4),CC(i,k,4))
00202       PM(cr2,ci5,dr5,dr2)
00203       PM(ci2,cr5,di2,di5)
00204       PM(cr3,ci4,dr4,dr3)
00205       PM(ci3,cr4,di3,di4)
00206       CH(i-1,0,k)=CC(i-1,k,0)+cr2+cr3;
00207       CH(i  ,0,k)=CC(i  ,k,0)+ci2+ci3;
00208       tr2=CC(i-1,k,0)+tr11*cr2+tr12*cr3;
00209       ti2=CC(i  ,k,0)+tr11*ci2+tr12*ci3;
00210       tr3=CC(i-1,k,0)+tr12*cr2+tr11*cr3;
00211       ti3=CC(i  ,k,0)+tr12*ci2+tr11*ci3;
00212       MULPM(tr5,tr4,cr5,cr4,ti11,ti12)
00213       MULPM(ti5,ti4,ci5,ci4,ti11,ti12)
00214       PM(CH(i-1,2,k),CH(ic-1,1,k),tr2,tr5)
00215       PM(CH(i  ,2,k),CH(ic  ,1,k),ti5,ti2)
00216       PM(CH(i-1,4,k),CH(ic-1,3,k),tr3,tr4)
00217       PM(CH(i  ,4,k),CH(ic  ,3,k),ti4,ti3)
00218       }
00219   }
00220 
00221 #undef CH
00222 #undef CC
00223 #define CH(a,b,c) ch[(a)+ido*((b)+l1*(c))]
00224 #define CC(a,b,c) cc[(a)+ido*((b)+cdim*(c))]
00225 #define C1(a,b,c) cc[(a)+ido*((b)+l1*(c))]
00226 #define C2(a,b) cc[(a)+idl1*(b)]
00227 #define CH2(a,b) ch[(a)+idl1*(b)]
00228 static void radfg(size_t ido, size_t ip, size_t l1, size_t idl1,
00229   FLOAT *cc, FLOAT *ch, const FLOAT *wa)
00230   {
00231   const size_t cdim=ip;
00232   static const FLOAT twopi=6.28318530717958647692;
00233   size_t idij, ipph, i, j, k, l, j2, ic, jc, lc, ik;
00234   FLOAT ai1, ai2, ar1, ar2, arg;
00235   FLOAT *csarr;
00236   size_t aidx;
00237 
00238   ipph=(ip+1)/ 2;
00239   if(ido!=1)
00240     {
00241     memcpy(ch,cc,idl1*sizeof(FLOAT));
00242 
00243     for(j=1; j<ip; j++)
00244       for(k=0; k<l1; k++)
00245         {
00246         CH(0,k,j)=C1(0,k,j);
00247         idij=(j-1)*ido+1;
00248         for(i=2; i<ido; i+=2,idij+=2)
00249           MULPM(CH(i-1,k,j),CH(i,k,j),wa[idij-1],wa[idij],C1(i-1,k,j),C1(i,k,j))
00250         }
00251 
00252     for(j=1,jc=ip-1; j<ipph; j++,jc--)
00253       for(k=0; k<l1; k++)
00254         for(i=2; i<ido; i+=2)
00255           {
00256           PM(C1(i-1,k,j),C1(i  ,k,jc),CH(i-1,k,jc),CH(i-1,k,j ))
00257           PM(C1(i  ,k,j),C1(i-1,k,jc),CH(i  ,k,j ),CH(i  ,k,jc))
00258           }
00259     }
00260   else
00261     memcpy(cc,ch,idl1*sizeof(FLOAT));
00262 
00263   for(j=1,jc=ip-1; j<ipph; j++,jc--)
00264     for(k=0; k<l1; k++)
00265       PM(C1(0,k,j),C1(0,k,jc),CH(0,k,jc),CH(0,k,j))
00266 
00267   csarr=RALLOC(FLOAT,2*ip);
00268   arg=twopi / ip;
00269   csarr[0]=1.;
00270   csarr[1]=0.;
00271   csarr[2]=csarr[2*ip-2]=cos(arg);
00272   csarr[3]=sin(arg); csarr[2*ip-1]=-csarr[3];
00273   for (i=2; i<=ip/2; ++i)
00274     {
00275     csarr[2*i]=csarr[2*ip-2*i]=cos(i*arg);
00276     csarr[2*i+1]=sin(i*arg);
00277     csarr[2*ip-2*i+1]=-csarr[2*i+1];
00278     }
00279   for(l=1,lc=ip-1; l<ipph; l++,lc--)
00280     {
00281     ar1=csarr[2*l];
00282     ai1=csarr[2*l+1];
00283     for(ik=0; ik<idl1; ik++)
00284       {
00285       CH2(ik,l)=C2(ik,0)+ar1*C2(ik,1);
00286       CH2(ik,lc)=ai1*C2(ik,ip-1);
00287       }
00288     aidx=2*l;
00289     for(j=2,jc=ip-2; j<ipph; j++,jc--)
00290       {
00291       aidx+=2*l;
00292       if (aidx>=2*ip) aidx-=2*ip;
00293       ar2=csarr[aidx];
00294       ai2=csarr[aidx+1];
00295       for(ik=0; ik<idl1; ik++)
00296         {
00297         CH2(ik,l )+=ar2*C2(ik,j );
00298         CH2(ik,lc)+=ai2*C2(ik,jc);
00299         }
00300       }
00301     }
00302   DEALLOC(csarr);
00303 
00304   for(j=1; j<ipph; j++)
00305     for(ik=0; ik<idl1; ik++)
00306       CH2(ik,0)+=C2(ik,j);
00307 
00308   for(k=0; k<l1; k++)
00309     memcpy(&CC(0,0,k),&CH(0,k,0),ido*sizeof(FLOAT));
00310   for(j=1; j<ipph; j++)
00311     {
00312     jc=ip-j;
00313     j2=2*j;
00314     for(k=0; k<l1; k++)
00315       {
00316       CC(ido-1,j2-1,k) = CH(0,k,j );
00317       CC(0    ,j2  ,k) = CH(0,k,jc);
00318       }
00319     }
00320   if(ido==1) return;
00321 
00322   for(j=1; j<ipph; j++)
00323     {
00324     jc=ip-j;
00325     j2=2*j;
00326     for(k=0; k<l1; k++)
00327       for(i=2; i<ido; i+=2)
00328         {
00329         ic=ido-i;
00330         PM (CC(i-1,j2,k),CC(ic-1,j2-1,k),CH(i-1,k,j ),CH(i-1,k,jc))
00331         PM (CC(i  ,j2,k),CC(ic  ,j2-1,k),CH(i  ,k,jc),CH(i  ,k,j ))
00332         }
00333     }
00334   }
00335 
00336 #undef CC
00337 #undef CH
00338 #define CH(a,b,c) ch[(a)+ido*((b)+l1*(c))]
00339 #define CC(a,b,c) cc[(a)+ido*((b)+cdim*(c))]
00340 
00341 static void radb2(size_t ido, size_t l1, const FLOAT *cc, FLOAT *ch,
00342   const FLOAT *wa)
00343   {
00344   const size_t cdim=2;
00345   size_t i, k, ic;
00346   FLOAT ti2, tr2;
00347 
00348   for (k=0; k<l1; k++)
00349     PM (CH(0,k,0),CH(0,k,1),CC(0,0,k),CC(ido-1,1,k))
00350   if ((ido&1)==0)
00351     for (k=0; k<l1; k++)
00352       {
00353       CH(ido-1,k,0) =  2*CC(ido-1,0,k);
00354       CH(ido-1,k,1) = -2*CC(0    ,1,k);
00355       }
00356   if (ido<=2) return;
00357   for (k=0; k<l1;++k)
00358     for (i=2; i<ido; i+=2)
00359       {
00360       ic=ido-i;
00361       PM (CH(i-1,k,0),tr2,CC(i-1,0,k),CC(ic-1,1,k))
00362       PM (ti2,CH(i  ,k,0),CC(i  ,0,k),CC(ic  ,1,k))
00363       MULPM (CH(i,k,1),CH(i-1,k,1),WA(0,i-2),WA(0,i-1),ti2,tr2)
00364       }
00365   }
00366 
00367 static void radb3(size_t ido, size_t l1, const FLOAT *cc, FLOAT *ch,
00368   const FLOAT *wa)
00369   {
00370   const size_t cdim=3;
00371   static const FLOAT taur=-0.5, taui=0.86602540378443864676;
00372   size_t i, k, ic;
00373   FLOAT ci2, ci3, di2, di3, cr2, cr3, dr2, dr3, ti2, tr2;
00374 
00375   for (k=0; k<l1; k++)
00376     {
00377     tr2=2*CC(ido-1,1,k);
00378     cr2=CC(0,0,k)+taur*tr2;
00379     CH(0,k,0)=CC(0,0,k)+tr2;
00380     ci3=2*taui*CC(0,2,k);
00381     PM (CH(0,k,2),CH(0,k,1),cr2,ci3);
00382     }
00383   if (ido==1) return;
00384   for (k=0; k<l1; k++)
00385     for (i=2; i<ido; i+=2)
00386       {
00387       ic=ido-i;
00388       tr2=CC(i-1,2,k)+CC(ic-1,1,k);
00389       ti2=CC(i  ,2,k)-CC(ic  ,1,k);
00390       cr2=CC(i-1,0,k)+taur*tr2;
00391       ci2=CC(i  ,0,k)+taur*ti2;
00392       CH(i-1,k,0)=CC(i-1,0,k)+tr2;
00393       CH(i  ,k,0)=CC(i  ,0,k)+ti2;
00394       cr3=taui*(CC(i-1,2,k)-CC(ic-1,1,k));
00395       ci3=taui*(CC(i  ,2,k)+CC(ic  ,1,k));
00396       PM(dr3,dr2,cr2,ci3)
00397       PM(di2,di3,ci2,cr3)
00398       MULPM(CH(i,k,1),CH(i-1,k,1),WA(0,i-2),WA(0,i-1),di2,dr2)
00399       MULPM(CH(i,k,2),CH(i-1,k,2),WA(1,i-2),WA(1,i-1),di3,dr3)
00400       }
00401   }
00402 
00403 static void radb4(size_t ido, size_t l1, const FLOAT *cc, FLOAT *ch,
00404   const FLOAT *wa)
00405   {
00406   const size_t cdim=4;
00407   static const FLOAT sqrt2=1.41421356237309504880;
00408   size_t i, k, ic;
00409   FLOAT ci2, ci3, ci4, cr2, cr3, cr4, ti1, ti2, ti3, ti4, tr1, tr2, tr3, tr4;
00410 
00411   for (k=0; k<l1; k++)
00412     {
00413     PM (tr2,tr1,CC(0,0,k),CC(ido-1,3,k))
00414     tr3=2*CC(ido-1,1,k);
00415     tr4=2*CC(0,2,k);
00416     PM (CH(0,k,0),CH(0,k,2),tr2,tr3)
00417     PM (CH(0,k,3),CH(0,k,1),tr1,tr4)
00418     }
00419   if ((ido&1)==0)
00420     for (k=0; k<l1; k++)
00421       {
00422       PM (ti1,ti2,CC(0    ,3,k),CC(0    ,1,k))
00423       PM (tr2,tr1,CC(ido-1,0,k),CC(ido-1,2,k))
00424       CH(ido-1,k,0)=tr2+tr2;
00425       CH(ido-1,k,1)=sqrt2*(tr1-ti1);
00426       CH(ido-1,k,2)=ti2+ti2;
00427       CH(ido-1,k,3)=-sqrt2*(tr1+ti1);
00428       }
00429   if (ido<=2) return;
00430   for (k=0; k<l1;++k)
00431     for (i=2; i<ido; i+=2)
00432       {
00433       ic=ido-i;
00434       PM (tr2,tr1,CC(i-1,0,k),CC(ic-1,3,k))
00435       PM (ti1,ti2,CC(i  ,0,k),CC(ic  ,3,k))
00436       PM (tr4,ti3,CC(i  ,2,k),CC(ic  ,1,k))
00437       PM (tr3,ti4,CC(i-1,2,k),CC(ic-1,1,k))
00438       PM (CH(i-1,k,0),cr3,tr2,tr3)
00439       PM (CH(i  ,k,0),ci3,ti2,ti3)
00440       PM (cr4,cr2,tr1,tr4)
00441       PM (ci2,ci4,ti1,ti4)
00442       MULPM (CH(i,k,1),CH(i-1,k,1),WA(0,i-2),WA(0,i-1),ci2,cr2)
00443       MULPM (CH(i,k,2),CH(i-1,k,2),WA(1,i-2),WA(1,i-1),ci3,cr3)
00444       MULPM (CH(i,k,3),CH(i-1,k,3),WA(2,i-2),WA(2,i-1),ci4,cr4)
00445       }
00446   }
00447 
00448 static void radb5(size_t ido, size_t l1, const FLOAT *cc, FLOAT *ch,
00449   const FLOAT *wa)
00450   {
00451   const size_t cdim=5;
00452   static const FLOAT tr11= 0.3090169943749474241, ti11=0.95105651629515357212,
00453                       tr12=-0.8090169943749474241, ti12=0.58778525229247312917;
00454   size_t i, k, ic;
00455   FLOAT ci2, ci3, ci4, ci5, di3, di4, di5, di2, cr2, cr3, cr5, cr4,
00456          ti2, ti3, ti4, ti5, dr3, dr4, dr5, dr2, tr2, tr3, tr4, tr5;
00457 
00458   for (k=0; k<l1; k++)
00459     {
00460     ti5=2*CC(0,2,k);
00461     ti4=2*CC(0,4,k);
00462     tr2=2*CC(ido-1,1,k);
00463     tr3=2*CC(ido-1,3,k);
00464     CH(0,k,0)=CC(0,0,k)+tr2+tr3;
00465     cr2=CC(0,0,k)+tr11*tr2+tr12*tr3;
00466     cr3=CC(0,0,k)+tr12*tr2+tr11*tr3;
00467     MULPM(ci5,ci4,ti5,ti4,ti11,ti12)
00468     PM(CH(0,k,4),CH(0,k,1),cr2,ci5)
00469     PM(CH(0,k,3),CH(0,k,2),cr3,ci4)
00470     }
00471   if (ido==1) return;
00472   for (k=0; k<l1;++k)
00473     for (i=2; i<ido; i+=2)
00474       {
00475       ic=ido-i;
00476       PM(tr2,tr5,CC(i-1,2,k),CC(ic-1,1,k))
00477       PM(ti5,ti2,CC(i  ,2,k),CC(ic  ,1,k))
00478       PM(tr3,tr4,CC(i-1,4,k),CC(ic-1,3,k))
00479       PM(ti4,ti3,CC(i  ,4,k),CC(ic  ,3,k))
00480       CH(i-1,k,0)=CC(i-1,0,k)+tr2+tr3;
00481       CH(i  ,k,0)=CC(i  ,0,k)+ti2+ti3;
00482       cr2=CC(i-1,0,k)+tr11*tr2+tr12*tr3;
00483       ci2=CC(i  ,0,k)+tr11*ti2+tr12*ti3;
00484       cr3=CC(i-1,0,k)+tr12*tr2+tr11*tr3;
00485       ci3=CC(i  ,0,k)+tr12*ti2+tr11*ti3;
00486       MULPM(cr5,cr4,tr5,tr4,ti11,ti12)
00487       MULPM(ci5,ci4,ti5,ti4,ti11,ti12)
00488       PM(dr4,dr3,cr3,ci4)
00489       PM(di3,di4,ci3,cr4)
00490       PM(dr5,dr2,cr2,ci5)
00491       PM(di2,di5,ci2,cr5)
00492       MULPM(CH(i,k,1),CH(i-1,k,1),WA(0,i-2),WA(0,i-1),di2,dr2)
00493       MULPM(CH(i,k,2),CH(i-1,k,2),WA(1,i-2),WA(1,i-1),di3,dr3)
00494       MULPM(CH(i,k,3),CH(i-1,k,3),WA(2,i-2),WA(2,i-1),di4,dr4)
00495       MULPM(CH(i,k,4),CH(i-1,k,4),WA(3,i-2),WA(3,i-1),di5,dr5)
00496       }
00497   }
00498 
00499 static void radbg(size_t ido, size_t ip, size_t l1, size_t idl1,
00500   FLOAT *cc, FLOAT *ch, const FLOAT *wa)
00501   {
00502   const size_t cdim=ip;
00503   static const FLOAT twopi=6.28318530717958647692;
00504   size_t idij, ipph, i, j, k, l, j2, ic, jc, lc, ik;
00505   FLOAT ai1, ai2, ar1, ar2, arg;
00506   FLOAT *csarr;
00507   size_t aidx;
00508 
00509   ipph=(ip+1)/ 2;
00510   for(k=0; k<l1; k++)
00511     memcpy(&CH(0,k,0),&CC(0,0,k),ido*sizeof(FLOAT));
00512   for(j=1; j<ipph; j++)
00513     {
00514     jc=ip-j;
00515     j2=2*j;
00516     for(k=0; k<l1; k++)
00517       {
00518       CH(0,k,j )=2*CC(ido-1,j2-1,k);
00519       CH(0,k,jc)=2*CC(0    ,j2  ,k);
00520       }
00521     }
00522 
00523   if(ido!=1)
00524     for(j=1,jc=ip-1; j<ipph; j++,jc--)
00525       for(k=0; k<l1; k++)
00526         for(i=2; i<ido; i+=2)
00527           {
00528           ic=ido-i;
00529           PM (CH(i-1,k,j ),CH(i-1,k,jc),CC(i-1,2*j,k),CC(ic-1,2*j-1,k))
00530           PM (CH(i  ,k,jc),CH(i  ,k,j ),CC(i  ,2*j,k),CC(ic  ,2*j-1,k))
00531           }
00532 
00533   csarr=RALLOC(FLOAT,2*ip);
00534   arg=twopi/ip;
00535   csarr[0]=1.;
00536   csarr[1]=0.;
00537   csarr[2]=csarr[2*ip-2]=cos(arg);
00538   csarr[3]=sin(arg); csarr[2*ip-1]=-csarr[3];
00539   for (i=2; i<=ip/2; ++i)
00540     {
00541     csarr[2*i]=csarr[2*ip-2*i]=cos(i*arg);
00542     csarr[2*i+1]=sin(i*arg);
00543     csarr[2*ip-2*i+1]=-csarr[2*i+1];
00544     }
00545   for(l=1; l<ipph; l++)
00546     {
00547     lc=ip-l;
00548     ar1=csarr[2*l];
00549     ai1=csarr[2*l+1];
00550     for(ik=0; ik<idl1; ik++)
00551       {
00552       C2(ik,l)=CH2(ik,0)+ar1*CH2(ik,1);
00553       C2(ik,lc)=ai1*CH2(ik,ip-1);
00554       }
00555     aidx=2*l;
00556     for(j=2; j<ipph; j++)
00557       {
00558       jc=ip-j;
00559       aidx+=2*l;
00560       if (aidx>=2*ip) aidx-=2*ip;
00561       ar2=csarr[aidx];
00562       ai2=csarr[aidx+1];
00563       for(ik=0; ik<idl1; ik++)
00564         {
00565         C2(ik,l )+=ar2*CH2(ik,j );
00566         C2(ik,lc)+=ai2*CH2(ik,jc);
00567         }
00568       }
00569     }
00570   DEALLOC(csarr);
00571 
00572   for(j=1; j<ipph; j++)
00573     for(ik=0; ik<idl1; ik++)
00574       CH2(ik,0)+=CH2(ik,j);
00575 
00576   for(j=1,jc=ip-1; j<ipph; j++,jc--)
00577     for(k=0; k<l1; k++)
00578       PM (CH(0,k,jc),CH(0,k,j),C1(0,k,j),C1(0,k,jc))
00579 
00580   if(ido==1)
00581     return;
00582   for(j=1,jc=ip-1; j<ipph; j++,jc--)
00583     for(k=0; k<l1; k++)
00584       for(i=2; i<ido; i+=2)
00585         {
00586         PM (CH(i-1,k,jc),CH(i-1,k,j ),C1(i-1,k,j),C1(i  ,k,jc))
00587         PM (CH(i  ,k,j ),CH(i  ,k,jc),C1(i  ,k,j),C1(i-1,k,jc))
00588         }
00589   memcpy(cc,ch,idl1*sizeof(FLOAT));
00590 
00591   for(j=1; j<ip; j++)
00592     for(k=0; k<l1; k++)
00593       {
00594       C1(0,k,j)=CH(0,k,j);
00595       idij=(j-1)*ido+1;
00596       for(i=2; i<ido; i+=2,idij+=2)
00597         MULPM (C1(i,k,j),C1(i-1,k,j),wa[idij-1],wa[idij],CH(i,k,j),CH(i-1,k,j))
00598       }
00599   }
00600 
00601 #undef CC
00602 #undef CH
00603 #undef PM
00604 #undef MULPM
00605 
00606 
00607 /*----------------------------------------------------------------------
00608    cfftf1, cfftb1, cfftf, cfftb, cffti1, cffti. Complex FFTs.
00609   ----------------------------------------------------------------------*/
00610 
00611 static void cfft1(size_t n, cmplx c[], cmplx ch[], const cmplx wa[],
00612   const size_t ifac[], int isign)
00613   {
00614   size_t k1, l1=1, nf=ifac[1], iw=0;
00615   cmplx *p1=c, *p2=ch;
00616 
00617   for(k1=0; k1<nf; k1++)
00618     {
00619     size_t ip=ifac[k1+2];
00620     size_t l2=ip*l1;
00621     size_t ido = n/l2;
00622     if(ip==4)
00623       (isign>0) ? passb4(ido, l1, p1, p2, wa+iw)
00624                 : passf4(ido, l1, p1, p2, wa+iw);
00625     else if(ip==2)
00626       (isign>0) ? passb2(ido, l1, p1, p2, wa+iw)
00627                 : passf2(ido, l1, p1, p2, wa+iw);
00628     else if(ip==3)
00629       (isign>0) ? passb3(ido, l1, p1, p2, wa+iw)
00630                 : passf3(ido, l1, p1, p2, wa+iw);
00631     else if(ip==5)
00632       (isign>0) ? passb5(ido, l1, p1, p2, wa+iw)
00633                 : passf5(ido, l1, p1, p2, wa+iw);
00634     else if(ip==6)
00635       (isign>0) ? passb6(ido, l1, p1, p2, wa+iw)
00636                 : passf6(ido, l1, p1, p2, wa+iw);
00637     else
00638       (isign>0) ? passbg(ido, ip, l1, p1, p2, wa+iw)
00639                 : passfg(ido, ip, l1, p1, p2, wa+iw);
00640     SWAP(p1,p2,cmplx *);
00641     l1=l2;
00642     iw+=(ip-1)*ido;
00643     }
00644   if (p1!=c)
00645     memcpy (c,p1,n*sizeof(cmplx));
00646   }
00647 
00648 void cfftf(size_t n, FLOAT c[], FLOAT wsave[])
00649   {
00650   if (n!=1)
00651     cfft1(n, (cmplx*)c, (cmplx*)wsave, (cmplx*)(wsave+2*n),
00652           (size_t*)(wsave+4*n),-1);
00653   }
00654 
00655 void cfftb(size_t n, FLOAT c[], FLOAT wsave[])
00656   {
00657   if (n!=1)
00658     cfft1(n, (cmplx*)c, (cmplx*)wsave, (cmplx*)(wsave+2*n),
00659           (size_t*)(wsave+4*n),+1);
00660   }
00661 
00662 static void factorize (size_t n, const size_t *pf, size_t npf, size_t *ifac)
00663   {
00664   size_t nl=n, nf=0, ntry=0, j=0, i;
00665 
00666 startloop:
00667   j++;
00668   ntry = (j<=npf) ? pf[j-1] : ntry+2;
00669   do
00670     {
00671     size_t nq=nl / ntry;
00672     size_t nr=nl-ntry*nq;
00673     if (nr!=0)
00674       goto startloop;
00675     nf++;
00676     ifac[nf+1]=ntry;
00677     nl=nq;
00678     if ((ntry==2) && (nf!=1))
00679       {
00680       for (i=nf+1; i>2; --i)
00681         ifac[i]=ifac[i-1];
00682       ifac[2]=2;
00683       }
00684     }
00685   while(nl!=1);
00686   ifac[0]=n;
00687   ifac[1]=nf;
00688   }
00689 
00690 static void cffti1(size_t n, FLOAT wa[], size_t ifac[])
00691   {
00692   static const size_t ntryh[5]={4,6,3,2,5};
00693   static const FLOAT twopi=6.28318530717958647692;
00694   size_t j, k, fi;
00695 
00696   FLOAT argh=twopi/n;
00697   size_t i=0, l1=1;
00698   factorize (n,ntryh,5,ifac);
00699   for(k=1; k<=ifac[1]; k++)
00700     {
00701     size_t ip=ifac[k+1];
00702     size_t ido=n/(l1*ip);
00703     for(j=1; j<ip; j++)
00704       {
00705       size_t is = i;
00706       FLOAT argld=j*l1*argh;
00707       wa[i  ]=1;
00708       wa[i+1]=0;
00709       for(fi=1; fi<=ido; fi++)
00710         {
00711         FLOAT arg=fi*argld;
00712         i+=2;
00713         wa[i  ]=cos(arg);
00714         wa[i+1]=sin(arg);
00715         }
00716       if(ip>6)
00717         {
00718         wa[is  ]=wa[i  ];
00719         wa[is+1]=wa[i+1];
00720         }
00721       }
00722     l1*=ip;
00723     }
00724   }
00725 
00726 void cffti(size_t n, FLOAT wsave[])
00727   { if (n!=1) cffti1(n, wsave+2*n,(size_t*)(wsave+4*n)); }
00728 
00729 
00730 /*----------------------------------------------------------------------
00731    rfftf1, rfftb1, rfftf, rfftb, rffti1, rffti. Real FFTs.
00732   ----------------------------------------------------------------------*/
00733 
00734 static void rfftf1(size_t n, FLOAT c[], FLOAT ch[], const FLOAT wa[],
00735   const size_t ifac[])
00736   {
00737   size_t k1, l1=n, nf=ifac[1], iw=n-1;
00738   FLOAT *p1=ch, *p2=c;
00739 
00740   for(k1=1; k1<=nf;++k1)
00741     {
00742     size_t ip=ifac[nf-k1+2];
00743     size_t ido=n / l1;
00744     l1 /= ip;
00745     iw-=(ip-1)*ido;
00746     SWAP (p1,p2,FLOAT *);
00747     if(ip==4)
00748       radf4(ido, l1, p1, p2, wa+iw);
00749     else if(ip==2)
00750       radf2(ido, l1, p1, p2, wa+iw);
00751     else if(ip==3)
00752       radf3(ido, l1, p1, p2, wa+iw);
00753     else if(ip==5)
00754       radf5(ido, l1, p1, p2, wa+iw);
00755     else
00756       {
00757       if (ido==1)
00758         SWAP (p1,p2,FLOAT *);
00759       radfg(ido, ip, l1, ido*l1, p1, p2, wa+iw);
00760       SWAP (p1,p2,FLOAT *);
00761       }
00762     }
00763   if (p1==c)
00764     memcpy (c,ch,n*sizeof(FLOAT));
00765   }
00766 
00767 static void rfftb1(size_t n, FLOAT c[], FLOAT ch[], const FLOAT wa[],
00768   const size_t ifac[])
00769   {
00770   size_t k1, l1=1, nf=ifac[1], iw=0;
00771   FLOAT *p1=c, *p2=ch;
00772 
00773   for(k1=1; k1<=nf; k1++)
00774     {
00775     size_t ip = ifac[k1+1],
00776            ido= n/(ip*l1);
00777     if(ip==4)
00778       radb4(ido, l1, p1, p2, wa+iw);
00779     else if(ip==2)
00780       radb2(ido, l1, p1, p2, wa+iw);
00781     else if(ip==3)
00782       radb3(ido, l1, p1, p2, wa+iw);
00783     else if(ip==5)
00784       radb5(ido, l1, p1, p2, wa+iw);
00785     else
00786       {
00787       radbg(ido, ip, l1, ido*l1, p1, p2, wa+iw);
00788       if (ido!=1)
00789         SWAP (p1,p2,FLOAT *);
00790       }
00791     SWAP (p1,p2,FLOAT *);
00792     l1*=ip;
00793     iw+=(ip-1)*ido;
00794     }
00795   if (p1!=c)
00796     memcpy (c,ch,n*sizeof(FLOAT));
00797   }
00798 
00799 void rfftf(size_t n, FLOAT r[], FLOAT wsave[])
00800   { if(n!=1) rfftf1(n, r, wsave, wsave+n,(size_t*)(wsave+2*n)); }
00801 
00802 void rfftb(size_t n, FLOAT r[], FLOAT wsave[])
00803   { if(n!=1) rfftb1(n, r, wsave, wsave+n,(size_t*)(wsave+2*n)); }
00804 
00805 static void rffti1(size_t n, FLOAT wa[], size_t ifac[])
00806   {
00807   static const size_t ntryh[4]={4,2,3,5};
00808   static const FLOAT twopi=6.28318530717958647692;
00809   size_t i, j, k, fi;
00810 
00811   FLOAT argh=twopi/n;
00812   size_t is=0, l1=1;
00813   factorize (n,ntryh,4,ifac);
00814   for (k=1; k<ifac[1]; k++)
00815     {
00816     size_t ip=ifac[k+1],
00817            ido=n/(l1*ip);
00818     for (j=1; j<ip; ++j)
00819       {
00820       FLOAT argld=j*l1*argh;
00821       for(i=is,fi=1; i<=ido+is-3; i+=2,++fi)
00822         {
00823         FLOAT arg=fi*argld;
00824         wa[i  ]=cos(arg);
00825         wa[i+1]=sin(arg);
00826         }
00827       is+=ido;
00828       }
00829     l1*=ip;
00830     }
00831   }
00832 
00833 void rffti(size_t n, FLOAT wsave[])
00834   { if (n!=1) rffti1(n, wsave+n,(size_t*)(wsave+2*n)); }