DW5S/DW5S.Basic/PhysicsHelper.cs

using System;
using System.IO;
using System.Security.Cryptography;
using static System.Math;

/// <summary>
/// 物理公式帮助类
/// </summary>
public static class PhysicsHelper
{
    readonly static double PI = Math.PI;//圆周率
    public readonly static double C = 299792458.458;//光速 m/s


    ///// <summary>
    ///// 地球半径(m)
    ///// </summary>
    //public static double R = 6378.1414 * 1000; //地球半径(m)

    /// <summary>
    /// GEO转ECEF地心地固坐标
    /// </summary>
    /// <param name="geo">经纬高(m)三元组</param>
    /// <returns>返回x、y、z三元组,单位m</returns>
    public static (double x, double y, double z) GeoToEcef((double, double, double) geo)
    {
        var (lon, lat, alt) = geo;
        var c45 = 180 / PI;
        //double lr = 6378137;//地球长半轴
        //double sr = 6356752.3142;//地球短半轴

        double lr = 6378136.49;//地球长半轴
        double sr = 6356755.00;//地球短半轴

        var e2 = (lr + sr) * (lr - sr) / lr / lr;
        var lonArc = lon / c45;//弧度
        var latArc = lat / c45;//弧度
        var r = lr / Sqrt(1 - e2 * Pow(Sin(latArc), 2));//地球曲率半径
        double x = (r + alt) * Cos(latArc) * Cos(lonArc);
        double y = (r + alt) * Cos(latArc) * Sin(lonArc);
        double z = (r * (1 - e2) + alt) * Sin(latArc);
        return (x, y, z);
    }
    //public static (double x, double y, double z) GeoToEcef((double, double, double) geo)
    //{
    //    /*
    //    R=6378.1414;
    //    p=p/180*pi;
    //    q=q/180*pi;
    //    r=[R*cos(q)*cos(p) R*cos(q)*sin(p) R*sin(q)];
    //     * */

    //    //double r = 6378.1414; //地球半径(Km)
    //    var (lon, lat, alt) = geo;
    //    double p = lon * Math.PI / 180;
    //    double q = lat * Math.PI / 180;

    //    var x = R * Math.Cos(q) * Math.Cos(p);
    //    var y= R * Math.Cos(q) * Math.Sin(p);
    //    var z = R * Math.Sin(q);

    //    return (x,y,z);
    //}

    /// <summary>
    /// ECEF转GEO
    /// </summary>
    /// <param name="ecef">ECEF地心地固坐标x、y、z三元组</param>
    /// <returns>返回经度(°)、纬度(°)、高度(m)三元组</returns>
    public static (double lon, double lat, double alt) EcefToGeo((double, double, double) ecef)
    {
        var (x, y, z) = ecef;
        double lr = 6378137;//地球长半轴
        double sr = 6356752.3142;//地球短半轴
        var e2 = (lr + sr) * (lr - sr) / (double)lr / lr;//轨道第一偏心率平方
        double B0 = Atan2(z, Sqrt(x * x + y * y));
        double N;//卯酉圈曲率半径
        double lon = Atan2(y, x);
        double lat, alt;
        lon *= (180 / PI);
        while (true)//纬度度多次逼近
        {
            N = lr / Sqrt(1 - e2 * Sin(B0) * Sin(B0));
            lat = Atan2(z + N * e2 * Sin(B0), Sqrt(x * x + y * y));
            if (Abs(B0 - lat) < 1e-6) break;
            B0 = lat;
        }
        alt = Sqrt(x * x + y * y) / Cos(lat) - N;
        lat *= (180 / PI);
        return (lon, lat, alt);
    }

    /// <summary>
    /// 求ECEF坐标系下两个点的直线距离(单位:m)
    /// </summary>
    /// <param name="ecef1">ECEF地心地固坐标1,x、y、z三元组</param>
    /// <param name="ecef2">ECEF地心地固坐标2,x、y、z三元组</param>
    /// <returns>返回距离,单位m</returns>
    public static double DistanceEcf((double, double, double) ecef1, (double, double, double) ecef2)
    {
        double xr = ecef1.Item1 - ecef2.Item1;
        double yr = ecef1.Item2 - ecef2.Item2;
        double zr = ecef1.Item3 - ecef2.Item3;

        double xr2 = xr * xr;
        double yr2 = yr * yr;
        double zr2 = zr * zr;

        double s = xr2 + yr2 + zr2;

        double distanse = Sqrt(s);
        return distanse;
    }

    /// <summary>
    /// <para>求Geo坐标系下两个点的直线距离(单位:m)</para>
    /// </summary>
    /// <param name="geo1">geo位置1,经纬高三元组</param>
    /// <param name="geo2">geo位置2,经纬高三元组</param>
    /// <returns>返回距离,单位m</returns>
    public static double DistanceGeo((double, double, double) geo1, (double, double, double) geo2)
    {
        var posEcef1 = GeoToEcef(geo1);
        var posEcef2 = GeoToEcef(geo2);
        return DistanceEcf(posEcef1, posEcef2);
    }

    /// <summary>
    /// <para>求Geo坐标系下两个点的测地线距离(单位:m),没有高度</para>
    /// <para>得到的是地球表面最短路径,而不是直线距离</para>
    /// </summary>
    /// <param name="geo1">geo位置1,经纬度二元组</param>
    /// <param name="geo2">geo位置2,经纬度二元组</param>
    /// <returns>测地线距离,单位m</returns>
    public static double DistanceArcGeo((double, double) geo1, (double, double) geo2)
    {
        var (lon1, lat1) = geo1;
        var (lon2, lat2) = geo2;

        double dLat1InRad = lat1 * (PI / 180);
        double dLong1InRad = lon1 * (PI / 180);
        double dLat2InRad = lat2 * (PI / 180);
        double dLong2InRad = lon2 * (PI / 180);
        double dLongitude = dLong2InRad - dLong1InRad;
        double dLatitude = dLat2InRad - dLat1InRad;
        double a = Pow(Sin(dLatitude / 2), 2) + Cos(dLat1InRad) * Cos(dLat2InRad) * Pow(Sin(dLongitude / 2), 2);
        double c = 2 * Atan2(Sqrt(a), Sqrt(1 - a));
        double dDistance = 6378137 * c;//地球长轴半径
        return dDistance;
    }

    /// <summary>
    /// 求ecef坐标系下两个点的光速时差(单位:s)
    /// </summary>
    /// <param name="ecef1">ECEF地心地固坐标1,x、y、z三元组</param>
    /// <param name="ecef2">ECEF地心地固坐标2,x、y、z三元组</param>
    /// <returns>光速走过的时间,单位s,不会出现负数。交换参数1和参数2的位置不影响结果</returns>
    public static double Dto((double, double, double) ecef1, (double, double, double) ecef2)
    {
        var distance = DistanceEcf(ecef1, ecef2);
        return distance / C;
    }

    /// <summary>
    /// 求ecef坐标系下三个点的光速时差(单位:s)
    /// </summary>
    /// <param name="ecef1">ECEF地心地固坐标1,x、y、z三元组</param>
    /// <param name="ecef2">ECEF地心地固坐标2,x、y、z三元组</param>
    /// <param name="ecef3">ECEF地心地固坐标3,x、y、z三元组</param>
    /// <returns>光速走过的时间,单位s,不会出现负数。交换参数位置不影响结果</returns>
    public static double Dto((double, double, double) ecef1, (double, double, double) ecef2, (double, double, double) ecef3)
    {
        var distance = DistanceEcf(ecef1, ecef2) + DistanceEcf(ecef2, ecef3);
        return distance / C;
    }

    /// <summary>
    /// 求ecef坐标系下两个目标的多普勒
    /// </summary>
    /// <param name="f0">目标载频(上行频点Hz)</param>
    /// <param name="ecef1"></param>
    /// <param name="ecef2"></param>
    /// <param name="v1"></param>
    /// <param name="v2"></param>
    /// <returns></returns>
    public static double Doppler(double f0, (double, double, double) ecef1, (double, double, double) ecef2,
        (double, double, double) v1, (double, double, double) v2)
    {
        var distance = DistanceEcf(ecef1, ecef2);
        if (distance == 0) return 0;
        var (vx1, vy1, vz1) = v1;
        var (vx2, vy2, vz2) = v2;
        var (x1, y1, z1) = ecef1;
        var (x2, y2, z2) = ecef2;
        var fm = (vx1 - vx2) * (x1 - x2) + (vy1 - vy2) * (y1 - y2) + (vz1 - vz2) * (z1 - z2);
        var f = f0 / C * fm / distance;
        return f;
    }

}