DW5S/DW5S.KxcApi/AddIns/定位/PosApi.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Security.Policy;
using System.Text;
using System.Threading.Tasks;
using DW5S.Entity;

namespace DW5S.KxcApi
{
    /// <summary>
    ///TODO 定位算法接口.在这里调用罗博士的算法库
    /// </summary>
    public static class PosApi
    {
        //置信度统计次数
        private static int confidenceCount = 1000;

        //置信度有效距离(m)
        private static int confidenceDistance = 10000;

        #region cpp dll Interop 

        //两星一地和三星解析定位算法(精度差、速度快，适用于置信度等统计时的多次调用)
        private const string gzPos = @"AddIns\定位\DLL_GZDW.dll";

        //一星一地测向带参定位
        private const string XDCX = @"AddIns\定位\DLL_DTO_DOA_DW.dll";

        //三星双时差带参、三星双时差无参、三星双频差带参、双星时频差带参、两星一地无参定位及时差线、一星两地
        private const string OtherPos = @"AddIns\定位\Position-New.dll";//DLL_11J_DW


        [DllImport(gzPos, EntryPoint = "DW_Analysis", CallingConvention = CallingConvention.Cdecl)]//两星一地和三星的解析定位算法(精度差、速度快)
        private extern static void DW_Analysis(double[] mainSatXYZ, double[] adja1XYZ, double[] adja2XYZ, double[] refStation, double tarDto1, double tarDto2, double refDto1, double refDto2, double[] posRes, int flag);


        [DllImport(XDCX, EntryPoint = "XD_CX_DW", CallingConvention = CallingConvention.Cdecl)]//一星一地测向带参
        private extern static void X1D1_Pos20240305_Core(double[] mainSat, double[] satStation, double[] cdbStation, double[] cxStation, double[] refStation, double[] zone, double theta, double tarDto, double refDto, double[] res);

        [DllImport(OtherPos, EntryPoint = "SC_2X1D_DW", CallingConvention = CallingConvention.Cdecl)]//两星一地带参
        private extern static void X2D1_Pos20240305_Core(double[] mainSat, double[] adjaSat, double[] cdbStation, double[] satStation1, double[] satStation2, double[] satStation3, double[] satStation4,
             double[] satStation5, double[] refStation, double[] zone, double tarSxDto, double tarXdDto, double samp_main_dto, double samp_neigh_dto, double[] res);

        [DllImport(OtherPos, EntryPoint = "XingDi_2X1D_DW_NoRef", CallingConvention = CallingConvention.Cdecl)]//两星一地无参
        private extern static void X2D1_PosNoRef20240305_Core(double[] mainSat, double[] adjaSat, double[] cdbStation, double[] mainSatTarStation, double[] adjaSat1TarStation, double[] adjaSat2TarStation
            , double[] zone, double tarSxDto, double tarXdDto, double[] res);

        [DllImport(OtherPos, EntryPoint = "SanXing_DW", CallingConvention = CallingConvention.Cdecl)]//三星双时差带参
        private extern static void X3_Pos20240305_Core(double[] mainSat, double[] adjaSat1, double[] adjaSat2, double[] mainSatTarStation, double[] adjaSat1TarStation, double[] adjaSat2TarStation
            , double[] mainSatRefStation, double[] adjaSat1RefStation, double[] adjaSat2RefStation, double[] refStation, double[] zone, double tarDto1, double tarDto2, double refDto1, double refDto2, double[] res);

        [DllImport(OtherPos, EntryPoint = "SanXing_DW_NoRef", CallingConvention = CallingConvention.Cdecl)]//三星双时差无参
        private extern static void X3_PosNoRef20240305_Core(double[] mainSat, double[] adjaSat1, double[] adjaSat2, double[] mainSatTarStation, double[] adjaSat1TarStation, double[] adjaSat2TarStation
           , double[] zone, double tarDto1, double tarDto2, double[] res);

        [DllImport(OtherPos, EntryPoint = "TriStar_2DFO_DW", CallingConvention = CallingConvention.Cdecl)]//三星双频差带参
        private extern static void X3_PosTwoDfo20240305_Core(double[] mainSat, double[] adjaSat1, double[] adjaSat2, double[] mainSatTarStation, double[] adjaSat1TarStation, double[] adjaSat2TarStation
          , double[] refStation, double[] zone, double tarDfo1, double tarDfo2, double refDfo1, double refDfo2, double fu1, double fd1, double fu2, double fd2, double[] res);

        [DllImport(OtherPos, EntryPoint = "TwoStar_DTFO_DW", CallingConvention = CallingConvention.Cdecl)]//双星时频差带参
        private extern static void X2_Pos20240305_Core(double[] mainSat, double[] adjaSat, double[] mainSatTarStation, double[] adjaSatTarStation, double[] refStation, double[] zone
            , double tarDto, double tarDfo, double refDto, double refDfo, double fu1, double fd1, double fu2, double fd2, double[] res);



        #endregion




        /// <summary>
        /// 一星一地带参,返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="cxRes">测向结果</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X1D1_Pos(CgRes cgRes, StationRes sRes, CxRes cxRes, bool CalcConfidence = false)
        {
            if (cgRes.Snr1 == 0) return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            if (cgRes.RefYbDto1 == null) return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            double[] mainSat = new double[3] { cgRes.MainX, cgRes.MainY, cgRes.MainZ };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] cdbStation = new double[3] { sRes.CdbTxLon.Value, sRes.CdbTxLat.Value, 0 };
            double[] cxStation = new double[3] { sRes.CxLon.Value, sRes.CxLat.Value, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double dtoCdb = cgRes.Dto1 / 1e6;
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double theta = cxRes.Fx;//单位°
            double[] res = new double[6];
            var ybDto1 = cgRes.RefYbDto1.Value / 1e6;
            X1D1_Pos20240305_Core(mainSat, satStation, cdbStation, cxStation, refStation, zone, theta, dtoCdb, ybDto1, res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX+RandomHelper.Normal(0,200),
                        cgRes.MainY+RandomHelper.Normal(0,200),
                        cgRes.MainZ+RandomHelper.Normal(0,200)
                    };
                    var dtoCdbNew = (dtoCdb * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var ybDtoNew = (ybDto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var thetaNew = theta + RandomHelper.Normal(0, 0.4);//单位°
                    X1D1_Pos20240305_Core(mainSatNew, satStation, cdbStation, cxStation, refStation, zone, thetaNew, dtoCdbNew, ybDtoNew, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }


        /// <summary>
        /// 两星一地带参解析定位(精度差速度快,主要用于置信度分析等需要多次调用的情况)
        /// </summary>
        /// <param name="cgRes"></param>
        /// <param name="sRes"></param>
        /// <returns></returns>
        public static double[] X2D1_GzPos(CgRes cgRes, StationRes sRes)
        {
            if (cgRes.Snr1 == 0 || cgRes.Snr2 == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            double[] mainSat = new double[3] { cgRes.MainX, cgRes.MainY, cgRes.MainZ };
            double[] adjaSat = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] cdbStation = new double[3] { sRes.CdbTxLon.Value, sRes.CdbTxLat.Value, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double dto1 = cgRes.Dto1 / 1e6;
            double dtoCdb = cgRes.Dto2.Value / 1e6;
            double ybDto1 = cgRes.RefYbDto1.Value / 1e6;
            double ybDto2 = cgRes.RefYbDto2.Value / 1e6;
            double ybDto = ybDto1 - ybDto2;
            double[] posRes = new double[3];
            DW_Analysis(mainSat, adjaSat, cdbStation, refStation, dto1, -dtoCdb, ybDto, -ybDto1, posRes, 1);
            return posRes;
        }


        /// <summary>
        /// 两星一地带参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X2D1_Pos(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            if (cgRes.Snr1 == 0 || cgRes.Snr2 == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            if (cgRes.RefYbDto1 == null || cgRes.RefYbDto2 == null)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            double[] mainSat = new double[3] { cgRes.MainX, cgRes.MainY, cgRes.MainZ };
            double[] adjaSat = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] cdbStation = new double[3] { sRes.CdbTxLon.Value, sRes.CdbTxLat.Value, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double dto1 = cgRes.Dto1 / 1e6;
            double dtoCdb = cgRes.Dto2.Value / 1e6;
            double ybDto1 = cgRes.RefYbDto1.Value / 1e6;
            double ybDto2 = cgRes.RefYbDto2.Value / 1e6;
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double[] res = new double[6];
            X2D1_Pos20240305_Core(mainSat, adjaSat, cdbStation, satStation, satStation, satStation, satStation, satStation, refStation, zone, dto1, dtoCdb, ybDto1, ybDto2, res);
            ConvertToGeoPoint(res);
            var posRes = ConvertToGeoPoint(res);//精确搜索值
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                var posResGz = X2D1_GzPos(cgRes, sRes);
                if (IsGeoPoint2(posResGz))
                {
                    int succeedCount = 0;
                    var cgResNew = cgRes.Clone();
                    for (int i = 0; i < confidenceCount; i++)
                    {
                        cgResNew.MainX = cgRes.MainX + RandomHelper.Normal(0, 200);
                        cgResNew.MainY = cgRes.MainY + RandomHelper.Normal(0, 200);
                        cgResNew.MainZ = cgRes.MainZ + RandomHelper.Normal(0, 200);
                        cgResNew.Adja1X = cgRes.Adja1X + RandomHelper.Normal(0, 200);
                        cgResNew.Adja1Y = cgRes.Adja1Y + RandomHelper.Normal(0, 200);
                        cgResNew.Adja1Z = cgRes.Adja1Z + RandomHelper.Normal(0, 200);
                        cgResNew.Dto1 = (cgRes.Dto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                        cgResNew.Dto2 = (cgRes.Dto2 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                        cgResNew.RefYbDto1 = (cgRes.RefYbDto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                        cgResNew.RefYbDto2 = (cgRes.RefYbDto2 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                        res = X2D1_GzPos(cgResNew, sRes);
                        var posResNew = ConvertToGeoPoint(res);
                        if (IsGeoPoint2(posResNew))
                        {
                            var distance = PhysicsHelper.DistanceGeo((posResGz[0], posResGz[1], 0), (posResNew[0], posResNew[1], 0));
                            if (distance <= confidenceDistance)
                            {
                                succeedCount++;
                            }
                        }
                    }
                    posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
                }
                else
                {
                    posRes[6] = 0;
                }
            }
            return posRes;
        }

        /// <summary>
        /// 两星一地无参，返回经度、纬度、高度、镜像点、置信度,数组长度为7
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="CalcConfidence">是否计算置信度</param>
        /// <returns></returns>
        public static double[] X2D1_PosNoRef(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            if (cgRes.Snr1 == 0 || cgRes.Snr2 == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            double[] mainSat = new double[3] { cgRes.MainX, cgRes.MainY, cgRes.MainZ };
            double[] adjaSat = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] cdbStation = new double[3] { sRes.CdbTxLon.Value, sRes.CdbTxLat.Value, 0 };
            double dto1 = cgRes.Dto1 / 1e6;
            double dtoCdb = cgRes.Dto2.Value / 1e6;
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double[] res = new double[6];
            X2D1_PosNoRef20240305_Core(mainSat, adjaSat, cdbStation, satStation, satStation, satStation, zone, dto1, dtoCdb, res);
            var posRes = ConvertToGeoPoint(res);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                for (int i = 0; i < confidenceCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX+RandomHelper.Normal(0,200),
                        cgRes.MainY+RandomHelper.Normal(0,200),
                        cgRes.MainZ+RandomHelper.Normal(0,200)

                    };
                    var adjaSatNew = new double[3]
                    {
                        cgRes.Adja1X.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Y.Value+RandomHelper.Normal(0,200),
                        cgRes.Adja1Z.Value+RandomHelper.Normal(0,200)
                    };
                    var dto1New = (dto1 * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    var dtoCdbNew = (dtoCdb * 1e6 + RandomHelper.Normal(0, 1)) / 1e6;
                    X2D1_PosNoRef20240305_Core(mainSatNew, adjaSatNew, cdbStation, satStation, satStation, satStation, zone, dto1New, dtoCdbNew, res);
                    var posResNew = ConvertToGeoPoint(res);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                posRes[6] = (int)(succeedCount / (double)confidenceCount * 100);
            }
            return posRes;
        }

        public static double[] X2D1_PosNoRef_ZL(CgRes cgRes, StationRes sRes, bool CalcConfidence = false)
        {
            if (cgRes.Snr1 == 0 || cgRes.Snr2 == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            var cdbStation = new double[2] { sRes.CdbTxLon.Value, sRes.CdbTxLat.Value };
            var satStation = new double[2] { sRes.SatTxLon, sRes.SatTxLat };
            var mainXYZ = new double[3] { cgRes.MainX, cgRes.MainY, cgRes.MainZ };
            var adjaXYZ = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            var dtoSxus = cgRes.Dto1;
            var dtoXdus = cgRes.Dto2.Value;
            var posRes = X2D1_PosNoRefCore_Zl(cdbStation, satStation, mainXYZ, adjaXYZ, dtoSxus, dtoXdus);
            posRes = ConvertToGeoPoint(posRes);
            if (CalcConfidence && IsGeoPoint2(posRes))
            {
                int succeedCount = 0;
                int totalCount = 100;
                for (int i = 0; i < totalCount; i++)
                {
                    var mainSatNew = new double[3]
                    {
                        cgRes.MainX+RandomHelper.Normal(0,500),
                        cgRes.MainY+RandomHelper.Normal(0,500),
                        cgRes.MainZ+RandomHelper.Normal(0,500)

                    };
                    var adjaSatNew = new double[3]
                    {
                        cgRes.Adja1X.Value+RandomHelper.Normal(0,500),
                        cgRes.Adja1Y.Value+RandomHelper.Normal(0,500),
                        cgRes.Adja1Z.Value+RandomHelper.Normal(0,500)
                    };
                    var dtoSxusNew = dtoSxus + RandomHelper.Normal(0, 1);
                    var dtoXdusNew = dtoXdus + RandomHelper.Normal(0, 1);
                    var posResNew = X2D1_PosNoRefCore_Zl(cdbStation, satStation, mainSatNew, adjaSatNew, dtoSxusNew, dtoXdusNew);
                    posResNew = ConvertToGeoPoint(posResNew);
                    if (IsGeoPoint2(posResNew))
                    {
                        var distance = PhysicsHelper.DistanceGeo((posRes[0], posRes[1], 0), (posResNew[0], posResNew[1], 0));
                        if (distance <= confidenceDistance)
                        {
                            succeedCount++;
                        }
                    }
                }
                var confidence = (int)(succeedCount / (double)totalCount * 100);
                if (confidence == 0)
                    confidence = 1;
                if (confidence == 100)
                    confidence = 99;
                posRes[6] = confidence;
            }
            return posRes;
        }

        private static double[] X2D1_PosNoRefCore_Zl(double[] cdbStation, double[] satStation, double[] mainXYZ, double[] adjaXYZ, double dtoSxus, double dtoXdus)
        {

            double startLon = (int)cdbStation[0] - 10;
            double endLon = (int)cdbStation[0] + 10;
            double startLat = (int)cdbStation[1] - 10;
            double endLat = (int)cdbStation[1] + 10;
            List<(double, double, double, double)> list = new List<(double, double, double, double)>();
            var recXYZ = PhysicsHelper.GeoToEcef((satStation[0], satStation[1], 0));
            var cdbXYZ = PhysicsHelper.GeoToEcef((cdbStation[0], cdbStation[1], 0));
            for (double lon = startLon; lon < endLon; lon += 0.0001)
            {
                int flag = 0;
                for (double lat = startLat; lat < endLat; lat += 0.0001)
                {
                    var posXYZ = PhysicsHelper.GeoToEcef((lon, lat, 0));

                    //目标-主星-接收站的时间(us)
                    var dt1 = PhysicsHelper.Dto(posXYZ, (mainXYZ[0], mainXYZ[1], mainXYZ[2]), recXYZ) * 1e6;

                    //目标-超短站的时间(us)
                    var dt3 = PhysicsHelper.Dto(posXYZ, cdbXYZ) * 1e6;
                    var dto2 = Math.Abs(dt1 - dt3 - dtoXdus);
                    if (dto2 > 400)
                    {
                        lat += 1;
                        if (flag < 1)
                            flag = 1;
                        continue;
                    }
                    else if (dto2 > 200)
                    {
                        lat += 0.5;
                        if (flag < 2)
                            flag = 2;
                        continue;
                    }
                    else if (dto2 > 100)
                    {
                        lat += 0.2;
                        if (flag < 3)
                            flag = 3;
                        continue;
                    }
                    else if (dto2 > 10)
                    {
                        lat += 0.01;
                        if (flag < 4)
                            flag = 4;
                        continue;
                    }
                    //else if (dto2 > 2)
                    //{
                    //    lat += 0.001;
                    //    if (flag < 5)
                    //        flag = 5;
                    //    continue;
                    //}
                    //目标-邻星-接收站的时间(us)
                    var dt2 = PhysicsHelper.Dto(posXYZ, (adjaXYZ[0], adjaXYZ[1], adjaXYZ[2]), recXYZ) * 1e6;
                    var dto1 = Math.Abs(dt1 - dt2 - dtoSxus);
                    if (dto1 > 400)
                    {
                        lat += 1;
                        if (flag < 1)
                            flag = 1;
                        continue;
                    }
                    if (dto1 > 200)
                    {
                        lat += 0.5;
                        if (flag < 2)
                            flag = 2;
                        continue;
                    }
                    else if (dto1 > 100)
                    {
                        lat += 0.2;
                        if (flag < 3)
                            flag = 3;
                        continue;
                    }
                    else if (dto1 > 10)
                    {
                        lat += 0.01;
                        if (flag < 4)
                            flag = 4;
                        continue;
                    }
                    //else if (dto1 > 2)
                    //{
                    //    lat += 0.001;
                    //    if (flag < 5)
                    //        flag = 5;
                    //    continue;
                    //}
                    list.Add((lon, lat, dto1, dto2));
                }
                if (flag == 1)
                    lon += 1;
                else if (flag == 2)
                    lon += 0.5;
                else if (flag == 3)
                    lon += 0.2;
                else if (flag == 4)
                    lon += 0.01;
                else if (flag == 5)
                    lon += 0.001;
            }
            double[] posRes;
            var p1 = list.OrderBy(p => p.Item4 * p.Item4 + p.Item3 * p.Item3).FirstOrDefault();
            if (p1 == default)
            {
                posRes = new double[6] { 999, 999, 0, 999, 999, 0 };
                return posRes;
            }
            var p2 = list.Where(p => PhysicsHelper.DistanceGeo((p1.Item1, p1.Item2, 0), (p.Item1, p.Item2, 0)) > 10000).OrderBy(p => p.Item4 * p.Item4 + p.Item3 * p.Item3).FirstOrDefault();
            if (p2 == default)
                posRes = new double[6] { p1.Item1, p1.Item2, 0, 999, 999, 0 };
            else
                posRes = new double[6] { p1.Item1, p1.Item2, 0, p2.Item1, p2.Item2, 0 };
            if (posRes[3] != 999)
            {
                var dis1 = PhysicsHelper.DistanceArcGeo((posRes[3], posRes[4]), (cdbStation[0], cdbStation[1]));
                var dis2 = PhysicsHelper.DistanceArcGeo((posRes[0], posRes[1]), (cdbStation[0], cdbStation[1]));
                if (dis1 < dis2)
                {
                    var tmp1 = posRes[3];
                    var tmp2 = posRes[4];
                    posRes[3] = posRes[0];
                    posRes[4] = posRes[1];
                    posRes[0] = tmp1;
                    posRes[1] = tmp2;
                }
            }
            return posRes;
        }


        private static double[] ConvertToGeoPoint(double[] res)
        {
            if (res == null || res.Length == 0)
            {
                return new double[7] { 999, 999, 0, 999, 999, 0, -1 };
            }
            else if (res.Length != 3 && res.Length != 6)
            {
                throw new Exception("定位结果解析异常,长度不是3或6");
            }

            var list = res.Select(p => Math.Round(p, 4)).ToArray();
            if (list.Length == 3)
            {
                list = list.Concat(new double[4] { 999, 999, 0, -1 }).ToArray();
            }
            list[2] = list[5] = 0;//高度

            if (double.IsNaN(list[0]) || double.IsNaN(list[1]))
            {
                list[0] = list[1] = 999;
            }
            if (double.IsNaN(list[3]) || double.IsNaN(list[4]))
            {
                list[3] = list[4] = 999;
            }
            if (list[0] < -180 || list[0] > 180)
            {
                list[0] = list[1] = 999;
            }
            else if (list[1] < -90 || list[1] > 90)
            {
                list[0] = list[1] = 999;
            }
            if (list[3] < -180 || list[3] > 180)
            {
                list[3] = list[4] = 999;
            }
            else if (list[4] < -90 || list[4] > 90)
            {
                list[3] = list[4] = 999;
            }
            if (list.Length == 6)
            {
                var listNew = list.ToList();
                listNew.Add(-1);
                list = listNew.ToArray();
            }
            return list;
        }

        private static bool IsGeoPoint(double[] res)
        {
            if (res.Length != 3) return false;
            if (res[0] < -180 || res[0] > 180)
            {
                return false;
            }
            else if (res[1] < -90 || res[1] > 90)
            {
                return false;
            }
            return true;
        }
        private static bool IsGeoPoint2(double[] res)
        {
            if (res.Length < 3) return false;
            if (res[0] < -180 || res[0] > 180)
            {
                return false;
            }
            else if (res[1] < -90 || res[1] > 90)
            {
                return false;
            }
            return true;
        }
    }
}