You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
384 lines
8.7 KiB
384 lines
8.7 KiB
|
|
#include <math.h>
|
|
//#include "chapro.h"
|
|
//#include "cha_ff.h"
|
|
|
|
/***********************************************************/
|
|
// FFT functions adapted from G. D. Bergland, "Subroutines FAST and FSST," (1979).
|
|
// In IEEE Acoustics, Speech, and Signal Processing Society.
|
|
// "Programs for Digital Signal Processing," IEEE Press, New York,
|
|
|
|
static __inline int
|
|
ilog2(int n)
|
|
{
|
|
int m;
|
|
|
|
for (m = 1; m < 32; m++)
|
|
if (n == (1 << m))
|
|
return (m);
|
|
return (-1);
|
|
}
|
|
|
|
static __inline int
|
|
bitrev(int ii, int m)
|
|
{
|
|
register int jj;
|
|
|
|
jj = ii & 1;
|
|
--m;
|
|
while (--m > 0) {
|
|
ii >>= 1;
|
|
jj <<= 1;
|
|
jj |= ii & 1;
|
|
}
|
|
return (jj);
|
|
}
|
|
|
|
static __inline void
|
|
rad2(int ii, float *x0, float *x1)
|
|
{
|
|
int k;
|
|
float t;
|
|
|
|
for (k = 0; k < ii; k++) {
|
|
t = x0[k] + x1[k];
|
|
x1[k] = x0[k] - x1[k];
|
|
x0[k] = t;
|
|
}
|
|
}
|
|
|
|
static __inline void
|
|
reorder1(int m, float *x)
|
|
{
|
|
int j, k, kl, n;
|
|
float t;
|
|
|
|
k = 4;
|
|
kl = 2;
|
|
n = 1 << m;
|
|
for (j = 4; j <= n; j += 2) {
|
|
if (k > j) {
|
|
t = x[j - 1];
|
|
x[j - 1] = x[k - 1];
|
|
x[k - 1] = t;
|
|
}
|
|
k -= 2;
|
|
if (k <= kl) {
|
|
k = 2 * j;
|
|
kl = j;
|
|
}
|
|
}
|
|
}
|
|
|
|
static __inline void
|
|
reorder2(int m, float *x)
|
|
{
|
|
int ji, ij, n;
|
|
float t;
|
|
|
|
n = 1 << m;
|
|
for (ij = 0; ij <= (n - 2); ij += 2) {
|
|
ji = bitrev(ij >> 1, m) << 1;
|
|
if (ij < ji) {
|
|
t = x[ij];
|
|
x[ij] = x[ji];
|
|
x[ji] = t;
|
|
t = x[ij + 1];
|
|
x[ij + 1] = x[ji + 1];
|
|
x[ji + 1] = t;
|
|
}
|
|
}
|
|
}
|
|
|
|
/***********************************************************/
|
|
|
|
// rcfft
|
|
|
|
static void
|
|
rcrad4(int ii, int nn,
|
|
float *x0, float *x1, float *x2, float *x3,
|
|
float *x4, float *x5, float *x6, float *x7)
|
|
{
|
|
double arg, tpiovn;
|
|
float c1, c2, c3, s1, s2, s3, pr, pi, r1, r5;
|
|
float t0, t1, t2, t3, t4, t5, t6, t7;
|
|
int i0, i4, j, j0, ji, jl, jr, jlast, k, k0, kl, m, n, ni;
|
|
|
|
n = nn / 4;
|
|
for (m = 1; (1 << m) < n; m++)
|
|
continue;
|
|
tpiovn = 2 * M_PI / nn;
|
|
ji = 3;
|
|
jl = 2;
|
|
jr = 2;
|
|
ni = (n + 1) / 2;
|
|
for (i0 = 0; i0 < ni; i0++) {
|
|
if (i0 == 0) {
|
|
for (k = 0; k < ii; k++) {
|
|
t0 = x0[k] + x2[k];
|
|
t1 = x1[k] + x3[k];
|
|
x2[k] = x0[k] - x2[k];
|
|
x3[k] = x1[k] - x3[k];
|
|
x0[k] = t0 + t1;
|
|
x1[k] = t0 - t1;
|
|
}
|
|
if (nn > 4) {
|
|
k0 = ii * 4;
|
|
kl = k0 + ii;
|
|
for (k = k0; k < kl; k++) {
|
|
pr = (float) (M_SQRT1_2 * (x1[k] - x3[k]));
|
|
pi = (float) (M_SQRT1_2 * (x1[k] + x3[k]));
|
|
x3[k] = x2[k] + pi;
|
|
x1[k] = pi - x2[k];
|
|
x2[k] = x0[k] - pr;
|
|
x0[k] += pr;
|
|
}
|
|
}
|
|
} else {
|
|
arg = tpiovn * bitrev(i0, m);
|
|
c1 = cosf(arg);
|
|
s1 = sinf(arg);
|
|
c2 = c1 * c1 - s1 * s1;
|
|
s2 = c1 * s1 + c1 * s1;
|
|
c3 = c1 * c2 - s1 * s2;
|
|
s3 = c2 * s1 + s2 * c1;
|
|
i4 = ii * 4;
|
|
j0 = jr * i4;
|
|
k0 = ji * i4;
|
|
jlast = j0 + ii;
|
|
for (j = j0; j < jlast; j++) {
|
|
k = k0 + j - j0;
|
|
r1 = x1[j] * c1 - x5[k] * s1;
|
|
r5 = x1[j] * s1 + x5[k] * c1;
|
|
t2 = x2[j] * c2 - x6[k] * s2;
|
|
t6 = x2[j] * s2 + x6[k] * c2;
|
|
t3 = x3[j] * c3 - x7[k] * s3;
|
|
t7 = x3[j] * s3 + x7[k] * c3;
|
|
t0 = x0[j] + t2;
|
|
t4 = x4[k] + t6;
|
|
t2 = x0[j] - t2;
|
|
t6 = x4[k] - t6;
|
|
t1 = r1 + t3;
|
|
t5 = r5 + t7;
|
|
t3 = r1 - t3;
|
|
t7 = r5 - t7;
|
|
x0[j] = t0 + t1;
|
|
x7[k] = t4 + t5;
|
|
x6[k] = t0 - t1;
|
|
x1[j] = t5 - t4;
|
|
x2[j] = t2 - t7;
|
|
x5[k] = t6 + t3;
|
|
x4[k] = t2 + t7;
|
|
x3[j] = t3 - t6;
|
|
}
|
|
jr += 2;
|
|
ji -= 2;
|
|
if (ji <= jl) {
|
|
ji = 2 * jr - 1;
|
|
jl = jr;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------
|
|
|
|
static int
|
|
rcfft2(float *x, int m)
|
|
{
|
|
int ii, nn, m2, it, n;
|
|
|
|
n = 1 << m;;
|
|
m2 = m / 2;
|
|
|
|
// radix 2
|
|
|
|
if (m <= m2 * 2) {
|
|
nn = 1;
|
|
} else {
|
|
nn = 2;
|
|
ii = n / nn;
|
|
rad2(ii, x, x + ii);
|
|
}
|
|
|
|
// radix 4
|
|
|
|
if (m2 != 0) {
|
|
for (it = 0; it < m2; it++) {
|
|
nn = nn * 4;
|
|
ii = n / nn;
|
|
rcrad4(ii, nn, x, x + ii, x + 2 * ii, x + 3 * ii,
|
|
x, x + ii, x + 2 * ii, x + 3 * ii);
|
|
}
|
|
}
|
|
|
|
// re-order
|
|
|
|
reorder1(m, x);
|
|
reorder2(m, x);
|
|
for (it = 3; it < n; it += 2)
|
|
x[it] = -x[it];
|
|
x[n] = x[1];
|
|
x[1] = 0.0;
|
|
x[n + 1] = 0.0;
|
|
|
|
return (0);
|
|
}
|
|
|
|
/***********************************************************/
|
|
|
|
// rcfft
|
|
|
|
static void
|
|
crrad4(int jj, int nn,
|
|
float *x0, float *x1, float *x2, float *x3,
|
|
float *x4, float *x5, float *x6, float *x7)
|
|
{
|
|
double arg, tpiovn;
|
|
float c1, c2, c3, s1, s2, s3;
|
|
float t0, t1, t2, t3, t4, t5, t6, t7;
|
|
int ii, j, j0, ji, jr, jl, jlast, j4, k, k0, kl, m, n, ni;
|
|
|
|
tpiovn = 2 * M_PI / nn;
|
|
ji = 3;
|
|
jl = 2;
|
|
jr = 2;
|
|
n = nn / 4;
|
|
for (m = 1; (1 << m) < n; m++)
|
|
continue;
|
|
ni = (n + 1) / 2;
|
|
for (ii = 0; ii < ni; ii++) {
|
|
if (ii == 0) {
|
|
for (k = 0; k < jj; k++) {
|
|
t0 = x0[k] + x1[k];
|
|
t1 = x0[k] - x1[k];
|
|
t2 = x2[k] * 2;
|
|
t3 = x3[k] * 2;
|
|
x0[k] = t0 + t2;
|
|
x2[k] = t0 - t2;
|
|
x1[k] = t1 + t3;
|
|
x3[k] = t1 - t3;
|
|
}
|
|
if (nn > 4) {
|
|
k0 = jj * 4;
|
|
kl = k0 + jj;
|
|
for (k = k0; k < kl; k++) {
|
|
t2 = x0[k] - x2[k];
|
|
t3 = x1[k] + x3[k];
|
|
x0[k] = (x0[k] + x2[k]) * 2;
|
|
x2[k] = (x3[k] - x1[k]) * 2;
|
|
x1[k] = (float) ((t2 + t3) * M_SQRT2);
|
|
x3[k] = (float) ((t3 - t2) * M_SQRT2);
|
|
}
|
|
}
|
|
} else {
|
|
arg = tpiovn * bitrev(ii, m);
|
|
c1 = cosf(arg);
|
|
s1 = -sinf(arg);
|
|
c2 = c1 * c1 - s1 * s1;
|
|
s2 = c1 * s1 + c1 * s1;
|
|
c3 = c1 * c2 - s1 * s2;
|
|
s3 = c2 * s1 + s2 * c1;
|
|
j4 = jj * 4;
|
|
j0 = jr * j4;
|
|
k0 = ji * j4;
|
|
jlast = j0 + jj;
|
|
for (j = j0; j < jlast; j++) {
|
|
k = k0 + j - j0;
|
|
t0 = x0[j] + x6[k];
|
|
t1 = x7[k] - x1[j];
|
|
t2 = x0[j] - x6[k];
|
|
t3 = x7[k] + x1[j];
|
|
t4 = x2[j] + x4[k];
|
|
t5 = x5[k] - x3[j];
|
|
t6 = x5[k] + x3[j];
|
|
t7 = x4[k] - x2[j];
|
|
x0[j] = t0 + t4;
|
|
x4[k] = t1 + t5;
|
|
x1[j] = (t2 + t6) * c1 - (t3 + t7) * s1;
|
|
x5[k] = (t2 + t6) * s1 + (t3 + t7) * c1;
|
|
x2[j] = (t0 - t4) * c2 - (t1 - t5) * s2;
|
|
x6[k] = (t0 - t4) * s2 + (t1 - t5) * c2;
|
|
x3[j] = (t2 - t6) * c3 - (t3 - t7) * s3;
|
|
x7[k] = (t2 - t6) * s3 + (t3 - t7) * c3;
|
|
}
|
|
jr += 2;
|
|
ji -= 2;
|
|
if (ji <= jl) {
|
|
ji = 2 * jr - 1;
|
|
jl = jr;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------
|
|
|
|
static int
|
|
crfft2(float *x, int m)
|
|
{
|
|
int n, i, it, nn, jj, m2;
|
|
|
|
n = 1 << m;
|
|
x[1] = x[n];
|
|
m2 = m / 2;
|
|
|
|
// re-order
|
|
|
|
for (i = 3; i < n; i += 2)
|
|
x[i] = -x[i];
|
|
reorder2(m, x);
|
|
reorder1(m, x);
|
|
|
|
// radix 4
|
|
|
|
if (m2 != 0) {
|
|
nn = 4 * n;
|
|
for (it = 0; it < m2; it++) {
|
|
nn = nn / 4;
|
|
jj = n / nn;
|
|
crrad4(jj, nn, x, x + jj, x + 2 * jj, x + 3 * jj,
|
|
x, x + jj, x + 2 * jj, x + 3 * jj);
|
|
}
|
|
}
|
|
|
|
// radix 2
|
|
|
|
if (m > m2 * 2) {
|
|
jj = n / 2;
|
|
rad2(jj, x, x + jj);
|
|
}
|
|
|
|
return (0);
|
|
}
|
|
|
|
/***********************************************************/
|
|
|
|
// real-to-complex FFT
|
|
|
|
//FUNC(void)
|
|
void cha_fft_rc(float *x, int n)
|
|
{
|
|
int m;
|
|
|
|
// assume n is a power of two
|
|
m = ilog2(n);
|
|
rcfft2(x, m);
|
|
}
|
|
|
|
// complex-to-real inverse FFT
|
|
|
|
//FUNC(void)
|
|
void cha_fft_cr(float *x, int n)
|
|
{
|
|
int i, m;
|
|
|
|
// assume n is a power of two
|
|
m = ilog2(n);
|
|
crfft2(x, m);
|
|
// scale inverse by 1/n
|
|
for (i = 0; i < n; i++) {
|
|
x[i] /= n;
|
|
}
|
|
}
|
|
|
|
|