ghost/monty/lift/smallft.c

   1 /********************************************************************
   2  *                                                                  *
   3  * THIS FILE IS PART OF THE OggGhost SOFTWARE CODEC SOURCE CODE.    *
   4  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
   5  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
   6  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
   7  *                                                                  *
   8  * THE OggGhost SOURCE CODE IS (C) COPYRIGHT 1994-2007              *
   9  * the Xiph.Org FOundation http://www.xiph.org/                     *
  10  *                                                                  *
  11  ********************************************************************
  12
  13  function: unnormalized powers-of-two forward fft transform
  14  last mod: $Id: smallft.c 7573 2004-08-16 01:26:52Z conrad $
  15
  16  ********************************************************************/
  17
  18 /* FFT implementation from OggSquish, minus cosine transforms,
  19  * minus all but radix 2/4 case.  In Vorbis we only need this
  20  * cut-down version.
  21  *
  22  * To do more than just power-of-two sized vectors, see the full
  23  * version I wrote for NetLib.
  24  *
  25  * Note that the packing is a little strange; rather than the FFT r/i
  26  * packing following R_0, R_n, R_1, I_1, R_2, I_2 ... R_n-1, I_n-1,
  27  * it follows R_0, R_1, I_1, R_2, I_2 ... R_n-1, I_n-1, I_n like the
  28  * FORTRAN version
  29  */
  30
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <math.h>
  34 #include "smallft.h"
  35
  36 static int drfti1(int n, double *wa, int *ifac){
  37   static int ntryh[2] = { 4,2 };
  38   static double tpi = 6.28318530717958648f;
  39   double arg,argh,argld,fi;
  40   int ntry=0,i,j=-1;
  41   int k1, l1, l2, ib;
  42   int ld, ii, ip, is, nq, nr;
  43   int ido, ipm, nfm1;
  44   int nl=n;
  45   int nf=0;
  46
  47  L101:
  48   j++;
  49   if (j < 2)
  50     ntry=ntryh[j];
  51   else // powers of two only in this one, sorry
  52     return -1;
  53
  54  L104:
  55   nq=nl/ntry;
  56   nr=nl-ntry*nq;
  57   if (nr!=0) goto L101;
  58
  59   nf++;
  60   ifac[nf+1]=ntry;
  61   nl=nq;
  62   if(ntry!=2)goto L107;
  63   if(nf==1)goto L107;
  64
  65   for (i=1;i<nf;i++){
  66     ib=nf-i+1;
  67     ifac[ib+1]=ifac[ib];
  68   }
  69   ifac[2] = 2;
  70
  71  L107:
  72   if(nl!=1)goto L104;
  73   ifac[0]=n;
  74   ifac[1]=nf;
  75   argh=tpi/n;
  76   is=0;
  77   nfm1=nf-1;
  78   l1=1;
  79
  80   if(nfm1==0)return -1;
  81
  82   for (k1=0;k1<nfm1;k1++){
  83     ip=ifac[k1+2];
  84     ld=0;
  85     l2=l1*ip;
  86     ido=n/l2;
  87     ipm=ip-1;
  88
  89     for (j=0;j<ipm;j++){
  90       ld+=l1;
  91       i=is;
  92       argld=(double)ld*argh;
  93       fi=0.;
  94       for (ii=2;ii<ido;ii+=2){
  95         fi+=1.;
  96         arg=fi*argld;
  97         wa[i++]=cos(arg);
  98         wa[i++]=sin(arg);
  99       }
 100       is+=ido;
 101     }
 102     l1=l2;
 103   }
 104   return 0;
 105 }
 106
 107 static int fdrffti(int n, double *wsave, int *ifac){
 108   if (n == 1) return 0;
 109   return drfti1(n, wsave, ifac);
 110 }
 111
 112 static void dradf2(int ido,int l1,double *cc,double *ch,double *wa1){
 113   int i,k;
 114   double ti2,tr2;
 115   int t0,t1,t2,t3,t4,t5,t6;
 116
 117   t1=0;
 118   t0=(t2=l1*ido);
 119   t3=ido<<1;
 120   for(k=0;k<l1;k++){
 121     ch[t1<<1]=cc[t1]+cc[t2];
 122     ch[(t1<<1)+t3-1]=cc[t1]-cc[t2];
 123     t1+=ido;
 124     t2+=ido;
 125   }
 126
 127   if(ido<2)return;
 128   if(ido==2)goto L105;
 129
 130   t1=0;
 131   t2=t0;
 132   for(k=0;k<l1;k++){
 133     t3=t2;
 134     t4=(t1<<1)+(ido<<1);
 135     t5=t1;
 136     t6=t1+t1;
 137     for(i=2;i<ido;i+=2){
 138       t3+=2;
 139       t4-=2;
 140       t5+=2;
 141       t6+=2;
 142       tr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
 143       ti2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
 144       ch[t6]=cc[t5]+ti2;
 145       ch[t4]=ti2-cc[t5];
 146       ch[t6-1]=cc[t5-1]+tr2;
 147       ch[t4-1]=cc[t5-1]-tr2;
 148     }
 149     t1+=ido;
 150     t2+=ido;
 151   }
 152
 153   if(ido%2==1)return;
 154
 155  L105:
 156   t3=(t2=(t1=ido)-1);
 157   t2+=t0;
 158   for(k=0;k<l1;k++){
 159     ch[t1]=-cc[t2];
 160     ch[t1-1]=cc[t3];
 161     t1+=ido<<1;
 162     t2+=ido;
 163     t3+=ido;
 164   }
 165 }
 166
 167 static void dradf4(int ido,int l1,double *cc,double *ch,double *wa1,
 168             double *wa2,double *wa3){
 169   static double hsqt2 = .70710678118654752f;
 170   int i,k,t0,t1,t2,t3,t4,t5,t6;
 171   double ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
 172   t0=l1*ido;
 173
 174   t1=t0;
 175   t4=t1<<1;
 176   t2=t1+(t1<<1);
 177   t3=0;
 178
 179   for(k=0;k<l1;k++){
 180     tr1=cc[t1]+cc[t2];
 181     tr2=cc[t3]+cc[t4];
 182
 183     ch[t5=t3<<2]=tr1+tr2;
 184     ch[(ido<<2)+t5-1]=tr2-tr1;
 185     ch[(t5+=(ido<<1))-1]=cc[t3]-cc[t4];
 186     ch[t5]=cc[t2]-cc[t1];
 187
 188     t1+=ido;
 189     t2+=ido;
 190     t3+=ido;
 191     t4+=ido;
 192   }
 193
 194   if(ido<2)return;
 195   if(ido==2)goto L105;
 196
 197
 198   t1=0;
 199   for(k=0;k<l1;k++){
 200     t2=t1;
 201     t4=t1<<2;
 202     t5=(t6=ido<<1)+t4;
 203     for(i=2;i<ido;i+=2){
 204       t3=(t2+=2);
 205       t4+=2;
 206       t5-=2;
 207
 208       t3+=t0;
 209       cr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
 210       ci2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
 211       t3+=t0;
 212       cr3=wa2[i-2]*cc[t3-1]+wa2[i-1]*cc[t3];
 213       ci3=wa2[i-2]*cc[t3]-wa2[i-1]*cc[t3-1];
 214       t3+=t0;
 215       cr4=wa3[i-2]*cc[t3-1]+wa3[i-1]*cc[t3];
 216       ci4=wa3[i-2]*cc[t3]-wa3[i-1]*cc[t3-1];
 217
 218       tr1=cr2+cr4;
 219       tr4=cr4-cr2;
 220       ti1=ci2+ci4;
 221       ti4=ci2-ci4;
 222
 223       ti2=cc[t2]+ci3;
 224       ti3=cc[t2]-ci3;
 225       tr2=cc[t2-1]+cr3;
 226       tr3=cc[t2-1]-cr3;
 227
 228       ch[t4-1]=tr1+tr2;
 229       ch[t4]=ti1+ti2;
 230
 231       ch[t5-1]=tr3-ti4;
 232       ch[t5]=tr4-ti3;
 233
 234       ch[t4+t6-1]=ti4+tr3;
 235       ch[t4+t6]=tr4+ti3;
 236
 237       ch[t5+t6-1]=tr2-tr1;
 238       ch[t5+t6]=ti1-ti2;
 239     }
 240     t1+=ido;
 241   }
 242   if(ido&1)return;
 243
 244  L105:
 245
 246   t2=(t1=t0+ido-1)+(t0<<1);
 247   t3=ido<<2;
 248   t4=ido;
 249   t5=ido<<1;
 250   t6=ido;
 251
 252   for(k=0;k<l1;k++){
 253     ti1=-hsqt2*(cc[t1]+cc[t2]);
 254     tr1=hsqt2*(cc[t1]-cc[t2]);
 255
 256     ch[t4-1]=tr1+cc[t6-1];
 257     ch[t4+t5-1]=cc[t6-1]-tr1;
 258
 259     ch[t4]=ti1-cc[t1+t0];
 260     ch[t4+t5]=ti1+cc[t1+t0];
 261
 262     t1+=ido;
 263     t2+=ido;
 264     t4+=t3;
 265     t6+=ido;
 266   }
 267 }
 268
 269 static void drftf1(int n,double *c,double *ch,double *wa,int *ifac){
 270   int i,k1,l1,l2;
 271   int na,kh,nf;
 272   int ip,iw,ido,idl1,ix2,ix3;
 273
 274   nf=ifac[1];
 275   na=1;
 276   l2=n;
 277   iw=n;
 278
 279   for(k1=0;k1<nf;k1++){
 280     kh=nf-k1;
 281     ip=ifac[kh+1];
 282     l1=l2/ip;
 283     ido=n/l2;
 284     idl1=ido*l1;
 285     iw-=(ip-1)*ido;
 286     na=1-na;
 287
 288     if(ip!=4)goto L102;
 289
 290     ix2=iw+ido;
 291     ix3=ix2+ido;
 292     if(na!=0)
 293       dradf4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
 294     else
 295       dradf4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
 296     goto L110;
 297
 298  L102:
 299     if(na!=0)goto L103;
 300
 301     dradf2(ido,l1,c,ch,wa+iw-1);
 302     goto L110;
 303
 304   L103:
 305     dradf2(ido,l1,ch,c,wa+iw-1);
 306
 307   L110:
 308     l2=l1;
 309   }
 310
 311   if(na==1)return;
 312
 313   for(i=0;i<n;i++)c[i]=ch[i];
 314 }
 315
 316 void drft_forward(drft_lookup *l,double *data){
 317   double work[l->n];
 318   if(l->n==1)return;
 319   drftf1(l->n,data,work,l->trigcache,l->splitcache);
 320 }
 321
 322 int drft_init(drft_lookup *l,int n){
 323   l->n=n;
 324   l->trigcache=calloc(2*n,sizeof(*l->trigcache));
 325   l->splitcache=calloc(32,sizeof(*l->splitcache));
 326   return fdrffti(n, l->trigcache, l->splitcache);
 327 }
 328
 329 void drft_clear(drft_lookup *l){
 330   if(l){
 331     if(l->trigcache)free(l->trigcache);
 332     if(l->splitcache)free(l->splitcache);
 333     memset(l,0,sizeof(*l));
 334   }
 335 }