public KeyFrequency(int f, int c, int rad, ComplexF[] indata, int center)
{
this.frequency = f;
this.channel = c;
this.radius = rad;
this.data = new double[radius * 2 + 1];
//rescale numbers >=0
double min = Double.PositiveInfinity;
double max = Double.NegativeInfinity;
for (int i = 0; i < this.data.Length; i++)
{
this.data[i] = mag2db(indata[center-rad+i]);
if (this.data[i] < min)
min = this.data[i];
}
for (int i = 0; i < this.data.Length; i++)
{
this.data[i] -= min;
if (this.data[i] > max)
max = this.data[i];
}
for (int i = 0; i < this.data.Length; i++)
this.data[i] /= max;
isBoth = false;
inverse_state = 0;
prior = 0;
state = this.classify();
}
/// <summary>
/// Initialize the acceleration processing class
/// </summary>
public static void Initialize()
{
Accel_Data = new float[POINTS_NEEDED, 3];
Gyro_Data = new float[POINTS_NEEDED, 3];
Time_Data = new float[POINTS_NEEDED];
accelAmplitudes = new ComplexF[POINTS_NEEDED + PADDING_NEEDED];
gyroAmplitudes = new ComplexF[POINTS_NEEDED + PADDING_NEEDED];
Sample_Frequency = 46;
Points_Collected = 0;
}
/// <summary>
/// Calculate the square root of a complex number
/// </summary>
/// <param name="c"></param>
/// <returns></returns>
static public ComplexF Sqrt( ComplexF c ) {
double x = c.Re;
double y = c.Im;
double modulus = Math.Sqrt( x*x + y*y );
int sign = ( y < 0 ) ? -1 : 1;
c.Re = (float)( _halfOfRoot2 * Math.Sqrt( modulus + x ) );
c.Im = (float)( _halfOfRoot2 * sign * Math.Sqrt( modulus - x ) );
return c;
}
public int GetMandelbrotDepth( ComplexF c ) {
int depth = 0;
ComplexF a = ComplexF.Zero;
// iterate until either the limit is reached or it is
// certain that a(k+1) = a(k)^2 + c goes to infinity.
do {
a = a*a + c;
depth += 8;
} while ( ( depth < 512 ) && ( a.GetModulusSquared() < 4.0f ) );
if( a.GetModulusSquared() < 4.0f ) {
return 255;
}
return ( depth % 256 );
}
static private ComplexF SumRecursion( ComplexF[] data, int start, int end ) {
Debug.Assert( 0 <= start, "start = " + start );
Debug.Assert( start < end, "start = " + start + " and end = " + end );
Debug.Assert( end <= data.Length, "end = " + end + " and data.Length = " + data.Length );
if( ( end - start ) <= 1000 ) {
ComplexF sum = ComplexF.Zero;
for( int i = start; i < end; i ++ ) {
sum += data[ i ];
}
return sum;
}
else {
int middle = ( start + end ) >> 1;
return SumRecursion( data, start, middle ) + SumRecursion( data, middle, end );
}
}
/// <summary>
/// Apply the filter to a complex array
/// </summary>
/// <param name="subject">The array to apply the mask to</param>
/// <param name="width">The width of the image the array relates to</param>
/// <returns>A new ComplexF with masking applied</returns>
public ComplexF[] apply(ComplexF[] subject, int width)
{
//Loop through all regions in the mask
foreach (RectangleF region in regions)
{
//Consider each pixel the mask covers
for (int x = (int)region.X; x < region.X + region.Width; x++)
{
for (int y = (int)region.Y; y < region.Y + region.Height; y++)
{
//Set the values to 0
subject[y * width + x] = new ComplexF(0, 0);
}
}
}
return subject;
}
public unsafe CImage( string fileName ) {
Bitmap bitmap = new Bitmap( fileName );
Size correctSize = new Size(
(int) Math.Pow( 2, Math.Ceiling( Math.Log( bitmap.Width, 2 ) ) ),
(int) Math.Pow( 2, Math.Ceiling( Math.Log( bitmap.Height, 2 ) ) ) );
if( correctSize != bitmap.Size ) {
bitmap = new Bitmap( bitmap, correctSize );
}
_size = correctSize;
_data = new ComplexF[ this.Width * this.Height ];
Rectangle rect = new Rectangle( 0, 0, this.Width, this.Height );
BitmapData bitmapData = bitmap.LockBits( rect, ImageLockMode.ReadOnly, PixelFormat.Format32bppArgb );
int* colorData = (int*) bitmapData.Scan0.ToPointer();
for( int i = 0; i < this.Width * this.Height; i ++ ) {
Color c = Color.FromArgb( colorData[ i ] );
_data[ i ].Re = ( (float)c.R + (float)c.G + (float)c.B ) / ( 3f * 256f );
}
bitmap.UnlockBits( bitmapData );
}
private void DataArrived(IntPtr data, int size)
{
System.Runtime.InteropServices.Marshal.Copy(data, _recorderBuffer, 0, size);
float[] waved = new float[4096];
int h = 0;
for (int i = 0; i < _recorderBuffer.Length; i += 4)
{
waved[h] = (float)BitConverter.ToInt16(_recorderBuffer, i);
h++;
}
Exocortex.DSP.ComplexF[] complexData = new Exocortex.DSP.ComplexF[4096];
for (int i = 0; i < 4096; ++i)
{
// Fill the complex data
complexData[i].Re = waved[i]; // Add your real part here
complexData[i].Im = waved[i] + 5; // Add your imaginary part here
}
// FFT the time domain data to get frequency domain data
Exocortex.DSP.Fourier.FFT(complexData, Exocortex.DSP.FourierDirection.Forward);
VMODEL.Add(complexData);
if (s == 2)
{
s = 0;
SEC--;
if (SEC == 0)
{
Recorded = true;
s_WaveIn.Dispose();
}
}
else
{
s++;
}
}
public float[] GetVPFromWave(string wavefile)
{
string w = Path.GetDirectoryName(wavefile) + @"\n_" + Path.GetFileName(wavefile);
Normalize(wavefile, w);
WAVFile wave = new WAVFile();
wave.Open(w, WAVFile.WAVFileMode.READ);
float[] samples = new float[wave.NumSamples];
for (int i = 0; i < wave.NumSamples; i++)
{
samples[i] = wave.GetNextSample_16bit();
}
int cp = 0;
int t = samples.Length / 1024;
if (t > 0)
{
int r = samples.Length % 1024;
for (int x = 1; x <= t; x++)
{
Exocortex.DSP.ComplexF[] complexData = new Exocortex.DSP.ComplexF[1024];
for (int i = cp; i <= 1023; i++)
{
complexData[i].Re = samples[cp + i];
complexData[i].Im = samples[cp + i] + 5;
}
Exocortex.DSP.Fourier.FFT(complexData, complexData.Length, FourierDirection.Forward);
for (int i = 0; i <= 1023; i++)
{
samples[cp + i] = complexData[i].Re;
}
cp += 1024;
}
}
wave.Close();
return(samples);
}
//--------------------------------------------------------------------------------------
public unsafe CImage(Bitmap img)
{
Bitmap bitmap = new Bitmap(img);
Size correctSize = new Size(
(int)Math.Pow(2, Math.Ceiling(Math.Log(bitmap.Width, 2))),
(int)Math.Pow(2, Math.Ceiling(Math.Log(bitmap.Height, 2))));
if (correctSize != bitmap.Size)
{
bitmap = new Bitmap(correctSize.Width, correctSize.Height);
}
int ow, oh;
ow = (correctSize.Width - img.Width) / 2;
oh = (correctSize.Height - img.Height) / 2;
for (int y = 0; y < img.Height; y++)
{
for (int x = 0; x < img.Width; x++)
{
bitmap.SetPixel(x + ow, y + oh, img.GetPixel(x, y));
}
}
_size = correctSize;
_data = new ComplexF[this.Width * this.Height];
Rectangle rect = new Rectangle(0, 0, this.Width, this.Height);
BitmapData bitmapData = bitmap.LockBits(rect, ImageLockMode.ReadOnly, PixelFormat.Format32bppArgb);
int* colorData = (int*)bitmapData.Scan0.ToPointer();
for (int i = 0; i < this.Width * this.Height; i++)
{
Color c = Color.FromArgb(colorData[i]);
_data[i].Re = ((float)c.R + (float)c.G + (float)c.B) / (3f * 256f);
}
bitmap.UnlockBits(bitmapData);
}
/// <summary>
/// Create a complex number based on an existing complex number
/// </summary>
/// <param name="c"></param>
public ComplexF(ComplexF c)
{
Re = c.Re;
Im = c.Im;
}
/// <summary>
/// Multiply each element in the array by a specific value
/// </summary>
/// <param name="array"></param>
/// <param name="scale"></param>
/// <param name="start"></param>
/// <param name="length"></param>
static public void Scale( ComplexF[] array, ComplexF scale, int start, int length ) {
Debug.Assert( array != null );
Debug.Assert( start >= 0 );
Debug.Assert( length >= 0 );
Debug.Assert( ( start + length ) < array.Length );
for( int i = 0; i < length; i ++ ) {
array[i + start] *= scale;
}
}
//---------------------------------------------------------------------------------------------------
/// <summary>
/// Calculate the power of a complex number
/// </summary>
/// <param name="c"></param>
/// <param name="exponent"></param>
/// <returns></returns>
static public ComplexF Pow( ComplexF c, double exponent ) {
double x = c.Re;
double y = c.Im;
double modulus = Math.Pow( x*x + y*y, exponent * 0.5 );
double argument = Math.Atan2( y, x ) * exponent;
c.Re = (float)( modulus * System.Math.Cos( argument ) );
c.Im = (float)( modulus * System.Math.Sin( argument ) );
return c;
}
/// <summary>
/// Swap two complex numbers
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
static public void Swap( ref ComplexF a, ref ComplexF b ) {
ComplexF temp = a;
a = b;
b = temp;
}
/// <summary>
/// Invert each element in the array
/// </summary>
/// <param name="array"></param>
static public void Invert( ComplexF[] array ) {
for( int i = 0; i < array.Length; i ++ ) {
array[i] = ((ComplexF) 1 ) / array[i];
}
}
static private void LockWorkspaceF( int length, ref ComplexF[] workspace ) {
Debug.Assert( _workspaceFLocked == false );
_workspaceFLocked = true;
if( length >= _workspaceF.Length ) {
_workspaceF = new ComplexF[ length ];
}
workspace = _workspaceF;
}
//-----------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------
/// <summary>
/// Determine whether two complex numbers are almost (i.e. within the tolerance) equivalent.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="tolerance"></param>
/// <returns></returns>
static public bool IsEqual(ComplexF a, ComplexF b, float tolerance)
{
return
((Math.Abs(a.Re - b.Re) < tolerance) &&
(Math.Abs(a.Im - b.Im) < tolerance));
}
/// <summary>
/// Scale and offset the elements in the array so that the
/// overall range is [0, 1]
/// </summary>
/// <param name="array"></param>
static public void Normalize( ComplexF[] array ) {
float min = 0, max = 0;
GetLengthRange( array, ref min, ref max );
Scale( array, ( 1 / ( max - min ) ) );
Offset( array, ( - min / ( max - min ) ) );
}
private static void LinearFFT_Quick( ComplexF[] data, int start, int inc, int length, FourierDirection direction ) {
/*Debug.Assert( data != null );
Debug.Assert( start >= 0 );
Debug.Assert( inc >= 1 );
Debug.Assert( length >= 1 );
Debug.Assert( ( start + inc * ( length - 1 ) ) < data.Length ); */
// copy to buffer
ComplexF[] buffer = null;
LockBufferCF( length, ref buffer );
int j = start;
for( int i = 0; i < length; i ++ ) {
buffer[ i ] = data[ j ];
j += inc;
}
FFT( buffer, length, direction );
// copy from buffer
j = start;
for( int i = 0; i < length; i ++ ) {
data[ j ] = buffer[ i ];
j += inc;
}
UnlockBufferCF( ref buffer );
}
/// <summary>
/// Compute a 1D fast Fourier transform of a dataset of complex numbers.
/// </summary>
/// <param name="data"></param>
/// <param name="length"></param>
/// <param name="direction"></param>
public static void FFT_Quick( ComplexF[] data, int length, FourierDirection direction ) {
/*if( data == null ) {
throw new ArgumentNullException( "data" );
}
if( data.Length < length ) {
throw new ArgumentOutOfRangeException( "length", length, "must be at least as large as 'data.Length' parameter" );
}
if( Fourier.IsPowerOf2( length ) == false ) {
throw new ArgumentOutOfRangeException( "length", length, "must be a power of 2" );
}
Fourier.SyncLookupTableLength( length );*/
int ln = Fourier.Log2( length );
// reorder array
Fourier.ReorderArray( data );
// successive doubling
int N = 1;
int signIndex = ( direction == FourierDirection.Forward ) ? 0 : 1;
for( int level = 1; level <= ln; level ++ ) {
int M = N;
N <<= 1;
float[] uRLookup = _uRLookupF[ level, signIndex ];
float[] uILookup = _uILookupF[ level, signIndex ];
for( int j = 0; j < M; j ++ ) {
float uR = uRLookup[j];
float uI = uILookup[j];
for( int even = j; even < length; even += N ) {
int odd = even + M;
float r = data[ odd ].Re;
float i = data[ odd ].Im;
float odduR = r * uR - i * uI;
float odduI = r * uI + i * uR;
r = data[ even ].Re;
i = data[ even ].Im;
data[ even ].Re = r + odduR;
data[ even ].Im = i + odduI;
data[ odd ].Re = r - odduR;
data[ odd ].Im = i - odduI;
}
}
}
}
static private void LockBufferCF( int length, ref ComplexF[] buffer ) {
Debug.Assert( length >= 0 );
Debug.Assert( _bufferCFLocked == false );
_bufferCFLocked = true;
if( length != _bufferCF.Length ) {
_bufferCF = new ComplexF[ length ];
}
buffer = _bufferCF;
}
/// <summary>
/// Compute a 3D fast fourier transform on a data set of complex numbers
/// </summary>
/// <param name="data"></param>
/// <param name="xLength"></param>
/// <param name="yLength"></param>
/// <param name="zLength"></param>
/// <param name="direction"></param>
public static void FFT3( ComplexF[] data, int xLength, int yLength, int zLength, FourierDirection direction ) {
if( data == null ) {
throw new ArgumentNullException( "data" );
}
if( data.Length < xLength*yLength*zLength ) {
throw new ArgumentOutOfRangeException( "data.Length", data.Length, "must be at least as large as 'xLength * yLength * zLength' parameter" );
}
if( Fourier.IsPowerOf2( xLength ) == false ) {
throw new ArgumentOutOfRangeException( "xLength", xLength, "must be a power of 2" );
}
if( Fourier.IsPowerOf2( yLength ) == false ) {
throw new ArgumentOutOfRangeException( "yLength", yLength, "must be a power of 2" );
}
if( Fourier.IsPowerOf2( zLength ) == false ) {
throw new ArgumentOutOfRangeException( "zLength", zLength, "must be a power of 2" );
}
int xInc = 1;
int yInc = xLength;
int zInc = xLength * yLength;
if( xLength > 1 ) {
Fourier.SyncLookupTableLength( xLength );
for( int z = 0; z < zLength; z ++ ) {
for( int y = 0; y < yLength; y ++ ) {
int xStart = y * yInc + z * zInc;
Fourier.LinearFFT_Quick( data, xStart, xInc, xLength, direction );
}
}
}
if( yLength > 1 ) {
Fourier.SyncLookupTableLength( yLength );
for( int z = 0; z < zLength; z ++ ) {
for( int x = 0; x < xLength; x ++ ) {
int yStart = z * zInc + x * xInc;
Fourier.LinearFFT_Quick( data, yStart, yInc, yLength, direction );
}
}
}
if( zLength > 1 ) {
Fourier.SyncLookupTableLength( zLength );
for( int y = 0; y < yLength; y ++ ) {
for( int x = 0; x < xLength; x ++ ) {
int zStart = y * yInc + x * xInc;
Fourier.LinearFFT_Quick( data, zStart, zInc, zLength, direction );
}
}
}
}
/// <summary>
/// Create a complex number based on an existing complex number
/// </summary>
/// <param name="c"></param>
public ComplexF(ComplexF c)
{
this.Re = c.Re;
this.Im = c.Im;
}
/// <summary>
/// Multiply each element in the array by a specific value
/// </summary>
/// <param name="array"></param>
/// <param name="scale"></param>
static public void Scale( ComplexF[] array, ComplexF scale ) {
Debug.Assert( array != null );
int length = array.Length;
for( int i = 0; i < length; i ++ ) {
array[i] *= scale;
}
}
/// <summary>
/// Add a specific value to each element in the array
/// </summary>
/// <param name="array"></param>
/// <param name="offset"></param>
static public void Offset( ComplexF[] array, ComplexF offset ) {
int length = array.Length;
for( int i = 0; i < length; i ++ ) {
array[i] += offset;
}
}
private static ComplexF[] GetImageFFTArray(Bitmap bitmap) {
float scale = 1F / (float) System.Math.Sqrt(bitmap.Width * bitmap.Height);
ComplexF[] data = new ComplexF [bitmap.Width * bitmap.Height * 4];
int offset = 0;
for( int y = 0; y < bitmap.Height; y ++ )
for( int x = 0; x < bitmap.Width; x ++ ) {
Color c = bitmap.GetPixel (x, y);
float s = 1F;
if( (( x + y ) & 0x1 ) != 0 ) {
s = -1F;
}
data [offset++] = new ComplexF( c.A * s / 256F, 0);
data [offset++] = new ComplexF( c.R * s / -256F, 0);
data [offset++] = new ComplexF( c.G * s / 256F, 0);
data [offset++] = new ComplexF( c.B * s / -256F, 0);
}
Fourier.FFT3( data, 4, bitmap.Width, bitmap.Height, FourierDirection.Forward );
for( int i = 0; i < data.Length; i ++ ) {
data[i] *= scale;
}
return data;
}
/// <summary>
/// Multiply each element in target array with corresponding element in rhs array
/// </summary>
/// <param name="target"></param>
/// <param name="rhs"></param>
static public void Multiply( ComplexF[] target, ComplexF[] rhs ) {
ComplexArray.Multiply( target, rhs, target );
}
static private void ReorderArray( ComplexF[] data ) {
Debug.Assert( data != null );
int length = data.Length;
Debug.Assert( Fourier.IsPowerOf2( length ) == true );
Debug.Assert( length >= cMinLength );
Debug.Assert( length <= cMaxLength );
int[] reversedBits = Fourier.GetReversedBits( Fourier.Log2( length ) );
for( int i = 0; i < length; i ++ ) {
int swap = reversedBits[ i ];
if( swap > i ) {
ComplexF temp = data[ i ];
data[ i ] = data[ swap ];
data[ swap ] = temp;
}
}
}
/// <summary>
/// Multiply each element in lhs array with corresponding element in rhs array and
/// put product in result array
/// </summary>
/// <param name="lhs"></param>
/// <param name="rhs"></param>
/// <param name="result"></param>
static public void Multiply( ComplexF[] lhs, ComplexF[] rhs, ComplexF[] result ) {
Debug.Assert( lhs != null );
Debug.Assert( rhs != null );
Debug.Assert( result != null );
Debug.Assert( lhs.Length == rhs.Length );
Debug.Assert( lhs.Length == result.Length );
int length = lhs.Length;
for( int i = 0; i < length; i ++ ) {
result[i] = lhs[i] * rhs[i];
}
}
static private void UnlockBufferCF( ref ComplexF[] buffer ) {
Debug.Assert( _bufferCF == buffer );
Debug.Assert( _bufferCFLocked == true );
_bufferCFLocked = false;
buffer = null;
}
/// <summary>
/// Divide each element in target array with corresponding element in rhs array
/// </summary>
/// <param name="target"></param>
/// <param name="rhs"></param>
static public void Divide( ComplexF[] target, ComplexF[] rhs ) {
ComplexArray.Divide( target, rhs, target );
}
static private void Swap( ref ComplexF a, ref ComplexF b ) {
ComplexF temp = a;
a = b;
b = temp;
}
/// <summary>
/// Divide each element in lhs array with corresponding element in rhs array and
/// put product in result array
/// </summary>
/// <param name="lhs"></param>
/// <param name="rhs"></param>
/// <param name="result"></param>
static public void Divide( ComplexF[] lhs, ComplexF[] rhs, ComplexF[] result ) {
Debug.Assert( lhs != null );
Debug.Assert( rhs != null );
Debug.Assert( result != null );
Debug.Assert( lhs.Length == rhs.Length );
Debug.Assert( lhs.Length == result.Length );
ComplexF zero = ComplexF.Zero;
int length = lhs.Length;
for( int i = 0; i < length; i ++ ) {
if( rhs[i] != zero ) {
result[i] = lhs[i] / rhs[i];
}
else {
result[i] = zero;
}
}
}
/// <summary>
/// Compute a 1D fast Fourier transform of a dataset of complex numbers.
/// </summary>
/// <param name="data"></param>
/// <param name="direction"></param>
public static void FFT( ComplexF[] data, FourierDirection direction ) {
if( data == null ) {
throw new ArgumentNullException( "data" );
}
Fourier.FFT( data, data.Length, direction );
}
/// <summary>
/// Copy an array
/// </summary>
/// <param name="dest"></param>
/// <param name="source"></param>
static public void Copy( ComplexF[] dest, ComplexF[] source ) {
Debug.Assert( dest != null );
Debug.Assert( source != null );
Debug.Assert( dest.Length == source.Length );
for( int i = 0; i < dest.Length; i ++ ) {
dest[i] = source[i];
}
}
