1 /********************************************************************
2 * *
3 * THIS FILE IS PART OF THE OggVorbis 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 OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2001 *
9 * by the XIPHOPHORUS Company http://www.xiph.org/ *
10 * *
11 ********************************************************************
12
13 function: *unnormalized* fft transform
14 last mod: $Id: smallft.c 18922 2007-11-21 22:46:57Z rjongbloed $
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, I_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 #ifdef HAVE_CONFIG_H
32 #include "config.h"
33 #endif
34
35 #ifdef _WIN32
36 #pragma warning(disable:4244)
37 #endif
38
39 #include <math.h>
40 #include "smallft.h"
41 #include "misc.h"
42
drfti1(int n,float * wa,int * ifac)43 static void drfti1(int n, float *wa, int *ifac){
44 static int ntryh[4] = { 4,2,3,5 };
45 static float tpi = 6.28318530717958648f;
46 float arg,argh,argld,fi;
47 int ntry=0,i,j=-1;
48 int k1, l1, l2, ib;
49 int ld, ii, ip, is, nq, nr;
50 int ido, ipm, nfm1;
51 int nl=n;
52 int nf=0;
53
54 L101:
55 j++;
56 if (j < 4)
57 ntry=ntryh[j];
58 else
59 ntry+=2;
60
61 L104:
62 nq=nl/ntry;
63 nr=nl-ntry*nq;
64 if (nr!=0) goto L101;
65
66 nf++;
67 ifac[nf+1]=ntry;
68 nl=nq;
69 if(ntry!=2)goto L107;
70 if(nf==1)goto L107;
71
72 for (i=1;i<nf;i++){
73 ib=nf-i+1;
74 ifac[ib+1]=ifac[ib];
75 }
76 ifac[2] = 2;
77
78 L107:
79 if(nl!=1)goto L104;
80 ifac[0]=n;
81 ifac[1]=nf;
82 argh=tpi/n;
83 is=0;
84 nfm1=nf-1;
85 l1=1;
86
87 if(nfm1==0)return;
88
89 for (k1=0;k1<nfm1;k1++){
90 ip=ifac[k1+2];
91 ld=0;
92 l2=l1*ip;
93 ido=n/l2;
94 ipm=ip-1;
95
96 for (j=0;j<ipm;j++){
97 ld+=l1;
98 i=is;
99 argld=(float)ld*argh;
100 fi=0.f;
101 for (ii=2;ii<ido;ii+=2){
102 fi+=1.f;
103 arg=fi*argld;
104 wa[i++]=cos(arg);
105 wa[i++]=sin(arg);
106 }
107 is+=ido;
108 }
109 l1=l2;
110 }
111 }
112
fdrffti(int n,float * wsave,int * ifac)113 static void fdrffti(int n, float *wsave, int *ifac){
114
115 if (n == 1) return;
116 drfti1(n, wsave+n, ifac);
117 }
118
dradf2(int ido,int l1,float * cc,float * ch,float * wa1)119 static void dradf2(int ido,int l1,float *cc,float *ch,float *wa1){
120 int i,k;
121 float ti2,tr2;
122 int t0,t1,t2,t3,t4,t5,t6;
123
124 t1=0;
125 t0=(t2=l1*ido);
126 t3=ido<<1;
127 for(k=0;k<l1;k++){
128 ch[t1<<1]=cc[t1]+cc[t2];
129 ch[(t1<<1)+t3-1]=cc[t1]-cc[t2];
130 t1+=ido;
131 t2+=ido;
132 }
133
134 if(ido<2)return;
135 if(ido==2)goto L105;
136
137 t1=0;
138 t2=t0;
139 for(k=0;k<l1;k++){
140 t3=t2;
141 t4=(t1<<1)+(ido<<1);
142 t5=t1;
143 t6=t1+t1;
144 for(i=2;i<ido;i+=2){
145 t3+=2;
146 t4-=2;
147 t5+=2;
148 t6+=2;
149 tr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
150 ti2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
151 ch[t6]=cc[t5]+ti2;
152 ch[t4]=ti2-cc[t5];
153 ch[t6-1]=cc[t5-1]+tr2;
154 ch[t4-1]=cc[t5-1]-tr2;
155 }
156 t1+=ido;
157 t2+=ido;
158 }
159
160 if(ido%2==1)return;
161
162 L105:
163 t3=(t2=(t1=ido)-1);
164 t2+=t0;
165 for(k=0;k<l1;k++){
166 ch[t1]=-cc[t2];
167 ch[t1-1]=cc[t3];
168 t1+=ido<<1;
169 t2+=ido;
170 t3+=ido;
171 }
172 }
173
dradf4(int ido,int l1,float * cc,float * ch,float * wa1,float * wa2,float * wa3)174 static void dradf4(int ido,int l1,float *cc,float *ch,float *wa1,
175 float *wa2,float *wa3){
176 static float hsqt2 = .70710678118654752f;
177 int i,k,t0,t1,t2,t3,t4,t5,t6;
178 float ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
179 t0=l1*ido;
180
181 t1=t0;
182 t4=t1<<1;
183 t2=t1+(t1<<1);
184 t3=0;
185
186 for(k=0;k<l1;k++){
187 tr1=cc[t1]+cc[t2];
188 tr2=cc[t3]+cc[t4];
189
190 ch[t5=t3<<2]=tr1+tr2;
191 ch[(ido<<2)+t5-1]=tr2-tr1;
192 ch[(t5+=(ido<<1))-1]=cc[t3]-cc[t4];
193 ch[t5]=cc[t2]-cc[t1];
194
195 t1+=ido;
196 t2+=ido;
197 t3+=ido;
198 t4+=ido;
199 }
200
201 if(ido<2)return;
202 if(ido==2)goto L105;
203
204
205 t1=0;
206 for(k=0;k<l1;k++){
207 t2=t1;
208 t4=t1<<2;
209 t5=(t6=ido<<1)+t4;
210 for(i=2;i<ido;i+=2){
211 t3=(t2+=2);
212 t4+=2;
213 t5-=2;
214
215 t3+=t0;
216 cr2=wa1[i-2]*cc[t3-1]+wa1[i-1]*cc[t3];
217 ci2=wa1[i-2]*cc[t3]-wa1[i-1]*cc[t3-1];
218 t3+=t0;
219 cr3=wa2[i-2]*cc[t3-1]+wa2[i-1]*cc[t3];
220 ci3=wa2[i-2]*cc[t3]-wa2[i-1]*cc[t3-1];
221 t3+=t0;
222 cr4=wa3[i-2]*cc[t3-1]+wa3[i-1]*cc[t3];
223 ci4=wa3[i-2]*cc[t3]-wa3[i-1]*cc[t3-1];
224
225 tr1=cr2+cr4;
226 tr4=cr4-cr2;
227 ti1=ci2+ci4;
228 ti4=ci2-ci4;
229
230 ti2=cc[t2]+ci3;
231 ti3=cc[t2]-ci3;
232 tr2=cc[t2-1]+cr3;
233 tr3=cc[t2-1]-cr3;
234
235 ch[t4-1]=tr1+tr2;
236 ch[t4]=ti1+ti2;
237
238 ch[t5-1]=tr3-ti4;
239 ch[t5]=tr4-ti3;
240
241 ch[t4+t6-1]=ti4+tr3;
242 ch[t4+t6]=tr4+ti3;
243
244 ch[t5+t6-1]=tr2-tr1;
245 ch[t5+t6]=ti1-ti2;
246 }
247 t1+=ido;
248 }
249 if(ido&1)return;
250
251 L105:
252
253 t2=(t1=t0+ido-1)+(t0<<1);
254 t3=ido<<2;
255 t4=ido;
256 t5=ido<<1;
257 t6=ido;
258
259 for(k=0;k<l1;k++){
260 ti1=-hsqt2*(cc[t1]+cc[t2]);
261 tr1=hsqt2*(cc[t1]-cc[t2]);
262
263 ch[t4-1]=tr1+cc[t6-1];
264 ch[t4+t5-1]=cc[t6-1]-tr1;
265
266 ch[t4]=ti1-cc[t1+t0];
267 ch[t4+t5]=ti1+cc[t1+t0];
268
269 t1+=ido;
270 t2+=ido;
271 t4+=t3;
272 t6+=ido;
273 }
274 }
275
dradfg(int ido,int ip,int l1,int idl1,float * cc,float * c1,float * c2,float * ch,float * ch2,float * wa)276 static void dradfg(int ido,int ip,int l1,int idl1,float *cc,float *c1,
277 float *c2,float *ch,float *ch2,float *wa){
278
279 static float tpi=6.283185307179586f;
280 int idij,ipph,i,j,k,l,ic,ik,is;
281 int t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10;
282 float dc2,ai1,ai2,ar1,ar2,ds2;
283 int nbd;
284 float dcp,arg,dsp,ar1h,ar2h;
285 int idp2,ipp2;
286
287 arg=tpi/(float)ip;
288 dcp=cos(arg);
289 dsp=sin(arg);
290 ipph=(ip+1)>>1;
291 ipp2=ip;
292 idp2=ido;
293 nbd=(ido-1)>>1;
294 t0=l1*ido;
295 t10=ip*ido;
296
297 if(ido==1)goto L119;
298 for(ik=0;ik<idl1;ik++)ch2[ik]=c2[ik];
299
300 t1=0;
301 for(j=1;j<ip;j++){
302 t1+=t0;
303 t2=t1;
304 for(k=0;k<l1;k++){
305 ch[t2]=c1[t2];
306 t2+=ido;
307 }
308 }
309
310 is=-ido;
311 t1=0;
312 if(nbd>l1){
313 for(j=1;j<ip;j++){
314 t1+=t0;
315 is+=ido;
316 t2= -ido+t1;
317 for(k=0;k<l1;k++){
318 idij=is-1;
319 t2+=ido;
320 t3=t2;
321 for(i=2;i<ido;i+=2){
322 idij+=2;
323 t3+=2;
324 ch[t3-1]=wa[idij-1]*c1[t3-1]+wa[idij]*c1[t3];
325 ch[t3]=wa[idij-1]*c1[t3]-wa[idij]*c1[t3-1];
326 }
327 }
328 }
329 }else{
330
331 for(j=1;j<ip;j++){
332 is+=ido;
333 idij=is-1;
334 t1+=t0;
335 t2=t1;
336 for(i=2;i<ido;i+=2){
337 idij+=2;
338 t2+=2;
339 t3=t2;
340 for(k=0;k<l1;k++){
341 ch[t3-1]=wa[idij-1]*c1[t3-1]+wa[idij]*c1[t3];
342 ch[t3]=wa[idij-1]*c1[t3]-wa[idij]*c1[t3-1];
343 t3+=ido;
344 }
345 }
346 }
347 }
348
349 t1=0;
350 t2=ipp2*t0;
351 if(nbd<l1){
352 for(j=1;j<ipph;j++){
353 t1+=t0;
354 t2-=t0;
355 t3=t1;
356 t4=t2;
357 for(i=2;i<ido;i+=2){
358 t3+=2;
359 t4+=2;
360 t5=t3-ido;
361 t6=t4-ido;
362 for(k=0;k<l1;k++){
363 t5+=ido;
364 t6+=ido;
365 c1[t5-1]=ch[t5-1]+ch[t6-1];
366 c1[t6-1]=ch[t5]-ch[t6];
367 c1[t5]=ch[t5]+ch[t6];
368 c1[t6]=ch[t6-1]-ch[t5-1];
369 }
370 }
371 }
372 }else{
373 for(j=1;j<ipph;j++){
374 t1+=t0;
375 t2-=t0;
376 t3=t1;
377 t4=t2;
378 for(k=0;k<l1;k++){
379 t5=t3;
380 t6=t4;
381 for(i=2;i<ido;i+=2){
382 t5+=2;
383 t6+=2;
384 c1[t5-1]=ch[t5-1]+ch[t6-1];
385 c1[t6-1]=ch[t5]-ch[t6];
386 c1[t5]=ch[t5]+ch[t6];
387 c1[t6]=ch[t6-1]-ch[t5-1];
388 }
389 t3+=ido;
390 t4+=ido;
391 }
392 }
393 }
394
395 L119:
396 for(ik=0;ik<idl1;ik++)c2[ik]=ch2[ik];
397
398 t1=0;
399 t2=ipp2*idl1;
400 for(j=1;j<ipph;j++){
401 t1+=t0;
402 t2-=t0;
403 t3=t1-ido;
404 t4=t2-ido;
405 for(k=0;k<l1;k++){
406 t3+=ido;
407 t4+=ido;
408 c1[t3]=ch[t3]+ch[t4];
409 c1[t4]=ch[t4]-ch[t3];
410 }
411 }
412
413 ar1=1.f;
414 ai1=0.f;
415 t1=0;
416 t2=ipp2*idl1;
417 t3=(ip-1)*idl1;
418 for(l=1;l<ipph;l++){
419 t1+=idl1;
420 t2-=idl1;
421 ar1h=dcp*ar1-dsp*ai1;
422 ai1=dcp*ai1+dsp*ar1;
423 ar1=ar1h;
424 t4=t1;
425 t5=t2;
426 t6=t3;
427 t7=idl1;
428
429 for(ik=0;ik<idl1;ik++){
430 ch2[t4++]=c2[ik]+ar1*c2[t7++];
431 ch2[t5++]=ai1*c2[t6++];
432 }
433
434 dc2=ar1;
435 ds2=ai1;
436 ar2=ar1;
437 ai2=ai1;
438
439 t4=idl1;
440 t5=(ipp2-1)*idl1;
441 for(j=2;j<ipph;j++){
442 t4+=idl1;
443 t5-=idl1;
444
445 ar2h=dc2*ar2-ds2*ai2;
446 ai2=dc2*ai2+ds2*ar2;
447 ar2=ar2h;
448
449 t6=t1;
450 t7=t2;
451 t8=t4;
452 t9=t5;
453 for(ik=0;ik<idl1;ik++){
454 ch2[t6++]+=ar2*c2[t8++];
455 ch2[t7++]+=ai2*c2[t9++];
456 }
457 }
458 }
459
460 t1=0;
461 for(j=1;j<ipph;j++){
462 t1+=idl1;
463 t2=t1;
464 for(ik=0;ik<idl1;ik++)ch2[ik]+=c2[t2++];
465 }
466
467 if(ido<l1)goto L132;
468
469 t1=0;
470 t2=0;
471 for(k=0;k<l1;k++){
472 t3=t1;
473 t4=t2;
474 for(i=0;i<ido;i++)cc[t4++]=ch[t3++];
475 t1+=ido;
476 t2+=t10;
477 }
478
479 goto L135;
480
481 L132:
482 for(i=0;i<ido;i++){
483 t1=i;
484 t2=i;
485 for(k=0;k<l1;k++){
486 cc[t2]=ch[t1];
487 t1+=ido;
488 t2+=t10;
489 }
490 }
491
492 L135:
493 t1=0;
494 t2=ido<<1;
495 t3=0;
496 t4=ipp2*t0;
497 for(j=1;j<ipph;j++){
498
499 t1+=t2;
500 t3+=t0;
501 t4-=t0;
502
503 t5=t1;
504 t6=t3;
505 t7=t4;
506
507 for(k=0;k<l1;k++){
508 cc[t5-1]=ch[t6];
509 cc[t5]=ch[t7];
510 t5+=t10;
511 t6+=ido;
512 t7+=ido;
513 }
514 }
515
516 if(ido==1)return;
517 if(nbd<l1)goto L141;
518
519 t1=-ido;
520 t3=0;
521 t4=0;
522 t5=ipp2*t0;
523 for(j=1;j<ipph;j++){
524 t1+=t2;
525 t3+=t2;
526 t4+=t0;
527 t5-=t0;
528 t6=t1;
529 t7=t3;
530 t8=t4;
531 t9=t5;
532 for(k=0;k<l1;k++){
533 for(i=2;i<ido;i+=2){
534 ic=idp2-i;
535 cc[i+t7-1]=ch[i+t8-1]+ch[i+t9-1];
536 cc[ic+t6-1]=ch[i+t8-1]-ch[i+t9-1];
537 cc[i+t7]=ch[i+t8]+ch[i+t9];
538 cc[ic+t6]=ch[i+t9]-ch[i+t8];
539 }
540 t6+=t10;
541 t7+=t10;
542 t8+=ido;
543 t9+=ido;
544 }
545 }
546 return;
547
548 L141:
549
550 t1=-ido;
551 t3=0;
552 t4=0;
553 t5=ipp2*t0;
554 for(j=1;j<ipph;j++){
555 t1+=t2;
556 t3+=t2;
557 t4+=t0;
558 t5-=t0;
559 for(i=2;i<ido;i+=2){
560 t6=idp2+t1-i;
561 t7=i+t3;
562 t8=i+t4;
563 t9=i+t5;
564 for(k=0;k<l1;k++){
565 cc[t7-1]=ch[t8-1]+ch[t9-1];
566 cc[t6-1]=ch[t8-1]-ch[t9-1];
567 cc[t7]=ch[t8]+ch[t9];
568 cc[t6]=ch[t9]-ch[t8];
569 t6+=t10;
570 t7+=t10;
571 t8+=ido;
572 t9+=ido;
573 }
574 }
575 }
576 }
577
drftf1(int n,float * c,float * ch,float * wa,int * ifac)578 static void drftf1(int n,float *c,float *ch,float *wa,int *ifac){
579 int i,k1,l1,l2;
580 int na,kh,nf;
581 int ip,iw,ido,idl1,ix2,ix3;
582
583 nf=ifac[1];
584 na=1;
585 l2=n;
586 iw=n;
587
588 for(k1=0;k1<nf;k1++){
589 kh=nf-k1;
590 ip=ifac[kh+1];
591 l1=l2/ip;
592 ido=n/l2;
593 idl1=ido*l1;
594 iw-=(ip-1)*ido;
595 na=1-na;
596
597 if(ip!=4)goto L102;
598
599 ix2=iw+ido;
600 ix3=ix2+ido;
601 if(na!=0)
602 dradf4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
603 else
604 dradf4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
605 goto L110;
606
607 L102:
608 if(ip!=2)goto L104;
609 if(na!=0)goto L103;
610
611 dradf2(ido,l1,c,ch,wa+iw-1);
612 goto L110;
613
614 L103:
615 dradf2(ido,l1,ch,c,wa+iw-1);
616 goto L110;
617
618 L104:
619 if(ido==1)na=1-na;
620 if(na!=0)goto L109;
621
622 dradfg(ido,ip,l1,idl1,c,c,c,ch,ch,wa+iw-1);
623 na=1;
624 goto L110;
625
626 L109:
627 dradfg(ido,ip,l1,idl1,ch,ch,ch,c,c,wa+iw-1);
628 na=0;
629
630 L110:
631 l2=l1;
632 }
633
634 if(na==1)return;
635
636 for(i=0;i<n;i++)c[i]=ch[i];
637 }
638
dradb2(int ido,int l1,float * cc,float * ch,float * wa1)639 static void dradb2(int ido,int l1,float *cc,float *ch,float *wa1){
640 int i,k,t0,t1,t2,t3,t4,t5,t6;
641 float ti2,tr2;
642
643 t0=l1*ido;
644
645 t1=0;
646 t2=0;
647 t3=(ido<<1)-1;
648 for(k=0;k<l1;k++){
649 ch[t1]=cc[t2]+cc[t3+t2];
650 ch[t1+t0]=cc[t2]-cc[t3+t2];
651 t2=(t1+=ido)<<1;
652 }
653
654 if(ido<2)return;
655 if(ido==2)goto L105;
656
657 t1=0;
658 t2=0;
659 for(k=0;k<l1;k++){
660 t3=t1;
661 t5=(t4=t2)+(ido<<1);
662 t6=t0+t1;
663 for(i=2;i<ido;i+=2){
664 t3+=2;
665 t4+=2;
666 t5-=2;
667 t6+=2;
668 ch[t3-1]=cc[t4-1]+cc[t5-1];
669 tr2=cc[t4-1]-cc[t5-1];
670 ch[t3]=cc[t4]-cc[t5];
671 ti2=cc[t4]+cc[t5];
672 ch[t6-1]=wa1[i-2]*tr2-wa1[i-1]*ti2;
673 ch[t6]=wa1[i-2]*ti2+wa1[i-1]*tr2;
674 }
675 t2=(t1+=ido)<<1;
676 }
677
678 if(ido%2==1)return;
679
680 L105:
681 t1=ido-1;
682 t2=ido-1;
683 for(k=0;k<l1;k++){
684 ch[t1]=cc[t2]+cc[t2];
685 ch[t1+t0]=-(cc[t2+1]+cc[t2+1]);
686 t1+=ido;
687 t2+=ido<<1;
688 }
689 }
690
dradb3(int ido,int l1,float * cc,float * ch,float * wa1,float * wa2)691 static void dradb3(int ido,int l1,float *cc,float *ch,float *wa1,
692 float *wa2){
693 static float taur = -.5f;
694 static float taui = .8660254037844386f;
695 int i,k,t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10;
696 float ci2,ci3,di2,di3,cr2,cr3,dr2,dr3,ti2,tr2;
697 t0=l1*ido;
698
699 t1=0;
700 t2=t0<<1;
701 t3=ido<<1;
702 t4=ido+(ido<<1);
703 t5=0;
704 for(k=0;k<l1;k++){
705 tr2=cc[t3-1]+cc[t3-1];
706 cr2=cc[t5]+(taur*tr2);
707 ch[t1]=cc[t5]+tr2;
708 ci3=taui*(cc[t3]+cc[t3]);
709 ch[t1+t0]=cr2-ci3;
710 ch[t1+t2]=cr2+ci3;
711 t1+=ido;
712 t3+=t4;
713 t5+=t4;
714 }
715
716 if(ido==1)return;
717
718 t1=0;
719 t3=ido<<1;
720 for(k=0;k<l1;k++){
721 t7=t1+(t1<<1);
722 t6=(t5=t7+t3);
723 t8=t1;
724 t10=(t9=t1+t0)+t0;
725
726 for(i=2;i<ido;i+=2){
727 t5+=2;
728 t6-=2;
729 t7+=2;
730 t8+=2;
731 t9+=2;
732 t10+=2;
733 tr2=cc[t5-1]+cc[t6-1];
734 cr2=cc[t7-1]+(taur*tr2);
735 ch[t8-1]=cc[t7-1]+tr2;
736 ti2=cc[t5]-cc[t6];
737 ci2=cc[t7]+(taur*ti2);
738 ch[t8]=cc[t7]+ti2;
739 cr3=taui*(cc[t5-1]-cc[t6-1]);
740 ci3=taui*(cc[t5]+cc[t6]);
741 dr2=cr2-ci3;
742 dr3=cr2+ci3;
743 di2=ci2+cr3;
744 di3=ci2-cr3;
745 ch[t9-1]=wa1[i-2]*dr2-wa1[i-1]*di2;
746 ch[t9]=wa1[i-2]*di2+wa1[i-1]*dr2;
747 ch[t10-1]=wa2[i-2]*dr3-wa2[i-1]*di3;
748 ch[t10]=wa2[i-2]*di3+wa2[i-1]*dr3;
749 }
750 t1+=ido;
751 }
752 }
753
dradb4(int ido,int l1,float * cc,float * ch,float * wa1,float * wa2,float * wa3)754 static void dradb4(int ido,int l1,float *cc,float *ch,float *wa1,
755 float *wa2,float *wa3){
756 static float sqrt2=1.414213562373095f;
757 int i,k,t0,t1,t2,t3,t4,t5,t6,t7,t8;
758 float ci2,ci3,ci4,cr2,cr3,cr4,ti1,ti2,ti3,ti4,tr1,tr2,tr3,tr4;
759 t0=l1*ido;
760
761 t1=0;
762 t2=ido<<2;
763 t3=0;
764 t6=ido<<1;
765 for(k=0;k<l1;k++){
766 t4=t3+t6;
767 t5=t1;
768 tr3=cc[t4-1]+cc[t4-1];
769 tr4=cc[t4]+cc[t4];
770 tr1=cc[t3]-cc[(t4+=t6)-1];
771 tr2=cc[t3]+cc[t4-1];
772 ch[t5]=tr2+tr3;
773 ch[t5+=t0]=tr1-tr4;
774 ch[t5+=t0]=tr2-tr3;
775 ch[t5+=t0]=tr1+tr4;
776 t1+=ido;
777 t3+=t2;
778 }
779
780 if(ido<2)return;
781 if(ido==2)goto L105;
782
783 t1=0;
784 for(k=0;k<l1;k++){
785 t5=(t4=(t3=(t2=t1<<2)+t6))+t6;
786 t7=t1;
787 for(i=2;i<ido;i+=2){
788 t2+=2;
789 t3+=2;
790 t4-=2;
791 t5-=2;
792 t7+=2;
793 ti1=cc[t2]+cc[t5];
794 ti2=cc[t2]-cc[t5];
795 ti3=cc[t3]-cc[t4];
796 tr4=cc[t3]+cc[t4];
797 tr1=cc[t2-1]-cc[t5-1];
798 tr2=cc[t2-1]+cc[t5-1];
799 ti4=cc[t3-1]-cc[t4-1];
800 tr3=cc[t3-1]+cc[t4-1];
801 ch[t7-1]=tr2+tr3;
802 cr3=tr2-tr3;
803 ch[t7]=ti2+ti3;
804 ci3=ti2-ti3;
805 cr2=tr1-tr4;
806 cr4=tr1+tr4;
807 ci2=ti1+ti4;
808 ci4=ti1-ti4;
809
810 ch[(t8=t7+t0)-1]=wa1[i-2]*cr2-wa1[i-1]*ci2;
811 ch[t8]=wa1[i-2]*ci2+wa1[i-1]*cr2;
812 ch[(t8+=t0)-1]=wa2[i-2]*cr3-wa2[i-1]*ci3;
813 ch[t8]=wa2[i-2]*ci3+wa2[i-1]*cr3;
814 ch[(t8+=t0)-1]=wa3[i-2]*cr4-wa3[i-1]*ci4;
815 ch[t8]=wa3[i-2]*ci4+wa3[i-1]*cr4;
816 }
817 t1+=ido;
818 }
819
820 if(ido%2 == 1)return;
821
822 L105:
823
824 t1=ido;
825 t2=ido<<2;
826 t3=ido-1;
827 t4=ido+(ido<<1);
828 for(k=0;k<l1;k++){
829 t5=t3;
830 ti1=cc[t1]+cc[t4];
831 ti2=cc[t4]-cc[t1];
832 tr1=cc[t1-1]-cc[t4-1];
833 tr2=cc[t1-1]+cc[t4-1];
834 ch[t5]=tr2+tr2;
835 ch[t5+=t0]=sqrt2*(tr1-ti1);
836 ch[t5+=t0]=ti2+ti2;
837 ch[t5+=t0]=-sqrt2*(tr1+ti1);
838
839 t3+=ido;
840 t1+=t2;
841 t4+=t2;
842 }
843 }
844
dradbg(int ido,int ip,int l1,int idl1,float * cc,float * c1,float * c2,float * ch,float * ch2,float * wa)845 static void dradbg(int ido,int ip,int l1,int idl1,float *cc,float *c1,
846 float *c2,float *ch,float *ch2,float *wa){
847 static float tpi=6.283185307179586f;
848 int idij,ipph,i,j,k,l,ik,is,t0,t1,t2,t3,t4,t5,t6,t7,t8,t9,t10,
849 t11,t12;
850 float dc2,ai1,ai2,ar1,ar2,ds2;
851 int nbd;
852 float dcp,arg,dsp,ar1h,ar2h;
853 int ipp2;
854
855 t10=ip*ido;
856 t0=l1*ido;
857 arg=tpi/(float)ip;
858 dcp=cos(arg);
859 dsp=sin(arg);
860 nbd=(ido-1)>>1;
861 ipp2=ip;
862 ipph=(ip+1)>>1;
863 if(ido<l1)goto L103;
864
865 t1=0;
866 t2=0;
867 for(k=0;k<l1;k++){
868 t3=t1;
869 t4=t2;
870 for(i=0;i<ido;i++){
871 ch[t3]=cc[t4];
872 t3++;
873 t4++;
874 }
875 t1+=ido;
876 t2+=t10;
877 }
878 goto L106;
879
880 L103:
881 t1=0;
882 for(i=0;i<ido;i++){
883 t2=t1;
884 t3=t1;
885 for(k=0;k<l1;k++){
886 ch[t2]=cc[t3];
887 t2+=ido;
888 t3+=t10;
889 }
890 t1++;
891 }
892
893 L106:
894 t1=0;
895 t2=ipp2*t0;
896 t7=(t5=ido<<1);
897 for(j=1;j<ipph;j++){
898 t1+=t0;
899 t2-=t0;
900 t3=t1;
901 t4=t2;
902 t6=t5;
903 for(k=0;k<l1;k++){
904 ch[t3]=cc[t6-1]+cc[t6-1];
905 ch[t4]=cc[t6]+cc[t6];
906 t3+=ido;
907 t4+=ido;
908 t6+=t10;
909 }
910 t5+=t7;
911 }
912
913 if (ido == 1)goto L116;
914 if(nbd<l1)goto L112;
915
916 t1=0;
917 t2=ipp2*t0;
918 t7=0;
919 for(j=1;j<ipph;j++){
920 t1+=t0;
921 t2-=t0;
922 t3=t1;
923 t4=t2;
924
925 t7+=(ido<<1);
926 t8=t7;
927 for(k=0;k<l1;k++){
928 t5=t3;
929 t6=t4;
930 t9=t8;
931 t11=t8;
932 for(i=2;i<ido;i+=2){
933 t5+=2;
934 t6+=2;
935 t9+=2;
936 t11-=2;
937 ch[t5-1]=cc[t9-1]+cc[t11-1];
938 ch[t6-1]=cc[t9-1]-cc[t11-1];
939 ch[t5]=cc[t9]-cc[t11];
940 ch[t6]=cc[t9]+cc[t11];
941 }
942 t3+=ido;
943 t4+=ido;
944 t8+=t10;
945 }
946 }
947 goto L116;
948
949 L112:
950 t1=0;
951 t2=ipp2*t0;
952 t7=0;
953 for(j=1;j<ipph;j++){
954 t1+=t0;
955 t2-=t0;
956 t3=t1;
957 t4=t2;
958 t7+=(ido<<1);
959 t8=t7;
960 t9=t7;
961 for(i=2;i<ido;i+=2){
962 t3+=2;
963 t4+=2;
964 t8+=2;
965 t9-=2;
966 t5=t3;
967 t6=t4;
968 t11=t8;
969 t12=t9;
970 for(k=0;k<l1;k++){
971 ch[t5-1]=cc[t11-1]+cc[t12-1];
972 ch[t6-1]=cc[t11-1]-cc[t12-1];
973 ch[t5]=cc[t11]-cc[t12];
974 ch[t6]=cc[t11]+cc[t12];
975 t5+=ido;
976 t6+=ido;
977 t11+=t10;
978 t12+=t10;
979 }
980 }
981 }
982
983 L116:
984 ar1=1.f;
985 ai1=0.f;
986 t1=0;
987 t9=(t2=ipp2*idl1);
988 t3=(ip-1)*idl1;
989 for(l=1;l<ipph;l++){
990 t1+=idl1;
991 t2-=idl1;
992
993 ar1h=dcp*ar1-dsp*ai1;
994 ai1=dcp*ai1+dsp*ar1;
995 ar1=ar1h;
996 t4=t1;
997 t5=t2;
998 t6=0;
999 t7=idl1;
1000 t8=t3;
1001 for(ik=0;ik<idl1;ik++){
1002 c2[t4++]=ch2[t6++]+ar1*ch2[t7++];
1003 c2[t5++]=ai1*ch2[t8++];
1004 }
1005 dc2=ar1;
1006 ds2=ai1;
1007 ar2=ar1;
1008 ai2=ai1;
1009
1010 t6=idl1;
1011 t7=t9-idl1;
1012 for(j=2;j<ipph;j++){
1013 t6+=idl1;
1014 t7-=idl1;
1015 ar2h=dc2*ar2-ds2*ai2;
1016 ai2=dc2*ai2+ds2*ar2;
1017 ar2=ar2h;
1018 t4=t1;
1019 t5=t2;
1020 t11=t6;
1021 t12=t7;
1022 for(ik=0;ik<idl1;ik++){
1023 c2[t4++]+=ar2*ch2[t11++];
1024 c2[t5++]+=ai2*ch2[t12++];
1025 }
1026 }
1027 }
1028
1029 t1=0;
1030 for(j=1;j<ipph;j++){
1031 t1+=idl1;
1032 t2=t1;
1033 for(ik=0;ik<idl1;ik++)ch2[ik]+=ch2[t2++];
1034 }
1035
1036 t1=0;
1037 t2=ipp2*t0;
1038 for(j=1;j<ipph;j++){
1039 t1+=t0;
1040 t2-=t0;
1041 t3=t1;
1042 t4=t2;
1043 for(k=0;k<l1;k++){
1044 ch[t3]=c1[t3]-c1[t4];
1045 ch[t4]=c1[t3]+c1[t4];
1046 t3+=ido;
1047 t4+=ido;
1048 }
1049 }
1050
1051 if(ido==1)goto L132;
1052 if(nbd<l1)goto L128;
1053
1054 t1=0;
1055 t2=ipp2*t0;
1056 for(j=1;j<ipph;j++){
1057 t1+=t0;
1058 t2-=t0;
1059 t3=t1;
1060 t4=t2;
1061 for(k=0;k<l1;k++){
1062 t5=t3;
1063 t6=t4;
1064 for(i=2;i<ido;i+=2){
1065 t5+=2;
1066 t6+=2;
1067 ch[t5-1]=c1[t5-1]-c1[t6];
1068 ch[t6-1]=c1[t5-1]+c1[t6];
1069 ch[t5]=c1[t5]+c1[t6-1];
1070 ch[t6]=c1[t5]-c1[t6-1];
1071 }
1072 t3+=ido;
1073 t4+=ido;
1074 }
1075 }
1076 goto L132;
1077
1078 L128:
1079 t1=0;
1080 t2=ipp2*t0;
1081 for(j=1;j<ipph;j++){
1082 t1+=t0;
1083 t2-=t0;
1084 t3=t1;
1085 t4=t2;
1086 for(i=2;i<ido;i+=2){
1087 t3+=2;
1088 t4+=2;
1089 t5=t3;
1090 t6=t4;
1091 for(k=0;k<l1;k++){
1092 ch[t5-1]=c1[t5-1]-c1[t6];
1093 ch[t6-1]=c1[t5-1]+c1[t6];
1094 ch[t5]=c1[t5]+c1[t6-1];
1095 ch[t6]=c1[t5]-c1[t6-1];
1096 t5+=ido;
1097 t6+=ido;
1098 }
1099 }
1100 }
1101
1102 L132:
1103 if(ido==1)return;
1104
1105 for(ik=0;ik<idl1;ik++)c2[ik]=ch2[ik];
1106
1107 t1=0;
1108 for(j=1;j<ip;j++){
1109 t2=(t1+=t0);
1110 for(k=0;k<l1;k++){
1111 c1[t2]=ch[t2];
1112 t2+=ido;
1113 }
1114 }
1115
1116 if(nbd>l1)goto L139;
1117
1118 is= -ido-1;
1119 t1=0;
1120 for(j=1;j<ip;j++){
1121 is+=ido;
1122 t1+=t0;
1123 idij=is;
1124 t2=t1;
1125 for(i=2;i<ido;i+=2){
1126 t2+=2;
1127 idij+=2;
1128 t3=t2;
1129 for(k=0;k<l1;k++){
1130 c1[t3-1]=wa[idij-1]*ch[t3-1]-wa[idij]*ch[t3];
1131 c1[t3]=wa[idij-1]*ch[t3]+wa[idij]*ch[t3-1];
1132 t3+=ido;
1133 }
1134 }
1135 }
1136 return;
1137
1138 L139:
1139 is= -ido-1;
1140 t1=0;
1141 for(j=1;j<ip;j++){
1142 is+=ido;
1143 t1+=t0;
1144 t2=t1;
1145 for(k=0;k<l1;k++){
1146 idij=is;
1147 t3=t2;
1148 for(i=2;i<ido;i+=2){
1149 idij+=2;
1150 t3+=2;
1151 c1[t3-1]=wa[idij-1]*ch[t3-1]-wa[idij]*ch[t3];
1152 c1[t3]=wa[idij-1]*ch[t3]+wa[idij]*ch[t3-1];
1153 }
1154 t2+=ido;
1155 }
1156 }
1157 }
1158
drftb1(int n,float * c,float * ch,float * wa,int * ifac)1159 static void drftb1(int n, float *c, float *ch, float *wa, int *ifac){
1160 int i,k1,l1,l2;
1161 int na;
1162 int nf,ip,iw,ix2,ix3,ido,idl1;
1163
1164 nf=ifac[1];
1165 na=0;
1166 l1=1;
1167 iw=1;
1168
1169 for(k1=0;k1<nf;k1++){
1170 ip=ifac[k1 + 2];
1171 l2=ip*l1;
1172 ido=n/l2;
1173 idl1=ido*l1;
1174 if(ip!=4)goto L103;
1175 ix2=iw+ido;
1176 ix3=ix2+ido;
1177
1178 if(na!=0)
1179 dradb4(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1);
1180 else
1181 dradb4(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1);
1182 na=1-na;
1183 goto L115;
1184
1185 L103:
1186 if(ip!=2)goto L106;
1187
1188 if(na!=0)
1189 dradb2(ido,l1,ch,c,wa+iw-1);
1190 else
1191 dradb2(ido,l1,c,ch,wa+iw-1);
1192 na=1-na;
1193 goto L115;
1194
1195 L106:
1196 if(ip!=3)goto L109;
1197
1198 ix2=iw+ido;
1199 if(na!=0)
1200 dradb3(ido,l1,ch,c,wa+iw-1,wa+ix2-1);
1201 else
1202 dradb3(ido,l1,c,ch,wa+iw-1,wa+ix2-1);
1203 na=1-na;
1204 goto L115;
1205
1206 L109:
1207 /* The radix five case can be translated later..... */
1208 /* if(ip!=5)goto L112;
1209
1210 ix2=iw+ido;
1211 ix3=ix2+ido;
1212 ix4=ix3+ido;
1213 if(na!=0)
1214 dradb5(ido,l1,ch,c,wa+iw-1,wa+ix2-1,wa+ix3-1,wa+ix4-1);
1215 else
1216 dradb5(ido,l1,c,ch,wa+iw-1,wa+ix2-1,wa+ix3-1,wa+ix4-1);
1217 na=1-na;
1218 goto L115;
1219
1220 L112:*/
1221 if(na!=0)
1222 dradbg(ido,ip,l1,idl1,ch,ch,ch,c,c,wa+iw-1);
1223 else
1224 dradbg(ido,ip,l1,idl1,c,c,c,ch,ch,wa+iw-1);
1225 if(ido==1)na=1-na;
1226
1227 L115:
1228 l1=l2;
1229 iw+=(ip-1)*ido;
1230 }
1231
1232 if(na==0)return;
1233
1234 for(i=0;i<n;i++)c[i]=ch[i];
1235 }
1236
spx_drft_forward(struct drft_lookup * l,float * data)1237 void spx_drft_forward(struct drft_lookup *l,float *data){
1238 if(l->n==1)return;
1239 drftf1(l->n,data,l->trigcache,l->trigcache+l->n,l->splitcache);
1240 }
1241
spx_drft_backward(struct drft_lookup * l,float * data)1242 void spx_drft_backward(struct drft_lookup *l,float *data){
1243 if (l->n==1)return;
1244 drftb1(l->n,data,l->trigcache,l->trigcache+l->n,l->splitcache);
1245 }
1246
spx_drft_init(struct drft_lookup * l,int n)1247 void spx_drft_init(struct drft_lookup *l,int n)
1248 {
1249 l->n=n;
1250 l->trigcache=(float*)speex_alloc(3*n*sizeof(*l->trigcache));
1251 l->splitcache=(int*)speex_alloc(32*sizeof(*l->splitcache));
1252 fdrffti(n, l->trigcache, l->splitcache);
1253 }
1254
spx_drft_clear(struct drft_lookup * l)1255 void spx_drft_clear(struct drft_lookup *l)
1256 {
1257 if(l)
1258 {
1259 if(l->trigcache)
1260 speex_free(l->trigcache);
1261 if(l->splitcache)
1262 speex_free(l->splitcache);
1263 }
1264 }
1265