cannam@0: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
cannam@0: 
cannam@0: /*
cannam@0:     QM DSP Library
cannam@0: 
cannam@0:     Centre for Digital Music, Queen Mary, University of London.
cannam@0:     This file is based on Don Cross's public domain FFT implementation.
cannam@0: */
cannam@0: 
cannam@0: #include "FFT.h"
cannam@55: 
cannam@55: #include "maths/MathUtilities.h"
cannam@55: 
cannam@0: #include <cmath>
cannam@0: 
cannam@55: #include <iostream>
cannam@55: 
Chris@114: FFT::FFT(int n) :
Chris@114:     m_n(n)
cannam@0: {
cannam@64:     if( !MathUtilities::isPowerOfTwo(m_n) )
cannam@64:     {
cannam@64:         std::cerr << "ERROR: FFT: Non-power-of-two FFT size "
cannam@64:                   << m_n << " not supported in this implementation"
cannam@64:                   << std::endl;
cannam@64: 	return;
cannam@64:     }
cannam@0: }
cannam@0: 
cannam@0: FFT::~FFT()
cannam@0: {
cannam@0: 
cannam@0: }
cannam@0: 
Chris@114: FFTReal::FFTReal(int n) :
cannam@64:     m_n(n),
Chris@114:     m_fft(new FFT(n)),
Chris@114:     m_r(new double[n]),
Chris@114:     m_i(new double[n]),
Chris@114:     m_discard(new double[n])
cannam@0: {
Chris@114:     for (int i = 0; i < n; ++i) {
Chris@114:         m_r[i] = 0;
Chris@114:         m_i[i] = 0;
Chris@114:         m_discard[i] = 0;
Chris@114:     }
cannam@64: }
cannam@0: 
cannam@64: FFTReal::~FFTReal()
cannam@64: {
Chris@114:     delete m_fft;
Chris@114:     delete[] m_discard;
Chris@114:     delete[] m_r;
Chris@114:     delete[] m_i;
cannam@64: }
cannam@64: 
cannam@64: void
Chris@114: FFTReal::forward(const double *realIn,
cannam@64:                  double *realOut, double *imagOut)
cannam@64: {
Chris@114:     m_fft->process(false, realIn, 0, realOut, imagOut);
cannam@64: }
cannam@64: 
Chris@114: void
Chris@114: FFTReal::inverse(const double *realIn, const double *imagIn,
Chris@114:                  double *realOut)
Chris@114: {
Chris@114:     for (int i = 0; i < m_n/2 + 1; ++i) {
Chris@114:         m_r[i] = realIn[i];
Chris@114:         m_i[i] = imagIn[i];
Chris@114:         if (i > 0 && i < m_n/2) {
Chris@114:             m_r[m_n - i] = realIn[i];
Chris@114:             m_i[m_n - i] = -imagIn[i];
Chris@114:         }
Chris@114:     }
Chris@114:     m_fft->process(true, m_r, m_i, realOut, m_discard);
Chris@114: }
Chris@114: 
Chris@114: static int numberOfBitsNeeded(int p_nSamples)
cannam@64: {	
cannam@64:     int i;
cannam@64: 
cannam@64:     if( p_nSamples < 2 )
cannam@64:     {
cannam@64: 	return 0;
cannam@64:     }
cannam@64: 
cannam@64:     for ( i=0; ; i++ )
cannam@64:     {
cannam@64: 	if( p_nSamples & (1 << i) ) return i;
cannam@64:     }
cannam@64: }
cannam@64: 
Chris@114: static int reverseBits(int p_nIndex, int p_nBits)
cannam@64: {
Chris@114:     int i, rev;
cannam@64: 
cannam@64:     for(i=rev=0; i < p_nBits; i++)
cannam@64:     {
cannam@64: 	rev = (rev << 1) | (p_nIndex & 1);
cannam@64: 	p_nIndex >>= 1;
cannam@64:     }
cannam@64: 
cannam@64:     return rev;
cannam@64: }
cannam@64: 
cannam@64: void
cannam@64: FFT::process(bool p_bInverseTransform,
cannam@64:              const double *p_lpRealIn, const double *p_lpImagIn,
cannam@64:              double *p_lpRealOut, double *p_lpImagOut)
cannam@64: {
cannam@66:     if (!p_lpRealIn || !p_lpRealOut || !p_lpImagOut) return;
cannam@66: 
cannam@66: //    std::cerr << "FFT::process(" << m_n << "," << p_bInverseTransform << ")" << std::endl;
cannam@0: 
Chris@114:     int NumBits;
Chris@114:     int i, j, k, n;
Chris@114:     int BlockSize, BlockEnd;
cannam@0: 
cannam@0:     double angle_numerator = 2.0 * M_PI;
cannam@0:     double tr, ti;
cannam@0: 
cannam@64:     if( !MathUtilities::isPowerOfTwo(m_n) )
cannam@0:     {
cannam@55:         std::cerr << "ERROR: FFT::process: Non-power-of-two FFT size "
cannam@64:                   << m_n << " not supported in this implementation"
cannam@55:                   << std::endl;
cannam@0: 	return;
cannam@0:     }
cannam@0: 
cannam@0:     if( p_bInverseTransform ) angle_numerator = -angle_numerator;
cannam@0: 
cannam@64:     NumBits = numberOfBitsNeeded ( m_n );
cannam@0: 
cannam@0: 
Chris@114: 
cannam@64:     for( i=0; i < m_n; i++ )
cannam@0:     {
cannam@0: 	j = reverseBits ( i, NumBits );
cannam@0: 	p_lpRealOut[j] = p_lpRealIn[i];
cannam@0: 	p_lpImagOut[j] = (p_lpImagIn == 0) ? 0.0 : p_lpImagIn[i];
cannam@0:     }
cannam@0: 
cannam@0: 
cannam@0:     BlockEnd = 1;
cannam@64:     for( BlockSize = 2; BlockSize <= m_n; BlockSize <<= 1 )
cannam@0:     {
cannam@0: 	double delta_angle = angle_numerator / (double)BlockSize;
cannam@0: 	double sm2 = -sin ( -2 * delta_angle );
cannam@0: 	double sm1 = -sin ( -delta_angle );
cannam@0: 	double cm2 = cos ( -2 * delta_angle );
cannam@0: 	double cm1 = cos ( -delta_angle );
cannam@0: 	double w = 2 * cm1;
cannam@0: 	double ar[3], ai[3];
cannam@0: 
cannam@64: 	for( i=0; i < m_n; i += BlockSize )
cannam@0: 	{
cannam@0: 
cannam@0: 	    ar[2] = cm2;
cannam@0: 	    ar[1] = cm1;
cannam@0: 
cannam@0: 	    ai[2] = sm2;
cannam@0: 	    ai[1] = sm1;
cannam@0: 
cannam@0: 	    for ( j=i, n=0; n < BlockEnd; j++, n++ )
cannam@0: 	    {
cannam@0: 
cannam@0: 		ar[0] = w*ar[1] - ar[2];
cannam@0: 		ar[2] = ar[1];
cannam@0: 		ar[1] = ar[0];
cannam@0: 
cannam@0: 		ai[0] = w*ai[1] - ai[2];
cannam@0: 		ai[2] = ai[1];
cannam@0: 		ai[1] = ai[0];
cannam@0: 
cannam@0: 		k = j + BlockEnd;
cannam@0: 		tr = ar[0]*p_lpRealOut[k] - ai[0]*p_lpImagOut[k];
cannam@0: 		ti = ar[0]*p_lpImagOut[k] + ai[0]*p_lpRealOut[k];
cannam@0: 
cannam@0: 		p_lpRealOut[k] = p_lpRealOut[j] - tr;
cannam@0: 		p_lpImagOut[k] = p_lpImagOut[j] - ti;
cannam@0: 
cannam@0: 		p_lpRealOut[j] += tr;
cannam@0: 		p_lpImagOut[j] += ti;
cannam@0: 
cannam@0: 	    }
cannam@0: 	}
cannam@0: 
cannam@0: 	BlockEnd = BlockSize;
cannam@0: 
cannam@0:     }
cannam@0: 
cannam@0: 
cannam@0:     if( p_bInverseTransform )
cannam@0:     {
cannam@64: 	double denom = (double)m_n;
cannam@0: 
cannam@64: 	for ( i=0; i < m_n; i++ )
cannam@0: 	{
cannam@0: 	    p_lpRealOut[i] /= denom;
cannam@0: 	    p_lpImagOut[i] /= denom;
cannam@0: 	}
cannam@0:     }
cannam@0: }
cannam@0: