devs/foxbox
1
0
Fork 0
forked from len0rd/rockbox
Code Pull requests Activity
foxbox/apps/plugins/pdbox/PDa/src/d_mayer_fft.c
Peter D'Hoye 0d4560cb03 Accept FS #10244 by Wincent Balin: more pdbox work done for GSoC; also some keyword and line-ending fixes by me 
git-svn-id: svn://svn.rockbox.org/rockbox/trunk@21626 a1c6a512-1295-4272-9138-f99709370657
2009-07-03 22:16:11 +00:00

428 lines
13 KiB
C
Raw Blame History

/*
** 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
*/
#ifdef MSW
#pragma warning( disable : 4305 ) /* uncast const double to float */
#pragma warning( disable : 4244 ) /* uncast double to float */
#pragma warning( disable : 4101 ) /* unused local variables */
#endif
#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_VARS \
int t_lam=0;
#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
#if defined(FAST_TRIG)
#define TRIG_VARS \
REAL t_c,t_s;
#define TRIG_INIT(k,c,s) \
{ \
t_c = costab[k]; \
t_s = sintab[k]; \
c = 1; \
s = 0; \
}
#define TRIG_NEXT(k,c,s) \
{ \
REAL t = c; \
c = t*t_c - s*t_s; \
s = t*t_s + s*t_c; \
}
#define TRIG_RESET(k,c,s)
#endif
static REAL halsec[20]=
{
0,
0,
.54119610014619698439972320536638942006107206337801,
.50979557910415916894193980398784391368261849190893,
.50241928618815570551167011928012092247859337193963,
.50060299823519630134550410676638239611758632599591,
.50015063602065098821477101271097658495974913010340,
.50003765191554772296778139077905492847503165398345,
.50000941253588775676512870469186533538523133757983,
.50000235310628608051401267171204408939326297376426,
.50000058827484117879868526730916804925780637276181,
.50000014706860214875463798283871198206179118093251,
.50000003676714377807315864400643020315103490883972,
.50000000919178552207366560348853455333939112569380,
.50000000229794635411562887767906868558991922348920,
.50000000057448658687873302235147272458812263401372
};
static REAL costab[20]=
{
.00000000000000000000000000000000000000000000000000,
.70710678118654752440084436210484903928483593768847,
.92387953251128675612818318939678828682241662586364,
.98078528040323044912618223613423903697393373089333,
.99518472667219688624483695310947992157547486872985,
.99879545620517239271477160475910069444320361470461,
.99969881869620422011576564966617219685006108125772,
.99992470183914454092164649119638322435060646880221,
.99998117528260114265699043772856771617391725094433,
.99999529380957617151158012570011989955298763362218,
.99999882345170190992902571017152601904826792288976,
.99999970586288221916022821773876567711626389934930,
.99999992646571785114473148070738785694820115568892,
.99999998161642929380834691540290971450507605124278,
.99999999540410731289097193313960614895889430318945,
.99999999885102682756267330779455410840053741619428
};
static REAL sintab[20]=
{
1.0000000000000000000000000000000000000000000000000,
.70710678118654752440084436210484903928483593768846,
.38268343236508977172845998403039886676134456248561,
.19509032201612826784828486847702224092769161775195,
.09801714032956060199419556388864184586113667316749,
.04906767432741801425495497694268265831474536302574,
.02454122852291228803173452945928292506546611923944,
.01227153828571992607940826195100321214037231959176,
.00613588464915447535964023459037258091705788631738,
.00306795676296597627014536549091984251894461021344,
.00153398018628476561230369715026407907995486457522,
.00076699031874270452693856835794857664314091945205,
.00038349518757139558907246168118138126339502603495,
.00019174759731070330743990956198900093346887403385,
.00009587379909597734587051721097647635118706561284,
.00004793689960306688454900399049465887274686668768
};
static REAL coswrk[20]=
{
.00000000000000000000000000000000000000000000000000,
.70710678118654752440084436210484903928483593768847,
.92387953251128675612818318939678828682241662586364,
.98078528040323044912618223613423903697393373089333,
.99518472667219688624483695310947992157547486872985,
.99879545620517239271477160475910069444320361470461,
.99969881869620422011576564966617219685006108125772,
.99992470183914454092164649119638322435060646880221,
.99998117528260114265699043772856771617391725094433,
.99999529380957617151158012570011989955298763362218,
.99999882345170190992902571017152601904826792288976,
.99999970586288221916022821773876567711626389934930,
.99999992646571785114473148070738785694820115568892,
.99999998161642929380834691540290971450507605124278,
.99999999540410731289097193313960614895889430318945,
.99999999885102682756267330779455410840053741619428
};
static REAL sinwrk[20]=
{
1.0000000000000000000000000000000000000000000000000,
.70710678118654752440084436210484903928483593768846,
.38268343236508977172845998403039886676134456248561,
.19509032201612826784828486847702224092769161775195,
.09801714032956060199419556388864184586113667316749,
.04906767432741801425495497694268265831474536302574,
.02454122852291228803173452945928292506546611923944,
.01227153828571992607940826195100321214037231959176,
.00613588464915447535964023459037258091705788631738,
.00306795676296597627014536549091984251894461021344,
.00153398018628476561230369715026407907995486457522,
.00076699031874270452693856835794857664314091945205,
.00038349518757139558907246168118138126339502603495,
.00019174759731070330743990956198900093346887403385,
.00009587379909597734587051721097647635118706561284,
.00004793689960306688454900399049465887274686668768
};
#define SQRT2_2 0.70710678118654752440084436210484
#define SQRT2 2*0.70710678118654752440084436210484
void mayer_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;
TRIG_VARS;
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 mayer_realfft(int n, REAL *real)
{
#ifdef ROCKBOX
REAL a,b;
#else
REAL a,b,c,d;
#endif
int i,j,k;
mayer_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 mayer_realifft(int n, REAL *real)
{
#ifdef ROCKBOX
REAL a,b;
#else
REAL a,b,c,d;
#endif
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);
}
mayer_fht(real,n);
}
View git blame Copy permalink