191 lines
4.4 KiB
C++
191 lines
4.4 KiB
C++
#include "TRACK.H"
|
|
|
|
/*-------------------------------------------------------------------------
|
|
This computes an in-place complex-to-complex FFT
|
|
x and y are the real and imaginary arrays of 2^m points.
|
|
dir = 1 gives forward transform
|
|
dir = -1 gives reverse transform
|
|
|
|
Formula: forward
|
|
N-1
|
|
---
|
|
1 \ - j k 2 pi n / N
|
|
X(n) = --- > x(k) e = forward transform
|
|
N / n=0..N-1
|
|
---
|
|
k=0
|
|
|
|
Formula: reverse
|
|
N-1
|
|
---
|
|
\ j k 2 pi n / N
|
|
X(n) = > x(k) e = forward transform
|
|
/ n=0..N-1
|
|
---
|
|
k=0
|
|
*/
|
|
int FFT2D(FFT_Complex **c,int nx,int ny,int dir);
|
|
int FFT(int dir,int m,TYPE_Of_PREC *x,TYPE_Of_PREC *y);
|
|
int FFT(int dir,int m,TYPE_Of_PREC *x,TYPE_Of_PREC *y)
|
|
{
|
|
long nn,i,i1,j,k,i2,l,l1,l2;
|
|
TYPE_Of_PREC c1,c2,tx,ty,t1,t2,u1,u2,z;
|
|
|
|
/* Calculate the number of points */
|
|
nn = 1;
|
|
for (i=0;i<m;i++)
|
|
nn *= 2;
|
|
|
|
/* Do the bit reversal */
|
|
i2 = nn >> 1;
|
|
j = 0;
|
|
for (i=0;i<nn-1;i++) {
|
|
if (i < j) {
|
|
tx = x[i];
|
|
ty = y[i];
|
|
x[i] = x[j];
|
|
y[i] = y[j];
|
|
x[j] = tx;
|
|
y[j] = ty;
|
|
}
|
|
k = i2;
|
|
while (k <= j) {
|
|
j -= k;
|
|
k >>= 1;
|
|
}
|
|
j += k;
|
|
}
|
|
|
|
/* Compute the FFT */
|
|
c1 = -1.0;
|
|
c2 = 0.0;
|
|
l2 = 1;
|
|
for (l=0;l<m;l++) {
|
|
l1 = l2;
|
|
l2 <<= 1;
|
|
u1 = 1.0;
|
|
u2 = 0.0;
|
|
for (j=0;j<l1;j++) {
|
|
for (i=j;i<nn;i+=l2) {
|
|
i1 = i + l1;
|
|
t1 = u1 * x[i1] - u2 * y[i1];
|
|
t2 = u1 * y[i1] + u2 * x[i1];
|
|
x[i1] = x[i] - t1;
|
|
y[i1] = y[i] - t2;
|
|
x[i] += t1;
|
|
y[i] += t2;
|
|
}
|
|
z = u1 * c1 - u2 * c2;
|
|
u2 = u1 * c2 + u2 * c1;
|
|
u1 = z;
|
|
}
|
|
c2 = sqrt((1.0 - c1) / 2.0);
|
|
if (dir == 1)
|
|
c2 = -c2;
|
|
c1 = sqrt((1.0 + c1) / 2.0);
|
|
}
|
|
|
|
/* Scaling for forward transform */
|
|
if (dir == 1) {
|
|
for (i=0;i<nn;i++) {
|
|
x[i] /= (TYPE_Of_PREC)nn;
|
|
y[i] /= (TYPE_Of_PREC)nn;
|
|
}
|
|
}
|
|
|
|
|
|
return(TRUE);
|
|
}
|
|
/*-------------------------------------------------------------------------
|
|
Perform a 2D FFT inplace given a complex 2D array
|
|
The direction dir, 1 for forward, -1 for reverse
|
|
The size of the array (nx,ny)
|
|
Return false if there are memory problems or
|
|
the dimensions are not powers of 2
|
|
*/
|
|
int FFT2D(FFT_Complex *c,int nx,int ny,int dir)
|
|
{
|
|
int i,j,PX,PY;
|
|
int m,twopm;
|
|
TYPE_Of_PREC *real,*imag;
|
|
|
|
/* Transform the rows */
|
|
PX = m = GetLogUpperPO2(nx);
|
|
PY = m = GetLogUpperPO2(ny);
|
|
//if (dir != 10)
|
|
{
|
|
real = (TYPE_Of_PREC *)CC_malloc(nx * sizeof(TYPE_Of_PREC));
|
|
imag = (TYPE_Of_PREC *)CC_malloc(nx * sizeof(TYPE_Of_PREC));
|
|
if (real == NULL || imag == NULL)
|
|
return(FALSE);
|
|
|
|
for (j=0;j<ny;j++) {
|
|
for (i=0;i<nx;i++) {
|
|
real[i] = c[(j<<PX)+i].re;
|
|
imag[i] = c[(j<<PX)+i].im;
|
|
}
|
|
FFT(dir,m,real,imag);
|
|
for (i=0;i<nx;i++) {
|
|
c[(j<<PX)+i].re = real[i];
|
|
c[(j<<PX)+i].im = imag[i];
|
|
}
|
|
}
|
|
CC_free(real);
|
|
CC_free(imag);
|
|
}
|
|
|
|
if (dir == 10) return(TRUE);
|
|
|
|
/* Transform the columns */
|
|
real = (TYPE_Of_PREC *)CC_malloc(ny * sizeof(TYPE_Of_PREC));
|
|
imag = (TYPE_Of_PREC *)CC_malloc(ny * sizeof(TYPE_Of_PREC));
|
|
if (real == NULL || imag == NULL)
|
|
return(FALSE);
|
|
PY = m = GetLogUpperPO2(ny);
|
|
for (i=0;i<nx;i++) {
|
|
for (j=0;j<ny;j++) {
|
|
real[j] = c[(j<<PX)+i].re;
|
|
imag[j] = c[(j<<PX)+i].im;
|
|
}
|
|
FFT(dir,m,real,imag);
|
|
for (j=0;j<ny;j++) {
|
|
c[(j<<PX)+i].re = real[j];
|
|
c[(j<<PX)+i].im = imag[j];
|
|
}
|
|
}
|
|
CC_free(real);
|
|
CC_free(imag);
|
|
|
|
return(TRUE);
|
|
}
|
|
|
|
|
|
int OFFSETHALF(FFT_Complex *c,int nx,int ny)
|
|
{
|
|
int i,j;
|
|
/* Switch the columns */
|
|
for (i=0;i<nx>>1;i++)
|
|
{
|
|
for (j=0;j<ny;j++)
|
|
{
|
|
FFT_Complex SWAP;
|
|
SWAP = c[(j*nx)+i];
|
|
c[(j*nx)+i] = c[(j*nx)+i+(nx>>1)];
|
|
c[(j*nx)+i+(nx>>1)] = SWAP;
|
|
}
|
|
}
|
|
|
|
/* Switch the rows */
|
|
for (i=0;i<nx;i++)
|
|
{
|
|
for (j=0;j<ny>>1;j++)
|
|
{
|
|
FFT_Complex SWAP;
|
|
SWAP = c[(j*nx)+i];
|
|
c[(j*nx)+i] = c[((j+(ny>>1))*nx)+i];
|
|
c[((j+(ny>>1))*nx)+i] = SWAP;
|
|
}
|
|
}
|
|
|
|
return(TRUE);
|
|
}
|