The Math Thread - Page 6

#include <math.h>
#include <stdint.h>

#define PI 3.14159265359f

uint32_t Rev32( uint32_t x )
{
	x = ((x & 0xAAAAAAAA) >> 1) | ((x & 0x55555555) << 1);
	x = ((x & 0xCCCCCCCC) >> 2) | ((x & 0x33333333) << 2);
	x = ((x & 0xF0F0F0F0) >> 4) | ((x & 0x0F0F0F0F) << 4);
	x = ((x & 0xFF00FF00) >> 8) | ((x & 0x00FF00FF) << 8);
	return (x >> 16) | (x << 16);
}

uint32_t PopCount( uint32_t x )
{
	uint32_t a = x - ((x >> 1) & 0x55555555);
	uint32_t b = (((a >> 2) & 0x33333333) + (a & 0x33333333));
	uint32_t c = (((b >> 4) + b) & 0x0f0f0f0f);
	uint32_t d = c + (c >> 8);
	uint32_t e = d + (d >> 16);
	uint32_t f = e & 0x0000003f;
	return f;
}

uint32_t Log2( uint32_t x )
{
	uint32_t a = x | ( x >> 1 );
	uint32_t b = a | ( a >> 2 );
	uint32_t c = b | ( b >> 4 );
	uint32_t d = c | ( c >> 8 );
	uint32_t e = d | ( d >> 16 );
	uint32_t f = e >> 1;
	return PopCount( f );
}

// x contains real inputs
// y contains imaginary inputs
// count must be a power of 2
// sign must be 1.0 (forward transform) or -1.0f (inverse transform)
void FFT( float* x, float* y, int count, float sign )
{
	int exponent = (int)Log2( (uint32_t)count );

	// bit reversal stage
	// swap all elements with their bit reversed index within the
	// lowest level of the Cooley-Tukey recursion tree
	for ( int i = 1; i < count - 1; i++ )
	{
		uint32_t j = Rev32( (uint32_t)i );
		j >>= (32 - exponent);
		if ( i < j )
		{
			float tx = x[ i ];
			float ty = y[ i ];
			x[ i ] = x[ j ];
			y[ i ] = y[ j ];
			x[ j ] = tx;
			y[ j ] = ty;
		}
	}

	// for each recursive iteration
	for ( int iter = 0, L = 1; iter < exponent; ++iter )
	{
		int Ls = L;
		L <<= 1;
		float u1 = 1.0f; // cos( pi / 2 )
		float u2 = 0;    // sin( pi / 2 )

		float arg = 3.14159265359f / (float)Ls;
		float wr = cosf( arg );
		float wi = -sign * sinf( arg );

		// rows in DFT submatrix
		for ( int j = 0; j < Ls; ++j )
		{
			// do butterflies upon DFT row elements
			for ( int i = j; i < count; i += L )
			{
				int index = i + Ls;
				float t1 = u1 * x[ index ] - u2 * y[ index ];
				float t2 = u1 * y[ index ] + u2 * x[ index ];
				x[ index ] = x[ i ] - t1; 
				y[ index ] = y[ i ] - t2;
				x[ i ] += t1;
				y[ i ] += t2;
			}

			// Rotate u1 and u2 via Givens rotations (2d planar rotation).
			// This keeps cos/sin calls in the outermost loop.
			// Floating point error is scaled proportionally to Ls.
			float z = u1 * wr - u2 * wi;
			u2 = u1 * wi + u2 * wr;
			u1 = z;
		}
	}

	// scale factor for forward transform
	if ( sign > 0 )
	{
		float inv_count = 1.0f / (float)count;
		for ( int i = 0; i < count; i++ )
		{
			x[ i ] *= inv_count;
			y[ i ] *= inv_count;
		}
	}
}