Fixes to dsp
[platform/upstream/libvorbis.git] / 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 }