private void OnDataReceiverEvent(int[] real, int[] imaginary, int length, rx_metadata_t md)
{
if (length < numberOfPoints) // Insufficient data for FFT
{
return;
}
for (int i = 0; i < numberOfPoints; i++)
{
data[i] = new AForge.Math.Complex(real[i], imaginary[i]);
}
FourierTransform.FFT(data, FourierTransform.Direction.Forward);
int count = 0;
for (int i = numberOfPoints / 2; i < numberOfPoints; i++)
{
testValues[i, 1] = data[count++].Magnitude;
}
for (int i = 0; i < numberOfPoints / 2; i++)
{
testValues[i, 1] = data[count++].Magnitude;
}
chart.UpdateDataSeries("Test", testValues);
//chart.RangeY = new AForge.Range(0, 2048);
}
public static object FFT(double[] Data, object BackwardFlagOpt)
{
bool backwards = Utils.GetOptionalParameter(BackwardFlagOpt, false);
af.FourierTransform.Direction direction = (backwards ? af.FourierTransform.Direction.Backward : af.FourierTransform.Direction.Forward);
af.Complex[] cData = new af.Complex[Data.Length];
for (int i = 0; i < Data.Length; ++i)
{
cData[i] = new af.Complex(Data[i], 0);
}
try
{
af.FourierTransform.FFT(cData, direction);
}
catch (Exception e)
{
Debug.WriteLine(e.ToString());
}
double[,] ret = new double[Data.Length, 1];
for (int i = 0; i < Data.Length; ++i)
{
ret[i, 0] = cData[i].Re;
}
return(ret);
}
//public int[] FFT(int[] y)
public async Task <int[]> FFT(int[] y)
{
WvlLogger.Log(LogType.TraceAll, "FFT()");
var input = new AForge.Math.Complex[y.Length];
for (int i = 0; i < y.Length; i++)
{
input[i] = new AForge.Math.Complex(y[i], 0);
}
FourierTransform.FFT(input, FourierTransform.Direction.Forward);
var result = new int[y.Length / 2];
// getting magnitude
for (int i = 0; i < y.Length / 2 - 1; i++)
{
var current = Math.Sqrt(input[i].Re * input[i].Re + input[i].Im * input[i].Im);
current = Math.Log10(current) * 10;
result[i] = (int)current;
}
samplesUpdated(this, new SamplesUpdatedEventArgs(result));
//PrepareHeatmapDataSeries(result);
return(result);
}
/// <summary>
/// Computes the absolute value of an array of complex numbers.
/// </summary>
public static Complex[] Abs(this Complex[] x)
{
var r = new Complex[x.Length];
for (int i = 0; i < x.Length; i++)
r[i] = new Complex(x[i].Magnitude, 0);
return r;
}
/// <summary>
/// Elementwise multiplication of two complex vectors.
/// </summary>
public static Complex[] Multiply(this Complex[] a, Complex[] b)
{
var r = new Complex[a.Length];
for (int i = 0; i < a.Length; i++)
{
r[i] = Complex.Multiply(a[i], b[i]);
}
return r;
}
/// <summary>
/// Performs the transformation over a double[] array.
/// </summary>
public static void FHT(double[] data, FourierTransform.Direction direction)
{
int N = data.Length;
// Forward operation
if (direction == FourierTransform.Direction.Forward)
{
// Copy the input to a complex array which can be processed
// in the complex domain by the FFT
var cdata = new Complex[N];
for (int i = 0; i < N; i++)
cdata[i].Re = data[i];
// Perform FFT
FourierTransform.FFT(cdata, FourierTransform.Direction.Forward);
//double positive frequencies
for (int i = 1; i < (N/2); i++)
{
cdata[i].Re *= 2.0;
cdata[i].Im *= 2.0;
}
// zero out negative frequencies
// (leaving out the dc component)
for (int i = (N/2) + 1; i < N; i++)
{
cdata[i].Re = 0.0;
cdata[i].Im = 0.0;
}
// Reverse the FFT
FourierTransform.FFT(cdata, FourierTransform.Direction.Backward);
// Convert back to our initial double array
for (int i = 0; i < N; i++)
data[i] = cdata[i].Im;
}
else // Backward operation
{
// The inverse Hilbert can be calculated by
// negating the transform and reapplying the
// transformation.
//
// H^–1{h(t)} = –H{h(t)}
FHT(data, FourierTransform.Direction.Forward);
for (int i = 0; i < data.Length; i++)
data[i] = -data[i];
}
}
/// <summary>
/// Processes the filter.
/// </summary>
///
protected override void ProcessFilter(ComplexSignal sourceData, ComplexSignal destinationData)
{
int length = sourceData.Length;
unsafe
{
Complex* src = (Complex*)sourceData.Data.ToPointer();
Complex* dst = (Complex*)destinationData.Data.ToPointer();
Complex d = new Complex();
for (int i = 0; i < length - 1; i++, src++, dst++)
{
d.Re = src[i + 1].Re - src[i].Re;
// Retain only if difference is positive
*dst = (d.Re > 0) ? d : Complex.Zero;
}
}
}
/// <summary>
/// 1-D Discrete Fourier Transform.
/// </summary>
///
/// <param name="data">The data to transform..</param>
/// <param name="direction">The transformation direction.</param>
///
public static void DFT(Complex[] data, FourierTransform.Direction direction)
{
int n = data.Length;
var c = new Complex[n];
// for each destination element
for (int i = 0; i < c.Length; i++)
{
double sumRe = 0;
double sumIm = 0;
double phim = 2 * Math.PI * i / (double)n;
// sum source elements
for (int j = 0; j < n; j++)
{
double re = data[j].Re();
double im = data[j].Im();
double cosw = Math.Cos(phim * j);
double sinw = Math.Sin(phim * j);
if (direction == FourierTransform.Direction.Backward)
sinw = -sinw;
sumRe += (re * cosw + im * sinw);
sumIm += (im * cosw - re * sinw);
}
c[i] = new Complex(sumRe, sumIm);
}
if (direction == FourierTransform.Direction.Backward)
{
for (int i = 0; i < c.Length; i++)
data[i] = c[i] / n;
}
else
{
for (int i = 0; i < c.Length; i++)
data[i] = c[i];
}
}
public int[] FFT(int[] y)
{
WvlLogger.Log(LogType.TraceAll, "FFT()");
WvlLogger.Log(LogType.TraceValues, "FFT() - int[] y.length : " + y.Length.ToString() + "y.Sum : " + y.Sum().ToString());
var input = new AForge.Math.Complex[y.Length];
for (int i = 0; i < y.Length; i++)
{
input[i] = new AForge.Math.Complex(y[i], 0);
}
FourierTransform.FFT(input, FourierTransform.Direction.Forward);
var result = new int[y.Length / 2];
// getting magnitude
for (int i = 0; i < y.Length / 2 - 1; i++)
{
var current = Math.Sqrt(input[i].Re * input[i].Re + input[i].Im * input[i].Im);
current = Math.Log10(current) * 10;
result[i] = (int)current;
}
// debug
/*
* int myMin = 0;
* int myMax = 0;
* myMin = result.Min();
* myMax = result.Max();
* debugMin = (debugMin <= myMin) ? debugMin : myMin;
* debugMax = (debugMax >= myMax) ? debugMax : myMax;
*/
WvlLogger.Log(LogType.TraceValues, "FFT() - result : " + result.Length.ToString() + "y.Sum : " + result.Sum().ToString());
return(result);
}
public int[] FFT(int[] y)
{
var input = new AForge.Math.Complex[y.Length];
for (int i = 0; i < y.Length; i++)
{
input[i] = new AForge.Math.Complex(y[i], 0);
}
FourierTransform.FFT(input, FourierTransform.Direction.Forward);
var result = new int[y.Length / 2];
// getting magnitude
for (int i = 0; i < y.Length / 2 - 1; i++)
{
var current = Math.Sqrt(input[i].Re * input[i].Re + input[i].Im * input[i].Im);
current = Math.Log10(current) * 10;
result[i] = (int)current;
}
return(result);
}
/// <summary>
/// Adds scalar value to a complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="s">A scalar value.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the sum of specified
/// complex number and scalar value.</returns>
///
public static Complex Add(Complex a, double s)
{
return(new Complex(a.Re + s, a.Im));
}
/// <summary>
/// Subtracts a scalar value from a complex number and puts the result into another complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance to subtract from.</param>
/// <param name="s">A scalar value to be subtracted.</param>
/// <param name="result">A <see cref="Complex"/> instance to hold the result.</param>
///
public static void Subtract(Complex a, double s, ref Complex result)
{
result.Re = a.Re - s;
result.Im = a.Im;
}
/// <summary>
/// Multiplies a complex number by a scalar value.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="s">A scalar value.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the result of multiplication.</returns>
///
public static Complex Multiply(Complex a, double s)
{
return(new Complex(a.Re * s, a.Im * s));
}
/// <summary>
/// Adds two complex numbers and puts the result into the third complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="b">A <see cref="Complex"/> instance.</param>
/// <param name="result">A <see cref="Complex"/> instance to hold the result.</param>
///
public static void Add(Complex a, Complex b, ref Complex result)
{
result.Re = a.Re + b.Re;
result.Im = a.Im + b.Im;
}
/// <summary>
/// Subtracts one complex number from another.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance to subtract from.</param>
/// <param name="b">A <see cref="Complex"/> instance to be subtracted.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the subtraction result (<b>a - b</b>).</returns>
///
public static Complex Subtract(Complex a, Complex b)
{
return(new Complex(a.Re - b.Re, a.Im - b.Im));
}
/// <summary>
/// Subtracts a complex number from a scalar value.
/// </summary>
///
/// <param name="s">A scalar value to subtract from.</param>
/// <param name="a">A <see cref="Complex"/> instance to be subtracted.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the subtraction result (<b>s - a</b>).</returns>
///
public static Complex Subtract(double s, Complex a)
{
return(new Complex(s - a.Re, a.Im));
}
/// <summary>
/// Subtracts one complex number from another and puts the result in the third complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance to subtract from.</param>
/// <param name="b">A <see cref="Complex"/> instance to be subtracted.</param>
/// <param name="result">A <see cref="Complex"/> instance to hold the result.</param>
///
public static void Subtract(Complex a, Complex b, ref Complex result)
{
result.Re = a.Re - b.Re;
result.Im = a.Im - b.Im;
}
/// <summary>
/// Adds a complex number and a scalar value.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="s">A scalar value.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the sum.</returns>
///
public static Complex operator +(double s, Complex a)
{
return(Complex.Add(a, s));
}
/// <summary>
/// Adds two complex numbers.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="b">A <see cref="Complex"/> instance.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the sum.</returns>
///
public static Complex operator +(Complex a, Complex b)
{
return(Complex.Add(a, b));
}
/// <summary>
/// Negates the complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the negated values.</returns>
///
public static Complex operator -(Complex a)
{
return(Complex.Negate(a));
}
/// <summary>
/// Tests whether two complex numbers are approximately equal using default tolerance value.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="b">A <see cref="Complex"/> instance.</param>
///
/// <returns>Return <see langword="true"/> if the two vectors are approximately equal or <see langword="false"/> otherwise.</returns>
///
/// <remarks><para>The default tolerance value, which is used for the test, equals to 8.8817841970012523233891E-16.</para></remarks>
///
public static bool ApproxEqual(Complex a, Complex b)
{
return(ApproxEqual(a, b, 8.8817841970012523233891E-16));
}
/// <summary>
/// Negates a complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the negated values.</returns>
///
public static Complex Negate(Complex a)
{
return(new Complex(-a.Re, -a.Im));
}
/// <summary>
/// Multiplies a complex number by a scalar value and puts the result into another complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="s">A scalar value.</param>
/// <param name="result">A <see cref="Complex"/> instance to hold the result.</param>
///
public static void Multiply(Complex a, double s, ref Complex result)
{
result.Re = a.Re * s;
result.Im = a.Im * s;
}
void solve()
{
show_result_2.Clear();
if (a == 0)
{
if (b != 0)
{
root_1.Clear();
root_2.Clear();
root_1.Text = root_1.Text + Convert.ToString(Math.Round(-c / b, 4));
show_result_2.Text += "Phương trình có 1 nghiệm duy nhất";
;
}
else
{
root_1.Clear();
root_2.Clear();
root_1.Text = root_1.Text + "No solution";
root_2.Text = root_2.Text + "No solution";
show_result_2.Text += "Phương trình vô nghiệm";
}
}
else
{
double delta = b * b - 4 * a * c;
if (delta >= 0)
{
double x1, x2;
x1 = (-b + Math.Sqrt(delta)) / (2 * a);
x2 = (-b - Math.Sqrt(delta)) / (2 * a);
root_1.Clear();
root_1.Text = root_1.Text + Convert.ToString(Math.Round(x1, 4));
root_2.Clear();
root_2.Text = root_2.Text + Convert.ToString(Math.Round(x2, 4));
if (delta == 0)
{
show_result_2.Text += "Phương trình có nghiệm kép";
}
else
{
show_result_2.Text += "Phương trình có 2 nghiệm phân biệt";
}
}
else
{
AForge.Math.Complex x1;
AForge.Math.Complex delta_1 = new AForge.Math.Complex(delta, 0);
x1 = (-b + AForge.Math.Complex.Sqrt(delta_1)) / (2 * a);
root_1.Clear();
root_1.Text = root_1.Text + Convert.ToString(Math.Round(x1.Re, 4));
if (x1.Im > 0)
{
root_1.Text = root_1.Text + "+";
}
root_1.Text = root_1.Text + Convert.ToString(Math.Round(x1.Im, 4));
root_1.Text = root_1.Text + "*i";
root_2.Clear();
root_2.Text = root_2.Text + Convert.ToString(Math.Round(x1.Re, 4));
if (x1.Im < 0)
{
root_2.Text = root_1.Text + "+";
}
root_2.Text = root_2.Text + Convert.ToString(Math.Round(-x1.Im, 4));
root_2.Text = root_2.Text + "*i";
show_result_2.Text += "Phương trình có 2 nghiệm phân biệt";
}
}
}
/// <summary>
/// Performs the transformation over a complex[] array.
/// </summary>
public static void FHT(Complex[] data, FourierTransform.Direction direction)
{
int N = data.Length;
// Forward operation
if (direction == FourierTransform.Direction.Forward)
{
// Makes a copy of the data so we don't lose the
// original information to build our final signal
var shift = (Complex[]) data.Clone();
// Perform FFT
FourierTransform.FFT(shift, FourierTransform.Direction.Backward);
//double positive frequencies
for (int i = 1; i < (N/2); i++)
{
shift[i].Re *= 2.0;
shift[i].Im *= 2.0;
}
// zero out negative frequencies
// (leaving out the dc component)
for (int i = (N/2) + 1; i < N; i++)
{
shift[i].Re = 0.0;
shift[i].Im = 0.0;
}
// Reverse the FFT
FourierTransform.FFT(shift, FourierTransform.Direction.Forward);
// Put the Hilbert transform in the Imaginary part
// of the input signal, creating a Analytic Signal
for (int i = 0; i < N; i++)
data[i].Im = shift[i].Im;
}
else // Backward operation
{
// Just discard the imaginary part
for (int i = 0; i < data.Length; i++)
data[i].Im = 0.0;
}
}
/// <summary>
/// Applies the filter to a signal.
/// </summary>
protected unsafe override void ProcessFilter(Signal sourceData, Signal destinationData)
{
SampleFormat format = sourceData.SampleFormat;
int samples = sourceData.Samples;
if (format == SampleFormat.Format32BitIeeeFloat)
{
float* src = (float*)sourceData.Data.ToPointer();
float* dst = (float*)destinationData.Data.ToPointer();
for (int i = 0; i < samples; i++, dst++, src++)
*dst = System.Math.Abs(*src);
}
else if (format == SampleFormat.Format128BitComplex)
{
Complex* src = (Complex*)sourceData.Data.ToPointer();
Complex* dst = (Complex*)destinationData.Data.ToPointer();
Complex c = new Complex();
for (int i = 0; i < samples; i++, dst++, src++)
{
c.Re = (*src).Magnitude;
*dst = c;
}
}
}
/// <summary>
/// Splits a signal using the window.
/// </summary>
///
public virtual ComplexSignal Apply(ComplexSignal complexSignal, int sampleIndex)
{
Complex[,] resultData = new Complex[Length, complexSignal.Channels];
ComplexSignal result = ComplexSignal.FromArray(resultData, complexSignal.SampleRate);
int channels = result.Channels;
int minLength = System.Math.Min(complexSignal.Length - sampleIndex, Length);
unsafe
{
for (int c = 0; c < complexSignal.Channels; c++)
{
Complex* dst = (Complex*)result.Data.ToPointer() + c;
Complex* src = (Complex*)complexSignal.Data.ToPointer() + c + channels * sampleIndex;
for (int i = 0; i < minLength; i++, dst += channels, src += channels)
{
*dst = window[i] * (*src);
}
}
}
return result;
}
/// <summary>
/// 1-D Fast Fourier Transform.
/// </summary>
///
/// <param name="data">The data to transform..</param>
/// <param name="direction">The transformation direction.</param>
///
public static void FFT(Complex[] data, FourierTransform.Direction direction)
{
int n = data.Length;
if (n == 0)
return;
if (direction == FourierTransform.Direction.Backward)
{
for (int i = 0; i < data.Length; i++)
data[i] = new Complex(data[i].Im(), data[i].Re());
}
if ((n & (n - 1)) == 0)
{
// Is power of 2
TransformRadix2(data);
}
else
{
// More complicated algorithm for arbitrary sizes
TransformBluestein(data);
}
if (direction == FourierTransform.Direction.Backward)
{
for (int i = 0; i < data.Length; i++)
{
double im = data[i].Im();
double re = data[i].Re();
data[i] = new Complex(im / n, re / n);
}
}
}
/// <summary>
/// Subtracts a scalar from a complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance to subtract from.</param>
/// <param name="s">A scalar value to be subtracted.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the subtraction result (<b>a - s</b>).</returns>
///
public static Complex Subtract(Complex a, double s)
{
return(new Complex(a.Re - s, a.Im));
}
/// <summary>
/// Computes the inverse discrete Fourier transform (IDFT) of the given complex
/// vector, storing the result back into the vector. The vector can have any length.
/// This is a wrapper function. This transform does not perform scaling, so the
/// inverse is not a true inverse.
/// </summary>
///
private static void IDFT(Complex[] data)
{
int n = data.Length;
if (n == 0)
return;
for (int i = 0; i < data.Length; i++)
data[i] = new Complex(data[i].Im(), data[i].Re());
if ((n & (n - 1)) == 0)
{
// Is power of 2
TransformRadix2(data);
}
else
{
// More complicated algorithm for arbitrary sizes
TransformBluestein(data);
}
for (int i = 0; i < data.Length; i++)
{
double im = data[i].Im();
double re = data[i].Re();
data[i] = new Complex(im, re);
}
}
/// <summary>
/// Adds scalar value to a complex number and puts the result into another complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="s">A scalar value.</param>
/// <param name="result">A <see cref="Complex"/> instance to hold the result.</param>
///
public static void Add(Complex a, double s, ref Complex result)
{
result.Re = a.Re + s;
result.Im = a.Im;
}
private static void TransformBluestein(Complex[] data)
{
int n = data.Length;
int m = HighestOneBit(n * 2 + 1) << 1;
// Trignometric tables
var cosTable = new double[n];
var sinTable = new double[n];
for (int i = 0; i < cosTable.Length; i++)
{
int j = (int)((long)i * i % (n * 2)); // This is more accurate than j = i * i
cosTable[i] = Math.Cos(Math.PI * j / n);
sinTable[i] = Math.Sin(Math.PI * j / n);
}
// Temporary vectors and preprocessing
var areal = new double[m];
var aimag = new double[m];
for (int i = 0; i < data.Length; i++)
{
double re = data[i].Re();
double im = data[i].Im();
areal[i] = +re * cosTable[i] + im * sinTable[i];
aimag[i] = -re * sinTable[i] + im * cosTable[i];
}
var breal = new double[m];
var bimag = new double[m];
breal[0] = cosTable[0];
bimag[0] = sinTable[0];
for (int i = 1; i < cosTable.Length; i++)
{
breal[i] = breal[m - i] = cosTable[i];
bimag[i] = bimag[m - i] = sinTable[i];
}
// Convolution
var creal = new double[m];
var cimag = new double[m];
Convolve(areal, aimag, breal, bimag, creal, cimag);
// Postprocessing
for (int i = 0; i < data.Length; i++)
{
double re = +creal[i] * cosTable[i] + cimag[i] * sinTable[i];
double im = -creal[i] * sinTable[i] + cimag[i] * cosTable[i];
data[i] = new Complex(re, im);
}
}
/// <summary>
/// Subtracts one complex number from another complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="b">A <see cref="Complex"/> instance.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the difference.</returns>
///
public static Complex operator -(Complex a, Complex b)
{
return(Complex.Subtract(a, b));
}
/// <summary>
/// Subtracts a complex number from a scalar value and puts the result into another complex number.
/// </summary>
///
/// <param name="s">A scalar value to subtract from.</param>
/// <param name="a">A <see cref="Complex"/> instance to be subtracted.</param>
/// <param name="result">A <see cref="Complex"/> instance to hold the result.</param>
///
public static void Subtract(double s, Complex a, ref Complex result)
{
result.Re = s - a.Re;
result.Im = a.Im;
}
private void AllFrameAvailable(object sender, MultiSourceFrameArrivedEventArgs e)
{
bool processed = true;
bool dataReceived = true;
using (MultiSourceFrame multiFrame = e.FrameReference.AcquireFrame())
{
#region Image Display
using (ColorFrame colorFrame = multiFrame.ColorFrameReference.AcquireFrame())
{
if (multiFrame != null && colorFrame != null)
{
hrData[j] = multiFrame;
j++;
if (j == 60)
{
j = 0;
}
FrameDescription colorFrameDescription = colorFrame.FrameDescription;
// verify data and write the new color frame data to the display bitmap
if ((colorFrameDescription.Width == this.wBitmap.PixelWidth) && (colorFrameDescription.Height == this.wBitmap.PixelHeight))
{
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(this.colorPixels);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(this.colorPixels, ColorImageFormat.Bgra);
}
processed = true;
}
}
// we got a frame, render
if (processed)
{
RenderColorPixels(this.colorPixels);
}
#endregion
#region Body Tracking
using (BodyFrame bodyFrame = multiFrame.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
if (dataReceived)
{
StatusTexts[0] = PersonStatus1;
StatusTexts[1] = PersonStatus2;
StatusTexts[2] = PersonStatus3;
StatusTexts[3] = PersonStatus4;
StatusTexts[4] = PersonStatus5;
StatusTexts[5] = PersonStatus6;
for (int bodyIndex = 0; bodyIndex < this.bodies.Length - 1; bodyIndex++)
{
Body body = this.bodies[bodyIndex];
if (body != null && body.IsTracked)
{
// mainImage.Visibility = Visibility.Visible;
StatusTexts[bodyIndex].Visibility = Visibility.Visible;
if (!this.currentlyTakingVitals)
{
VisualStateManager.GoToState(this, VSG_TakingVitals.Name, false);
//mainImage.Opacity = 1;
//this.storyboard.Stop();
//this.takeVitals.Begin();
this.currentlyTakingVitals = true;
}
//Get Facial expressions
var expressions = body.Expressions;
//foreach(var expressionItem in expressions)
//{
// var expression = expressionItem.Value;
// var expName = expressionItem.Key;
//}
//var appearanceItem = body.Appearance;
//foreach (var appearanceItem in body.Appearance)
//{
// var appearance = appearanceItem.Value;
// var appName = appearanceItem.Key;
//}
//var activities = body.Activities;
//foreach(var activityItem in activities)
//{
// var activity = activityItem.Value;
// var actName = activityItem.Key;
//}
var jointPointsInDepthSpace = new Dictionary <JointType, Point>();
Joint head = body.Joints[JointType.Head];
Rect headRct, chestRct;
var foundHead = GetHeadRegion(body, ref headRct);
if (foundHead)
{
//start calculating heart Rate
using (InfraredFrame irFrame = multiFrame.InfraredFrameReference.AcquireFrame())
{
if (irFrame != null)
{
var timestamp = irFrame.RelativeTime;
irFrame.CopyFrameDataToArray(this.irData);
var avg = GetAverageIRIntensity(headRct, this.irData);
this.irAverages[matrixCounter[bodyIndex], bodyIndex] = avg;
this.matrices[matrixCounter[bodyIndex], bodyIndex].BodyIndex = bodyIndex;
this.matrices[matrixCounter[bodyIndex], bodyIndex].AverageIRIntensity = avg;
this.matrices[matrixCounter[bodyIndex], bodyIndex].TimeStamp = timestamp;
if (matrixCounter[bodyIndex] == 0)
{
this.matrices[matrixCounter[bodyIndex], bodyIndex].AverageIRIntensity = 0;
this.matrices[matrixCounter[bodyIndex], bodyIndex].RateOfChange = 0;
}
else
{
var timeDelta = this.matrices[matrixCounter[bodyIndex], bodyIndex].TimeStamp - this.matrices[matrixCounter[bodyIndex] - 1, bodyIndex].TimeStamp;
var freqDelta = this.matrices[matrixCounter[bodyIndex], bodyIndex].AverageIRIntensity - this.matrices[matrixCounter[bodyIndex] - 1, bodyIndex].AverageIRIntensity;
var rateOfChange = freqDelta / timeDelta.Milliseconds;
this.matrices[matrixCounter[bodyIndex], bodyIndex].RateOfChange = rateOfChange;
}
matrixCounter[bodyIndex]++;
//every 6 seconds, or 1800 frames calculate a value...
if ((matrixCounter[bodyIndex]) % 256 == 0)
{
AForge.Math.Complex[] cmpxArray = new AForge.Math.Complex[256];
var cnt = matrixCounter[bodyIndex] - 256;
for (int i = 0; i < 256; i++)
{
cmpxArray[i] = new AForge.Math.Complex(this.irAverages[cnt, bodyIndex], 0);
cnt++;
}
var cmplx = Hann(cmpxArray);
FourierTransform.FFT(cmplx, FourierTransform.Direction.Forward);
double bpm = 0;
for (int j = 1; j < cmpxArray.Length; j++)
{
if (cmpxArray[j].Re > cmpxArray[j - 1].Re)
{
bpm = cmpxArray[j].Re * 9;
//was 12;
}
}
HeartRateCount = (int)bpm;
StatusTexts[bodyIndex].Text = string.Format("{0} bpm", HeartRateCount);
if (!currentlyShowingResults && currentlyTakingVitals)
{
VisualStateManager.GoToState(this, VSG_ShowResults.Name, true);
//this.takeVitals.Stop();
this.currentlyTakingVitals = false;
this.title5.Text = string.Format("Person tracked at: {0}", bodyIndex);
this.title6.Text = string.Format("has a HeartRate Of {0} bpm", HeartRateCount);
//this.showVitalResults.Begin();
currentlyShowingResults = true;
}
}
if (matrixCounter[bodyIndex] >= maxSeconds)
{
// //we're finished counting...
VisualStateManager.GoToState(this, VSG_ShowResults.Name, true);
matrixCounter[bodyIndex] = 0;
}
// var size = matrixCounter[bodyIndex] - 1;
// var calcAvg = AverageRateOfChange(size, bodyIndex);
// var calcStdDev = Deviation(calcAvg, size, bodyIndex);
// var heartRateCount = CalculateRateSpikesMoreThan3Deviations(calcAvg, calcStdDev, size, bodyIndex);
// HeartRateCount = heartRateCount;
// StatusTexts[bodyIndex].Text = string.Format("{0} bpm", HeartRateCount);
// matrixCounter[bodyIndex] = 0;
//}
}
}
}
// var foundChest = GetChestRegion(body, ref chestRct);
CameraSpacePoint position = body.Joints[JointType.Head].Position;
DepthSpacePoint depthSpacePoint = coordinateMapper.MapCameraPointToDepthSpace(position);
jointPointsInDepthSpace[JointType.Head] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
StatusTexts[bodyIndex].SetValue(Canvas.TopProperty, 140 + jointPointsInDepthSpace[JointType.Head].Y);
StatusTexts[bodyIndex].SetValue(Canvas.LeftProperty, 180 + jointPointsInDepthSpace[JointType.Head].X);
showStartScreenCounter = 0;
VisualStateManager.GoToState(this, VSG_TakingVitals.Name, true);
}
else
{
//if
//if(currentlyShowingResults || currentlyTakingVitals)
//{
// VisualStateManager.GoToState(this, VSG_Attract.Name, true);
//this.showVitalResults.Stop();
//this.takeVitals.Stop();
//this.storyboard.Begin();
//this.currentlyShowingResults = false;
//this.currentlyTakingVitals = false;
//
// }
showStartScreenCounter++;
if (showStartScreenCounter > 600)
{
VisualStateManager.GoToState(this, VSG_Attract.Name, true);
showStartScreenCounter = 0;
}
StatusTexts[bodyIndex].Visibility = Visibility.Collapsed;
// mainImage.Visibility = Visibility.Collapsed;
}
}
}
else
{
VisualStateManager.GoToState(this, VSG_Attract.Name, true);
}
}
#endregion
//#region "Heart Rate Section"
//using (BodyFrame bodyFrame = multiFrame.BodyFrameReference.AcquireFrame())
//{
// if (bodyFrame == null)
// {
// return;
// }
// using (BodyIndexFrame bodyIndexFrame = multiFrame.BodyIndexFrameReference.AcquireFrame())
// {
// if (bodyIndexFrame == null)
// {
// return;
// }
// using (InfraredFrame irFrame = multiFrame.InfraredFrameReference.AcquireFrame())
// {
// if (irFrame == null)
// {
// return;
// }
// needsMoreFrames = m_heartRate.GetCurrentHeartRate(bodyFrame, bodyIndexFrame, irFrame);
// }
// }
//}
//#endregion
}
}
/// <summary>
/// Subtracts a scalar value from a complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="s">A scalar value.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the difference.</returns>
///
public static Complex operator -(Complex a, double s)
{
return(Complex.Subtract(a, s));
}
/// <summary>
/// 2-D Discrete Fourier Transform.
/// </summary>
///
/// <param name="data">The data to transform.</param>
/// <param name="direction">The transformation direction.</param>
///
public static void DFT2(Complex[][] data, FourierTransform.Direction direction)
{
int n = data.Length;
int m = data[0].Length;
// process rows
var row = new Complex[m];
for (int i = 0; i < n; i++)
{
// copy row
for (int j = 0; j < row.Length; j++)
row[j] = data[i][j];
// transform it
DFT(row, direction);
// copy back
for (int j = 0; j < row.Length; j++)
data[i][j] = row[j];
}
// process columns
var col = new Complex[n];
for (int j = 0; j < n; j++)
{
// copy column
for (int i = 0; i < col.Length; i++)
col[i] = data[i][j];
// transform it
DFT(col, direction);
// copy back
for (int i = 0; i < col.Length; i++)
data[i][j] = col[i];
}
}
/// <summary>
/// Subtracts a complex number from a scalar value.
/// </summary>
///
/// <param name="s">A scalar value.</param>
/// <param name="a">A <see cref="Complex"/> instance.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the difference.</returns>
///
public static Complex operator -(double s, Complex a)
{
return(Complex.Subtract(s, a));
}
/// <summary>
/// 2-D Fast Fourier Transform.
/// </summary>
///
/// <param name="data">The data to transform..</param>
/// <param name="direction">The Transformation direction.</param>
///
public static void FFT2(Complex[][] data, FourierTransform.Direction direction)
{
int n = data.Length;
int m = data[0].Length;
// process rows
for (int i = 0; i < data.Length; i++)
{
// transform it
FFT(data[i], direction);
}
// process columns
var col = new Complex[n];
for (int j = 0; j < m; j++)
{
// copy column
for (int i = 0; i < col.Length; i++)
col[i] = data[i][j];
// transform it
FFT(col, direction);
// copy back
for (int i = 0; i < col.Length; i++)
data[i][j] = col[i];
}
}
/// <summary>
/// Multiplies two complex numbers.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="b">A <see cref="Complex"/> instance.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the result of multiplication.</returns>
///
public static Complex operator *(Complex a, Complex b)
{
return(Complex.Multiply(a, b));
}
/// <summary>
/// Computes the discrete Fourier transform (DFT) of the given complex vector, storing
/// the result back into the vector. The vector's length must be a power of 2. Uses the
/// Cooley-Tukey decimation-in-time radix-2 algorithm.
/// </summary>
///
/// <exception cref="System.ArgumentException">Length is not a power of 2.</exception>
///
private static void TransformRadix2(Complex[] complex)
{
int n = complex.Length;
int levels = (int)Math.Floor(Math.Log(n, 2));
if (1 << levels != n)
throw new ArgumentException("Length is not a power of 2");
// TODO: keep those tables in memory
var cosTable = new double[n / 2];
var sinTable = new double[n / 2];
for (int i = 0; i < n / 2; i++)
{
cosTable[i] = Math.Cos(2 * Math.PI * i / n);
sinTable[i] = Math.Sin(2 * Math.PI * i / n);
}
// Bit-reversed addressing permutation
for (int i = 0; i < complex.Length; i++)
{
int j = unchecked((int)((uint)Reverse(i) >> (32 - levels)));
if (j > i)
{
var temp = complex[i];
complex[i] = complex[j];
complex[j] = temp;
}
}
// Cooley-Tukey decimation-in-time radix-2 FFT
for (int size = 2; size <= n; size *= 2)
{
int halfsize = size / 2;
int tablestep = n / size;
for (int i = 0; i < n; i += size)
{
for (int j = i, k = 0; j < i + halfsize; j++, k += tablestep)
{
int h = j + halfsize;
double re = complex[h].Re();
double im = complex[h].Im();
double tpre = +re * cosTable[k] + im * sinTable[k];
double tpim = -re * sinTable[k] + im * cosTable[k];
double rej = complex[j].Re();
double imj = complex[j].Im();
complex[h] = new Complex(rej - tpre, imj - tpim);
complex[j] = new Complex(rej + tpre, imj + tpim);
}
}
// Prevent overflow in 'size *= 2'
if (size == n)
break;
}
}
/// <summary>
/// Multiplies a complex number by a scalar value.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="s">A scalar value.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the result of multiplication.</returns>
///
public static Complex operator *(Complex a, double s)
{
return(Complex.Multiply(a, s));
}
/// <summary>
/// Computes the circular convolution of the given complex
/// vectors. All vectors must have the same length.
/// </summary>
///
public static void Convolve(Complex[] x, Complex[] y, Complex[] result)
{
FFT(x, FourierTransform.Direction.Forward);
FFT(y, FourierTransform.Direction.Forward);
for (int i = 0; i < x.Length; i++)
{
double xreal = x[i].Re();
double ximag = x[i].Im();
double yreal = y[i].Re();
double yimag = y[i].Im();
double re = xreal * yreal - ximag * yimag;
double im = ximag * yreal + xreal * yimag;
x[i] = new Complex(re, im);
}
IDFT(x);
// Scaling (because this FFT implementation omits it)
for (int i = 0; i < x.Length; i++)
{
result[i] = x[i] / x.Length;
}
}
/// <summary>
/// Divides one complex number by another complex number.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="b">A <see cref="Complex"/> instance.</param>
///
/// <returns>A new Complex instance containing the result.</returns>
/// <returns>Returns new <see cref="Complex"/> instance containing the result of division.</returns>
///
public static Complex operator /(Complex a, Complex b)
{
return(Complex.Divide(a, b));
}
/// <summary>
/// Processes the filter.
/// </summary>
///
protected override void ProcessFilter(ComplexSignal sourceData, ComplexSignal destinationData)
{
SampleFormat format = sourceData.SampleFormat;
int channels = sourceData.Channels;
int length = sourceData.Length;
int samples = sourceData.Samples;
unsafe
{
Complex* src = (Complex*)sourceData.Data.ToPointer();
Complex* dst = (Complex*)destinationData.Data.ToPointer();
Complex* comb = (Complex*)combSignal.Data.ToPointer();
Complex d = new Complex();
for (int i = 0; i < samples; i++, src++, dst++, comb++)
{
d.Re = (src[0] * comb[0]).Magnitude;
*dst = d;
}
}
}
/// <summary>
/// Divides a complex number by a scalar value.
/// </summary>
///
/// <param name="a">A <see cref="Complex"/> instance.</param>
/// <param name="s">A scalar value.</param>
///
/// <returns>Returns new <see cref="Complex"/> instance containing the result of division.</returns>
///
public static Complex operator /(Complex a, double s)
{
return(Complex.Divide(a, s));
}
