308 lines
9.7 KiB
C++
308 lines
9.7 KiB
C++
/* This is the FFT routine taken from PureData, a great piece of
|
|
software by Miller S. Puckette.
|
|
http://crca.ucsd.edu/~msp/software.html */
|
|
|
|
/*
|
|
** FFT and FHT routines
|
|
** Copyright 1988, 1993; Ron Mayer
|
|
**
|
|
** mayer_fht(fz,n);
|
|
** Does a hartley transform of "n" points in the array "fz".
|
|
** mayer_fft(n,real,imag)
|
|
** Does a fourier transform of "n" points of the "real" and
|
|
** "imag" arrays.
|
|
** mayer_ifft(n,real,imag)
|
|
** Does an inverse fourier transform of "n" points of the "real"
|
|
** and "imag" arrays.
|
|
** mayer_realfft(n,real)
|
|
** Does a real-valued fourier transform of "n" points of the
|
|
** "real" array. The real part of the transform ends
|
|
** up in the first half of the array and the imaginary part of the
|
|
** transform ends up in the second half of the array.
|
|
** mayer_realifft(n,real)
|
|
** The inverse of the realfft() routine above.
|
|
**
|
|
**
|
|
** 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
|
|
**
|
|
*/
|
|
|
|
/* This is a slightly modified version of Mayer's contribution; write
|
|
* msp@ucsd.edu for the original code. Kudos to Mayer for a fine piece
|
|
* of work. -msp
|
|
*/
|
|
|
|
#include "mayer_fft.h"
|
|
|
|
#define REAL float
|
|
#define GOOD_TRIG
|
|
|
|
#ifdef GOOD_TRIG
|
|
#else
|
|
#define FAST_TRIG
|
|
#endif
|
|
|
|
#if defined(GOOD_TRIG)
|
|
#define FHT_SWAP(a,b,t) {(t)=(a);(a)=(b);(b)=(t);}
|
|
|
|
#define TRIG_INIT(k,c,s) \
|
|
{ \
|
|
int i; \
|
|
for (i=2 ; i<=k ; i++) \
|
|
{coswrk[i]=costab[i];sinwrk[i]=sintab[i];} \
|
|
t_lam = 0; \
|
|
c = 1; \
|
|
s = 0; \
|
|
}
|
|
#define TRIG_NEXT(k,c,s) \
|
|
{ \
|
|
int i,j; \
|
|
(t_lam)++; \
|
|
for (i=0 ; !((1<<i)&t_lam) ; i++); \
|
|
i = k-i; \
|
|
s = sinwrk[i]; \
|
|
c = coswrk[i]; \
|
|
if (i>1) \
|
|
{ \
|
|
for (j=k-i+2 ; (1<<j)&t_lam ; j++); \
|
|
j = k - j; \
|
|
sinwrk[i] = halsec[i] * (sinwrk[i-1] + sinwrk[j]); \
|
|
coswrk[i] = halsec[i] * (coswrk[i-1] + coswrk[j]); \
|
|
} \
|
|
}
|
|
#define TRIG_RESET(k,c,s)
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#define SQRT2_2 0.70710678118654752440084436210484
|
|
#define SQRT2 2*0.70710678118654752440084436210484
|
|
|
|
void MayerFFT::fht(REAL* fz, int n)
|
|
{
|
|
/* REAL a,b;
|
|
REAL c1,s1,s2,c2,s3,c3,s4,c4;
|
|
REAL f0,g0,f1,g1,f2,g2,f3,g3; */
|
|
int k, k1, k2, k3, k4, kx;
|
|
REAL* fi, * fn, * gi;
|
|
int t_lam = 0;
|
|
|
|
for (k1 = 1, k2 = 0; k1 < n; k1++)
|
|
{
|
|
REAL aa;
|
|
for (k = n >> 1; (!((k2 ^= k) & k)); k >>= 1);
|
|
if (k1 > k2)
|
|
{
|
|
aa = fz[k1]; fz[k1] = fz[k2]; fz[k2] = aa;
|
|
}
|
|
}
|
|
for (k = 0; (1 << k) < n; k++);
|
|
k &= 1;
|
|
if (k == 0)
|
|
{
|
|
for (fi = fz, fn = fz + n; fi < fn; fi += 4)
|
|
{
|
|
REAL f0, f1, f2, f3;
|
|
f1 = fi[0] - fi[1];
|
|
f0 = fi[0] + fi[1];
|
|
f3 = fi[2] - fi[3];
|
|
f2 = fi[2] + fi[3];
|
|
fi[2] = (f0 - f2);
|
|
fi[0] = (f0 + f2);
|
|
fi[3] = (f1 - f3);
|
|
fi[1] = (f1 + f3);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (fi = fz, fn = fz + n, gi = fi + 1; fi < fn; fi += 8, gi += 8)
|
|
{
|
|
REAL bs1, bc1, bs2, bc2, bs3, bc3, bs4, bc4,
|
|
bg0, bf0, bf1, bg1, bf2, bg2, bf3, bg3;
|
|
bc1 = fi[0] - gi[0];
|
|
bs1 = fi[0] + gi[0];
|
|
bc2 = fi[2] - gi[2];
|
|
bs2 = fi[2] + gi[2];
|
|
bc3 = fi[4] - gi[4];
|
|
bs3 = fi[4] + gi[4];
|
|
bc4 = fi[6] - gi[6];
|
|
bs4 = fi[6] + gi[6];
|
|
bf1 = (bs1 - bs2);
|
|
bf0 = (bs1 + bs2);
|
|
bg1 = (bc1 - bc2);
|
|
bg0 = (bc1 + bc2);
|
|
bf3 = (bs3 - bs4);
|
|
bf2 = (bs3 + bs4);
|
|
bg3 = SQRT2 * bc4;
|
|
bg2 = SQRT2 * bc3;
|
|
fi[4] = bf0 - bf2;
|
|
fi[0] = bf0 + bf2;
|
|
fi[6] = bf1 - bf3;
|
|
fi[2] = bf1 + bf3;
|
|
gi[4] = bg0 - bg2;
|
|
gi[0] = bg0 + bg2;
|
|
gi[6] = bg1 - bg3;
|
|
gi[2] = bg1 + bg3;
|
|
}
|
|
}
|
|
if (n < 16) return;
|
|
|
|
do
|
|
{
|
|
REAL s1, c1;
|
|
int ii;
|
|
k += 2;
|
|
k1 = 1 << k;
|
|
k2 = k1 << 1;
|
|
k4 = k2 << 1;
|
|
k3 = k2 + k1;
|
|
kx = k1 >> 1;
|
|
fi = fz;
|
|
gi = fi + kx;
|
|
fn = fz + n;
|
|
do
|
|
{
|
|
REAL g0, f0, f1, g1, f2, g2, f3, g3;
|
|
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;
|
|
g1 = gi[0] - gi[k1];
|
|
g0 = gi[0] + gi[k1];
|
|
g3 = SQRT2 * gi[k3];
|
|
g2 = SQRT2 * gi[k2];
|
|
gi[k2] = g0 - g2;
|
|
gi[0] = g0 + g2;
|
|
gi[k3] = g1 - g3;
|
|
gi[k1] = g1 + g3;
|
|
gi += k4;
|
|
fi += k4;
|
|
} while (fi < fn);
|
|
TRIG_INIT(k, c1, s1);
|
|
for (ii = 1; ii < kx; ii++)
|
|
{
|
|
REAL c2, s2;
|
|
TRIG_NEXT(k, c1, s1);
|
|
c2 = c1 * c1 - s1 * s1;
|
|
s2 = 2 * (c1 * s1);
|
|
fn = fz + n;
|
|
fi = fz + ii;
|
|
gi = fz + k1 - ii;
|
|
do
|
|
{
|
|
REAL 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);
|
|
}
|
|
TRIG_RESET(k, c1, s1);
|
|
} while (k4 < n);
|
|
}
|
|
|
|
//void mayer_fft(int n, REAL* real, REAL* imag)
|
|
//{
|
|
// REAL a, b, c, d;
|
|
// REAL q, r, s, t;
|
|
// int i, j, k;
|
|
// for (i = 1, j = n - 1, k = n / 2; i < k; i++, j--) {
|
|
// a = real[i]; b = real[j]; q = a + b; r = a - b;
|
|
// c = imag[i]; d = imag[j]; s = c + d; t = c - d;
|
|
// real[i] = (q + t) * .5; real[j] = (q - t) * .5;
|
|
// imag[i] = (s - r) * .5; imag[j] = (s + r) * .5;
|
|
// }
|
|
// mayer_fht(real, n);
|
|
// mayer_fht(imag, n);
|
|
//}
|
|
//
|
|
//void mayer_ifft(int n, REAL* real, REAL* imag)
|
|
//{
|
|
// REAL a, b, c, d;
|
|
// REAL q, r, s, t;
|
|
// int i, j, k;
|
|
// mayer_fht(real, n);
|
|
// mayer_fht(imag, n);
|
|
// for (i = 1, j = n - 1, k = n / 2; i < k; i++, j--) {
|
|
// a = real[i]; b = real[j]; q = a + b; r = a - b;
|
|
// c = imag[i]; d = imag[j]; s = c + d; t = c - d;
|
|
// imag[i] = (s + r) * 0.5; imag[j] = (s - r) * 0.5;
|
|
// real[i] = (q - t) * 0.5; real[j] = (q + t) * 0.5;
|
|
// }
|
|
//}
|
|
|
|
void MayerFFT::realfft(int n, REAL* real)
|
|
{
|
|
REAL a, b;
|
|
int i, j, k;
|
|
|
|
fht(real, n);
|
|
for (i = 1, j = n - 1, k = n / 2; i < k; i++, j--) {
|
|
a = real[i];
|
|
b = real[j];
|
|
real[j] = (a - b) * 0.5;
|
|
real[i] = (a + b) * 0.5;
|
|
}
|
|
}
|
|
|
|
void MayerFFT::realifft(int n, REAL* real)
|
|
{
|
|
REAL a, b;
|
|
int i, j, k;
|
|
|
|
for (i = 1, j = n - 1, k = n / 2; i < k; i++, j--) {
|
|
a = real[i];
|
|
b = real[j];
|
|
real[j] = (a - b);
|
|
real[i] = (a + b);
|
|
}
|
|
fht(real, n);
|
|
} |