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