public double Hor(DrawingContext dc, Target TargetA, Target TargetB ,bool show)
{
//3D
Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, TargetA.point3D().Z - TargetB.point3D().Z);
Vector3D vectorB = new Vector3D(0, 1, 0);
//2D
//Vector3D vectorA = new Vector3D(TargetA.point2D().X - TargetB.point2D().X, TargetA.point2D().Y - TargetB.point2D().Y, 0);
//Vector3D vectorB = new Vector3D(1, 0, 0);
double theta = Math.Abs(Vector3D.AngleBetween(vectorA, vectorB));
theta = 90 - theta;
//if (TargetA.point3D().Y < TargetB.point3D().Y) theta = -theta;
if (show) //show angle text
{
dc.DrawText(new FormattedText(theta.ToString("f0"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushDeepSkyBlue),
new Point(TargetB.point2D().X - 35, TargetB.point2D().Y - 35));
dc.DrawLine(PenDeepSkyBlue, TargetA.point2D(), TargetB.point2D()); //show angle line
dc.DrawLine(PenDeepSkyBlue, new Point(TargetA.point2D().X, TargetB.point2D().Y), TargetB.point2D());
}
return theta;
}
public double Ver(DrawingContext dc, Target TargetA, Target TargetB, bool show)
{
//3D
Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, 0);// TargetA.point3D().Z - TargetB.point3D().Z);
Vector3D vectorB = new Vector3D(0, 1, 0);
//2D
//Vector3D vectorA = new Vector3D(TargetA.point2D().X - TargetB.point2D().X, TargetA.point2D().Y - TargetB.point2D().Y, 0);
//Vector3D vectorB = new Vector3D(0, TargetA.point2D().Y - TargetB.point2D().Y, 0);
double theta = Math.Abs(Vector3D.AngleBetween(vectorA, vectorB));
if (TargetA.point3D().X < TargetB.point3D().X) theta = -theta;
if (show) //show angle text
{
dc.DrawText(new FormattedText(theta.ToString("f0"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushLemonChiffon),
new Point(TargetB.point2D().X - 35, TargetB.point2D().Y - 35));
dc.DrawLine(PenLemonChiffon, TargetA.point2D(), TargetB.point2D()); //show angle line
//dc.DrawLine(PenLemonChiffon, new Point(TargetB.point2D().X, TargetA.point2D().Y), new Point(TargetB.point2D().X, TargetB.point2D().Y)); //show Vertical line
}
return theta;
}
public void CalPhiTwoTargetTilt(Target TargetA, Target TargetB)
{
if (GlobalCount < upperbound)
{
Phi[GlobalCount, 0] = 2 * TargetA.point3D().X;
Phi[GlobalCount, 1] = 2 * TargetA.point3D().Y;
Phi[GlobalCount, 2] = 1;
Ybar[GlobalCount, 0] = Math.Pow(TargetA.point3D().X, 2) + Math.Pow(TargetA.point3D().Y, 2);
Phi[GlobalCount + 1, 0] = 2 * TargetB.point3D().X;
Phi[GlobalCount + 1, 1] = 2 * TargetB.point3D().Y;
Phi[GlobalCount + 1, 2] = 1;
Ybar[GlobalCount + 1, 0] = Math.Pow(TargetB.point3D().X, 2) + Math.Pow(TargetB.point3D().Y, 2);
if (GlobalCount == upperbound)
{
//Calulate Phi Transpose
for (int i = 0; i < GlobalCount; i++)
{
for (int j = 0; j < Phi.GetLength(1); j++)
{
PhiTranspose[j, i] = Phi[i, j];
}
}
PhiSquare = mProd(PhiTranspose, Phi); // 3*3 matrix
PhiSquareInverse = mInverse(PhiSquare);
Omega = mProd(mProd(PhiSquareInverse, PhiTranspose), Ybar);
double randius = Math.Sqrt(Omega[2, 0] + Math.Pow(Omega[0, 0], 2) + Math.Pow(Omega[1, 0], 2));
//Cal theta
for (int i = 0; i < GlobalCount; i++)
{
{
Vector vec = new Vector(Phi[i, 0] / 2 - Omega[0, 0], Phi[i, 1] / 2 - Omega[1, 0]); //(2X,2Y)/2 - (Rx,Ry)
Vector mid = new Vector(0, 1);
ThetaList.Add(Vector.AngleBetween(vec, mid));
}
}
thetaL = ThetaList.Max();
thetaR = ThetaList.Min();
theta = thetaL - thetaR;
drawpoints(theta, thetaL, thetaR);
}
GlobalCount += 2;
}
else GlobalCount = 0;
}
public void CalPhiTwoTargetSpin(Target TargetA, Target TargetB)
{
if (GlobalCount < upperbound)
{
Phi[GlobalCount, 0] = 2 * TargetA.point3D().X;
Phi[GlobalCount, 1] = 2 * TargetA.point3D().Z;
Phi[GlobalCount, 2] = 1;
Ybar[GlobalCount, 0] = Math.Pow(TargetA.point3D().X, 2) + Math.Pow(TargetA.point3D().Z, 2);
Phi[GlobalCount + 1, 0] = 2 * TargetB.point3D().X;
Phi[GlobalCount + 1, 1] = 2 * TargetB.point3D().Z;
Phi[GlobalCount + 1, 2] = 1;
Ybar[GlobalCount + 1, 0] = Math.Pow(TargetB.point3D().X, 2) + Math.Pow(TargetB.point3D().Z, 2);
if (GlobalCount == upperbound-2)
{
//Calulate Phi Transpose
for (int i = 0; i < GlobalCount; i++)
{
for (int j = 0; j < Phi.GetLength(1); j++)
{
PhiTranspose[j, i] = Phi[i, j];
}
}
PhiSquare = mProd(PhiTranspose, Phi); // 3*3 matrix
PhiSquareInverse = mInverse(PhiSquare);
Omega = mProd(mProd(PhiSquareInverse, PhiTranspose), Ybar);
double randius = Math.Sqrt(Omega[2, 0] + Math.Pow(Omega[0, 0], 2) + Math.Pow(Omega[1, 0], 2));
for (int i = 0; i < GlobalCount; i++)//Cal theta
{
if (i % 2 == 0)
{
Vector vec = new Vector(Phi[i, 0] / 2 - Omega[0, 0], Phi[i, 1] / 2 - Omega[1, 0]); //(X,Y)-(Rx,Ry)
Vector mid = new Vector(0, 1);
ThetaListRight.Add(Vector.AngleBetween(vec, mid));
}
else
{
Vector vec = new Vector(Phi[i, 0] / 2 - Omega[0, 0], Phi[i, 1] / 2 - Omega[1, 0]); //(X,Y)-(Rx,Ry)
Vector mid = new Vector(0, 1);
ThetaListLeft.Add(Vector.AngleBetween(vec, mid));
}
}
tmaxR = ThetaListRight.Max();
tminR = ThetaListRight.Min();
thetaR =Math.Abs( tmaxR - tminR);
tmaxL = ThetaListLeft.Max();
tminL = ThetaListLeft.Min();
thetaL = Math.Abs(tmaxL - tminL);
theta = (thetaR + thetaL) / 2;
drawpoints(theta, thetaR, thetaL);
}
GlobalCount += 2;
}
else GlobalCount = 0;
}
public double HeadSpin(Target TargetA, Target TargetB)
{
Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, 0, TargetA.point3D().Z - TargetB.point3D().Z);
Vector3D vectorB = new Vector3D(0, 0, -1);
double theta = Vector3D.AngleBetween(vectorA, vectorB);
if ((TargetA.point3D().X > TargetB.point3D().X))
theta = -theta;
return theta;
}
public double HeadTilt(Target TargetA, Target TargetB)
{
Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y,0);
Vector3D vectorB = new Vector3D(0, -1, 0); //待驗證
return Vector3D.AngleBetween(vectorA, vectorB);
}
public double TrunkSpin(DrawingContext dc, Target TargetA, Target TargetB)
{
Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, 0, TargetA.point3D().Z - TargetB.point3D().Z);
Vector3D vectorB = new Vector3D(-1, 0, 0);
double theta = Vector3D.AngleBetween(vectorA, vectorB);
if (TargetA.point3D().Z > TargetB.point3D().Z)
theta = -theta;
dc.DrawText(new FormattedText(theta.ToString("f0"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushGreenYellow),
new Point(TargetB.point2D().X - 35, TargetB.point2D().Y - 35));
return theta;
}
public double TrunkTilt(Target TargetA, Target TargetB)
{
Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, 0);
Vector3D vectorB = new Vector3D(-1, 0, 0);
return Vector3D.AngleBetween(vectorA, vectorB);
}
public VerticalData VerticalDistance(DrawingContext dc, Target TargetA, Target TargetB, double FootCenter3DX, double FootCenter2DX, bool show)
{
double AC = Math.Ceiling(Math.Sqrt(Math.Pow(TargetA.point3D().X - FootCenter3DX, 2)) * 100);
double BC = Math.Ceiling(Math.Sqrt(Math.Pow(TargetB.point3D().X - FootCenter3DX, 2)) * 100);
double CC = (TargetA.point3D().Y - TargetB.point3D().Y)* 100;
VerticalData abc = new VerticalData();
abc.Right = AC;
abc.Left = BC;
abc.Mid = CC;
if (show) //show angle text
{
dc.DrawText(new FormattedText(AC.ToString("f0"), //AC
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushLightGreen),
new Point(TargetA.point2D().X - 35, TargetA.point2D().Y - 25));
dc.DrawText(new FormattedText(BC.ToString("f0"), //BC
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushLightGreen),
new Point(TargetB.point2D().X , TargetB.point2D().Y - 25));
dc.DrawText(new FormattedText(CC.ToString("f0"), //CC
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushLightGreen),
new Point(FootCenter2DX-10, TargetB.point2D().Y/2 + TargetA.point2D().Y/2));
dc.DrawLine(PenLightGreen, TargetA.point2D(), new Point(FootCenter2DX, TargetA.point2D().Y)); //AC
dc.DrawLine(PenLightGreen, TargetB.point2D(), new Point(FootCenter2DX, TargetB.point2D().Y)); //BC
dc.DrawLine(PenLightGreen, new Point(FootCenter2DX, TargetA.point2D().Y), new Point(FootCenter2DX, TargetB.point2D().Y)); //CC
}
return abc;
}
public double Length(DrawingContext dc, Target TargetA, Target TargetB)
{
double AA = Math.Ceiling(Math.Sqrt((Math.Pow(TargetA.point3D().X - TargetB.point3D().X, 2) + Math.Pow(TargetA.point3D().Y - TargetB.point3D().Y, 2) + Math.Pow(TargetA.point3D().Z - TargetB.point3D().Z, 2))) * 100);
dc.DrawLine(PenYellow, TargetA.point2D(), TargetB.point2D());
dc.DrawText(new FormattedText(AA.ToString("f0"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushYellow),
new Point(TargetB.point2D().X - 55, TargetB.point2D().Y - 15));
return AA;
}
public double threePts(DrawingContext dc, Target TargetA, Target TargetB, Target TargetC, bool show)
{
//Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, TargetA.point3D().Z - TargetB.point3D().Z);
//Vector3D vectorB = new Vector3D(TargetB.point3D().X - TargetC.point3D().X, TargetB.point3D().Y - TargetC.point3D().Y, TargetB.point3D().Z - TargetC.point3D().Z);
Vector3D vectorA = new Vector3D(TargetA.point2D().X - TargetB.point2D().X, TargetA.point2D().Y - TargetB.point2D().Y, 0);
Vector3D vectorB = new Vector3D(TargetB.point2D().X - TargetC.point2D().X, TargetB.point2D().Y - TargetC.point2D().Y, 0);
double theta = Math.Abs(180 - Vector3D.AngleBetween(vectorA, vectorB));
if (show) //show angle text
{
dc.DrawText(new FormattedText(theta.ToString("f0"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushYellow),
new Point(TargetB.point2D().X - 55, TargetB.point2D().Y - 15));
dc.DrawLine(PenYellow, TargetA.point2D(), TargetB.point2D()); //show angle line
dc.DrawLine(PenYellow, TargetB.point2D(), TargetC.point2D());
}
return theta;
}
public double twoLines(DrawingContext dc, Target TargetA, Target TargetB, Target TargetC, Target TargetD, bool show)
{
//3D
//Vector3D vectorA = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, TargetA.point3D().Z - TargetB.point3D().Z);
//Vector3D vectorB = new Vector3D(TargetC.point3D().X - TargetD.point3D().X, TargetC.point3D().Y - TargetD.point3D().Y, TargetC.point3D().Z - TargetD.point3D().Z);
//2D
Vector3D vectorA = new Vector3D(TargetA.point2D().X - TargetB.point2D().X, TargetA.point2D().Y - TargetB.point2D().Y, 0);
Vector3D vectorB = new Vector3D(TargetC.point2D().X - TargetD.point2D().X, TargetC.point2D().Y - TargetD.point2D().Y, 0);
double theta = Math.Abs(Vector3D.AngleBetween(vectorA, vectorB));
if (show)
{
dc.DrawText(new FormattedText(theta.ToString("f0"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushOrange),
new Point((TargetA.point2D().X + TargetB.point2D().X) / 2, (TargetA.point2D().Y + TargetC.point2D().Y-40) / 2));
dc.DrawLine(PenOrange, TargetA.point2D(), TargetB.point2D()); //show angle line
dc.DrawLine(PenOrange, TargetC.point2D(), TargetD.point2D());
}
return theta;
}
private void WindowLoaded(object sender, RoutedEventArgs e)
{
//SplashScreen splashScreen = new SplashScreen("Images/SplashScreenImage.bmp");
//splashScreen.Show(true);
this.drawingGroup = new DrawingGroup(); // Create the drawing group we'll use for drawing
this.imageSource = new DrawingImage(this.drawingGroup); // Create an image source that we can use in our image control
Image.Source = this.imageSource; // Display the drawing using our image control
for (int t = 0; t <= NumbersOfTarget; t++)
{
Target target = new Target(t);
TargetList.Add(target);
}
for (int d = 0; d <= 93; d++) //指標特徵運算A1~A66
{
List<double> array = new List<double>() { };
DataList.Add(array);
Data.Add(0);
}
// Look through all sensors and start the first connected one.
// This requires that a Kinect is connected at the time of app startup.
// To make your app robust against plug/unplug,
// it is recommended to use KinectSensorChooser provided in Microsoft.Kinect.Toolkit (See components in Toolkit Browser).
foreach (var potentialSensor in KinectSensor.KinectSensors)
{
if (potentialSensor.Status == KinectStatus.Connected)
{
this.sensor = potentialSensor;
break;
}
}
if (null != this.sensor) //Initailization
{
this.sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
this.sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
this.sensor.SkeletonStream.Enable();
this.colorPixels = new byte[this.sensor.ColorStream.FramePixelDataLength];
this.depthPixels = new DepthImagePixel[this.sensor.DepthStream.FramePixelDataLength];
this.depthShortPixels = new short[this.sensor.DepthStream.FramePixelDataLength];
this.boolPixels = new int[this.sensor.DepthStream.FramePixelDataLength];
this.sensor.AllFramesReady += this.SensorAllFramesReady;
this.colorBitmap = new WriteableBitmap(this.sensor.ColorStream.FrameWidth, this.sensor.ColorStream.FrameHeight, 96.0, 96.0, PixelFormats.Bgr32, null);
this.depth4background = new DepthImagePixel[this.sensor.DepthStream.FramePixelDataLength];
// Start the sensor!
try
{
this.sensor.Start();
}
catch (IOException)
{
this.sensor = null;
}
}
if (null == this.sensor)
{
this.statusBarText.Text = Properties.Resources.NoKinectReady;
}
AngleTimer = new System.Timers.Timer(1000);
AngleTimer.Elapsed += new System.Timers.ElapsedEventHandler(theout);
AngleTimer.AutoReset = true;
AngleTimer.Enabled = true;
}