commit 5a5a5cfd577cfaf5a3e3b2976bd1b68cf4fdb193 Author: Jens Ahrensfeld Date: Sat Jul 19 07:44:42 2014 +0000 Initial import git-svn-id: http://moon:8086/svn/software/trunk/libsrc/avfft@1 b431acfa-c32f-4a4a-93f1-934dc6c82436 diff --git a/avfft.c b/avfft.c new file mode 100755 index 0000000..7fda399 --- /dev/null +++ b/avfft.c @@ -0,0 +1,632 @@ +/***************************************************************************/ +/* FFT.C +/* Fast-Fourier-Transformation +/* Author: Jens Ahrensfeld +/* Datum : 24.06.1999 +/* letzte Änderung: 09.06.2000 +/***************************************************************************/ +#define AVNEEDFLOAT + +#include +#include +#include + +#include "avtypes.h" +#include "averror.h" +#include "avrtl.h" +#include "avfft.h" + +#define PI 3.1415926535897932384626433832795 + +/***************************************************************************/ +/* Konstruktor FFT-Objekt */ +/* */ +/***************************************************************************/ +void FFTinit(FFT *pFFT, UINT32 N) +{ + pFFT->m_numPoints = 0; + pFFT->m_numStages = 0; + + pFFT->pTwfRe = NULL; + pFFT->pTwfIm = NULL; + + if(IsPowerOfTwo(N) == AV_E_FALSE) + { + printf("\n%u is not a power of 2!\n",N); + exit(1); + } + + pFFT->m_numStages = (UINT32) (1e-06 + log((avfloat_t)N)/log(2.0)); + pFFT->m_numPoints = (UINT32 )N; + + pFFT->pTwfRe = (avfloat_t*)malloc(pFFT->m_numPoints * sizeof(avfloat_t) /2); + pFFT->pTwfIm = (avfloat_t*)malloc(pFFT->m_numPoints * sizeof(avfloat_t) /2); + + FFTCalcTwiddleTable(pFFT->pTwfRe, pFFT->pTwfIm, pFFT->m_numPoints); + +#ifdef FFTMsg + printf("\n%d-Point FFT object created.\n",pFFT->m_numPoints); +#endif +} +/***************************************************************************/ +/* Destruktor FFT-Objekt */ +/* */ +/***************************************************************************/ +void FFTfree(FFT *pFFT) +{ + if (pFFT->pTwfRe != NULL) + { + free(pFFT->pTwfRe); + pFFT->pTwfRe = NULL; + } + + if (pFFT->pTwfIm != NULL) + { + free(pFFT->pTwfIm); + pFFT->pTwfIm = NULL; + } + + pFFT->m_numPoints = 0; + pFFT->m_numStages = 0; + +#ifdef FFTMsg + printf("\n%d-Point FFT object deleted.\n",pFFT->m_numPoints); +#endif +} + +/***************************************************************************/ +/* FFT */ +/* FAST-FOURIER-TRANSFORMATION s(t) -> S(f) */ +/***************************************************************************/ +void fft(FFT *pFFT, avfloat_t *in_re, avfloat_t *in_im) +{ +register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j; +avfloat_t tempr, tempi, s, c; + +#ifdef FFTMsg +UINT32 numMul, numAdd; +printf ("\nStart %d-Point FFT.\n\n",pFFT->m_numPoints); +#endif + + // Do the bit reversal + i2 = pFFT->m_numPoints >> 1; + j = 0; + for (i=0; i m_numPoints-1;i++) { + if (i < j) { + tempr = in_re[i]; + tempi = in_im[i]; + in_re[i] = in_re[j]; + in_im[i] = in_im[j]; + in_re[j] = tempr; + in_im[j] = tempi; + } + k = i2; + while (k <= j) { + j = j-k; + k >>= 1; + } + j = j+k; + } + + // Calculate the FFT + +#ifdef FFTMsg +numMul =0; +numAdd =0; +#endif + numNodesPerStage = pFFT->m_numPoints; + for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) { + k=0; + numNodesPerStage = numNodesPerStage/2; + opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage); + +#ifdef FFTMsg +printf ("Processing STAGE # %2d",stageCnt); +#endif + for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++) { + twf = 0; + opCnt = opsPerNode; + + while (opCnt) { +#ifdef FFTMsg + numMul++; + numAdd +=2; +#endif + c = pFFT->pTwfRe[twf]; + s = - pFFT->pTwfIm[twf]; + kk = k + opsPerNode; + tempr = (c * in_re[kk]) - (s * in_im[kk]); + tempi = (s * in_re[kk]) + (c * in_im[kk]); + in_re[kk] = in_re[k] - tempr; + in_im[kk] = in_im[k] - tempi; + in_re[k] = in_re[k] + tempr; + in_im[k] = in_im[k] + tempi; + k++; + opCnt--; + twf += numNodesPerStage; + } + + k += opsPerNode; + } +#ifdef FFTMsg +printf (" ... finished.\n"); +#endif + } + +#ifdef FFTMsg +printf ("\n%d-Point FFT is completed.\n",pFFT->m_numPoints); +printf ("Total number of complex additions = %u\n",numAdd); +printf ("Total number of complex multiplications = %u\n",numMul); +#endif +} + +/***************************************************************************/ +/* IFFT */ +/* INVERSE-FAST-FOURIER-TRANSFORMATION S(f) -> s(t) */ +/***************************************************************************/ +void ifft(FFT *pFFT, avfloat_t *in_re, avfloat_t *in_im) +{ +register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j; +avfloat_t tempr, tempi, s, c; + + #ifdef FFTMsg + printf ("\nStart %d-Point IFFT.\n\n",pFFT->m_numPoints); + #endif + + // Do the bit reversal + i2 = pFFT->m_numPoints >> 1; + j = 0; + for (i=0; i m_numPoints-1;i++) { + if (i < j) { + tempr = in_re[i]; + tempi = in_im[i]; + in_re[i] = in_re[j]; + in_im[i] = in_im[j]; + in_re[j] = tempr; + in_im[j] = tempi; + } + k = i2; + while (k <= j) { + j = j-k; + k >>= 1; + } + j = j+k; + } + + // Calculate the IFFT + numNodesPerStage = pFFT->m_numPoints; + for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) { + k=0; + numNodesPerStage = numNodesPerStage/2; + opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage); + + #ifdef FFTMsg + printf ("Processing STAGE # %2d",stageCnt); + #endif + + for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++) { + twf = 0; + opCnt = opsPerNode; + + while (opCnt) { + c = pFFT->pTwfRe[twf]; + s = pFFT->pTwfIm[twf]; + kk = k + opsPerNode; + tempr = (c * in_re[kk]) - (s * in_im[kk]); + tempi = (s * in_re[kk]) + (c * in_im[kk]); + in_re[kk] = in_re[k] - tempr; + in_im[kk] = in_im[k] - tempi; + in_re[k] = in_re[k] + tempr; + in_im[k] = in_im[k] + tempi; + k++; + opCnt--; + twf += numNodesPerStage; + } + k += opsPerNode; + } + #ifdef FFTMsg + printf (" ... finished.\n"); + #endif + } +#ifdef FFTMsg +printf ("\n%d-Point IFFT is completed.\n",pFFT->m_numPoints); +#endif +} + +/***************************************************************************/ +/* FFTS Autoscale Version X = 1/N * FFT{x} +/* FAST-FOURIER-TRANSFORMATION s(t) -> S(f) +/***************************************************************************/ +void ffts(FFT *pFFT, avfloat_t *in_re, avfloat_t *in_im) +{ +register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j; +avfloat_t tempr, tempi, s, c; + +#ifdef FFTMsg +UINT32 numMul, numAdd; +printf ("\nStart %d-Point FFT.\n\n",pFFT->m_numPoints); +#endif + + // Do the bit reversal + i2 = pFFT->m_numPoints >> 1; + j = 0; + for (i=0; i m_numPoints-1;i++) { + if (i < j) { + tempr = in_re[i]; + tempi = in_im[i]; + in_re[i] = in_re[j]; + in_im[i] = in_im[j]; + in_re[j] = tempr; + in_im[j] = tempi; + } + k = i2; + while (k <= j) { + j = j-k; + k >>= 1; + } + j = j+k; + } + + // Calculate the FFT + +#ifdef FFTMsg +numMul =0; +numAdd =0; +#endif + numNodesPerStage = pFFT->m_numPoints; + for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) { + k=0; + numNodesPerStage = numNodesPerStage/2; + opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage); + +#ifdef FFTMsg +printf ("Processing STAGE # %2d",stageCnt); +#endif + for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++) { + twf = 0; + opCnt = opsPerNode; + + while (opCnt) { +#ifdef FFTMsg + numMul++; + numAdd +=2; +#endif + c = pFFT->pTwfRe[twf]; + s = - pFFT->pTwfIm[twf]; + kk = k + opsPerNode; + tempr = (c * in_re[kk]) - (s * in_im[kk]); + tempi = (s * in_re[kk]) + (c * in_im[kk]); + in_re[kk] = (in_re[k] - tempr)/2; + in_im[kk] = (in_im[k] - tempi)/2; + in_re[k] = (in_re[k] + tempr)/2; + in_im[k] = (in_im[k] + tempi)/2; + k++; + opCnt--; + twf += numNodesPerStage; + } + + k += opsPerNode; + } +#ifdef FFTMsg +printf (" ... finished.\n"); +#endif + } + +#ifdef FFTMsg +printf ("\n%d-Point FFT is completed.\n",pFFT->m_numPoints); +printf ("Total number of complex additions = %u\n",numAdd); +printf ("Total number of complex multiplications = %u\n",numMul); +#endif +} + +/***************************************************************************/ +/* TWIDDLE-FAKTOR-TABLE */ +/* Erstellt Twiddle-Faktor-Tabelle von WN^0 bis WN^N/2 */ +/***************************************************************************/ +void FFTCalcTwiddleTable (avfloat_t *pRealData, avfloat_t *pImagData, UINT32 numPoints) +{ +UINT32 i, size; +avfloat_t arg1; + + size = numPoints/2; + arg1 = (avfloat_t)(2.0 * PI / (avfloat_t)numPoints ); + + #ifdef FFTMsg + printf ("Calculating and initializing Twiddle-Table (%d kB)...",size*sizeof(COMPLEX)); + #endif + + for (i = 0; i < size; i++) { + pRealData[i] = (avfloat_t)cos(arg1* (avfloat_t)i); + pImagData[i] = (avfloat_t)sin(arg1* (avfloat_t)i); + } + #ifdef FFTMsg + printf(" completed.\n"); + #endif +} + +/***************************************************************************/ +/* MODULUS */ +/* Berechnet den Betrag einer komplexen Zahl */ +/***************************************************************************/ +void Modulus(avfloat_t *pRealData, avfloat_t *pImagData, UINT32 N) +{ +UINT32 i; +avfloat_t temp; + + for (i = 0; i < N; i++) { + temp = (avfloat_t)sqrt( pRealData[i] * pRealData[i] + pImagData[i] * pImagData[i]); + pImagData[i] = (avfloat_t)atan2(pImagData[i],pRealData[i]); + pRealData[i] = temp; + } +} +/***************************************************************************/ +/* Hanning */ +/* Legt das Hanningfenster auf die Abtastwerte im Zeitbereich der Groesse N */ +/***************************************************************************/ +void Hanning(avfloat_t *pRealData, avfloat_t *pImagData, UINT32 N, UINT32 maximum) +{ + UINT32 n; + avfloat_t arg; + + for (n=0; n < N; n++) + { + arg = (avfloat_t)(2*PI*n /(N-1) - 2*PI*maximum/N); + pRealData[n] = pRealData[n] * (avfloat_t)(1 + cos(arg)) /2; + pImagData[n] = pImagData[n] * (avfloat_t)(1 + cos(arg)) /2; + } +} + +/***************************************************************************/ +/* HANNING_K */ +/* Gibt einen Faktor k in Abhängigkeit von n bezogen auf N zurück. */ +/***************************************************************************/ +avfloat_t hanning_k(UINT32 n, UINT32 N) +{ +avfloat_t arg; +arg = (avfloat_t)(2 * PI / (avfloat_t)N); + +return ((1-(avfloat_t)cos(arg*(avfloat_t)n))/2); +} + +/***************************************************************************/ +/* GAUSS_K */ +/* Gibt einen Faktor k in Abhängigkeit von n bezogen auf N zurück. */ +/***************************************************************************/ +avfloat_t gauss_k(UINT32 n, UINT32 m, UINT32 s) +{ +avfloat_t arg; +arg = (avfloat_t)((n-m)*(n-m)/(2*s*s)); + +return (avfloat_t)(exp(-arg)); +} + +/***************************************************************************/ +/* Normalize) */ +/* */ +/***************************************************************************/ +void Scale(avfloat_t *pRealData, avfloat_t *pImagData, avfloat_t scaleFactor, UINT32 N) +{ + UINT32 k; + + // Scaling Data + for (k=0; k < N; k++) + { + pRealData[k] *= (avfloat_t)scaleFactor; + pImagData[k] *= (avfloat_t)scaleFactor; + } +} + +/***************************************************************************/ +/* BiPower() +/* checks if N is power of 2 +/***************************************************************************/ +AVERR IsPowerOfTwo(UINT32 N) +{ + UINT32 i, iterations; + iterations = sizeof(UINT32)*8; + + if (N == 0) + return AV_E_FALSE; + + for (i=0; i pXfft = NULL; + pFFT->pYfft = NULL; + + pFFT->m_Nx = Nx; + pFFT->m_Ny = Ny; + + pFFT->pXfft = (struct _sFFT*)malloc(sizeof(pFFT->pXfft)); + FFTinit(pFFT->pXfft, Nx); + + if (Nx == Ny) + pFFT->pYfft = pFFT->pXfft; + + else + { + pFFT->pYfft = (struct _sFFT*)malloc(sizeof(pFFT->pYfft)); + FFTinit(pFFT->pYfft, Ny); + } +} + +/***************************************************************************/ +/* FFT2Dfree() +/* +/***************************************************************************/ +void FFT2Dfree(struct _sFFT2D *pFFT) +{ + if (pFFT->pXfft != NULL) + { + FFTfree(pFFT->pXfft); + pFFT->pXfft = NULL; + } + if (pFFT->pYfft != NULL) + { + FFTfree(pFFT->pYfft); + pFFT->pYfft = NULL; + } + + pFFT->m_Nx = 0; + pFFT->m_Ny = 0; +} + +/***************************************************************************/ +/* fft2d() +/* +/***************************************************************************/ +void fft2d(struct _sFFT2D *pFFT, avfloat_t **ppReal, avfloat_t **ppImag) +{ + avfloat_t *pTempr, *pTempi; + UINT32 i, row; + + + pTempr = (avfloat_t*)malloc(pFFT->m_Ny*sizeof(avfloat_t)); + pTempi = (avfloat_t*)malloc(pFFT->m_Ny*sizeof(avfloat_t)); + + /* Transform ROWs */ + for (row=0; row m_Ny; row++) + fft(pFFT->pXfft, ppReal[row], ppImag[row]); + + /* Transform COLs */ + for (row=0; row m_Nx; row++) + { + for(i=0; i m_Ny; i++) + { + pTempr[i] = ppReal[i][row]; + pTempi[i] = ppImag[i][row]; + } + + fft(pFFT->pYfft, pTempr, pTempi); + for(i=0; i m_Ny; i++) + { + ppReal[i][row] = pTempr[i]; + ppImag[i][row] = pTempi[i]; + } + } + free(pTempr); + free(pTempi); + +} + +/***************************************************************************/ +/* ifft2d() +/* +/***************************************************************************/ +void ifft2d(struct _sFFT2D *pFFT, avfloat_t **ppReal, avfloat_t **ppImag) +{ + avfloat_t *pTempr, *pTempi; + UINT32 i, row; + + + pTempr = (avfloat_t*)malloc(pFFT->m_Ny*sizeof(avfloat_t)); + pTempi = (avfloat_t*)malloc(pFFT->m_Ny*sizeof(avfloat_t)); + + /* Transform COLs */ + for (row=0; row m_Nx; row++) + { + for(i=0; i m_Ny; i++) + { + pTempr[i] = ppReal[i][row]; + pTempi[i] = ppImag[i][row]; + } + + ifft(pFFT->pYfft, pTempr, pTempi); + for(i=0; i m_Ny; i++) + { + ppReal[i][row] = pTempr[i]; + ppImag[i][row] = pTempi[i]; + } + } + free(pTempr); + free(pTempi); + + /* Transform ROWs */ + for (row=0; row m_Ny; row++) + ifft(pFFT->pXfft, ppReal[row], ppImag[row]); + +} + +/***************************************************************************/ +/* DFT() +/* DISKRETE FOURIER-TRANSFORMATION S(f) -> s(t) */ +/***************************************************************************/ +void DFT(avfloat_t *pDataR, avfloat_t *pDataI, UINT32 N) +{ + avfloat_t *a, *b; + avfloat_t phi, c, s; + UINT32 i, j; + + a = (avfloat_t*)malloc(N * sizeof(avfloat_t)); + b = (avfloat_t*)malloc(N * sizeof(avfloat_t)); + + memcpy((avfloat_t*)a,(avfloat_t*)pDataR,N * sizeof(avfloat_t)); + memcpy((avfloat_t*)b,(avfloat_t*)pDataI,N * sizeof(avfloat_t)); + + for (i = 0; i < N; i++) + { + pDataR[i] = 0; + pDataI[i] = 0; + + for (j = 0; j < N; j++) + { + phi = (avfloat_t)(2*i*j*PI/N); + c = (avfloat_t)(0.5*cos(phi)); + s = (avfloat_t)(0.5*sin(phi)); + + pDataR[i] += c*a[j] - s*b[j]; + pDataI[i] += s*a[j] + c*b[j]; + } + } + free(a); + free(b); + +} + +/***************************************************************************/ +/* IDFT() +/* INVERSE DISKRETE FOURIER-TRANSFORMATION S(f) -> s(t) */ +/***************************************************************************/ +void IDFT(avfloat_t *pDataR, avfloat_t *pDataI, UINT32 N) +{ + avfloat_t *a, *b; + avfloat_t phi, c, s; + UINT32 i, j; + + a = (avfloat_t*)malloc(N * sizeof(avfloat_t)); + b = (avfloat_t*)malloc(N * sizeof(avfloat_t)); + + memcpy((avfloat_t*)a,(avfloat_t*)pDataR,N * sizeof(avfloat_t)); + memcpy((avfloat_t*)b,(avfloat_t*)pDataI,N * sizeof(avfloat_t)); + + for (i = 0; i < N; i++) + { + pDataR[i] = 0; + pDataI[i] = 0; + + for (j = 0; j < N; j++) + { + phi = (avfloat_t)(2*i*j*PI/N); + c = (avfloat_t) (0.5*(avfloat_t)cos(phi)); + s = (avfloat_t)(-0.5*(avfloat_t)sin(phi)); + + pDataR[i] += c*a[j] - s*b[j]; + pDataI[i] += s*a[j] + c*b[j]; + } + } + free(a); + free(b); + +} \ No newline at end of file diff --git a/avfft.h b/avfft.h new file mode 100755 index 0000000..ea7f754 --- /dev/null +++ b/avfft.h @@ -0,0 +1,53 @@ +/***************************************************************************/ +/* FFT.H */ +/* Fast-Fourier-Transformation +/* Author: Jens Ahrensfeld */ +/* Datum : 24.06.1999 */ +/* letzte Änderung: 09.06.2000 */ +/***************************************************************************/ +/* Tabellen und Funktionen für die FFT */ +/***************************************************************************/ +#ifndef FFT_H +#define FFT_H + +#define PI 3.1415926535897932384626433832795 + +typedef struct _sFFT +{ + UINT32 m_numPoints, m_numStages; + avfloat_t *pTwfRe, *pTwfIm; +} FFT; + +typedef struct _sFFT2D +{ + struct _sFFT *pXfft, *pYfft; + UINT32 m_Nx, m_Ny; +} FFT2D; + +/* DFT functions */ +extern void DFT(avfloat_t *pDataR, avfloat_t *pDataI, UINT32 N); +extern void IDFT(avfloat_t *pDataR, avfloat_t *pDataI, UINT32 N); + +/* FFT functions */ +extern void FFTinit(FFT *pFFT, UINT32 N); +extern void FFTfree(FFT *pFFT); +extern void fft(FFT *pFFT, avfloat_t*, avfloat_t*); +extern void ifft(FFT *pFFT, avfloat_t*, avfloat_t*); +extern void ffts(FFT *pFFT, avfloat_t *in_re, avfloat_t *in_im); +extern void FFTCalcTwiddleTable (avfloat_t *pRealData, avfloat_t *pImagData, UINT32 numPoints); + +/* 2D FFT functions */ +extern void FFT2Dinit(struct _sFFT2D *pFFT, UINT32 Nx, UINT32 Ny); +extern void FFT2Dfree(struct _sFFT2D *pFFT); +extern void fft2d(struct _sFFT2D *pFFT, avfloat_t **ppReal, avfloat_t **ppImag); +extern void ifft2d(struct _sFFT2D *pFFT, avfloat_t **ppReal, avfloat_t **ppImag); + +/* Helper functions */ +extern void Scale(avfloat_t *pRealData, avfloat_t *pImagData , avfloat_t scaleFactor, UINT32 N); +extern void Modulus(avfloat_t *pRealData, avfloat_t *pImagData, UINT32 N); +extern void Hanning (avfloat_t *pRealData, avfloat_t *pImagData, UINT32 N, UINT32 maximum); +extern avfloat_t hanning_k (UINT32, UINT32); +extern avfloat_t gauss_k(UINT32, UINT32, UINT32); +extern AVERR IsPowerOfTwo(UINT32 N); + +#endif /* FFT_H */ diff --git a/fft.c b/fft.c new file mode 100755 index 0000000..7c380ea --- /dev/null +++ b/fft.c @@ -0,0 +1,627 @@ +/***************************************************************************/ +/* FFT.C +/* Fast-Fourier-Transformation +/* Author: Jens Ahrensfeld +/* Datum : 24.06.1999 +/* letzte Änderung: 09.06.2000 +/***************************************************************************/ +#define AVNEEDFLOAT + +#include +#include +#include + +#define PI 3.1415926535897932384626433832795 + +/***************************************************************************/ +/* Konstruktor FFT-Objekt */ +/* */ +/***************************************************************************/ +void FFTinit(fft_t *pFFT, UINT32 N) +{ + pFFT->m_numPoints = 0; + pFFT->m_numStages = 0; + + pFFT->pTwfRe = NULL; + pFFT->pTwfIm = NULL; + + if(IsPowerOfTwo(N) == FFT_ERROR) + { + printf("\n%u is not a power of 2!\n",N); + exit(1); + } + + pFFT->m_numStages = (UINT32) (1e-06 + log((fft_float_t)N)/log(2.0)); + pFFT->m_numPoints = (UINT32 )N; + + pFFT->pTwfRe = (fft_float_t*)malloc(pFFT->m_numPoints * sizeof(fft_float_t) /2); + pFFT->pTwfIm = (fft_float_t*)malloc(pFFT->m_numPoints * sizeof(fft_float_t) /2); + + FFTCalcTwiddleTable(pFFT->pTwfRe, pFFT->pTwfIm, pFFT->m_numPoints); + +#ifdef FFTMsg + printf("\n%d-Point FFT object created.\n",pFFT->m_numPoints); +#endif +} +/***************************************************************************/ +/* Destruktor FFT-Objekt */ +/* */ +/***************************************************************************/ +void FFTfree(fft_t *pFFT) +{ + if (pFFT->pTwfRe != NULL) + { + free(pFFT->pTwfRe); + pFFT->pTwfRe = NULL; + } + + if (pFFT->pTwfIm != NULL) + { + free(pFFT->pTwfIm); + pFFT->pTwfIm = NULL; + } + + pFFT->m_numPoints = 0; + pFFT->m_numStages = 0; + +#ifdef FFTMsg + printf("\n%d-Point FFT object deleted.\n",pFFT->m_numPoints); +#endif +} + +/***************************************************************************/ +/* FFT */ +/* FAST-FOURIER-TRANSFORMATION s(t) -> S(f) */ +/***************************************************************************/ +void fft(fft_t *pFFT, fft_float_t *in_re, fft_float_t *in_im) +{ +register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j; +fft_float_t tempr, tempi, s, c; + +#ifdef FFTMsg +UINT32 numMul, numAdd; +printf ("\nStart %d-Point FFT.\n\n",pFFT->m_numPoints); +#endif + + // Do the bit reversal + i2 = pFFT->m_numPoints >> 1; + j = 0; + for (i=0; i m_numPoints-1;i++) { + if (i < j) { + tempr = in_re[i]; + tempi = in_im[i]; + in_re[i] = in_re[j]; + in_im[i] = in_im[j]; + in_re[j] = tempr; + in_im[j] = tempi; + } + k = i2; + while (k <= j) { + j = j-k; + k >>= 1; + } + j = j+k; + } + + // Calculate the FFT + +#ifdef FFTMsg +numMul =0; +numAdd =0; +#endif + numNodesPerStage = pFFT->m_numPoints; + for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) { + k=0; + numNodesPerStage = numNodesPerStage/2; + opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage); + +#ifdef FFTMsg +printf ("Processing STAGE # %2d",stageCnt); +#endif + for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++) { + twf = 0; + opCnt = opsPerNode; + + while (opCnt) { +#ifdef FFTMsg + numMul++; + numAdd +=2; +#endif + c = pFFT->pTwfRe[twf]; + s = - pFFT->pTwfIm[twf]; + kk = k + opsPerNode; + tempr = (c * in_re[kk]) - (s * in_im[kk]); + tempi = (s * in_re[kk]) + (c * in_im[kk]); + in_re[kk] = in_re[k] - tempr; + in_im[kk] = in_im[k] - tempi; + in_re[k] = in_re[k] + tempr; + in_im[k] = in_im[k] + tempi; + k++; + opCnt--; + twf += numNodesPerStage; + } + + k += opsPerNode; + } +#ifdef FFTMsg +printf (" ... finished.\n"); +#endif + } + +#ifdef FFTMsg +printf ("\n%d-Point FFT is completed.\n",pFFT->m_numPoints); +printf ("Total number of complex additions = %u\n",numAdd); +printf ("Total number of complex multiplications = %u\n",numMul); +#endif +} + +/***************************************************************************/ +/* IFFT */ +/* INVERSE-FAST-FOURIER-TRANSFORMATION S(f) -> s(t) */ +/***************************************************************************/ +void ifft(fft_t *pFFT, fft_float_t *in_re, fft_float_t *in_im) +{ +register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j; +fft_float_t tempr, tempi, s, c; + + #ifdef FFTMsg + printf ("\nStart %d-Point IFFT.\n\n",pFFT->m_numPoints); + #endif + + // Do the bit reversal + i2 = pFFT->m_numPoints >> 1; + j = 0; + for (i=0; i m_numPoints-1;i++) { + if (i < j) { + tempr = in_re[i]; + tempi = in_im[i]; + in_re[i] = in_re[j]; + in_im[i] = in_im[j]; + in_re[j] = tempr; + in_im[j] = tempi; + } + k = i2; + while (k <= j) { + j = j-k; + k >>= 1; + } + j = j+k; + } + + // Calculate the IFFT + numNodesPerStage = pFFT->m_numPoints; + for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) { + k=0; + numNodesPerStage = numNodesPerStage/2; + opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage); + + #ifdef FFTMsg + printf ("Processing STAGE # %2d",stageCnt); + #endif + + for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++) { + twf = 0; + opCnt = opsPerNode; + + while (opCnt) { + c = pFFT->pTwfRe[twf]; + s = pFFT->pTwfIm[twf]; + kk = k + opsPerNode; + tempr = (c * in_re[kk]) - (s * in_im[kk]); + tempi = (s * in_re[kk]) + (c * in_im[kk]); + in_re[kk] = in_re[k] - tempr; + in_im[kk] = in_im[k] - tempi; + in_re[k] = in_re[k] + tempr; + in_im[k] = in_im[k] + tempi; + k++; + opCnt--; + twf += numNodesPerStage; + } + k += opsPerNode; + } + #ifdef FFTMsg + printf (" ... finished.\n"); + #endif + } +#ifdef FFTMsg +printf ("\n%d-Point IFFT is completed.\n",pFFT->m_numPoints); +#endif +} + +/***************************************************************************/ +/* FFTS Autoscale Version X = 1/N * FFT{x} +/* FAST-FOURIER-TRANSFORMATION s(t) -> S(f) +/***************************************************************************/ +void ffts(fft_t *pFFT, fft_float_t *in_re, fft_float_t *in_im) +{ +register UINT32 stageCnt, k, kk, twf, numNodesPerStage, nodeCnt, opsPerNode, opCnt, i, i2, j; +fft_float_t tempr, tempi, s, c; + +#ifdef FFTMsg +UINT32 numMul, numAdd; +printf ("\nStart %d-Point FFT.\n\n",pFFT->m_numPoints); +#endif + + // Do the bit reversal + i2 = pFFT->m_numPoints >> 1; + j = 0; + for (i=0; i m_numPoints-1;i++) { + if (i < j) { + tempr = in_re[i]; + tempi = in_im[i]; + in_re[i] = in_re[j]; + in_im[i] = in_im[j]; + in_re[j] = tempr; + in_im[j] = tempi; + } + k = i2; + while (k <= j) { + j = j-k; + k >>= 1; + } + j = j+k; + } + + // Calculate the FFT + +#ifdef FFTMsg +numMul =0; +numAdd =0; +#endif + numNodesPerStage = pFFT->m_numPoints; + for (stageCnt=1; stageCnt <= pFFT->m_numStages; stageCnt++) { + k=0; + numNodesPerStage = numNodesPerStage/2; + opsPerNode = pFFT->m_numPoints/(2*numNodesPerStage); + +#ifdef FFTMsg +printf ("Processing STAGE # %2d",stageCnt); +#endif + for (nodeCnt=1; nodeCnt <= numNodesPerStage; nodeCnt++) { + twf = 0; + opCnt = opsPerNode; + + while (opCnt) { +#ifdef FFTMsg + numMul++; + numAdd +=2; +#endif + c = pFFT->pTwfRe[twf]; + s = - pFFT->pTwfIm[twf]; + kk = k + opsPerNode; + tempr = (c * in_re[kk]) - (s * in_im[kk]); + tempi = (s * in_re[kk]) + (c * in_im[kk]); + in_re[kk] = (in_re[k] - tempr)/2; + in_im[kk] = (in_im[k] - tempi)/2; + in_re[k] = (in_re[k] + tempr)/2; + in_im[k] = (in_im[k] + tempi)/2; + k++; + opCnt--; + twf += numNodesPerStage; + } + + k += opsPerNode; + } +#ifdef FFTMsg +printf (" ... finished.\n"); +#endif + } + +#ifdef FFTMsg +printf ("\n%d-Point FFT is completed.\n",pFFT->m_numPoints); +printf ("Total number of complex additions = %u\n",numAdd); +printf ("Total number of complex multiplications = %u\n",numMul); +#endif +} + +/***************************************************************************/ +/* TWIDDLE-FAKTOR-TABLE */ +/* Erstellt Twiddle-Faktor-Tabelle von WN^0 bis WN^N/2 */ +/***************************************************************************/ +void FFTCalcTwiddleTable (fft_float_t *pRealData, fft_float_t *pImagData, UINT32 numPoints) +{ +UINT32 i, size; +fft_float_t arg1; + + size = numPoints/2; + arg1 = (fft_float_t)(2.0 * PI / (fft_float_t)numPoints ); + + #ifdef FFTMsg + printf ("Calculating and initializing Twiddle-Table (%d kB)...",size*sizeof(COMPLEX)); + #endif + + for (i = 0; i < size; i++) { + pRealData[i] = (fft_float_t)cos(arg1* (fft_float_t)i); + pImagData[i] = (fft_float_t)sin(arg1* (fft_float_t)i); + } + #ifdef FFTMsg + printf(" completed.\n"); + #endif +} + +/***************************************************************************/ +/* MODULUS */ +/* Berechnet den Betrag einer komplexen Zahl */ +/***************************************************************************/ +void Modulus(fft_float_t *pRealData, fft_float_t *pImagData, UINT32 N) +{ +UINT32 i; +fft_float_t temp; + + for (i = 0; i < N; i++) { + temp = (fft_float_t)sqrt( pRealData[i] * pRealData[i] + pImagData[i] * pImagData[i]); + pImagData[i] = (fft_float_t)atan2(pImagData[i],pRealData[i]); + pRealData[i] = temp; + } +} +/***************************************************************************/ +/* Hanning */ +/* Legt das Hanningfenster auf die Abtastwerte im Zeitbereich der Groesse N */ +/***************************************************************************/ +void Hanning(fft_float_t *pRealData, fft_float_t *pImagData, UINT32 N, UINT32 maximum) +{ + UINT32 n; + fft_float_t arg; + + for (n=0; n < N; n++) + { + arg = (fft_float_t)(2*PI*n /(N-1) - 2*PI*maximum/N); + pRealData[n] = pRealData[n] * (fft_float_t)(1 + cos(arg)) /2; + pImagData[n] = pImagData[n] * (fft_float_t)(1 + cos(arg)) /2; + } +} + +/***************************************************************************/ +/* HANNING_K */ +/* Gibt einen Faktor k in Abhängigkeit von n bezogen auf N zurück. */ +/***************************************************************************/ +fft_float_t hanning_k(UINT32 n, UINT32 N) +{ +fft_float_t arg; +arg = (fft_float_t)(2 * PI / (fft_float_t)N); + +return ((1-(fft_float_t)cos(arg*(fft_float_t)n))/2); +} + +/***************************************************************************/ +/* GAUSS_K */ +/* Gibt einen Faktor k in Abhängigkeit von n bezogen auf N zurück. */ +/***************************************************************************/ +fft_float_t gauss_k(UINT32 n, UINT32 m, UINT32 s) +{ +fft_float_t arg; +arg = (fft_float_t)((n-m)*(n-m)/(2*s*s)); + +return (fft_float_t)(exp(-arg)); +} + +/***************************************************************************/ +/* Normalize) */ +/* */ +/***************************************************************************/ +void Scale(fft_float_t *pRealData, fft_float_t *pImagData, fft_float_t scaleFactor, UINT32 N) +{ + UINT32 k; + + // Scaling Data + for (k=0; k < N; k++) + { + pRealData[k] *= (fft_float_t)scaleFactor; + pImagData[k] *= (fft_float_t)scaleFactor; + } +} + +/***************************************************************************/ +/* BiPower() +/* checks if N is power of 2 +/***************************************************************************/ +UINT32 IsPowerOfTwo(UINT32 N) +{ + UINT32 i, iterations; + iterations = sizeof(UINT32)*8; + + if (N == 0) + return AV_E_FALSE; + + for (i=0; i pXfft = NULL; + pFFT->pYfft = NULL; + + pFFT->m_Nx = Nx; + pFFT->m_Ny = Ny; + + pFFT->pXfft = (fft_t*)malloc(sizeof(pFFT->pXfft)); + FFTinit(pFFT->pXfft, Nx); + + if (Nx == Ny) + pFFT->pYfft = pFFT->pXfft; + + else + { + pFFT->pYfft = (fft_t*)malloc(sizeof(pFFT->pYfft)); + FFTinit(pFFT->pYfft, Ny); + } +} + +/***************************************************************************/ +/* FFT2Dfree() +/* +/***************************************************************************/ +void FFT2Dfree(fft2_t *pFFT) +{ + if (pFFT->pXfft != NULL) + { + FFTfree(pFFT->pXfft); + pFFT->pXfft = NULL; + } + if (pFFT->pYfft != NULL) + { + FFTfree(pFFT->pYfft); + pFFT->pYfft = NULL; + } + + pFFT->m_Nx = 0; + pFFT->m_Ny = 0; +} + +/***************************************************************************/ +/* fft2d() +/* +/***************************************************************************/ +void fft2d(fft2_t *pFFT, fft_float_t **ppReal, fft_float_t **ppImag) +{ + fft_float_t *pTempr, *pTempi; + UINT32 i, row; + + + pTempr = (fft_float_t*)malloc(pFFT->m_Ny*sizeof(fft_float_t)); + pTempi = (fft_float_t*)malloc(pFFT->m_Ny*sizeof(fft_float_t)); + + /* Transform ROWs */ + for (row=0; row m_Ny; row++) + fft(pFFT->pXfft, ppReal[row], ppImag[row]); + + /* Transform COLs */ + for (row=0; row m_Nx; row++) + { + for(i=0; i m_Ny; i++) + { + pTempr[i] = ppReal[i][row]; + pTempi[i] = ppImag[i][row]; + } + + fft(pFFT->pYfft, pTempr, pTempi); + for(i=0; i m_Ny; i++) + { + ppReal[i][row] = pTempr[i]; + ppImag[i][row] = pTempi[i]; + } + } + free(pTempr); + free(pTempi); + +} + +/***************************************************************************/ +/* ifft2d() +/* +/***************************************************************************/ +void ifft2d(fft2_t *pFFT, fft_float_t **ppReal, fft_float_t **ppImag) +{ + fft_float_t *pTempr, *pTempi; + UINT32 i, row; + + + pTempr = (fft_float_t*)malloc(pFFT->m_Ny*sizeof(fft_float_t)); + pTempi = (fft_float_t*)malloc(pFFT->m_Ny*sizeof(fft_float_t)); + + /* Transform COLs */ + for (row=0; row m_Nx; row++) + { + for(i=0; i m_Ny; i++) + { + pTempr[i] = ppReal[i][row]; + pTempi[i] = ppImag[i][row]; + } + + ifft(pFFT->pYfft, pTempr, pTempi); + for(i=0; i m_Ny; i++) + { + ppReal[i][row] = pTempr[i]; + ppImag[i][row] = pTempi[i]; + } + } + free(pTempr); + free(pTempi); + + /* Transform ROWs */ + for (row=0; row m_Ny; row++) + ifft(pFFT->pXfft, ppReal[row], ppImag[row]); + +} + +/***************************************************************************/ +/* DFT() +/* DISKRETE FOURIER-TRANSFORMATION S(f) -> s(t) */ +/***************************************************************************/ +void DFT(fft_float_t *pDataR, fft_float_t *pDataI, UINT32 N) +{ + fft_float_t *a, *b; + fft_float_t phi, c, s; + UINT32 i, j; + + a = (fft_float_t*)malloc(N * sizeof(fft_float_t)); + b = (fft_float_t*)malloc(N * sizeof(fft_float_t)); + + memcpy((fft_float_t*)a,(fft_float_t*)pDataR,N * sizeof(fft_float_t)); + memcpy((fft_float_t*)b,(fft_float_t*)pDataI,N * sizeof(fft_float_t)); + + for (i = 0; i < N; i++) + { + pDataR[i] = 0; + pDataI[i] = 0; + + for (j = 0; j < N; j++) + { + phi = (fft_float_t)(2*i*j*PI/N); + c = (fft_float_t)(0.5*cos(phi)); + s = (fft_float_t)(0.5*sin(phi)); + + pDataR[i] += c*a[j] - s*b[j]; + pDataI[i] += s*a[j] + c*b[j]; + } + } + free(a); + free(b); + +} + +/***************************************************************************/ +/* IDFT() +/* INVERSE DISKRETE FOURIER-TRANSFORMATION S(f) -> s(t) */ +/***************************************************************************/ +void IDFT(fft_float_t *pDataR, fft_float_t *pDataI, UINT32 N) +{ + fft_float_t *a, *b; + fft_float_t phi, c, s; + UINT32 i, j; + + a = (fft_float_t*)malloc(N * sizeof(fft_float_t)); + b = (fft_float_t*)malloc(N * sizeof(fft_float_t)); + + memcpy((fft_float_t*)a,(fft_float_t*)pDataR,N * sizeof(fft_float_t)); + memcpy((fft_float_t*)b,(fft_float_t*)pDataI,N * sizeof(fft_float_t)); + + for (i = 0; i < N; i++) + { + pDataR[i] = 0; + pDataI[i] = 0; + + for (j = 0; j < N; j++) + { + phi = (fft_float_t)(2*i*j*PI/N); + c = (fft_float_t) (0.5*(fft_float_t)cos(phi)); + s = (fft_float_t)(-0.5*(fft_float_t)sin(phi)); + + pDataR[i] += c*a[j] - s*b[j]; + pDataI[i] += s*a[j] + c*b[j]; + } + } + free(a); + free(b); + +} \ No newline at end of file diff --git a/fft.h b/fft.h new file mode 100755 index 0000000..535912c --- /dev/null +++ b/fft.h @@ -0,0 +1,56 @@ +/***************************************************************************/ +/* FFT.H */ +/* Fast-Fourier-Transformation +/* Author: Jens Ahrensfeld */ +/* Datum : 24.06.1999 */ +/* letzte Änderung: 09.06.2000 */ +/***************************************************************************/ +/* Tabellen und Funktionen für die FFT */ +/***************************************************************************/ +#ifndef FFT_H +#define FFT_H + +#define PI 3.1415926535897932384626433832795 + +#define fft_float_t FLOAT32 +#define FFT_ERROR 0x80000000 + +typedef struct _sfft_t +{ + UINT32 m_numPoints, m_numStages; + fft_float_t *pTwfRe, *pTwfIm; +} fft_t; + +typedef struct _sfft2_t +{ + fft_t *pXfft, *pYfft; + UINT32 m_Nx, m_Ny; +} fft2_t; + +/* DFT functions */ +extern void DFT(fft_float_t *pDataR, fft_float_t *pDataI, UINT32 N); +extern void IDFT(fft_float_t *pDataR, fft_float_t *pDataI, UINT32 N); + +/* FFT functions */ +extern void FFTinit(fft_t *pFFT, UINT32 N); +extern void FFTfree(fft_t *pFFT); +extern void fft(fft_t *pFFT, fft_float_t*, fft_float_t*); +extern void ifft(fft_t *pFFT, fft_float_t*, fft_float_t*); +extern void ffts(fft_t *pFFT, fft_float_t *in_re, fft_float_t *in_im); +extern void FFTCalcTwiddleTable (fft_float_t *pRealData, fft_float_t *pImagData, UINT32 numPoints); + +/* 2D FFT functions */ +extern void FFT2Dinit(fft2_t *pFFT, UINT32 Nx, UINT32 Ny); +extern void FFT2Dfree(fft2_t *pFFT); +extern void fft2d(fft2_t *pFFT, fft_float_t **ppReal, fft_float_t **ppImag); +extern void ifft2d(fft2_t *pFFT, fft_float_t **ppReal, fft_float_t **ppImag); + +/* Helper functions */ +extern void Scale(fft_float_t *pRealData, fft_float_t *pImagData , fft_float_t scaleFactor, UINT32 N); +extern void Modulus(fft_float_t *pRealData, fft_float_t *pImagData, UINT32 N); +extern void Hanning (fft_float_t *pRealData, fft_float_t *pImagData, UINT32 N, UINT32 maximum); +extern fft_float_t hanning_k (UINT32, UINT32); +extern fft_float_t gauss_k(UINT32, UINT32, UINT32); +extern UINT32 IsPowerOfTwo(UINT32 N); + +#endif /* FFT_H */