c# - 计算端点给定的距离，方位角，起点

给定起点纬度/经度，方位角和距离，我试图找到终点。下面这个网站的计算器给了我想要的结果。

http://www.movable-type.co.uk/scripts/latlong.html

当我尝试通过代码实现相同的结果时，我没有得到正确的结果。

下面是我的代码-

    private  GLatLng pointRadialDistance(double lat1, double lon1,
               double radianBearing, double radialDistance)
    {
        double rEarth = 6371.01;
        lat1 = DegreeToRadian(lat1);
        lon1 = DegreeToRadian(lon1);
        radianBearing = DegreeToRadian(radianBearing);
        radialDistance = radialDistance / rEarth;
        double lat = Math.Asin(Math.Sin(lat1) * Math.Cos(radialDistance) + Math.Cos(lat1)
                        * Math.Sin(radialDistance) * Math.Cos(radianBearing));
        double lon;
        if (Math.Cos(lat) == 0)
        {  // Endpoint a pole
            lon = lon1;
        }
        else
        {
            lon = ((lon1 - Math.Asin(Math.Sin(radianBearing) * Math.Sin(radialDistance) / Math.Cos(lat))
                            + Math.PI) % (2 * Math.PI)) - Math.PI;
        }
        lat = RadianToDegree(lat);
        lon = RadianToDegree(lon);
        GLatLng newLatLng = new GLatLng(lat, lon);
        return newLatLng;
    }

    public  double Bearing(double lat1, double long1, double lat2, double long2)
    {
        //Convert input values to radians
        lat1 = DegreeToRadian(lat1);
        long1 = DegreeToRadian(long1);
        lat2 = DegreeToRadian(lat2);
        long2 = DegreeToRadian(long2);

        double deltaLong = long2 - long1;

        double y = Math.Sin(deltaLong) * Math.Cos(lat2);
        double x = Math.Cos(lat1) * Math.Sin(lat2) -
                Math.Sin(lat1) * Math.Cos(lat2) * Math.Cos(deltaLong);
        double bearing = Math.Atan2(y, x);
        return bearing;
    }

   public double DegreeToRadian(double angle)
    {
    return Math.PI * angle / 180.0;
    }

    public double RadianToDegree(double angle)
    {
        return 180.0 * angle / Math.PI;
    }

在主程序中，我按如下方式调用子程序-

double bearing = Bearing(-41.294444, 174.814444, -40.90521, 175.6604);
GLatLng endLatLng = pointRadialDistance(-41.294444, 174.814444, bearing, 80);

我得到以下结果-

Bearing=1.02749621782165
endLatLng=-40.5751022737927,174.797458881699

我期望的答案是-40.939722,175.646389(来自上面的网站链接)。

有人可以在这里建议我在代码中犯什么错误吗？

最佳答案

这是一些代码，可以实现您想要的操作。

public static GeoLocation FindPointAtDistanceFrom(GeoLocation startPoint, double initialBearingRadians, double distanceKilometres)
{
    const double radiusEarthKilometres = 6371.01;
    var distRatio = distanceKilometres / radiusEarthKilometres;
    var distRatioSine = Math.Sin(distRatio);
    var distRatioCosine = Math.Cos(distRatio);

    var startLatRad = DegreesToRadians(startPoint.Latitude);
    var startLonRad = DegreesToRadians(startPoint.Longitude);

    var startLatCos = Math.Cos(startLatRad);
    var startLatSin = Math.Sin(startLatRad);

    var endLatRads = Math.Asin((startLatSin * distRatioCosine) + (startLatCos * distRatioSine * Math.Cos(initialBearingRadians)));

    var endLonRads = startLonRad
        + Math.Atan2(
            Math.Sin(initialBearingRadians) * distRatioSine * startLatCos,
            distRatioCosine - startLatSin * Math.Sin(endLatRads));

    return new GeoLocation
    {
        Latitude = RadiansToDegrees(endLatRads),
        Longitude = RadiansToDegrees(endLonRads)
    };
}

public struct GeoLocation
{
    public double Latitude { get; set; }
    public double Longitude { get; set; }
}

public static double DegreesToRadians(double degrees)
{
    const double degToRadFactor = Math.PI / 180;
    return degrees * degToRadFactor;
}

public static double RadiansToDegrees(double radians)
{
    const double radToDegFactor = 180 / Math.PI;
    return radians * radToDegFactor;
}