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 <math.h>
  16 #include "smallft.h"
  17
  18 static void drfti1(int n, double *wa, int *ifac){
  19   static int ntryh[4] = { 4,2,3,5 };
  20   static double tpi = 6.28318530717958647692528676655900577;
  21   double arg,argh,argld,fi;
  22   int ntry=0,i,j=-1;
  23   int k1, l1, l2, ib;
  24   int ld, ii, ip, is, nq, nr;
  25   int ido, ipm, nfm1;
  26   int nl=n;
  27   int nf=0;
  28
  29  L101:
  30   j++;
  31   if (j < 4)
  32     ntry=ntryh[j];
  33   else
  34     ntry+=2;
  35
  36  L104:
  37   nq=nl/ntry;
  38   nr=nl-ntry*nq;
  39   if (nr!=0) goto L101;
  40
  41   nf++;
  42   ifac[nf+1]=ntry;
  43   nl=nq;
  44   if(ntry!=2)goto L107;
  45   if(nf==1)goto L107;
  46
  47   for (i=1;i<nf;i++){
  48     ib=nf-i+1;
  49     ifac[ib+1]=ifac[ib];
  50   }
  51   ifac[2] = 2;
  52
  53  L107:
  54   if(nl!=1)goto L104;
  55   ifac[0]=n;
  56   ifac[1]=nf;
  57   argh=tpi/n;
  58   is=0;
  59   nfm1=nf-1;
  60   l1=1;
  61
  62   if(nfm1==0)return;
  63
  64   for (k1=0;k1<nfm1;k1++){
  65     ip=ifac[k1+2];
  66     ld=0;
  67     l2=l1*ip;
  68     ido=n/l2;
  69     ipm=ip-1;
  70
  71     for (j=0;j<ipm;j++){
  72       ld+=l1;
  73       i=is;
  74       argld=(double)ld*argh;
  75       fi=0.;
  76       for (ii=2;ii<ido;ii+=2){
  77         fi+=1.;
  78         arg=fi*argld;
  79         wa[i++]=cos(arg);
  80         wa[i++]=sin(arg);
  81       }
  82       is+=ido;
  83     }
  84     l1=l2;
  85   }
  86 }
  87
  88 static void fdrffti(int n, double *wsave, int *ifac){
  89
  90   if (n == 1) return;
  91   drfti1(n, wsave+n, ifac);
  92 }
  93
  94 static void dradf2(int ido,int l1,double *cc,double *ch,double *wa1){
  95   int i,k;
  96   double ti2,tr2;
  97   int t0,t1,t2,t3,t4,t5,t6;
  98
  99   t1=0;
 100   t0=(t2=l1*ido);
 101   t3=ido<<1;
 102   for(k=0;k<l1;k++){
 103     ch[t1<<1]=cc[t1]+cc[t2];
 104     ch[(t1<<1)+t3-1]=cc[t1]-cc[t2];
 105     t1+=ido;
 106     t2+=ido;
 107   }
 108
 109   if(ido<2)return;
 110   if(ido==2)goto L105;
 111
 112   t1=0;
 113   t2=t0;
 114   for(k=0;k<l1;k++){
 115     t3=t2;
 116     t4=(t1<<1)+(ido<<1);
 117     t5=t1;
 118     t6=t1+t1;
 119     for(i=2;i<ido;i+=2){
 120       t3+=2;
 121       t4-=2;
 122       t5+=2;
 123       t6+=2;
 124       tr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
 125       ti2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
 126       ch[t6]=cc[t5]+ti2;
 127       ch[t4]=ti2-cc[t5];
 128       ch[t6-1]=cc[t5-1]+tr2;
 129       ch[t4-1]=cc[t5-1]-tr2;
 130     }
 131     t1+=ido;
 132     t2+=ido;
 133   }
 134
 135   if(ido%2==1)return;
 136
 137  L105:
 138   t3=(t2=(t1=ido)-1);
 139   t2+=t0;
 140   for(k=0;k<l1;k++){
 141     ch[t1]=-cc[t2];
 142     ch[t1-1]=cc[t3];
 143     t1+=ido<<1;
 144     t2+=ido;
 145     t3+=ido;
 146   }
 147 }
 148
 149 static void dradf4(int ido,int l1,double *cc,double *ch,double *wa1,
 150             double *wa2,double *wa3){
 151   static double hsqt2 = .70710678118654752440084436210485;
 152   int i,k,t0,t1,t2,t3,t4,t5,t6;
 153   double ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
 154   t0=l1*ido;
 155
 156   t1=t0;
 157   t4=t1<<1;
 158   t2=t1+(t1<<1);
 159   t3=0;
 160
 161   for(k=0;k<l1;k++){
 162     tr1=cc[t1]+cc[t2];
 163     tr2=cc[t3]+cc[t4];
 164
 165     ch[t5=t3<<2]=tr1+tr2;
 166     ch[(ido<<2)+t5-1]=tr2-tr1;
 167     ch[(t5+=(ido<<1))-1]=cc[t3]-cc[t4];
 168     ch[t5]=cc[t2]-cc[t1];
 169
 170     t1+=ido;
 171     t2+=ido;
 172     t3+=ido;
 173     t4+=ido;
 174   }
 175
 176   if(ido<2)return;
 177   if(ido==2)goto L105;
 178
 179
 180   t1=0;
 181   for(k=0;k<l1;k++){
 182     t2=t1;
 183     t4=t1<<2;
 184     t5=(t6=ido<<1)+t4;
 185     for(i=2;i<ido;i+=2){
 186       t3=(t2+=2);
 187       t4+=2;
 188       t5-=2;
 189
 190       t3+=t0;
 191       cr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
 192       ci2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
 193       t3+=t0;
 194       cr3=wa2[i-2]*cc[t3-1]+wa2[i-1]*cc[t3];
 195       ci3=wa2[i-2]*cc[t3]-wa2[i-1]*cc[t3-1];
 196       t3+=t0;
 197       cr4=wa3[i-2]*cc[t3-1]+wa3[i-1]*cc[t3];
 198       ci4=wa3[i-2]*cc[t3]-wa3[i-1]*cc[t3-1];
 199
 200       tr1=cr2+cr4;
 201       tr4=cr4-cr2;
 202       ti1=ci2+ci4;
 203       ti4=ci2-ci4;
 204
 205       ti2=cc[t2]+ci3;
 206       ti3=cc[t2]-ci3;
 207       tr2=cc[t2-1]+cr3;
 208       tr3=cc[t2-1]-cr3;
 209
 210       ch[t4-1]=tr1+tr2;
 211       ch[t4]=ti1+ti2;
 212
 213       ch[t5-1]=tr3-ti4;
 214       ch[t5]=tr4-ti3;
 215
 216       ch[t4+t6-1]=ti4+tr3;
 217       ch[t4+t6]=tr4+ti3;
 218
 219       ch[t5+t6-1]=tr2-tr1;
 220       ch[t5+t6]=ti1-ti2;
 221     }
 222     t1+=ido;
 223   }
 224   if(ido&1)return;
 225
 226  L105:
 227
 228   t2=(t1=t0+ido-1)+(t0<<1);
 229   t3=ido<<2;
 230   t4=ido;
 231   t5=ido<<1;
 232   t6=ido;
 233
 234   for(k=0;k<l1;k++){
 235     ti1=-hsqt2*(cc[t1]+cc[t2]);
 236     tr1=hsqt2*(cc[t1]-cc[t2]);
 237
 238     ch[t4-1]=tr1+cc[t6-1];
 239     ch[t4+t5-1]=cc[t6-1]-tr1;
 240
 241     ch[t4]=ti1-cc[t1+t0];
 242     ch[t4+t5]=ti1+cc[t1+t0];
 243
 244     t1+=ido;
 245     t2+=ido;
 246     t4+=t3;
 247     t6+=ido;
 248   }
 249 }
 250
 251 static void drftf1(int n,double *c,double *ch,double *wa,int *ifac){
 252   int i,k1,l1,l2;
 253   int na,kh,nf;
 254   int ip,iw,ido,idl1,ix2,ix3;
 255
 256   nf=ifac[1];
 257   na=1;
 258   l2=n;
 259   iw=n;
 260
 261   for(k1=0;k1<nf;k1++){
 262     kh=nf-k1;
 263     ip=ifac[kh+1];
 264     l1=l2/ip;
 265     ido=n/l2;
 266     idl1=ido*l1;
 267     iw-=(ip-1)*ido;
 268     na=1-na;
 269
 270     if(ip!=4)goto L102;
 271
 272     ix2=iw+ido;
 273     ix3=ix2+ido;
 274     if(na!=0)
 275       dradf4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
 276     else
 277       dradf4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
 278     goto L110;
 279
 280  L102:
 281     if(ip!=2)goto L104;
 282     if(na!=0)goto L103;
 283
 284     dradf2(ido,l1,c,ch,wa+iw-1);
 285     goto L110;
 286
 287   L103:
 288     dradf2(ido,l1,ch,c,wa+iw-1);
 289     goto L110;
 290
 291   L104:
 292     return; /* We're restricted to powers of two.  just fail */
 293
 294   L110:
 295     l2=l1;
 296   }
 297
 298   if(na==1)return;
 299
 300   for(i=0;i<n;i++)c[i]=ch[i];
 301 }
 302
 303 static void fdrfftf(int n,double *r,double *wsave,int *ifac){
 304   if(n==1)return;
 305   drftf1(n,r,wsave,wsave+n,ifac);
 306 }
 307
 308 static void dradb2(int ido,int l1,double *cc,double *ch,double *wa1){
 309   int i,k,t0,t1,t2,t3,t4,t5,t6;
 310   double ti2,tr2;
 311
 312   t0=l1*ido;
 313
 314   t1=0;
 315   t2=0;
 316   t3=(ido<<1)-1;
 317   for(k=0;k<l1;k++){
 318     ch[t1]=cc[t2]+cc[t3+t2];
 319     ch[t1+t0]=cc[t2]-cc[t3+t2];
 320     t2=(t1+=ido)<<1;
 321   }
 322
 323   if(ido<2)return;
 324   if(ido==2)goto L105;
 325
 326   t1=0;
 327   t2=0;
 328   for(k=0;k<l1;k++){
 329     t3=t1;
 330     t5=(t4=t2)+(ido<<1);
 331     t6=t0+t1;
 332     for(i=2;i<ido;i+=2){
 333       t3+=2;
 334       t4+=2;
 335       t5-=2;
 336       t6+=2;
 337       ch[t3-1]=cc[t4-1]+cc[t5-1];
 338       tr2=cc[t4-1]-cc[t5-1];
 339       ch[t3]=cc[t4]-cc[t5];
 340       ti2=cc[t4]+cc[t5];
 341       ch[t6-1]=wa1[i-2]*tr2-wa1[i-1]*ti2;
 342       ch[t6]=wa1[i-2]*ti2+wa1[i-1]*tr2;
 343     }
 344     t2=(t1+=ido)<<1;
 345   }
 346
 347   if(ido%2==1)return;
 348
 349 L105:
 350   t1=ido-1;
 351   t2=ido-1;
 352   for(k=0;k<l1;k++){
 353     ch[t1]=cc[t2]+cc[t2];
 354     ch[t1+t0]=-(cc[t2+1]+cc[t2+1]);
 355     t1+=ido;
 356     t2+=ido<<1;
 357   }
 358 }
 359
 360 static void dradb4(int ido,int l1,double *cc,double *ch,double *wa1,
 361                           double *wa2,double *wa3){
 362   static double sqrt2=1.4142135623730950488016887242097;
 363   int i,k,t0,t1,t2,t3,t4,t5,t6,t7,t8;
 364   double ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
 365   t0=l1*ido;
 366
 367   t1=0;
 368   t2=ido<<2;
 369   t3=0;
 370   t6=ido<<1;
 371   for(k=0;k<l1;k++){
 372     t4=t3+t6;
 373     t5=t1;
 374     tr3=cc[t4-1]+cc[t4-1];
 375     tr4=cc[t4]+cc[t4];
 376     tr1=cc[t3]-cc[(t4+=t6)-1];
 377     tr2=cc[t3]+cc[t4-1];
 378     ch[t5]=tr2+tr3;
 379     ch[t5+=t0]=tr1-tr4;
 380     ch[t5+=t0]=tr2-tr3;
 381     ch[t5+=t0]=tr1+tr4;
 382     t1+=ido;
 383     t3+=t2;
 384   }
 385
 386   if(ido<2)return;
 387   if(ido==2)goto L105;
 388
 389   t1=0;
 390   for(k=0;k<l1;k++){
 391     t5=(t4=(t3=(t2=t1<<2)+t6))+t6;
 392     t7=t1;
 393     for(i=2;i<ido;i+=2){
 394       t2+=2;
 395       t3+=2;
 396       t4-=2;
 397       t5-=2;
 398       t7+=2;
 399       ti1=cc[t2]+cc[t5];
 400       ti2=cc[t2]-cc[t5];
 401       ti3=cc[t3]-cc[t4];
 402       tr4=cc[t3]+cc[t4];
 403       tr1=cc[t2-1]-cc[t5-1];
 404       tr2=cc[t2-1]+cc[t5-1];
 405       ti4=cc[t3-1]-cc[t4-1];
 406       tr3=cc[t3-1]+cc[t4-1];
 407       ch[t7-1]=tr2+tr3;
 408       cr3=tr2-tr3;
 409       ch[t7]=ti2+ti3;
 410       ci3=ti2-ti3;
 411       cr2=tr1-tr4;
 412       cr4=tr1+tr4;
 413       ci2=ti1+ti4;
 414       ci4=ti1-ti4;
 415
 416       ch[(t8=t7+t0)-1]=wa1[i-2]*cr2-wa1[i-1]*ci2;
 417       ch[t8]=wa1[i-2]*ci2+wa1[i-1]*cr2;
 418       ch[(t8+=t0)-1]=wa2[i-2]*cr3-wa2[i-1]*ci3;
 419       ch[t8]=wa2[i-2]*ci3+wa2[i-1]*cr3;
 420       ch[(t8+=t0)-1]=wa3[i-2]*cr4-wa3[i-1]*ci4;
 421       ch[t8]=wa3[i-2]*ci4+wa3[i-1]*cr4;
 422     }
 423     t1+=ido;
 424   }
 425
 426   if(ido%2 == 1)return;
 427
 428  L105:
 429
 430   t1=ido;
 431   t2=ido<<2;
 432   t3=ido-1;
 433   t4=ido+(ido<<1);
 434   for(k=0;k<l1;k++){
 435     t5=t3;
 436     ti1=cc[t1]+cc[t4];
 437     ti2=cc[t4]-cc[t1];
 438     tr1=cc[t1-1]-cc[t4-1];
 439     tr2=cc[t1-1]+cc[t4-1];
 440     ch[t5]=tr2+tr2;
 441     ch[t5+=t0]=sqrt2*(tr1-ti1);
 442     ch[t5+=t0]=ti2+ti2;
 443     ch[t5+=t0]=-sqrt2*(tr1+ti1);
 444
 445     t3+=ido;
 446     t1+=t2;
 447     t4+=t2;
 448   }
 449 }
 450
 451 static void drftb1(int n, double *c, double *ch, double *wa, int *ifac){
 452   int i,k1,l1,l2;
 453   int na;
 454   int nf,ip,iw,ix2,ix3,ido,idl1;
 455
 456   nf=ifac[1];
 457   na=0;
 458   l1=1;
 459   iw=1;
 460
 461   for(k1=0;k1<nf;k1++){
 462     ip=ifac[k1 + 2];
 463     l2=ip*l1;
 464     ido=n/l2;
 465     idl1=ido*l1;
 466     if(ip!=4)goto L103;
 467     ix2=iw+ido;
 468     ix3=ix2+ido;
 469
 470     if(na!=0)
 471       dradb4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
 472     else
 473       dradb4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
 474     na=1-na;
 475     goto L115;
 476
 477   L103:
 478     if(ip!=2)goto L106;
 479
 480     if(na!=0)
 481       dradb2(ido,l1,ch,c,wa+iw-1);
 482     else
 483       dradb2(ido,l1,c,ch,wa+iw-1);
 484     na=1-na;
 485     goto L115;
 486
 487   L106:
 488     return; /* silently fail.  we only do powers of two in this version */
 489
 490   L115:
 491     l1=l2;
 492     iw+=(ip-1)*ido;
 493   }
 494
 495   if(na==0)return;
 496
 497   for(i=0;i<n;i++)c[i]=ch[i];
 498 }
 499
 500 static void fdrfftb(int n, double *r, double *wsave, int *ifac){
 501   if (n == 1)return;
 502   drftb1(n, r, wsave, wsave+n, ifac);
 503 }
 504
 505 void fft_forward(int n, double *buf,double *trigcache,int *splitcache){
 506   int flag=0;
 507
 508   if(!trigcache || !splitcache){
 509     trigcache=calloc(3*n,sizeof(double));
 510     splitcache=calloc(32,sizeof(int));
 511     fdrffti(n, trigcache, splitcache);
 512     flag=1;
 513   }
 514
 515   fdrfftf(n, buf, trigcache, splitcache);
 516
 517   if(flag){
 518     free(trigcache);
 519     free(splitcache);
 520   }
 521 }
 522
 523 void fft_backward(int n, double *buf, double *trigcache,int *splitcache){
 524   int i;
 525   int flag=0;
 526
 527   if(!trigcache || !splitcache){
 528     trigcache=calloc(3*n,sizeof(double));
 529     splitcache=calloc(32,sizeof(int));
 530     fdrffti(n, trigcache, splitcache);
 531     flag=1;
 532   }
 533
 534   fdrfftb(n, buf, trigcache, splitcache);
 535
 536   for(i=0;i<n;i++)buf[i]/=n;
 537
 538   if(flag){
 539     free(trigcache);
 540     free(splitcache);
 541   }
 542 }
 543
 544 void fft_i(int n, double **trigcache, int **splitcache){
 545   *trigcache=calloc(3*n,sizeof(double));
 546   *splitcache=calloc(32,sizeof(int));
 547   fdrffti(n, *trigcache, *splitcache);
 548 }