SimulationServerYL/Models/SimulationCommon/GeDianShengCheng.cs

using BHJD.DEMdll.Public;
using Unity.Mathematics;

namespace QuYuSaoMiao
{
    public class GeDianShengCheng
    {
        public static DemHelper DemHelper()
        {
            IDbHelper db = Factory.Load("10.130.100.5", "5432", "postgres", "postgres", "rescue_patrol_platform");
            DemHelper ddemHelper = new DemHelper(db);
            return ddemHelper;
        }

        public static double[,] MaxMinPoly(List<double[]> poly) //多边形矩形边界
        {
            List<double> PolyLon = new List<double>();
            List<double> PolyLat = new List<double>();
            foreach (double[] element in poly)
            {
                PolyLon.Add(element[0]);
                PolyLat.Add(element[1]);
            }

            double[,] MM1 = new double[2, 2];
            MM1[0, 0] = PolyLon.Min(); //经度最小（西界）
            MM1[0, 1] = PolyLon.Max(); //经度最大（东界）
            MM1[1, 0] = PolyLat.Min(); //纬度最小（南界）
            MM1[1, 1] = PolyLat.Max(); //纬度最大（北界）
            return MM1;
        }

        public static bool isinpoly(double[] zuobiao, List<double[]> poly, DemHelper demHelper) //已经替换成在边界内算法
        {
            double[,] polyOut = MaxMinPoly(poly);
            if (zuobiao[0] <= polyOut[0, 0] || zuobiao[0] >= polyOut[0, 1] || zuobiao[1] <= polyOut[1, 0] ||
                zuobiao[1] >= polyOut[1, 1])
            {
                return false;
            }
            else
            {
                string polystring = "MULTIPOLYGON(((";
                foreach (double[] element in poly)
                {
                    double e0 = element[0];
                    double e1 = element[1];
                    polystring = polystring + element[0] + " " + element[1] + ",";
                }

                polystring = polystring + poly[0][0] + " " + poly[0][1] + ")))";
                string pointstring = "POINT (" + zuobiao[0] + " " + zuobiao[1] + ")";
                // Console.WriteLine("isinpoly?");
                //Console.WriteLine(pointstring+polystring);
                string r1 = demHelper.doDetermine(pointstring, polystring);
                // Console.WriteLine(r1);
                //double[,] polyOut = MaxMinPoly(poly);
                //if (zuobiao[0] <= polyOut[0, 0] || zuobiao[0] >= polyOut[0, 1] || zuobiao[1] <= polyOut[1, 0] || zuobiao[1] >= polyOut[1, 1])
                // { return false; }
                // else
                //{
                return (r1 == "True" ? true : false);
                // }
            }
        }

        //格点生成方法：输入前四个为经纬度上下限，数组为多边形，后两个是经纬度分辨率。输出0经，1纬，2高，3是否被侦察的【01】变量。
        public static List<double[]> NPGenerate(double lonmin, double lonmax, double latmin, double latmax,
            List<double[]> juxing, double netscalelon, double netscalelat, DemHelper demHelper)

        {
            Console.WriteLine("格点生成");

            List<double[]> netpoints = new List<double[]>();
            for (double[] netpointprev = new double[3] { lonmin, latmax, 0 };
                 netpointprev[1] >= latmin;
                 netpointprev[1] = netpointprev[1] - netscalelat)
            {
                for (netpointprev[0] = lonmin;
                     netpointprev[0] <= lonmax;
                     netpointprev[0] = netpointprev[0] + netscalelon)
                {
                    if (isinpoly(netpointprev, juxing, demHelper) == true)
                    {
                        double HH = 0; //需要替换为该点【netpointprev[0], netpointprev[1]】高程
                        double[] netpointtemp = new double[3] { netpointprev[0], netpointprev[1], HH };
                        //netpointtemp = netpointprev;
                        netpoints.Add(netpointtemp);
                    }
                }
            }

            return netpoints;
        }

        //格点初始化
        public static List<bool> NPprem(List<double[]> NPcoo)
        {
            List<bool> NPprem0 = new List<bool>();
            int i = 0;
            foreach (double[] element in NPcoo)
            {
                //object[] NPrentemp = new object[4] { element[0], element[1], element[2], false };
                NPprem0.Add(false);
                i++;
            }

            return NPprem0;
        }

        //格点显示,可以不用
        public static void ListPoints(List<double[]> NPtoRead)
        {
            int i1 = 0;
            int i2 = 0;
            int i3;
            int irow;
            int iLine = 1;
            Console.Write("{");
            foreach (double[] element in NPtoRead)
            {
                if (i1 != 0 && element[1] != NPtoRead[i1 - 1][1])
                {
                    i3 = i1 - 1;
                    irow = i3 - i2 + 1;
                    i2 = i1;
                    iLine++;
                    Console.Write("<本行格点数：" + irow + ">}" + Environment.NewLine + "{");
                }

                string lontax = element[0].ToString("F5");
                string lattax = element[1].ToString("F5");
                Console.Write("【E" + lontax + " | N" + lattax + " | h" + element[2] + "】\t");
                i1++;
            }

            i3 = i1 - 1;
            irow = i3 - i2 + 1;
            i2 = i1;
            Console.Write("<本行格点数：" + irow + ">}" + Environment.NewLine);
            int numberP = NPtoRead.Count;
            Console.WriteLine("行数:" + iLine + ";格点数量:" + numberP);
        }

        //两点间距计算，输出米
        public static double distance(double[] P1, double[] P2, DemHelper demHelper)
        {
            //double DL = 6371.004 * Math.Acos(Math.Sin(Math.PI / 180 * P1[1]) * Math.Sin(Math.PI / 180 * P2[1]) + Math.Cos(Math.PI / 180 * P1[1]) * Math.Cos(Math.PI / 180 * P2[1]) * Math.Cos(Math.PI / 180 * (P1[0] - P2[0]))) * 1000;
            //double D = Math.Pow(Math.Pow(DL, 2) + Math.Pow(P1[2] - P2[2], 2), 0.5);
            //return D;
            double3 p1 = new double3();
            double3 p2 = new double3();
            p1.x = P1[0];
            p1.y = P1[1];
            p1.z = P1[2];
            p2.x = P2[0];
            p2.y = P2[1];
            p2.z = P2[2];


            double D1 = demHelper.getDistance(p1, p2);
            //double DD1 = D1;
            //Console.WriteLine("距离：" + D1);
            return D1;
        }

        //两点间高度角计算，从P1到P2
        public static double angleH(double[] P1, double[] P2, DemHelper demHelper)
        {
            if (P1[2] == P2[2])
            {
                return 0;
            }
            else if (P1[0] == P2[0] && P1[1] == P2[1])
            {
                if (P1[2] < P2[2])
                {
                    return 90;
                }
                else
                {
                    return -90;
                }
            }
            else
            {
                //if (P1[2] < P2[2])
                //{
                double angleH = Math.Asin((P2[2] - P1[2]) / distance(P1, P2, demHelper)) * (180 / Math.PI);
                return angleH;
                //}
                //else
                //{
                //    double angleH = Math.Asin((P2[2] - P1[2]) / distance(P1, P2)) * (180 / Math.PI);
                //    return angleH;
                //}
            }
        }

        //MoveTo，输入单位坐标、目标坐标，输出时步结束时的单位坐标
        public static double[] MoveTo(double[] aircraft, double[] target, double V, double TimeStep,
            DemHelper demHelper)
        {
            double[] direction =
            {
                (target[0] - aircraft[0]) / distance(aircraft, target, demHelper),
                (target[1] - aircraft[1]) / distance(aircraft, target, demHelper),
                (target[2] - aircraft[2]) / distance(aircraft, target, demHelper)
            };
            double[] delta = { direction[0] * V * TimeStep, direction[1] * V * TimeStep, direction[2] * V * TimeStep };
            double[] next = { aircraft[0] + delta[0], aircraft[1] + delta[1], aircraft[2] + delta[2] };

            return next;
        }

        public static bool isBlocked(double[] P1, double[] P2, DemHelper demHelper) //是否被地形遮挡
        {
            double3 p1 = new double3();
            double3 p2 = new double3();
            p1.x = P1[0];
            p1.y = P1[1];
            p1.z = P1[2];
            p2.x = P2[0];
            p2.y = P2[1];
            p2.z = P2[2];
            //string r2 = demHelper.doIntervisibility(p1, p2);//这三行代码用于运行判定
            // Console.WriteLine(r2);
            //return (r2 == "True" ? false : true);
            return false;
        }

        //是否符合观察条件
        public static bool IsInSight(double[] aircraft, double[] netpoint, double LSight, double angleH0,
            double angleH1, DemHelper demHelper) //-90，-45

        {
            if (distance(aircraft, netpoint, demHelper) <= LSight && angleH0 <= angleH(aircraft, netpoint, demHelper) &&
                angleH1 >= angleH(aircraft, netpoint, demHelper) && isBlocked(aircraft, netpoint, demHelper) == false)
            {
                return true;
            }
            else
            {
                return false;
            }
        }


        //路径生成，输入区域边界，扫描线间距，扫描高度

        /// <summary>
        /// 
        /// </summary>
        /// <param name="poly">想定文件 </param>
        /// <param name="ScanRange">想定文件 低空：300  高空：</param>
        /// <param name="hSC">想定文件 低空 300</param>
        /// <param name="isRealHeight">想定文件 true</param>
        /// <param name="isSurround">想定文件 true</param>
        /// <param name="SurroundNum">想定文件 1</param>
        /// <param name="demHelper">想定文件 </param>
        /// <returns></returns>
        public static List<double[]> scanroute(List<double[]> poly, double ScanRange, double hSC, bool isRealHeight,
            bool isSurround, int SurroundNum, DemHelper demHelper)
        {
            

            List<double[]> scanroute = new List<double[]>();
            double HH = 1; //需要替换为该点高程
            if (isSurround == false)
            {
                double[,] RactEdge = MaxMinPoly(poly);

                double[] searchpoint =
                {
                    RactEdge[0, 0] + 0.5 *
                    (ScanRange / (6371004 * Math.Cos(RactEdge[1, 0] * Math.PI / 180) * 2 * Math.PI / 360)),
                    RactEdge[1, 0], HH + hSC
                };
                bool isLastIn = false;
                int ZF = 1;
                if (isinpoly(searchpoint, poly, demHelper) == true)
                {
                    scanroute.Add(searchpoint);
                    isLastIn = true;
                }

                searchpoint[1] = searchpoint[1] + ZF * 0.5 * (ScanRange / (6371004 * 2 * Math.PI / 360));
                for (;; searchpoint[1] = searchpoint[1] + ZF * 0.5 * (ScanRange / (6371004 * 2 * Math.PI / 360)))
                {
                    double[] searchpoint1 =
                    {
                        searchpoint[0], searchpoint[1] - ZF * 0.5 * (ScanRange / (6371004 * 2 * Math.PI / 360)),
                        isRealHeight == true ? HH + hSC : hSC
                    };
                    double[] searchpoint0 = { searchpoint[0], searchpoint[1], isRealHeight == true ? HH + hSC : hSC };
                    if (isinpoly(searchpoint, poly, demHelper) == true && isLastIn == false)
                    {
                        scanroute.Add(searchpoint1);
                        isLastIn = true;
                    }
                    else if (isLastIn == true && isinpoly(searchpoint, poly, demHelper) == false)
                    {
                        scanroute.Add(searchpoint0);
                        isLastIn = false;
                        if (ZF == 1)
                        {
                            searchpoint[0] = searchpoint[0] + ScanRange /
                                (6371004 * Math.Cos(RactEdge[1, 0] * Math.PI / 180) * 2 * Math.PI / 360);
                            searchpoint[1] = RactEdge[1, 1] + 0.5 * (ScanRange / (6371004 * 2 * Math.PI / 360));
                        }
                        else
                        {
                            searchpoint[0] = searchpoint[0] + ScanRange /
                                (6371004 * Math.Cos(RactEdge[1, 0] * Math.PI / 180) * 2 * Math.PI / 360);
                            searchpoint[1] = RactEdge[1, 0] - 0.5 * (ScanRange / (6371004 * 2 * Math.PI / 360));
                        }

                        ZF = -ZF;
                    }

                    if (searchpoint[0] > RactEdge[0, 1])
                    {
                        break;
                    }
                }

                return scanroute;
            }
            else
            {
                int len = poly.Count;
                for (int i = 0; i < len; i++)
                {
                    double[] p = poly[i];
                    double[] p1 = poly[i == 0 ? len - 1 : i - 1];
                    double[] p2 = poly[i == len - 1 ? 0 : i + 1];
                    double v1EW = (p1[0] - p[0]) * (6371004 * Math.Cos(p[1] * Math.PI / 180) * 2 * Math.PI / 360);
                    double v1SN = (p1[1] - p[1]) * (6371004 * 2 * Math.PI / 360);
                    double n1 = Math.Sqrt(v1EW * v1EW + v1SN * v1SN);
                    v1EW /= n1;
                    v1SN /= n1;

                    double v2EW = (p2[0] - p[0]) * (6371004 * Math.Cos(p[1] * Math.PI / 180) * 2 * Math.PI / 360);
                    double v2SN = (p2[1] - p[1]) * (6371004 * 2 * Math.PI / 360);
                    double n2 = Math.Sqrt(v2EW * v2EW + v2SN * v2SN);
                    v2EW /= n2;
                    v2SN /= n2;

                    if (v1EW != v2EW)
                    {
                        double l = -ScanRange / Math.Sqrt((1 - (v1EW * v2EW + v1SN * v2SN)) / 2);

                        double vEW = v1EW + v2EW;

                        double vSN = v1SN + v2SN;

                        double n = l / Math.Sqrt(vEW * vEW + vSN * vSN);
                        vEW *= n;
                        vSN *= n;
                        double VEW = vEW / (6371004 * Math.Cos(p[1] * Math.PI / 180) * 2 * Math.PI / 360);
                        double VSN = vSN / (6371004 * 2 * Math.PI / 360);

                        double[] searchpoint =
                            { p[0] + VEW, p[1] + VSN, isRealHeight == true ? HH + hSC : hSC }; //H需替换为该点高程

                        scanroute.Add(searchpoint);
                    }
                }

                int len2 = scanroute.Count;
                if (SurroundNum >= 2)
                {
                    for (int i = 2; i <= SurroundNum; i++)
                    {
                        for (int j = 0; j < len2; j++)
                        {
                            scanroute.Add(scanroute[j]);
                        }
                    }
                }

                return scanroute;
            }
        }

        //航线转object
        public static List<object[]> RouteInput(string TurningPointName, List<double[]> TurningPointLocation,
            string TurningPointType, double TurningPointVelocity, double SegmentFlightFuelConsumption,
            double SegmentFlightTime, double TurningRadius)
        {
            List<object[]> SCIP = new List<object[]>();
            int i = 0;
            foreach (double[] element in TurningPointLocation)
            {
                object[] SCIPtemt =
                {
                    $"{TurningPointName}{i}", element, $"{TurningPointType}", TurningPointVelocity,
                    SegmentFlightFuelConsumption, SegmentFlightTime, TurningRadius
                };
                SCIP.Add(SCIPtemt);
                i++;
            }

            return SCIP;
        }

        //扫描线显示，不用管
        public static void ListscanRoute(List<double[]> SCtoRead)
        {
            Console.WriteLine("【showSC】");
            foreach (double[] element in SCtoRead)
            {
                string lontax = element[0].ToString("F5");
                string lattax = element[1].ToString("F5");
                Console.WriteLine("【E" + lontax + " | N" + lattax + " | h" + element[2] + "】\t");
            }
        }
    }
}