| /* |
| ** FFT and FHT routines |
| ** Copyright 1988, 1993; Ron Mayer |
| ** |
| ** fht(fz,n); |
| ** Does a hartley transform of "n" points in the array "fz". |
| ** |
| ** NOTE: This routine uses at least 2 patented algorithms, and may be |
| ** under the restrictions of a bunch of different organizations. |
| ** Although I wrote it completely myself; it is kind of a derivative |
| ** of a routine I once authored and released under the GPL, so it |
| ** may fall under the free software foundation's restrictions; |
| ** it was worked on as a Stanford Univ project, so they claim |
| ** some rights to it; it was further optimized at work here, so |
| ** I think this company claims parts of it. The patents are |
| ** held by R. Bracewell (the FHT algorithm) and O. Buneman (the |
| ** trig generator), both at Stanford Univ. |
| ** If it were up to me, I'd say go do whatever you want with it; |
| ** but it would be polite to give credit to the following people |
| ** if you use this anywhere: |
| ** Euler - probable inventor of the fourier transform. |
| ** Gauss - probable inventor of the FFT. |
| ** Hartley - probable inventor of the hartley transform. |
| ** Buneman - for a really cool trig generator |
| ** Mayer(me) - for authoring this particular version and |
| ** including all the optimizations in one package. |
| ** Thanks, |
| ** Ron Mayer; mayer@acuson.com |
| ** and added some optimization by |
| ** Mather - idea of using lookup table |
| ** Takehiro - some dirty hack for speed up |
| */ |
| |
| #include <math.h> |
| #include "util.h" |
| #include "psymodel.h" |
| #include "lame.h" |
| |
| #define TRI_SIZE (5-1) /* 1024 = 4**5 */ |
| static FLOAT costab[TRI_SIZE*2]; |
| static FLOAT window[BLKSIZE / 2], window_s[BLKSIZE_s / 2]; |
| |
| static INLINE void fht(FLOAT *fz, short n) |
| { |
| short k4; |
| FLOAT *fi, *fn, *gi; |
| FLOAT *tri; |
| |
| fn = fz + n; |
| tri = &costab[0]; |
| k4 = 4; |
| do { |
| FLOAT s1, c1; |
| short i, k1, k2, k3, kx; |
| kx = k4 >> 1; |
| k1 = k4; |
| k2 = k4 << 1; |
| k3 = k2 + k1; |
| k4 = k2 << 1; |
| fi = fz; |
| gi = fi + kx; |
| do { |
| FLOAT f0,f1,f2,f3; |
| f1 = fi[0] - fi[k1]; |
| f0 = fi[0] + fi[k1]; |
| f3 = fi[k2] - fi[k3]; |
| f2 = fi[k2] + fi[k3]; |
| fi[k2] = f0 - f2; |
| fi[0 ] = f0 + f2; |
| fi[k3] = f1 - f3; |
| fi[k1] = f1 + f3; |
| f1 = gi[0] - gi[k1]; |
| f0 = gi[0] + gi[k1]; |
| f3 = SQRT2 * gi[k3]; |
| f2 = SQRT2 * gi[k2]; |
| gi[k2] = f0 - f2; |
| gi[0 ] = f0 + f2; |
| gi[k3] = f1 - f3; |
| gi[k1] = f1 + f3; |
| gi += k4; |
| fi += k4; |
| } while (fi<fn); |
| c1 = tri[0]; |
| s1 = tri[1]; |
| for (i = 1; i < kx; i++) { |
| FLOAT c2,s2; |
| c2 = 1 - (2*s1)*s1; |
| s2 = (2*s1)*c1; |
| fi = fz + i; |
| gi = fz + k1 - i; |
| do { |
| FLOAT a,b,g0,f0,f1,g1,f2,g2,f3,g3; |
| b = s2*fi[k1] - c2*gi[k1]; |
| a = c2*fi[k1] + s2*gi[k1]; |
| f1 = fi[0 ] - a; |
| f0 = fi[0 ] + a; |
| g1 = gi[0 ] - b; |
| g0 = gi[0 ] + b; |
| b = s2*fi[k3] - c2*gi[k3]; |
| a = c2*fi[k3] + s2*gi[k3]; |
| f3 = fi[k2] - a; |
| f2 = fi[k2] + a; |
| g3 = gi[k2] - b; |
| g2 = gi[k2] + b; |
| b = s1*f2 - c1*g3; |
| a = c1*f2 + s1*g3; |
| fi[k2] = f0 - a; |
| fi[0 ] = f0 + a; |
| gi[k3] = g1 - b; |
| gi[k1] = g1 + b; |
| b = c1*g2 - s1*f3; |
| a = s1*g2 + c1*f3; |
| gi[k2] = g0 - a; |
| gi[0 ] = g0 + a; |
| fi[k3] = f1 - b; |
| fi[k1] = f1 + b; |
| gi += k4; |
| fi += k4; |
| } while (fi<fn); |
| c2 = c1; |
| c1 = c2 * tri[0] - s1 * tri[1]; |
| s1 = c2 * tri[1] + s1 * tri[0]; |
| } |
| tri += 2; |
| } while (k4<n); |
| } |
| |
| static const short rv_tbl[] = { |
| 0x00, 0x80, 0x40, 0xc0, 0x20, 0xa0, 0x60, 0xe0, |
| 0x10, 0x90, 0x50, 0xd0, 0x30, 0xb0, 0x70, 0xf0, |
| 0x08, 0x88, 0x48, 0xc8, 0x28, 0xa8, 0x68, 0xe8, |
| 0x18, 0x98, 0x58, 0xd8, 0x38, 0xb8, 0x78, 0xf8, |
| 0x04, 0x84, 0x44, 0xc4, 0x24, 0xa4, 0x64, 0xe4, |
| 0x14, 0x94, 0x54, 0xd4, 0x34, 0xb4, 0x74, 0xf4, |
| 0x0c, 0x8c, 0x4c, 0xcc, 0x2c, 0xac, 0x6c, 0xec, |
| 0x1c, 0x9c, 0x5c, 0xdc, 0x3c, 0xbc, 0x7c, 0xfc, |
| 0x02, 0x82, 0x42, 0xc2, 0x22, 0xa2, 0x62, 0xe2, |
| 0x12, 0x92, 0x52, 0xd2, 0x32, 0xb2, 0x72, 0xf2, |
| 0x0a, 0x8a, 0x4a, 0xca, 0x2a, 0xaa, 0x6a, 0xea, |
| 0x1a, 0x9a, 0x5a, 0xda, 0x3a, 0xba, 0x7a, 0xfa, |
| 0x06, 0x86, 0x46, 0xc6, 0x26, 0xa6, 0x66, 0xe6, |
| 0x16, 0x96, 0x56, 0xd6, 0x36, 0xb6, 0x76, 0xf6, |
| 0x0e, 0x8e, 0x4e, 0xce, 0x2e, 0xae, 0x6e, 0xee, |
| 0x1e, 0x9e, 0x5e, 0xde, 0x3e, 0xbe, 0x7e, 0xfe |
| }; |
| |
| |
| |
| |
| #define ch01(index) (buffer[chn][index]) |
| #define ch2(index) (((FLOAT)(0.5*SQRT2))*(buffer[0][index] + buffer[1][index])) |
| #define ch3(index) (((FLOAT)(0.5*SQRT2))*(buffer[0][index] - buffer[1][index])) |
| |
| #define ml00(f) (window[i ] * f(i)) |
| #define ml10(f) (window[0x1ff - i] * f(i + 0x200)) |
| #define ml20(f) (window[i + 0x100] * f(i + 0x100)) |
| #define ml30(f) (window[0x0ff - i] * f(i + 0x300)) |
| |
| #define ml01(f) (window[i + 0x001] * f(i + 0x001)) |
| #define ml11(f) (window[0x1fe - i] * f(i + 0x201)) |
| #define ml21(f) (window[i + 0x101] * f(i + 0x101)) |
| #define ml31(f) (window[0x0fe - i] * f(i + 0x301)) |
| |
| #define ms00(f) (window_s[i ] * f(i + k)) |
| #define ms10(f) (window_s[0x7f - i] * f(i + k + 0x80)) |
| #define ms20(f) (window_s[i + 0x40] * f(i + k + 0x40)) |
| #define ms30(f) (window_s[0x3f - i] * f(i + k + 0xc0)) |
| |
| #define ms01(f) (window_s[i + 0x01] * f(i + k + 0x01)) |
| #define ms11(f) (window_s[0x7e - i] * f(i + k + 0x81)) |
| #define ms21(f) (window_s[i + 0x41] * f(i + k + 0x41)) |
| #define ms31(f) (window_s[0x3e - i] * f(i + k + 0xc1)) |
| |
| |
| |
| void fft_short( |
| FLOAT x_real[3][BLKSIZE_s], int chn, short *buffer[2]) |
| { |
| short i, j, b; |
| |
| for (b = 0; b < 3; b++) { |
| FLOAT *x = &x_real[b][BLKSIZE_s / 2]; |
| short k = (576 / 3) * (b + 1); |
| j = BLKSIZE_s / 8 - 1; |
| if (chn < 2) { |
| do { |
| FLOAT f0,f1,f2,f3, w; |
| |
| i = rv_tbl[j << 2]; |
| |
| f0 = ms00(ch01); w = ms10(ch01); f1 = f0 - w; f0 = f0 + w; |
| f2 = ms20(ch01); w = ms30(ch01); f3 = f2 - w; f2 = f2 + w; |
| |
| x -= 4; |
| x[0] = f0 + f2; |
| x[2] = f0 - f2; |
| x[1] = f1 + f3; |
| x[3] = f1 - f3; |
| |
| f0 = ms01(ch01); w = ms11(ch01); f1 = f0 - w; f0 = f0 + w; |
| f2 = ms21(ch01); w = ms31(ch01); f3 = f2 - w; f2 = f2 + w; |
| |
| x[BLKSIZE_s / 2 + 0] = f0 + f2; |
| x[BLKSIZE_s / 2 + 2] = f0 - f2; |
| x[BLKSIZE_s / 2 + 1] = f1 + f3; |
| x[BLKSIZE_s / 2 + 3] = f1 - f3; |
| } while (--j >= 0); |
| } else if (chn == 2) { |
| do { |
| FLOAT f0,f1,f2,f3, w; |
| |
| i = rv_tbl[j << 2]; |
| |
| f0 = ms00(ch2); w = ms10(ch2); f1 = f0 - w; f0 = f0 + w; |
| f2 = ms20(ch2); w = ms30(ch2); f3 = f2 - w; f2 = f2 + w; |
| |
| x -= 4; |
| x[0] = f0 + f2; |
| x[2] = f0 - f2; |
| x[1] = f1 + f3; |
| x[3] = f1 - f3; |
| |
| f0 = ms01(ch2); w = ms11(ch2); f1 = f0 - w; f0 = f0 + w; |
| f2 = ms21(ch2); w = ms31(ch2); f3 = f2 - w; f2 = f2 + w; |
| |
| x[BLKSIZE_s / 2 + 0] = f0 + f2; |
| x[BLKSIZE_s / 2 + 2] = f0 - f2; |
| x[BLKSIZE_s / 2 + 1] = f1 + f3; |
| x[BLKSIZE_s / 2 + 3] = f1 - f3; |
| } while (--j >= 0); |
| } else { |
| do { |
| FLOAT f0,f1,f2,f3, w; |
| |
| i = rv_tbl[j << 2]; |
| |
| f0 = ms00(ch3); w = ms10(ch3); f1 = f0 - w; f0 = f0 + w; |
| f2 = ms20(ch3); w = ms30(ch3); f3 = f2 - w; f2 = f2 + w; |
| |
| x -= 4; |
| x[0] = f0 + f2; |
| x[2] = f0 - f2; |
| x[1] = f1 + f3; |
| x[3] = f1 - f3; |
| |
| f0 = ms01(ch3); w = ms11(ch3); f1 = f0 - w; f0 = f0 + w; |
| f2 = ms21(ch3); w = ms31(ch3); f3 = f2 - w; f2 = f2 + w; |
| |
| x[BLKSIZE_s / 2 + 0] = f0 + f2; |
| x[BLKSIZE_s / 2 + 2] = f0 - f2; |
| x[BLKSIZE_s / 2 + 1] = f1 + f3; |
| x[BLKSIZE_s / 2 + 3] = f1 - f3; |
| } while (--j >= 0); |
| } |
| |
| fht(x, BLKSIZE_s); |
| } |
| } |
| |
| void fft_long( |
| FLOAT x[BLKSIZE], int chn, short *buffer[2]) |
| { |
| short i,jj = BLKSIZE / 8 - 1; |
| x += BLKSIZE / 2; |
| |
| if (chn < 2) { |
| do { |
| FLOAT f0,f1,f2,f3, w; |
| |
| i = rv_tbl[jj]; |
| f0 = ml00(ch01); w = ml10(ch01); f1 = f0 - w; f0 = f0 + w; |
| f2 = ml20(ch01); w = ml30(ch01); f3 = f2 - w; f2 = f2 + w; |
| |
| x -= 4; |
| x[0] = f0 + f2; |
| x[2] = f0 - f2; |
| x[1] = f1 + f3; |
| x[3] = f1 - f3; |
| |
| f0 = ml01(ch01); w = ml11(ch01); f1 = f0 - w; f0 = f0 + w; |
| f2 = ml21(ch01); w = ml31(ch01); f3 = f2 - w; f2 = f2 + w; |
| |
| x[BLKSIZE / 2 + 0] = f0 + f2; |
| x[BLKSIZE / 2 + 2] = f0 - f2; |
| x[BLKSIZE / 2 + 1] = f1 + f3; |
| x[BLKSIZE / 2 + 3] = f1 - f3; |
| } while (--jj >= 0); |
| } else if (chn == 2) { |
| do { |
| FLOAT f0,f1,f2,f3, w; |
| |
| i = rv_tbl[jj]; |
| f0 = ml00(ch2); w = ml10(ch2); f1 = f0 - w; f0 = f0 + w; |
| f2 = ml20(ch2); w = ml30(ch2); f3 = f2 - w; f2 = f2 + w; |
| |
| x -= 4; |
| x[0] = f0 + f2; |
| x[2] = f0 - f2; |
| x[1] = f1 + f3; |
| x[3] = f1 - f3; |
| |
| f0 = ml01(ch2); w = ml11(ch2); f1 = f0 - w; f0 = f0 + w; |
| f2 = ml21(ch2); w = ml31(ch2); f3 = f2 - w; f2 = f2 + w; |
| |
| x[BLKSIZE / 2 + 0] = f0 + f2; |
| x[BLKSIZE / 2 + 2] = f0 - f2; |
| x[BLKSIZE / 2 + 1] = f1 + f3; |
| x[BLKSIZE / 2 + 3] = f1 - f3; |
| } while (--jj >= 0); |
| } else { |
| do { |
| FLOAT f0,f1,f2,f3, w; |
| |
| i = rv_tbl[jj]; |
| f0 = ml00(ch3); w = ml10(ch3); f1 = f0 - w; f0 = f0 + w; |
| f2 = ml20(ch3); w = ml30(ch3); f3 = f2 - w; f2 = f2 + w; |
| |
| x -= 4; |
| x[0] = f0 + f2; |
| x[2] = f0 - f2; |
| x[1] = f1 + f3; |
| x[3] = f1 - f3; |
| |
| f0 = ml01(ch3); w = ml11(ch3); f1 = f0 - w; f0 = f0 + w; |
| f2 = ml21(ch3); w = ml31(ch3); f3 = f2 - w; f2 = f2 + w; |
| |
| x[BLKSIZE / 2 + 0] = f0 + f2; |
| x[BLKSIZE / 2 + 2] = f0 - f2; |
| x[BLKSIZE / 2 + 1] = f1 + f3; |
| x[BLKSIZE / 2 + 3] = f1 - f3; |
| } while (--jj >= 0); |
| } |
| |
| fht(x, BLKSIZE); |
| } |
| |
| |
| void init_fft(void) |
| { |
| int i; |
| |
| FLOAT r = PI*0.125; |
| for (i = 0; i < TRI_SIZE; i++) { |
| costab[i*2 ] = cos(r); |
| costab[i*2+1] = sin(r); |
| r *= 0.25; |
| } |
| |
| /* |
| * calculate HANN window coefficients |
| */ |
| for (i = 0; i < BLKSIZE / 2; i++) |
| window[i] = 0.5 * (1.0 - cos(2.0 * PI * (i + 0.5) / BLKSIZE)); |
| for (i = 0; i < BLKSIZE_s / 2; i++) |
| window_s[i] = 0.5 * (1.0 - cos(2.0 * PI * (i + 0.5) / BLKSIZE_s)); |
| } |