DW5S/DW5S.KxcApi/AddIns/时差线/OutputHelper.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace DW5S.KxcApi
{
    public class OutputHelper
    {
        public const double WGS84a = 6378137.0;
        public const double f = 1.0 / 298.257224;

        public static List<DtoLinePoint> WriteDtoLine(double[] LOP_Value, int LOP_Len)
        {
            List<DtoLinePoint> list = new List<DtoLinePoint>();

            if (LOP_Len == 0) return list;
            var len = LOP_Len * 2;
            Lla[] line = new Lla[len + 1];
            double? prevLon = null, prevLat = null;
            for (int i = 0; i < LOP_Len; i++)
            {
                var lon1 = LOP_Value[3 * i + 1];
                var lon2 = LOP_Value[3 * i + 2];
                var lat = LOP_Value[3 * i];
                var p2Idx = len - 1 - i;
                if (lon1 != -1 && lon2 != -1)
                {
                    if (prevLon == null)
                    {
                        line[i] = new Lla(lon1, lat);
                        line[p2Idx] = new Lla(lon2, lat);
                    }
                    else
                    {
                        var disLon1 = DisLon(prevLon.Value, prevLat.Value, lon1, lat);
                        var disLon2 = DisLon(prevLon.Value, prevLat.Value, lon2, lat);
                        if (disLon1 < disLon2)
                        {
                            line[i] = new Lla(lon1, lat);
                            line[p2Idx] = new Lla(lon2, lat);
                        }
                        else
                        {
                            line[i] = new Lla(lon2, lat);
                            line[p2Idx] = new Lla(lon1, lat);
                        }
                    }
                    prevLon = line[i].Lon;
                    prevLat = line[i].Lat;
                }
            }
            var firstPoint = line.FirstOrDefault(m => m != null);
            if (firstPoint != null)
            {
                line[line.Length - 1] = new Lla(firstPoint.Lon, firstPoint.Lat);
            }
            Array.ForEach(line, lla =>
            {
                if (lla != null)
                {
                    DtoLinePoint dtoLine = new DtoLinePoint();
                    dtoLine.Lon = lla.Lon;
                    dtoLine.Lat = lla.Lat;
                    list.Add(dtoLine);
                }
            });
            return list;
        }

        public static double DisLon(double lon1, double lat1, double lon2, double lat2)
        {
            return DistinceLla(new double[] { lon1, lat1, 0 }, new double[] { lon2, lat2, 0 });
        }

        public static double DistinceLla(double[] lla1, double[] lla2)
        {
            double[] xyz1 = LLA2XYZ(lla1[0], lla1[1], lla1[2]), xyz2 = LLA2XYZ(lla2[0], lla2[1], lla2[2]);
            return Distince(xyz1[0], xyz1[1], xyz1[2], xyz2[0], xyz2[1], xyz2[2]);
        }

        public static double Distince(double x1, double y1, double z1, double x2, double y2, double z2)
        {
            double x = (x1 - x2);
            double y = (y1 - y2);
            double z = (z1 - z2);
            double dist = Math.Sqrt(x * x + y * y + z * z);
            return dist;
        }

        public static double[] LLA2XYZ(double lon, double lat, double alt)
        {
            lat = Math.PI * lat / 180.0;
            lon = Math.PI * lon / 180.0;

            double cosLat = Math.Cos(lat);
            double sinLat = Math.Sin(lat);

            double cosLong = Math.Cos(lon);
            double sinLong = Math.Sin(lon);

            double c = 1 / Math.Sqrt(cosLat * cosLat + (1 - f) * (1 - f) * sinLat * sinLat);
            double s = (1 - f) * (1 - f) * c;

            double x = (WGS84a * c + alt) * cosLat * cosLong;
            double y = (WGS84a * c + alt) * cosLat * sinLong;
            double z = (WGS84a * s + alt) * sinLat;

            return new double[3] { x, y, z };
        }
    }

    public class Lla
    {
        public Lla(double lon, double lat)
        {
            this.Lon = lon;
            this.Lat = lat;
        }

        public double Lon { get; set; }

        public double Lat { get; set; }

        public double Alt { get; set; } = 0;
    }

}