XdCxRhDW/XdCxRhDW.Api/AddIns/定位/PosApi.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;
using XdCxRhDW.Entity;

namespace XdCxRhDW.Api
{
    /// <summary>
    ///TODO 定位算法接口.在这里调用罗博士的算法库
    /// </summary>
    public static class PosApi
    {
        #region cpp dll Interop 

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

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

        [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>
        /// 一星一地带参,返返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="cxRes">测向结果</param>
        /// <returns></returns>
        public static double[] X1D1_Pos(CgRes cgRes, StationRes sRes, CxRes cxRes)
        {
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            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.DtoCdb.Value / 1e6;
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            double theta = cxRes.Fx;//单位°
            double[] res = new double[6];
            var ybDto1 = cgRes.YbMainDto.Value / 1e6;
            X1D1_Pos20240305_Core(mainSat, satStation, cdbStation, cxStation, refStation, zone, theta, dtoCdb, ybDto1, res);
            return ConvertToGeoPoint(res);
        }

        /// <summary>
        /// 两星一地带参，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <returns></returns>
        public static double[] X2D1_Pos(CgRes cgRes, StationRes sRes)
        {

            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            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.Value / 1e6;
            double dtoCdb = cgRes.DtoCdb.Value / 1e6;
            double ybDto1 = cgRes.YbMainDto.Value / 1e6;
            double ybDto2 = cgRes.YbAdja1Dto.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);
            return ConvertToGeoPoint(res);
        }

        /// <summary>
        /// 两星一地无参，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <returns></returns>
        public static double[] X2D1_PosNoRef(CgRes cgRes, StationRes sRes)
        {
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            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.Value / 1e6;
            double dtoCdb = cgRes.DtoCdb.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);
            return ConvertToGeoPoint(res);
        }

        /// <summary>
        /// 融合定位带参，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <param name="cxRes">测向结果</param>
        /// <returns></returns>
        public static double[] RH_Pos(CgRes cgRes, StationRes sRes, CxRes cxRes)
        {
            var res1 = X1D1_Pos(cgRes, sRes, cxRes);
            var res2 = X2D1_Pos(cgRes, sRes);
            double[] res = new double[] { 999, 999, 0, 999, 999, 0 };
            var p1 = res1.Take(3).ToArray();
            var p2 = res1.Skip(3).ToArray();
            var p3 = res2.Take(3).ToArray();
            var p4 = res2.Skip(3).ToArray();
            if (IsGeoPoint(p1) && IsGeoPoint(p2) && IsGeoPoint(p3) && IsGeoPoint(p4))
            {
                res = new double[6] {
                    (p1[0] + p3[0]) / 2 + 0.003,
                    (p1[1] + p3[1]) / 2 - 0.002,
                    0,
                    (p2[0] + p4[0]) / 2 + 0.003,
                    (p2[1] + p4[1]) / 2 - 0.002,
                    0,
                };
            }
            else if (IsGeoPoint(p1) && IsGeoPoint(p3))
            {
                res = new double[6] {
                    (p1[0] + p3[0]) / 2 + 0.003,
                    (p1[1] + p3[1]) / 2 - 0.002,
                    0,
                    999,
                    999,
                    0,
                };
            }
            else if (IsGeoPoint(p1) && IsGeoPoint(p2))
            {
                res = new double[6]
                {
                    p1[0] + 0.003,
                    p1[1] - 0.002,
                    0,
                    p2[0] + 0.003,
                    p2[1] - 0.002,
                    0,

                };
            }
            else if (IsGeoPoint(p3) && IsGeoPoint(p4))
            {
                res = new double[6]
               {
                    p3[0] + 0.003,
                    p3[1] - 0.002,
                    0,
                    p4[0] + 0.003,
                    p4[1] - 0.002,
                    0,

               };
            }
            return res;
        }

        /// <summary>
        /// 三星双时差带参，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <returns></returns>
        public static double[] X3_Pos(CgRes cgRes, StationRes sRes)
        {
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            double[] adjaSat1 = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] adjaSat2 = new double[3] { cgRes.Adja2X.Value, cgRes.Adja2Y.Value, cgRes.Adja2Z.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            var tarDto1 = cgRes.Dto1.Value / 1e6;
            var tarDto2 = cgRes.Dto2.Value / 1e6;
            var refDto1 = (cgRes.YbMainDto.Value - cgRes.YbAdja1Dto.Value) / 1e6;
            var refDto2 = (cgRes.YbMainDto.Value - cgRes.YbAdja2Dto.Value) / 1e6;
            double[] res = new double[6];
            X3_Pos20240305_Core(mainSat, adjaSat1, adjaSat2, satStation, satStation, satStation, satStation,
                satStation, satStation, refStation, zone, tarDto1, tarDto2, refDto1, refDto2, res);
            return ConvertToGeoPoint(res);
        }

        /// <summary>
        /// 三星双时差无参，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <returns></returns>
        public static double[] X3_PosNoRef(CgRes cgRes, StationRes sRes)
        {
            double[] mainSat = new double[3] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value };
            double[] adjaSat1 = new double[3] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value };
            double[] adjaSat2 = new double[3] { cgRes.Adja2X.Value, cgRes.Adja2Y.Value, cgRes.Adja2Z.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            var tarDto1 = cgRes.Dto1.Value / 1e6;
            var tarDto2 = cgRes.Dto2.Value / 1e6;
            double[] res = new double[6];
            X3_PosNoRef20240305_Core(mainSat, adjaSat1, adjaSat2, satStation, satStation, satStation, zone, tarDto1, tarDto2, res);
            return ConvertToGeoPoint(res);
        }

        /// <summary>
        /// 三星双频差带参，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <returns></returns>
        public static double[] X3_PosTwoDfo(CgRes cgRes, StationRes sRes)
        {
            double[] mainSat = new double[6] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value, cgRes.MainVx.Value, cgRes.MainVy.Value, cgRes.MainVz.Value };
            double[] adjaSat1 = new double[6] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value, cgRes.Adja1Vx.Value, cgRes.Adja1Vy.Value, cgRes.Adja1Vz.Value };
            double[] adjaSat2 = new double[6] { cgRes.Adja2X.Value, cgRes.Adja2Y.Value, cgRes.Adja2Z.Value, cgRes.Adja2Vx.Value, cgRes.Adja2Vy.Value, cgRes.Adja2Vz.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            var tarDfo1 = cgRes.Dfo1.Value;
            var tarDfo2 = cgRes.Dfo2.Value;
            var refDfo1 = cgRes.YbMainDfo.Value - cgRes.YbAdja1Dfo.Value;
            var refDfo2 = cgRes.YbMainDfo.Value - cgRes.YbAdja2Dfo.Value;
            double fu1 = cgRes.TarFreqUp.Value;
            double fd1 = cgRes.TarFreqDown.Value;
            double fu2 = cgRes.RefFreqUp.Value;
            double fd2 = cgRes.RefFreqDown.Value;
            double[] res = new double[6];
            X3_PosTwoDfo20240305_Core(mainSat, adjaSat1, adjaSat2, satStation, satStation, satStation,
                refStation, zone, tarDfo1, tarDfo2, refDfo1, refDfo2, fu1, fd1, fu2, fd2, res);
            return ConvertToGeoPoint(res);
        }

        /// <summary>
        /// 双星时频差带参，返回经度纬度高度及镜像点,数组长度为6
        /// </summary>
        /// <param name="cgRes">参估结果</param>
        /// <param name="sRes">站点信息</param>
        /// <returns></returns>
        public static double[] X2_PosDtoDfo(CgRes cgRes, StationRes sRes)
        {
            double[] mainSat = new double[6] { cgRes.MainX.Value, cgRes.MainY.Value, cgRes.MainZ.Value, cgRes.MainVx.Value, cgRes.MainVy.Value, cgRes.MainVz.Value };
            double[] adjaSat = new double[6] { cgRes.Adja1X.Value, cgRes.Adja1Y.Value, cgRes.Adja1Z.Value, cgRes.Adja1Vx.Value, cgRes.Adja1Vy.Value, cgRes.Adja1Vz.Value };
            double[] satStation = new double[3] { sRes.SatTxLon, sRes.SatTxLat, 0 };
            double[] refStation = new double[3] { sRes.RefLon.Value, sRes.RefLat.Value, 0 };
            double[] zone = new double[] { -85, 85, -180, 180 }; //定位区域
            var tarDto = cgRes.Dto1.Value / 1e6;
            var tarDfo = cgRes.Dfo1.Value;
            var refDto = (cgRes.YbMainDto.Value - cgRes.YbAdja1Dto.Value) / 1e6;
            var refDfo = cgRes.YbMainDfo.Value - cgRes.YbAdja1Dfo.Value;
            double fu1 = cgRes.TarFreqUp.Value;
            double fd1 = cgRes.TarFreqDown.Value;
            double fu2 = cgRes.RefFreqUp.Value;
            double fd2 = cgRes.RefFreqDown.Value;
            double[] res = new double[6];
            X2_Pos20240305_Core(mainSat, adjaSat, satStation, satStation, refStation, zone, tarDto, tarDfo, refDto, refDfo, fu1, fd1, fu2, fd2, res);
            return ConvertToGeoPoint(res);
        }

        private static double[] ConvertToGeoPoint(double[] res)
        {
            if (res == null || res.Length == 0)
            {
                return new double[6] { 999, 999, 0, 999, 999, 0 };
            }
            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[3] { 999, 999, 0 }).ToArray();
            }
            list[2] = list[5] = 0;//高度
            if (double.IsNaN(list[3]) || double.IsNaN(list[4]))
            {
                list[3] = list[4] = 999;
            }
            if (double.IsNaN(list[0]) || double.IsNaN(list[1]))
            {
                list[0] = list[1] = 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;
            }
            
            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;
        }

    }
}