///////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2006-2008 Ernest Laurentin (http://www.ernzo.com/)
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
//
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
//
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
//
// 3. This notice may not be removed or altered from any source
// distribution.
//
// File: FFT.hpp
// Version: 1.0
///////////////////////////////////////////////////////////////////////////////
// Based on 1998 original version by: Don Cross <dcross@intersrv.com>
///////////////////////////////////////////////////////////////////////////////
#ifndef FFT_HPP
#define FFT_HPP
#include <exception>
#include <math.h>
// Ernzo::DSP
namespace Ernzo { namespace DSP {
static const double DDC_PI = 3.14159265358979323846;
/**
* FFT class - static implementation only
* based on 1998 original version by: Don Cross <dcross@intersrv.com>
*/
class FFT
{
public:
/**
* Verifies a number is a power of two
* @param x Number to check
* @return true if number is a power two (i.e.:1,2,4,8,16,...)
*/
static bool IsPowerOfTwo ( size_t x )
{
return ((x != 0) && (x & (x-1)) == 0);
}
/**
* Get Next power of number.
* @param x Number to check
* @return A power of two number
*/
static size_t NextPowerOfTwo(size_t x)
{
x = x - 1;
x = x | (x >> 1);
x = x | (x >> 2);
x = x | (x >> 4);
x = x | (x >> 8);
x = x | (x >> 16);
return x + 1;
}
/**
* Get Number of bits needed for a power of two
* @param PowerOfTwo Power of two number
* @return Number of bits
*/
static size_t NumberOfBitsNeeded( size_t PowerOfTwo )
{
if ( PowerOfTwo > 0 )
{
for(size_t i = 0; ; i++ )
{
if ( PowerOfTwo & (1 << i) )
return i;
}
}
return 0; // error
}
/**
* Reverse bits
* @param index Bits
* @param NumBits Number of bits to reverse
* @return Reverse Bits
*/
static size_t ReverseBits( size_t index, size_t NumBits )
{
size_t i, rev;
for ( i=rev=0; i < NumBits; i++ )
{
rev = (rev << 1) | (index & 1);
index >>= 1;
}
return rev;
}
/**
* Return index to frequency based on number of samples
* @param Index sample index
* @param NumSamples number of samples
* @return Frequency index range
*/
static double IndexToFrequency( size_t Index, size_t NumSamples )
{
if ( Index >= NumSamples )
return 0.0;
else if ( Index <= NumSamples/2 )
return (double)Index / (double)NumSamples;
return -(double)(NumSamples-Index) / (double)NumSamples;
}
template<typename T>
static void Compute(size_t NumSamples, T *pRealIn, T *pImagIn,
T *pRealOut, T *pImagOut, bool bInverseTransform = false);
template<typename T>
static void Norm(size_t NumSamples, T *pReal, T *pImag, T* pAmpl);
template<typename T>
static double PeakFrequency(size_t NumSamples, const T *pAmpl, double samplingRate, size_t& index);
};
/**
* Compute FFT
* @param NumSamples Number of samples (must be power two)
* @param pRealIn Real samples
* @param pImagIn Imaginary (optional, may be NULL)
* @param pRealOut Real coefficient output
* @param pImagOut Imaginary coefficient output
* @param bInverseTransform when true, compute Inverse FFT
*/
template<typename T>
void FFT::Compute(size_t NumSamples, T *pRealIn, T *pImagIn,
T *pRealOut, T *pImagOut, bool bInverseTransform)
{
size_t NumBits; /* Number of bits needed to store indices */
size_t i, j, k, n;
size_t BlockSize, BlockEnd;
double angle_numerator = 2.0 * DDC_PI;
double tr, ti; /* temp real, temp imaginary */
if ( !pRealIn || !pRealOut || !pImagOut )
{
// error
throw std::exception("Invalid argument");
}
if ( !IsPowerOfTwo(NumSamples) )
{
// error
throw std::exception("Number of samples must be power of 2");
}
if ( bInverseTransform )
angle_numerator = -angle_numerator;
NumBits = NumberOfBitsNeeded ( NumSamples );
/*
** Do simultaneous data copy and bit-reversal ordering into outputs...
*/
for ( i=0; i < NumSamples; i++ )
{
j = ReverseBits ( i, NumBits );
pRealOut[j] = pRealIn[i];
pImagOut[j] = static_cast<T>((pImagIn == NULL) ? 0.0 : pImagIn[i]);
}
/*
** Do the FFT itself...
*/
BlockEnd = 1;
for ( BlockSize = 2; BlockSize <= NumSamples; BlockSize <<= 1 )
{
double delta_angle = angle_numerator / (double)BlockSize;
double sm2 = sin ( -2 * delta_angle );
double sm1 = sin ( -delta_angle );
double cm2 = cos ( -2 * delta_angle );
double cm1 = cos ( -delta_angle );
double w = 2 * cm1;
double ar[3], ai[3];
for ( i=0; i < NumSamples; i += BlockSize )
{
ar[2] = cm2;
ar[1] = cm1;
ai[2] = sm2;
ai[1] = sm1;
for ( j=i, n=0; n < BlockEnd; j++, n++ )
{
ar[0] = w*ar[1] - ar[2];
ar[2] = ar[1];
ar[1] = ar[0];
ai[0] = w*ai[1] - ai[2];
ai[2] = ai[1];
ai[1] = ai[0];
k = j + BlockEnd;
tr = ar[0]*pRealOut[k] - ai[0]*pImagOut[k];
ti = ar[0]*pImagOut[k] + ai[0]*pRealOut[k];
pRealOut[k] = static_cast<T>( pRealOut[j] - tr);
pImagOut[k] = static_cast<T>( pImagOut[j] - ti);
pRealOut[j] += static_cast<T>(tr);
pImagOut[j] += static_cast<T>(ti);
}
}
BlockEnd = BlockSize;
}
/*
** Need to normalize if inverse transform...
*/
if ( bInverseTransform )
{
T denom = static_cast<T>(NumSamples);
for ( i=0; i < NumSamples; i++ )
{
pRealOut[i] /= denom;
pImagOut[i] /= denom;
}
}
}
/**
* Calculate normal (power spectrum)
* @param NumSamples Number of samples
* @param pReal Real coefficient buffer
* @param pImag Imaginary coefficient buffer
* @param pAmpl Working buffer to hold amplitude Xps(m) = | X(m)^2 | = Xreal(m)^2 + Ximag(m)^2
*/
/**
*计算法向（功率谱）
*@参数numsamples样本数
*@参数预实数系数缓冲器
*@参数pimag虚系数缓冲
*@保持振幅xps（m）=x（m）^2=xreal（m）^2+ximag（m）^2的参数pampl工作缓冲器
*/
template<typename T>
void FFT::Norm(size_t NumSamples, T *pReal, T *pImag, T* pAmpl)
{
if ( !pReal || !pImag || !pAmpl )
{
// error
throw std::exception("Invalid argument");
}
// Calculate amplitude values in the buffer provided 计算所提供缓冲器的振幅值
for (size_t i=0; i<NumSamples; i++ )
{
pAmpl[i] = pReal[i]*pReal[i] + pIma
