lib/smallft.c

   1 /******************************************************************
   2  * CopyPolicy: GNU Public License 2 applies
   3  * Copyright (C) 1998 Monty xiphmont@mit.edu, monty@xiph.org
   4  *
   5  * FFT implementation from OggSquish, minus cosine transforms,
   6  * minus all but radix 2/4 case.  In Vorbis we only need this
   7  * cut-down version (used by the encoder, not decoder).
   8  *
   9  * To do more than just power-of-two sized vectors, see the full
  10  * version I wrote for NetLib.
  11  *
  12  ******************************************************************/
  13
  14 #include <stdlib.h>
  15 #include <string.h>
  16 #include <math.h>
  17 #include "smallft.h"
  18
  19 static void drfti1(int n, double *wa, int *ifac){
  20   static int ntryh[4] = { 4,2,3,5 };
  21   static double tpi = 6.28318530717958647692528676655900577;
  22   double arg,argh,argld,fi;
  23   int ntry=0,i,j=-1;
  24   int k1, l1, l2, ib;
  25   int ld, ii, ip, is, nq, nr;
  26   int ido, ipm, nfm1;
  27   int nl=n;
  28   int nf=0;
  29
  30  L101:
  31   j++;
  32   if (j < 4)
  33     ntry=ntryh[j];
  34   else
  35     ntry+=2;
  36
  37  L104:
  38   nq=nl/ntry;
  39   nr=nl-ntry*nq;
  40   if (nr!=0) goto L101;
  41
  42   nf++;
  43   ifac[nf+1]=ntry;
  44   nl=nq;
  45   if(ntry!=2)goto L107;
  46   if(nf==1)goto L107;
  47
  48   for (i=1;i<nf;i++){
  49     ib=nf-i+1;
  50     ifac[ib+1]=ifac[ib];
  51   }
  52   ifac[2] = 2;
  53
  54  L107:
  55   if(nl!=1)goto L104;
  56   ifac[0]=n;
  57   ifac[1]=nf;
  58   argh=tpi/n;
  59   is=0;
  60   nfm1=nf-1;
  61   l1=1;
  62
  63   if(nfm1==0)return;
  64
  65   for (k1=0;k1<nfm1;k1++){
  66     ip=ifac[k1+2];
  67     ld=0;
  68     l2=l1*ip;
  69     ido=n/l2;
  70     ipm=ip-1;
  71
  72     for (j=0;j<ipm;j++){
  73       ld+=l1;
  74       i=is;
  75       argld=(double)ld*argh;
  76       fi=0.;
  77       for (ii=2;ii<ido;ii+=2){
  78         fi+=1.;
  79         arg=fi*argld;
  80         wa[i++]=cos(arg);
  81         wa[i++]=sin(arg);
  82       }
  83       is+=ido;
  84     }
  85     l1=l2;
  86   }
  87 }
  88
  89 static void fdrffti(int n, double *wsave, int *ifac){
  90
  91   if (n == 1) return;
  92   drfti1(n, wsave+n, ifac);
  93 }
  94
  95 static void dradf2(int ido,int l1,double *cc,double *ch,double *wa1){
  96   int i,k;
  97   double ti2,tr2;
  98   int t0,t1,t2,t3,t4,t5,t6;
  99
 100   t1=0;
 101   t0=(t2=l1*ido);
 102   t3=ido<<1;
 103   for(k=0;k<l1;k++){
 104     ch[t1<<1]=cc[t1]+cc[t2];
 105     ch[(t1<<1)+t3-1]=cc[t1]-cc[t2];
 106     t1+=ido;
 107     t2+=ido;
 108   }
 109
 110   if(ido<2)return;
 111   if(ido==2)goto L105;
 112
 113   t1=0;
 114   t2=t0;
 115   for(k=0;k<l1;k++){
 116     t3=t2;
 117     t4=(t1<<1)+(ido<<1);
 118     t5=t1;
 119     t6=t1+t1;
 120     for(i=2;i<ido;i+=2){
 121       t3+=2;
 122       t4-=2;
 123       t5+=2;
 124       t6+=2;
 125       tr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
 126       ti2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
 127       ch[t6]=cc[t5]+ti2;
 128       ch[t4]=ti2-cc[t5];
 129       ch[t6-1]=cc[t5-1]+tr2;
 130       ch[t4-1]=cc[t5-1]-tr2;
 131     }
 132     t1+=ido;
 133     t2+=ido;
 134   }
 135
 136   if(ido%2==1)return;
 137
 138  L105:
 139   t3=(t2=(t1=ido)-1);
 140   t2+=t0;
 141   for(k=0;k<l1;k++){
 142     ch[t1]=-cc[t2];
 143     ch[t1-1]=cc[t3];
 144     t1+=ido<<1;
 145     t2+=ido;
 146     t3+=ido;
 147   }
 148 }
 149
 150 static void dradf4(int ido,int l1,double *cc,double *ch,double *wa1,
 151             double *wa2,double *wa3){
 152   static double hsqt2 = .70710678118654752440084436210485;
 153   int i,k,t0,t1,t2,t3,t4,t5,t6;
 154   double ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
 155   t0=l1*ido;
 156
 157   t1=t0;
 158   t4=t1<<1;
 159   t2=t1+(t1<<1);
 160   t3=0;
 161
 162   for(k=0;k<l1;k++){
 163     tr1=cc[t1]+cc[t2];
 164     tr2=cc[t3]+cc[t4];
 165
 166     ch[t5=t3<<2]=tr1+tr2;
 167     ch[(ido<<2)+t5-1]=tr2-tr1;
 168     ch[(t5+=(ido<<1))-1]=cc[t3]-cc[t4];
 169     ch[t5]=cc[t2]-cc[t1];
 170
 171     t1+=ido;
 172     t2+=ido;
 173     t3+=ido;
 174     t4+=ido;
 175   }
 176
 177   if(ido<2)return;
 178   if(ido==2)goto L105;
 179
 180
 181   t1=0;
 182   for(k=0;k<l1;k++){
 183     t2=t1;
 184     t4=t1<<2;
 185     t5=(t6=ido<<1)+t4;
 186     for(i=2;i<ido;i+=2){
 187       t3=(t2+=2);
 188       t4+=2;
 189       t5-=2;
 190
 191       t3+=t0;
 192       cr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
 193       ci2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
 194       t3+=t0;
 195       cr3=wa2[i-2]*cc[t3-1]+wa2[i-1]*cc[t3];
 196       ci3=wa2[i-2]*cc[t3]-wa2[i-1]*cc[t3-1];
 197       t3+=t0;
 198       cr4=wa3[i-2]*cc[t3-1]+wa3[i-1]*cc[t3];
 199       ci4=wa3[i-2]*cc[t3]-wa3[i-1]*cc[t3-1];
 200
 201       tr1=cr2+cr4;
 202       tr4=cr4-cr2;
 203       ti1=ci2+ci4;
 204       ti4=ci2-ci4;
 205
 206       ti2=cc[t2]+ci3;
 207       ti3=cc[t2]-ci3;
 208       tr2=cc[t2-1]+cr3;
 209       tr3=cc[t2-1]-cr3;
 210
 211       ch[t4-1]=tr1+tr2;
 212       ch[t4]=ti1+ti2;
 213
 214       ch[t5-1]=tr3-ti4;
 215       ch[t5]=tr4-ti3;
 216
 217       ch[t4+t6-1]=ti4+tr3;
 218       ch[t4+t6]=tr4+ti3;
 219
 220       ch[t5+t6-1]=tr2-tr1;
 221       ch[t5+t6]=ti1-ti2;
 222     }
 223     t1+=ido;
 224   }
 225   if(ido&1)return;
 226
 227  L105:
 228
 229   t2=(t1=t0+ido-1)+(t0<<1);
 230   t3=ido<<2;
 231   t4=ido;
 232   t5=ido<<1;
 233   t6=ido;
 234
 235   for(k=0;k<l1;k++){
 236     ti1=-hsqt2*(cc[t1]+cc[t2]);
 237     tr1=hsqt2*(cc[t1]-cc[t2]);
 238
 239     ch[t4-1]=tr1+cc[t6-1];
 240     ch[t4+t5-1]=cc[t6-1]-tr1;
 241
 242     ch[t4]=ti1-cc[t1+t0];
 243     ch[t4+t5]=ti1+cc[t1+t0];
 244
 245     t1+=ido;
 246     t2+=ido;
 247     t4+=t3;
 248     t6+=ido;
 249   }
 250 }
 251
 252 static void dradfg(int ido,int ip,int l1,int idl1,double *cc,double *c1,
 253                           double *c2,double *ch,double *ch2,double *wa){
 254
 255   static double tpi=6.28318530717958647692528676655900577;
 256   int idij,ipph,i,j,k,l,ic,ik,is;
 257   int t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10;
 258   double dc2,ai1,ai2,ar1,ar2,ds2;
 259   int nbd;
 260   double dcp,arg,dsp,ar1h,ar2h;
 261   int idp2,ipp2;
 262
 263   arg=tpi/(double)ip;
 264   dcp=cos(arg);
 265   dsp=sin(arg);
 266   ipph=(ip+1)>>1;
 267   ipp2=ip;
 268   idp2=ido;
 269   nbd=(ido-1)>>1;
 270   t0=l1*ido;
 271   t10=ip*ido;
 272
 273   if(ido==1)goto L119;
 274   for(ik=0;ik<idl1;ik++)ch2[ik]=c2[ik];
 275
 276   t1=0;
 277   for(j=1;j<ip;j++){
 278     t1+=t0;
 279     t2=t1;
 280     for(k=0;k<l1;k++){
 281       ch[t2]=c1[t2];
 282       t2+=ido;
 283     }
 284   }
 285
 286   is=-ido;
 287   t1=0;
 288   if(nbd>l1){
 289     for(j=1;j<ip;j++){
 290       t1+=t0;
 291       is+=ido;
 292       t2= -ido+t1;
 293       for(k=0;k<l1;k++){
 294         idij=is-1;
 295         t2+=ido;
 296         t3=t2;
 297         for(i=2;i<ido;i+=2){
 298           idij+=2;
 299           t3+=2;
 300           ch[t3-1]=wa[idij-1]*c1[t3-1]+wa[idij]*c1[t3];
 301           ch[t3]=wa[idij-1]*c1[t3]-wa[idij]*c1[t3-1];
 302         }
 303       }
 304     }
 305   }else{
 306
 307     for(j=1;j<ip;j++){
 308       is+=ido;
 309       idij=is-1;
 310       t1+=t0;
 311       t2=t1;
 312       for(i=2;i<ido;i+=2){
 313         idij+=2;
 314         t2+=2;
 315         t3=t2;
 316         for(k=0;k<l1;k++){
 317           ch[t3-1]=wa[idij-1]*c1[t3-1]+wa[idij]*c1[t3];
 318           ch[t3]=wa[idij-1]*c1[t3]-wa[idij]*c1[t3-1];
 319           t3+=ido;
 320         }
 321       }
 322     }
 323   }
 324
 325   t1=0;
 326   t2=ipp2*t0;
 327   if(nbd<l1){
 328     for(j=1;j<ipph;j++){
 329       t1+=t0;
 330       t2-=t0;
 331       t3=t1;
 332       t4=t2;
 333       for(i=2;i<ido;i+=2){
 334         t3+=2;
 335         t4+=2;
 336         t5=t3-ido;
 337         t6=t4-ido;
 338         for(k=0;k<l1;k++){
 339           t5+=ido;
 340           t6+=ido;
 341           c1[t5-1]=ch[t5-1]+ch[t6-1];
 342           c1[t6-1]=ch[t5]-ch[t6];
 343           c1[t5]=ch[t5]+ch[t6];
 344           c1[t6]=ch[t6-1]-ch[t5-1];
 345         }
 346       }
 347     }
 348   }else{
 349     for(j=1;j<ipph;j++){
 350       t1+=t0;
 351       t2-=t0;
 352       t3=t1;
 353       t4=t2;
 354       for(k=0;k<l1;k++){
 355         t5=t3;
 356         t6=t4;
 357         for(i=2;i<ido;i+=2){
 358           t5+=2;
 359           t6+=2;
 360           c1[t5-1]=ch[t5-1]+ch[t6-1];
 361           c1[t6-1]=ch[t5]-ch[t6];
 362           c1[t5]=ch[t5]+ch[t6];
 363           c1[t6]=ch[t6-1]-ch[t5-1];
 364         }
 365         t3+=ido;
 366         t4+=ido;
 367       }
 368     }
 369   }
 370
 371 L119:
 372   for(ik=0;ik<idl1;ik++)c2[ik]=ch2[ik];
 373
 374   t1=0;
 375   t2=ipp2*idl1;
 376   for(j=1;j<ipph;j++){
 377     t1+=t0;
 378     t2-=t0;
 379     t3=t1-ido;
 380     t4=t2-ido;
 381     for(k=0;k<l1;k++){
 382       t3+=ido;
 383       t4+=ido;
 384       c1[t3]=ch[t3]+ch[t4];
 385       c1[t4]=ch[t4]-ch[t3];
 386     }
 387   }
 388
 389   ar1=1.;
 390   ai1=0.;
 391   t1=0;
 392   t2=ipp2*idl1;
 393   t3=(ip-1)*idl1;
 394   for(l=1;l<ipph;l++){
 395     t1+=idl1;
 396     t2-=idl1;
 397     ar1h=dcp*ar1-dsp*ai1;
 398     ai1=dcp*ai1+dsp*ar1;
 399     ar1=ar1h;
 400     t4=t1;
 401     t5=t2;
 402     t6=t3;
 403     t7=idl1;
 404
 405     for(ik=0;ik<idl1;ik++){
 406       ch2[t4++]=c2[ik]+ar1*c2[t7++];
 407       ch2[t5++]=ai1*c2[t6++];
 408     }
 409
 410     dc2=ar1;
 411     ds2=ai1;
 412     ar2=ar1;
 413     ai2=ai1;
 414
 415     t4=idl1;
 416     t5=(ipp2-1)*idl1;
 417     for(j=2;j<ipph;j++){
 418       t4+=idl1;
 419       t5-=idl1;
 420
 421       ar2h=dc2*ar2-ds2*ai2;
 422       ai2=dc2*ai2+ds2*ar2;
 423       ar2=ar2h;
 424
 425       t6=t1;
 426       t7=t2;
 427       t8=t4;
 428       t9=t5;
 429       for(ik=0;ik<idl1;ik++){
 430         ch2[t6++]+=ar2*c2[t8++];
 431         ch2[t7++]+=ai2*c2[t9++];
 432       }
 433     }
 434   }
 435
 436   t1=0;
 437   for(j=1;j<ipph;j++){
 438     t1+=idl1;
 439     t2=t1;
 440     for(ik=0;ik<idl1;ik++)ch2[ik]+=c2[t2++];
 441   }
 442
 443   if(ido<l1)goto L132;
 444
 445   t1=0;
 446   t2=0;
 447   for(k=0;k<l1;k++){
 448     t3=t1;
 449     t4=t2;
 450     for(i=0;i<ido;i++)cc[t4++]=ch[t3++];
 451     t1+=ido;
 452     t2+=t10;
 453   }
 454
 455   goto L135;
 456
 457  L132:
 458   for(i=0;i<ido;i++){
 459     t1=i;
 460     t2=i;
 461     for(k=0;k<l1;k++){
 462       cc[t2]=ch[t1];
 463       t1+=ido;
 464       t2+=t10;
 465     }
 466   }
 467
 468  L135:
 469   t1=0;
 470   t2=ido<<1;
 471   t3=0;
 472   t4=ipp2*t0;
 473   for(j=1;j<ipph;j++){
 474
 475     t1+=t2;
 476     t3+=t0;
 477     t4-=t0;
 478
 479     t5=t1;
 480     t6=t3;
 481     t7=t4;
 482
 483     for(k=0;k<l1;k++){
 484       cc[t5-1]=ch[t6];
 485       cc[t5]=ch[t7];
 486       t5+=t10;
 487       t6+=ido;
 488       t7+=ido;
 489     }
 490   }
 491
 492   if(ido==1)return;
 493   if(nbd<l1)goto L141;
 494
 495   t1=-ido;
 496   t3=0;
 497   t4=0;
 498   t5=ipp2*t0;
 499   for(j=1;j<ipph;j++){
 500     t1+=t2;
 501     t3+=t2;
 502     t4+=t0;
 503     t5-=t0;
 504     t6=t1;
 505     t7=t3;
 506     t8=t4;
 507     t9=t5;
 508     for(k=0;k<l1;k++){
 509       for(i=2;i<ido;i+=2){
 510         ic=idp2-i;
 511         cc[i+t7-1]=ch[i+t8-1]+ch[i+t9-1];
 512         cc[ic+t6-1]=ch[i+t8-1]-ch[i+t9-1];
 513         cc[i+t7]=ch[i+t8]+ch[i+t9];
 514         cc[ic+t6]=ch[i+t9]-ch[i+t8];
 515       }
 516       t6+=t10;
 517       t7+=t10;
 518       t8+=ido;
 519       t9+=ido;
 520     }
 521   }
 522   return;
 523
 524  L141:
 525
 526   t1=-ido;
 527   t3=0;
 528   t4=0;
 529   t5=ipp2*t0;
 530   for(j=1;j<ipph;j++){
 531     t1+=t2;
 532     t3+=t2;
 533     t4+=t0;
 534     t5-=t0;
 535     for(i=2;i<ido;i+=2){
 536       t6=idp2+t1-i;
 537       t7=i+t3;
 538       t8=i+t4;
 539       t9=i+t5;
 540       for(k=0;k<l1;k++){
 541         cc[t7-1]=ch[t8-1]+ch[t9-1];
 542         cc[t6-1]=ch[t8-1]-ch[t9-1];
 543         cc[t7]=ch[t8]+ch[t9];
 544         cc[t6]=ch[t9]-ch[t8];
 545         t6+=t10;
 546         t7+=t10;
 547         t8+=ido;
 548         t9+=ido;
 549       }
 550     }
 551   }
 552 }
 553
 554 static void drftf1(int n,double *c,double *ch,double *wa,int *ifac){
 555   int i,k1,l1,l2;
 556   int na,kh,nf;
 557   int ip,iw,ido,idl1,ix2,ix3;
 558
 559   nf=ifac[1];
 560   na=1;
 561   l2=n;
 562   iw=n;
 563
 564   for(k1=0;k1<nf;k1++){
 565     kh=nf-k1;
 566     ip=ifac[kh+1];
 567     l1=l2/ip;
 568     ido=n/l2;
 569     idl1=ido*l1;
 570     iw-=(ip-1)*ido;
 571     na=1-na;
 572
 573     if(ip!=4)goto L102;
 574
 575     ix2=iw+ido;
 576     ix3=ix2+ido;
 577     if(na!=0)
 578       dradf4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
 579     else
 580       dradf4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
 581     goto L110;
 582
 583  L102:
 584     if(ip!=2)goto L104;
 585     if(na!=0)goto L103;
 586
 587     dradf2(ido,l1,c,ch,wa+iw-1);
 588     goto L110;
 589
 590   L103:
 591     dradf2(ido,l1,ch,c,wa+iw-1);
 592     goto L110;
 593
 594   L104:
 595     if(ido==1)na=1-na;
 596     if(na!=0)goto L109;
 597
 598     dradfg(ido,ip,l1,idl1,c,c,c,ch,ch,wa+iw-1);
 599     na=1;
 600     goto L110;
 601
 602   L109:
 603     dradfg(ido,ip,l1,idl1,ch,ch,ch,c,c,wa+iw-1);
 604     na=0;
 605
 606   L110:
 607     l2=l1;
 608   }
 609
 610   if(na==1)return;
 611
 612   for(i=0;i<n;i++)c[i]=ch[i];
 613 }
 614
 615 static void dradb2(int ido,int l1,double *cc,double *ch,double *wa1){
 616   int i,k,t0,t1,t2,t3,t4,t5,t6;
 617   double ti2,tr2;
 618
 619   t0=l1*ido;
 620
 621   t1=0;
 622   t2=0;
 623   t3=(ido<<1)-1;
 624   for(k=0;k<l1;k++){
 625     ch[t1]=cc[t2]+cc[t3+t2];
 626     ch[t1+t0]=cc[t2]-cc[t3+t2];
 627     t2=(t1+=ido)<<1;
 628   }
 629
 630   if(ido<2)return;
 631   if(ido==2)goto L105;
 632
 633   t1=0;
 634   t2=0;
 635   for(k=0;k<l1;k++){
 636     t3=t1;
 637     t5=(t4=t2)+(ido<<1);
 638     t6=t0+t1;
 639     for(i=2;i<ido;i+=2){
 640       t3+=2;
 641       t4+=2;
 642       t5-=2;
 643       t6+=2;
 644       ch[t3-1]=cc[t4-1]+cc[t5-1];
 645       tr2=cc[t4-1]-cc[t5-1];
 646       ch[t3]=cc[t4]-cc[t5];
 647       ti2=cc[t4]+cc[t5];
 648       ch[t6-1]=wa1[i-2]*tr2-wa1[i-1]*ti2;
 649       ch[t6]=wa1[i-2]*ti2+wa1[i-1]*tr2;
 650     }
 651     t2=(t1+=ido)<<1;
 652   }
 653
 654   if(ido%2==1)return;
 655
 656 L105:
 657   t1=ido-1;
 658   t2=ido-1;
 659   for(k=0;k<l1;k++){
 660     ch[t1]=cc[t2]+cc[t2];
 661     ch[t1+t0]=-(cc[t2+1]+cc[t2+1]);
 662     t1+=ido;
 663     t2+=ido<<1;
 664   }
 665 }
 666
 667 static void dradb3(int ido,int l1,double *cc,double *ch,double *wa1,
 668                           double *wa2){
 669   static double taur = -.5;
 670   static double taui = .86602540378443864676372317075293618;
 671   int i,k,t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10;
 672   double ci2,ci3,di2,di3,cr2,cr3,dr2,dr3,ti2,tr2;
 673   t0=l1*ido;
 674
 675   t1=0;
 676   t2=t0<<1;
 677   t3=ido<<1;
 678   t4=ido+(ido<<1);
 679   t5=0;
 680   for(k=0;k<l1;k++){
 681     tr2=cc[t3-1]+cc[t3-1];
 682     cr2=cc[t5]+(taur*tr2);
 683     ch[t1]=cc[t5]+tr2;
 684     ci3=taui*(cc[t3]+cc[t3]);
 685     ch[t1+t0]=cr2-ci3;
 686     ch[t1+t2]=cr2+ci3;
 687     t1+=ido;
 688     t3+=t4;
 689     t5+=t4;
 690   }
 691
 692   if(ido==1)return;
 693
 694   t1=0;
 695   t3=ido<<1;
 696   for(k=0;k<l1;k++){
 697     t7=t1+(t1<<1);
 698     t6=(t5=t7+t3);
 699     t8=t1;
 700     t10=(t9=t1+t0)+t0;
 701
 702     for(i=2;i<ido;i+=2){
 703       t5+=2;
 704       t6-=2;
 705       t7+=2;
 706       t8+=2;
 707       t9+=2;
 708       t10+=2;
 709       tr2=cc[t5-1]+cc[t6-1];
 710       cr2=cc[t7-1]+(taur*tr2);
 711       ch[t8-1]=cc[t7-1]+tr2;
 712       ti2=cc[t5]-cc[t6];
 713       ci2=cc[t7]+(taur*ti2);
 714       ch[t8]=cc[t7]+ti2;
 715       cr3=taui*(cc[t5-1]-cc[t6-1]);
 716       ci3=taui*(cc[t5]+cc[t6]);
 717       dr2=cr2-ci3;
 718       dr3=cr2+ci3;
 719       di2=ci2+cr3;
 720       di3=ci2-cr3;
 721       ch[t9-1]=wa1[i-2]*dr2-wa1[i-1]*di2;
 722       ch[t9]=wa1[i-2]*di2+wa1[i-1]*dr2;
 723       ch[t10-1]=wa2[i-2]*dr3-wa2[i-1]*di3;
 724       ch[t10]=wa2[i-2]*di3+wa2[i-1]*dr3;
 725     }
 726     t1+=ido;
 727   }
 728 }
 729
 730 static void dradb4(int ido,int l1,double *cc,double *ch,double *wa1,
 731                           double *wa2,double *wa3){
 732   static double sqrt2=1.4142135623730950488016887242097;
 733   int i,k,t0,t1,t2,t3,t4,t5,t6,t7,t8;
 734   double ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
 735   t0=l1*ido;
 736
 737   t1=0;
 738   t2=ido<<2;
 739   t3=0;
 740   t6=ido<<1;
 741   for(k=0;k<l1;k++){
 742     t4=t3+t6;
 743     t5=t1;
 744     tr3=cc[t4-1]+cc[t4-1];
 745     tr4=cc[t4]+cc[t4];
 746     tr1=cc[t3]-cc[(t4+=t6)-1];
 747     tr2=cc[t3]+cc[t4-1];
 748     ch[t5]=tr2+tr3;
 749     ch[t5+=t0]=tr1-tr4;
 750     ch[t5+=t0]=tr2-tr3;
 751     ch[t5+=t0]=tr1+tr4;
 752     t1+=ido;
 753     t3+=t2;
 754   }
 755
 756   if(ido<2)return;
 757   if(ido==2)goto L105;
 758
 759   t1=0;
 760   for(k=0;k<l1;k++){
 761     t5=(t4=(t3=(t2=t1<<2)+t6))+t6;
 762     t7=t1;
 763     for(i=2;i<ido;i+=2){
 764       t2+=2;
 765       t3+=2;
 766       t4-=2;
 767       t5-=2;
 768       t7+=2;
 769       ti1=cc[t2]+cc[t5];
 770       ti2=cc[t2]-cc[t5];
 771       ti3=cc[t3]-cc[t4];
 772       tr4=cc[t3]+cc[t4];
 773       tr1=cc[t2-1]-cc[t5-1];
 774       tr2=cc[t2-1]+cc[t5-1];
 775       ti4=cc[t3-1]-cc[t4-1];
 776       tr3=cc[t3-1]+cc[t4-1];
 777       ch[t7-1]=tr2+tr3;
 778       cr3=tr2-tr3;
 779       ch[t7]=ti2+ti3;
 780       ci3=ti2-ti3;
 781       cr2=tr1-tr4;
 782       cr4=tr1+tr4;
 783       ci2=ti1+ti4;
 784       ci4=ti1-ti4;
 785
 786       ch[(t8=t7+t0)-1]=wa1[i-2]*cr2-wa1[i-1]*ci2;
 787       ch[t8]=wa1[i-2]*ci2+wa1[i-1]*cr2;
 788       ch[(t8+=t0)-1]=wa2[i-2]*cr3-wa2[i-1]*ci3;
 789       ch[t8]=wa2[i-2]*ci3+wa2[i-1]*cr3;
 790       ch[(t8+=t0)-1]=wa3[i-2]*cr4-wa3[i-1]*ci4;
 791       ch[t8]=wa3[i-2]*ci4+wa3[i-1]*cr4;
 792     }
 793     t1+=ido;
 794   }
 795
 796   if(ido%2 == 1)return;
 797
 798  L105:
 799
 800   t1=ido;
 801   t2=ido<<2;
 802   t3=ido-1;
 803   t4=ido+(ido<<1);
 804   for(k=0;k<l1;k++){
 805     t5=t3;
 806     ti1=cc[t1]+cc[t4];
 807     ti2=cc[t4]-cc[t1];
 808     tr1=cc[t1-1]-cc[t4-1];
 809     tr2=cc[t1-1]+cc[t4-1];
 810     ch[t5]=tr2+tr2;
 811     ch[t5+=t0]=sqrt2*(tr1-ti1);
 812     ch[t5+=t0]=ti2+ti2;
 813     ch[t5+=t0]=-sqrt2*(tr1+ti1);
 814
 815     t3+=ido;
 816     t1+=t2;
 817     t4+=t2;
 818   }
 819 }
 820
 821 static void dradbg(int ido,int ip,int l1,int idl1,double *cc,double *c1,
 822             double *c2,double *ch,double *ch2,double *wa){
 823   static double tpi=6.28318530717958647692528676655900577;
 824   int idij,ipph,i,j,k,l,ik,is,t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10,
 825       t11,t12;
 826   double dc2,ai1,ai2,ar1,ar2,ds2;
 827   int nbd;
 828   double dcp,arg,dsp,ar1h,ar2h;
 829   int ipp2;
 830
 831   t10=ip*ido;
 832   t0=l1*ido;
 833   arg=tpi/(double)ip;
 834   dcp=cos(arg);
 835   dsp=sin(arg);
 836   nbd=(ido-1)>>1;
 837   ipp2=ip;
 838   ipph=(ip+1)>>1;
 839   if(ido<l1)goto L103;
 840
 841   t1=0;
 842   t2=0;
 843   for(k=0;k<l1;k++){
 844     t3=t1;
 845     t4=t2;
 846     for(i=0;i<ido;i++){
 847       ch[t3]=cc[t4];
 848       t3++;
 849       t4++;
 850     }
 851     t1+=ido;
 852     t2+=t10;
 853   }
 854   goto L106;
 855
 856  L103:
 857   t1=0;
 858   for(i=0;i<ido;i++){
 859     t2=t1;
 860     t3=t1;
 861     for(k=0;k<l1;k++){
 862       ch[t2]=cc[t3];
 863       t2+=ido;
 864       t3+=t10;
 865     }
 866     t1++;
 867   }
 868
 869  L106:
 870   t1=0;
 871   t2=ipp2*t0;
 872   t7=(t5=ido<<1);
 873   for(j=1;j<ipph;j++){
 874     t1+=t0;
 875     t2-=t0;
 876     t3=t1;
 877     t4=t2;
 878     t6=t5;
 879     for(k=0;k<l1;k++){
 880       ch[t3]=cc[t6-1]+cc[t6-1];
 881       ch[t4]=cc[t6]+cc[t6];
 882       t3+=ido;
 883       t4+=ido;
 884       t6+=t10;
 885     }
 886     t5+=t7;
 887   }
 888
 889   if (ido == 1)goto L116;
 890   if(nbd<l1)goto L112;
 891
 892   t1=0;
 893   t2=ipp2*t0;
 894   t7=0;
 895   for(j=1;j<ipph;j++){
 896     t1+=t0;
 897     t2-=t0;
 898     t3=t1;
 899     t4=t2;
 900
 901     t7+=(ido<<1);
 902     t8=t7;
 903     for(k=0;k<l1;k++){
 904       t5=t3;
 905       t6=t4;
 906       t9=t8;
 907       t11=t8;
 908       for(i=2;i<ido;i+=2){
 909         t5+=2;
 910         t6+=2;
 911         t9+=2;
 912         t11-=2;
 913         ch[t5-1]=cc[t9-1]+cc[t11-1];
 914         ch[t6-1]=cc[t9-1]-cc[t11-1];
 915         ch[t5]=cc[t9]-cc[t11];
 916         ch[t6]=cc[t9]+cc[t11];
 917       }
 918       t3+=ido;
 919       t4+=ido;
 920       t8+=t10;
 921     }
 922   }
 923   goto L116;
 924
 925  L112:
 926   t1=0;
 927   t2=ipp2*t0;
 928   t7=0;
 929   for(j=1;j<ipph;j++){
 930     t1+=t0;
 931     t2-=t0;
 932     t3=t1;
 933     t4=t2;
 934     t7+=(ido<<1);
 935     t8=t7;
 936     t9=t7;
 937     for(i=2;i<ido;i+=2){
 938       t3+=2;
 939       t4+=2;
 940       t8+=2;
 941       t9-=2;
 942       t5=t3;
 943       t6=t4;
 944       t11=t8;
 945       t12=t9;
 946       for(k=0;k<l1;k++){
 947         ch[t5-1]=cc[t11-1]+cc[t12-1];
 948         ch[t6-1]=cc[t11-1]-cc[t12-1];
 949         ch[t5]=cc[t11]-cc[t12];
 950         ch[t6]=cc[t11]+cc[t12];
 951         t5+=ido;
 952         t6+=ido;
 953         t11+=t10;
 954         t12+=t10;
 955       }
 956     }
 957   }
 958
 959 L116:
 960   ar1=1.;
 961   ai1=0.;
 962   t1=0;
 963   t9=(t2=ipp2*idl1);
 964   t3=(ip-1)*idl1;
 965   for(l=1;l<ipph;l++){
 966     t1+=idl1;
 967     t2-=idl1;
 968
 969     ar1h=dcp*ar1-dsp*ai1;
 970     ai1=dcp*ai1+dsp*ar1;
 971     ar1=ar1h;
 972     t4=t1;
 973     t5=t2;
 974     t6=0;
 975     t7=idl1;
 976     t8=t3;
 977     for(ik=0;ik<idl1;ik++){
 978       c2[t4++]=ch2[t6++]+ar1*ch2[t7++];
 979       c2[t5++]=ai1*ch2[t8++];
 980     }
 981     dc2=ar1;
 982     ds2=ai1;
 983     ar2=ar1;
 984     ai2=ai1;
 985
 986     t6=idl1;
 987     t7=t9-idl1;
 988     for(j=2;j<ipph;j++){
 989       t6+=idl1;
 990       t7-=idl1;
 991       ar2h=dc2*ar2-ds2*ai2;
 992       ai2=dc2*ai2+ds2*ar2;
 993       ar2=ar2h;
 994       t4=t1;
 995       t5=t2;
 996       t11=t6;
 997       t12=t7;
 998       for(ik=0;ik<idl1;ik++){
 999         c2[t4++]+=ar2*ch2[t11++];
1000         c2[t5++]+=ai2*ch2[t12++];
1001       }
1002     }
1003   }
1004
1005   t1=0;
1006   for(j=1;j<ipph;j++){
1007     t1+=idl1;
1008     t2=t1;
1009     for(ik=0;ik<idl1;ik++)ch2[ik]+=ch2[t2++];
1010   }
1011
1012   t1=0;
1013   t2=ipp2*t0;
1014   for(j=1;j<ipph;j++){
1015     t1+=t0;
1016     t2-=t0;
1017     t3=t1;
1018     t4=t2;
1019     for(k=0;k<l1;k++){
1020       ch[t3]=c1[t3]-c1[t4];
1021       ch[t4]=c1[t3]+c1[t4];
1022       t3+=ido;
1023       t4+=ido;
1024     }
1025   }
1026
1027   if(ido==1)goto L132;
1028   if(nbd<l1)goto L128;
1029
1030   t1=0;
1031   t2=ipp2*t0;
1032   for(j=1;j<ipph;j++){
1033     t1+=t0;
1034     t2-=t0;
1035     t3=t1;
1036     t4=t2;
1037     for(k=0;k<l1;k++){
1038       t5=t3;
1039       t6=t4;
1040       for(i=2;i<ido;i+=2){
1041         t5+=2;
1042         t6+=2;
1043         ch[t5-1]=c1[t5-1]-c1[t6];
1044         ch[t6-1]=c1[t5-1]+c1[t6];
1045         ch[t5]=c1[t5]+c1[t6-1];
1046         ch[t6]=c1[t5]-c1[t6-1];
1047       }
1048       t3+=ido;
1049       t4+=ido;
1050     }
1051   }
1052   goto L132;
1053
1054  L128:
1055   t1=0;
1056   t2=ipp2*t0;
1057   for(j=1;j<ipph;j++){
1058     t1+=t0;
1059     t2-=t0;
1060     t3=t1;
1061     t4=t2;
1062     for(i=2;i<ido;i+=2){
1063       t3+=2;
1064       t4+=2;
1065       t5=t3;
1066       t6=t4;
1067       for(k=0;k<l1;k++){
1068         ch[t5-1]=c1[t5-1]-c1[t6];
1069         ch[t6-1]=c1[t5-1]+c1[t6];
1070         ch[t5]=c1[t5]+c1[t6-1];
1071         ch[t6]=c1[t5]-c1[t6-1];
1072         t5+=ido;
1073         t6+=ido;
1074       }
1075     }
1076   }
1077
1078 L132:
1079   if(ido==1)return;
1080
1081   for(ik=0;ik<idl1;ik++)c2[ik]=ch2[ik];
1082
1083   t1=0;
1084   for(j=1;j<ip;j++){
1085     t2=(t1+=t0);
1086     for(k=0;k<l1;k++){
1087       c1[t2]=ch[t2];
1088       t2+=ido;
1089     }
1090   }
1091
1092   if(nbd>l1)goto L139;
1093
1094   is= -ido-1;
1095   t1=0;
1096   for(j=1;j<ip;j++){
1097     is+=ido;
1098     t1+=t0;
1099     idij=is;
1100     t2=t1;
1101     for(i=2;i<ido;i+=2){
1102       t2+=2;
1103       idij+=2;
1104       t3=t2;
1105       for(k=0;k<l1;k++){
1106         c1[t3-1]=wa[idij-1]*ch[t3-1]-wa[idij]*ch[t3];
1107         c1[t3]=wa[idij-1]*ch[t3]+wa[idij]*ch[t3-1];
1108         t3+=ido;
1109       }
1110     }
1111   }
1112   return;
1113
1114  L139:
1115   is= -ido-1;
1116   t1=0;
1117   for(j=1;j<ip;j++){
1118     is+=ido;
1119     t1+=t0;
1120     t2=t1;
1121     for(k=0;k<l1;k++){
1122       idij=is;
1123       t3=t2;
1124       for(i=2;i<ido;i+=2){
1125         idij+=2;
1126         t3+=2;
1127         c1[t3-1]=wa[idij-1]*ch[t3-1]-wa[idij]*ch[t3];
1128         c1[t3]=wa[idij-1]*ch[t3]+wa[idij]*ch[t3-1];
1129       }
1130       t2+=ido;
1131     }
1132   }
1133 }
1134
1135 static void drftb1(int n, double *c, double *ch, double *wa, int *ifac){
1136   int i,k1,l1,l2;
1137   int na;
1138   int nf,ip,iw,ix2,ix3,ido,idl1;
1139
1140   nf=ifac[1];
1141   na=0;
1142   l1=1;
1143   iw=1;
1144
1145   for(k1=0;k1<nf;k1++){
1146     ip=ifac[k1 + 2];
1147     l2=ip*l1;
1148     ido=n/l2;
1149     idl1=ido*l1;
1150     if(ip!=4)goto L103;
1151     ix2=iw+ido;
1152     ix3=ix2+ido;
1153
1154     if(na!=0)
1155       dradb4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
1156     else
1157       dradb4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
1158     na=1-na;
1159     goto L115;
1160
1161   L103:
1162     if(ip!=2)goto L106;
1163
1164     if(na!=0)
1165       dradb2(ido,l1,ch,c,wa+iw-1);
1166     else
1167       dradb2(ido,l1,c,ch,wa+iw-1);
1168     na=1-na;
1169     goto L115;
1170
1171   L106:
1172     if(ip!=3)goto L109;
1173
1174     ix2=iw+ido;
1175     if(na!=0)
1176       dradb3(ido,l1,ch,c,wa+iw-1,wa+ix2-1);
1177     else
1178       dradb3(ido,l1,c,ch,wa+iw-1,wa+ix2-1);
1179     na=1-na;
1180     goto L115;
1181
1182   L109:
1183 /*    The radix five case can be translated later..... */
1184 /*    if(ip!=5)goto L112;
1185
1186     ix2=iw+ido;
1187     ix3=ix2+ido;
1188     ix4=ix3+ido;
1189     if(na!=0)
1190       dradb5(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1,wa+ix4-1);
1191     else
1192       dradb5(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1,wa+ix4-1);
1193     na=1-na;
1194     goto L115;
1195
1196   L112:*/
1197     if(na!=0)
1198       dradbg(ido,ip,l1,idl1,ch,ch,ch,c,c,wa+iw-1);
1199     else
1200       dradbg(ido,ip,l1,idl1,c,c,c,ch,ch,wa+iw-1);
1201     if(ido==1)na=1-na;
1202
1203   L115:
1204     l1=l2;
1205     iw+=(ip-1)*ido;
1206   }
1207
1208   if(na==0)return;
1209
1210   for(i=0;i<n;i++)c[i]=ch[i];
1211 }
1212
1213 void drft_forward(drft_lookup *l,double *data){
1214   if(l->n==1)return;
1215   drftf1(l->n,data,l->trigcache,l->trigcache+l->n,l->splitcache);
1216 }
1217
1218 void drft_backward(drft_lookup *l,double *data){
1219   int i;
1220   double n1=1./l->n;
1221   if (l->n==1)return;
1222   drftb1(l->n,data,l->trigcache,l->trigcache+l->n,l->splitcache);
1223   for(i=0;i<l->n;i++)data[i]*=n1;
1224 }
1225
1226 void drft_init(drft_lookup *l,int n){
1227   l->n=n;
1228   l->trigcache=calloc(3*n,sizeof(double));
1229   l->splitcache=calloc(32,sizeof(int));
1230   fdrffti(n, l->trigcache, l->splitcache);
1231 }
1232
1233 void drft_clear(drft_lookup *l){
1234   if(l){
1235     if(l->trigcache)free(l->trigcache);
1236     if(l->splitcache)free(l->splitcache);
1237     memset(l,0,sizeof(drft_lookup));
1238   }
1239 }