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