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 drftf1(int n,double *c,double *ch,double *wa,int *ifac){
 253   int i,k1,l1,l2;
 254   int na,kh,nf;
 255   int ip,iw,ido,idl1,ix2,ix3;
 256
 257   nf=ifac[1];
 258   na=1;
 259   l2=n;
 260   iw=n;
 261
 262   for(k1=0;k1<nf;k1++){
 263     kh=nf-k1;
 264     ip=ifac[kh+1];
 265     l1=l2/ip;
 266     ido=n/l2;
 267     idl1=ido*l1;
 268     iw-=(ip-1)*ido;
 269     na=1-na;
 270
 271     if(ip!=4)goto L102;
 272
 273     ix2=iw+ido;
 274     ix3=ix2+ido;
 275     if(na!=0)
 276       dradf4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
 277     else
 278       dradf4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
 279     goto L110;
 280
 281  L102:
 282     if(ip!=2)goto L104;
 283     if(na!=0)goto L103;
 284
 285     dradf2(ido,l1,c,ch,wa+iw-1);
 286     goto L110;
 287
 288   L103:
 289     dradf2(ido,l1,ch,c,wa+iw-1);
 290     goto L110;
 291
 292   L104:
 293     return; /* We're restricted to powers of two.  just fail */
 294
 295   L110:
 296     l2=l1;
 297   }
 298
 299   if(na==1)return;
 300
 301   for(i=0;i<n;i++)c[i]=ch[i];
 302 }
 303
 304 static void dradb2(int ido,int l1,double *cc,double *ch,double *wa1){
 305   int i,k,t0,t1,t2,t3,t4,t5,t6;
 306   double ti2,tr2;
 307
 308   t0=l1*ido;
 309
 310   t1=0;
 311   t2=0;
 312   t3=(ido<<1)-1;
 313   for(k=0;k<l1;k++){
 314     ch[t1]=cc[t2]+cc[t3+t2];
 315     ch[t1+t0]=cc[t2]-cc[t3+t2];
 316     t2=(t1+=ido)<<1;
 317   }
 318
 319   if(ido<2)return;
 320   if(ido==2)goto L105;
 321
 322   t1=0;
 323   t2=0;
 324   for(k=0;k<l1;k++){
 325     t3=t1;
 326     t5=(t4=t2)+(ido<<1);
 327     t6=t0+t1;
 328     for(i=2;i<ido;i+=2){
 329       t3+=2;
 330       t4+=2;
 331       t5-=2;
 332       t6+=2;
 333       ch[t3-1]=cc[t4-1]+cc[t5-1];
 334       tr2=cc[t4-1]-cc[t5-1];
 335       ch[t3]=cc[t4]-cc[t5];
 336       ti2=cc[t4]+cc[t5];
 337       ch[t6-1]=wa1[i-2]*tr2-wa1[i-1]*ti2;
 338       ch[t6]=wa1[i-2]*ti2+wa1[i-1]*tr2;
 339     }
 340     t2=(t1+=ido)<<1;
 341   }
 342
 343   if(ido%2==1)return;
 344
 345 L105:
 346   t1=ido-1;
 347   t2=ido-1;
 348   for(k=0;k<l1;k++){
 349     ch[t1]=cc[t2]+cc[t2];
 350     ch[t1+t0]=-(cc[t2+1]+cc[t2+1]);
 351     t1+=ido;
 352     t2+=ido<<1;
 353   }
 354 }
 355
 356 static void dradb4(int ido,int l1,double *cc,double *ch,double *wa1,
 357                           double *wa2,double *wa3){
 358   static double sqrt2=1.4142135623730950488016887242097;
 359   int i,k,t0,t1,t2,t3,t4,t5,t6,t7,t8;
 360   double ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
 361   t0=l1*ido;
 362
 363   t1=0;
 364   t2=ido<<2;
 365   t3=0;
 366   t6=ido<<1;
 367   for(k=0;k<l1;k++){
 368     t4=t3+t6;
 369     t5=t1;
 370     tr3=cc[t4-1]+cc[t4-1];
 371     tr4=cc[t4]+cc[t4];
 372     tr1=cc[t3]-cc[(t4+=t6)-1];
 373     tr2=cc[t3]+cc[t4-1];
 374     ch[t5]=tr2+tr3;
 375     ch[t5+=t0]=tr1-tr4;
 376     ch[t5+=t0]=tr2-tr3;
 377     ch[t5+=t0]=tr1+tr4;
 378     t1+=ido;
 379     t3+=t2;
 380   }
 381
 382   if(ido<2)return;
 383   if(ido==2)goto L105;
 384
 385   t1=0;
 386   for(k=0;k<l1;k++){
 387     t5=(t4=(t3=(t2=t1<<2)+t6))+t6;
 388     t7=t1;
 389     for(i=2;i<ido;i+=2){
 390       t2+=2;
 391       t3+=2;
 392       t4-=2;
 393       t5-=2;
 394       t7+=2;
 395       ti1=cc[t2]+cc[t5];
 396       ti2=cc[t2]-cc[t5];
 397       ti3=cc[t3]-cc[t4];
 398       tr4=cc[t3]+cc[t4];
 399       tr1=cc[t2-1]-cc[t5-1];
 400       tr2=cc[t2-1]+cc[t5-1];
 401       ti4=cc[t3-1]-cc[t4-1];
 402       tr3=cc[t3-1]+cc[t4-1];
 403       ch[t7-1]=tr2+tr3;
 404       cr3=tr2-tr3;
 405       ch[t7]=ti2+ti3;
 406       ci3=ti2-ti3;
 407       cr2=tr1-tr4;
 408       cr4=tr1+tr4;
 409       ci2=ti1+ti4;
 410       ci4=ti1-ti4;
 411
 412       ch[(t8=t7+t0)-1]=wa1[i-2]*cr2-wa1[i-1]*ci2;
 413       ch[t8]=wa1[i-2]*ci2+wa1[i-1]*cr2;
 414       ch[(t8+=t0)-1]=wa2[i-2]*cr3-wa2[i-1]*ci3;
 415       ch[t8]=wa2[i-2]*ci3+wa2[i-1]*cr3;
 416       ch[(t8+=t0)-1]=wa3[i-2]*cr4-wa3[i-1]*ci4;
 417       ch[t8]=wa3[i-2]*ci4+wa3[i-1]*cr4;
 418     }
 419     t1+=ido;
 420   }
 421
 422   if(ido%2 == 1)return;
 423
 424  L105:
 425
 426   t1=ido;
 427   t2=ido<<2;
 428   t3=ido-1;
 429   t4=ido+(ido<<1);
 430   for(k=0;k<l1;k++){
 431     t5=t3;
 432     ti1=cc[t1]+cc[t4];
 433     ti2=cc[t4]-cc[t1];
 434     tr1=cc[t1-1]-cc[t4-1];
 435     tr2=cc[t1-1]+cc[t4-1];
 436     ch[t5]=tr2+tr2;
 437     ch[t5+=t0]=sqrt2*(tr1-ti1);
 438     ch[t5+=t0]=ti2+ti2;
 439     ch[t5+=t0]=-sqrt2*(tr1+ti1);
 440
 441     t3+=ido;
 442     t1+=t2;
 443     t4+=t2;
 444   }
 445 }
 446
 447 static void drftb1(int n, double *c, double *ch, double *wa, int *ifac){
 448   int i,k1,l1,l2;
 449   int na;
 450   int nf,ip,iw,ix2,ix3,ido,idl1;
 451
 452   nf=ifac[1];
 453   na=0;
 454   l1=1;
 455   iw=1;
 456
 457   for(k1=0;k1<nf;k1++){
 458     ip=ifac[k1 + 2];
 459     l2=ip*l1;
 460     ido=n/l2;
 461     idl1=ido*l1;
 462     if(ip!=4)goto L103;
 463     ix2=iw+ido;
 464     ix3=ix2+ido;
 465
 466     if(na!=0)
 467       dradb4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
 468     else
 469       dradb4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
 470     na=1-na;
 471     goto L115;
 472
 473   L103:
 474     if(ip!=2)goto L106;
 475
 476     if(na!=0)
 477       dradb2(ido,l1,ch,c,wa+iw-1);
 478     else
 479       dradb2(ido,l1,c,ch,wa+iw-1);
 480     na=1-na;
 481     goto L115;
 482
 483   L106:
 484     return; /* silently fail.  we only do powers of two in this version */
 485
 486   L115:
 487     l1=l2;
 488     iw+=(ip-1)*ido;
 489   }
 490
 491   if(na==0)return;
 492
 493   for(i=0;i<n;i++)c[i]=ch[i];
 494 }
 495
 496 void drft_forward(drft_lookup *l,double *data){
 497   if(l->n==1)return;
 498   drftf1(l->n,data,l->trigcache,l->trigcache+l->n,l->splitcache);
 499 }
 500
 501 void drft_backward(drft_lookup *l,double *data){
 502   int i;
 503   if (l->n==1)return;
 504   drftb1(l->n,data,l->trigcache,l->trigcache+l->n,l->splitcache);
 505   for(i=0;i<l->n;i++)data[i]/=l->n;
 506 }
 507
 508 void drft_init(drft_lookup *l,int n){
 509   l->n=n;
 510   l->trigcache=calloc(3*n,sizeof(double));
 511   l->splitcache=calloc(32,sizeof(int));
 512   fdrffti(n, l->trigcache, l->splitcache);
 513 }
 514
 515 void drft_clear(drft_lookup *l){
 516   if(l){
 517     if(l->trigcache)free(l->trigcache);
 518     if(l->splitcache)free(l->splitcache);
 519     memset(l,0,sizeof(drft_lookup));
 520   }
 521 }