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 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 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 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; }