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