skyline TEP学习心得（2）：一些常用的三维计算数学方法
由于skyline的功能并不强大，经常会遇到一些坐标的运算需要自己实现
PS：以下的类TDPoint代表一个点，有属性X，Y，Z
1.由X，Y，Z，yaw，pitch，height（长度）表达的三维线段转为两个三维点表达
public static void LineTo3DPoints(double X, double Y, double Z, double Yaw, double Pitch, double height, ref TDPoint tTDPoint1, ref TDPoint tTDPoint2)
{
tTDPoint1 = new TDPoint(X, Y, Z);
double x2 = height * Math.Sin(Yaw * Math.PI / 180) + X;
double y2 = height * Math.Cos(Yaw * Math.PI / 180) + Y;
double z2 = Math.Atan((Math.Abs(Pitch) - 90) * -1) * (x2 - X) + Z;
tTDPoint2 = new TDPoint(x2, y2, z2);
}
2.两点距离（三维）
public static double TwoPointDistance(TDPoint tPoint1, TDPoint tPoint2)
{
return Math.Sqrt(Math.Pow(tPoint1.X - tPoint2.X, 2) + Math.Pow(tPoint1.Y - tPoint2.Y, 2) + Math.Pow(tPoint1.Z - tPoint2.Z, 2));
}
3.两点距离（二维）
public static double TwoPointDistance(TDPoint tPoint1, TDPoint tPoint2)
{
return Math.Sqrt(Math.Pow(tPoint1.X - tPoint2.X, 2) + Math.Pow(tPoint1.Y - tPoint2.Y, 2));
}
4.两线段是否相交（二维）
public static bool TwoLineIsIntersect(TDPoint tTDPoint1Start, TDPoint tTDPoint1End, TDPoint tTDPoint2Start, TDPoint tTDPoint2End)
{
if (tTDPoint1Start == null || tTDPoint1End == null || tTDPoint2Start == null || tTDPoint2End == null) return false;
double A = tTDPoint1End.Y - tTDPoint1Start.Y;
double B = tTDPoint1End.X - tTDPoint1Start.X;
double C = A * tTDPoint1Start.X - B * tTDPoint1Start.Y;
double res1 = A * tTDPoint2Start.X - B * tTDPoint2Start.Y - C;
double res2 = A * tTDPoint2End.X - B * tTDPoint2End.Y - C;
if ((res1 > 0 && res2 > 0) || (res1 < 0 && res2 < 0))
{
return false;
}
A = tTDPoint2End.Y - tTDPoint2Start.Y;
B = tTDPoint2End.X - tTDPoint2Start.X;
C = A * tTDPoint2Start.X - B * tTDPoint2Start.Y;
res1 = A * tTDPoint1Start.X - B * tTDPoint1Start.Y - C;
res2 = A * tTDPoint1End.X - B * tTDPoint1End.Y - C;
if ((res1 > 0 && res2 > 0) || (res1 < 0 && res2 < 0))
{
return false;
}
return true;
}
5.求多边形重心（二维）
public static TDPoint GetPolygonZX(List<TDPoint> tPoints)
{
double area = 0;
TDPoint center = new TDPoint();
center.X = 0;
center.Y = 0;
int n = tPoints.Count;
for (int i = 0; i < tPoints.Count - 1; i++)
{
area += (tPoints[i].X * tPoints[i + 1].Y - tPoints[i + 1].X * tPoints[i].Y) / 2;
center.X += (tPoints[i].X * tPoints[i + 1].Y - tPoints[i + 1].X * tPoints[i].Y) * (tPoints[i].X + tPoints[i + 1].X);
center.Y += (tPoints[i].X * tPoints[i + 1].Y - tPoints[i + 1].X * tPoints[i].Y) * (tPoints[i].Y + tPoints[i + 1].Y);
}
area += (tPoints[n - 1].X * tPoints[0].Y - tPoints[0].X * tPoints[n - 1].Y) / 2;
center.X += (tPoints[n - 1].X * tPoints[0].Y - tPoints[0].X * tPoints[n - 1].Y) * (tPoints[n - 1].X + tPoints[0].X);
center.Y += (tPoints[n - 1].X * tPoints[0].Y - tPoints[0].X * tPoints[n - 1].Y) * (tPoints[n - 1].Y + tPoints[0].Y);
center.X /= 6 * area;
center.Y /= 6 * area;
return center;
}
6.三维线段由两个三维点的表达方式转为X，Y，Z，yaw，pitch，height（跟1是相反）
这方法还缺了height（长度的计算），可以用2计算三维两点距离来实现
S_XValue是起点X坐标，E_YValue是终点Y坐标，如此类推
public static void CalculateYawPitchRoll(double S_XValue, double S_YValue, double E_XValue, double E_YValue, double S_HIGHValue, double E_HIGHValue,
out double yaw, out double pitch)
{
double vectorXY_X = E_XValue - S_XValue;
double vectorXY_Y = E_YValue - S_YValue;
//计算yaw
yaw = Math.Atan2(vectorXY_X, vectorXY_Y) * 180 / Math.PI;
double vectorXZ_X = E_XValue - S_XValue;
double vectorXZ_Z = E_HIGHValue - S_HIGHValue;
//计算pitch
pitch = Math.Atan2(-Math.Sqrt(Math.Pow(vectorXY_X, 2) + Math.Pow(vectorXY_Y, 2)), vectorXZ_Z) * 180 / Math.PI;
}