SimulationServerYL/SimulationServer/Utils/Utility.cs

using Unity.Mathematics;

public static class Utility
{

    #region 距离面积坐标转换

    /********** 地球半径数据计算 ***************/

    /// <summary>
    /// 地球半径 m
    /// </summary>
    private const double EarthRadius = 6371393;

    /// <summary>
    /// 获取距离
    /// </summary>
    /// <param name="startPos">起点经纬度</param>
    /// <param name="endPos">终点经纬度</param>
    /// <returns></returns>
    public static double GetDiatance(double3 startPos, double3 endPos)
    {
        double xA = math.cos(Rad(startPos.y)) * math.cos(Rad(startPos.x));
        double yA = math.cos(Rad(startPos.y)) * math.sin(Rad(startPos.x));
        double zA = math.sin(Rad(startPos.y));

        double xB = math.cos(Rad(endPos.y)) * math.cos(Rad(endPos.x));
        double yB = math.cos(Rad(endPos.y)) * math.sin(Rad(endPos.x));
        double zB = math.sin(Rad(endPos.y));

        double angleL = (xA * xB + yA * yB + zA * zB) / math.sqrt((xA * xA + yA * yA + zA * zA) * (xB * xB + yB * yB + zB * zB));
        double distance = EarthRadius * math.acos(angleL);
        return distance;
    }

    /// <summary>
    /// 经纬度转化成弧度
    /// </summary>
    /// <param name="d"></param>
    /// <returns></returns>
    private static double Rad(double d)
    {
        return d * Math.PI / 180d;
    }
    #region 计算面积

    /// <summary>
    /// 计算面积
    /// </summary>
    /// <param name="points"></param>
    /// <returns></returns>
    public static double GetArea(List<double3> points)
    {
        double result = 0;
        for (int i = 0; i < points.Count - 2; i++)
        {
            int j = i + 1;
            int K = i + 2;

            double totalAngle = GetAngle(points[i], points[j], points[K]);
            double dis1 = GetDiatance(points[i], points[j]);
            double dis2 = GetDiatance(points[j], points[K]);

            result += dis1 * dis2 * Math.Abs(math.sin(Rad(totalAngle)));
        }
        return result;
    }
    /// <summary>
    /// 计算 3个点的角度
    /// </summary>
    /// <param name="start"></param>
    /// <param name="end"></param>
    /// <returns></returns>
    private static double GetAngle(double3 A, double3 B, double3 C)
    {
        double bearing1 = GetBearing(B, A);
        double bearing2 = GetBearing(B, C);

        double angle = bearing2 - bearing1;
        if (angle < 0)
        {
            angle += 360;
        }
        return angle;
    }


    /// <summary>
    /// 计算 从A点到B点相对于平面 X 轴的平面角度
    /// </summary>
    /// <param name="A"></param>
    /// <param name="B"></param>
    /// <returns></returns>
    private static double GetBearing(double3 A, double3 B)
    {
        double lat1 = Rad(A.y);
        double lon1 = Rad(A.x);
        double lat2 = Rad(B.y);
        double lon2 = Rad(B.x);

        double angle = -Math.Atan2(Math.Sin(lon1 - lon2) * Math.Cos(lat2), Math.Cos(lat1) * Math.Sin(lat2) - Math.Sin(lat1) * Math.Cos(lat2) * Math.Cos(lon1 - lon2));
        if (angle < 0)
        {
            angle += 2 * Math.PI;
        }
        return angle * 180d / Math.PI;
    }


    #endregion
    #endregion



}