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;
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
_sensor = KinectSensor.GetDefault();
for (int t = 0; t <= NumbersOfTarget; t++)
{
Target target = new Target(t);
TargetList.Add(target);
}
pointTarget = new Target(0);
if (_sensor != null)
{
_sensor.Open();
this.coordinateMapper = this._sensor.CoordinateMapper;
_reader = _sensor.OpenMultiSourceFrameReader(FrameSourceTypes.Color | FrameSourceTypes.Depth | FrameSourceTypes.Infrared | FrameSourceTypes.Body);
_reader.MultiSourceFrameArrived += Reader_MultiSourceFrameArrived;
this.colorBitmap = new WriteableBitmap(this._sensor.ColorFrameSource.FrameDescription.Width, this._sensor.ColorFrameSource.FrameDescription.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
this.boolPixels = new int[this._sensor.DepthFrameSource.FrameDescription.LengthInPixels];
checkBoxShowXYZ.IsChecked = true;
}
}
public Vector3D CrossProduct(Target TargetA, Target TargetB, Target TargetC)
{
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 - TargetB.point3D().X, TargetC.point3D().Y - TargetB.point3D().Y, TargetC.point3D().Z - TargetB.point3D().Z);
return Vector3D.CrossProduct(vectorA, vectorB);
}
public Vector3D TwoPointVector(Target TargetA, Target TargetB, DrawingContext dc, bool show)
{
Vector3D vectorC = new Vector3D(TargetA.point3D().X - TargetB.point3D().X, TargetA.point3D().Y - TargetB.point3D().Y, TargetA.point3D().Z - TargetB.point3D().Z);
if (show)
{
dc.DrawText(new FormattedText(vectorC.X.ToString("f2") + ", " + vectorC.Y.ToString("f2") + ", " + vectorC.Z.ToString("f2"),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Verdana"),
25, brushDeepSkyBlue),
new Point(TargetB.point2D().X + 20, TargetB.point2D().Y + 20));
}
return vectorC;
//dc.DrawLine(PenDeepSkyBlue, TargetB.point2D(), TargetA.point2D()); //show angle line
//dc.DrawLine(PenDeepSkyBlue, TargetB.point2D(), TargetC.point2D());
}
public double AngleBetween(Target TargetA, Target TargetB, Target TargetC)
{
//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 - TargetB.point3D().X, TargetC.point3D().Y - TargetB.point3D().Y, TargetC.point3D().Z - TargetB.point3D().Z);
//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));
double theta = Vector3D.AngleBetween(vectorA, vectorB);
//theta = 90 - theta;
//if (TargetA.point3D().Y < TargetB.point3D().Y) theta = -theta;
return theta;
}
public double Length(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);
return AA;
}
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 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 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 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 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;
}
