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