白塞尔大地主题反算

夏白分享社

于 2023-12-22 17:27:29 发布

阅读量730

点赞数 10

文章标签：算法 c++

本文链接：https://blog.csdn.net/weixin_74013176/article/details/135158055

版权

#include <stdio.h>
#include <math.h>

#define PI 3.14159265358979323846

double toRadians(double degree) {
    return degree * PI / 180.0;
}

double vincentyDistance(double lat1, double lon1, double lat2, double lon2) {
    double a = 6378137.0; // semi-major axis of the Earth in meters
    double f = 1/298.257223563; // flattening
    double b = (1 - f) * a; // semi-minor axis of the Earth in meters
    double L = toRadians(lon2 - lon1);
    double lambda = L;
    double sinU1 = sin(toRadians(lat1)), cosU1 = cos(toRadians(lat1));
    double sinU2 = sin(toRadians(lat2)), cosU2 = cos(toRadians(lat2));
    double cosSqAlpha, sinSigma, cos2SigmaM, cosSigma, sigma, sinLambda, cosLambda;
    double alpha, beta, cos2Sigma, C;
    do {
        sinLambda = sin(lambda);
        cosLambda = cos(lambda);
        sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
            (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) *
            (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
        if (sinSigma == 0) {
            return 0;  // co-incident points
        }
        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        sigma = atan2(sinSigma, cosSigma);
        double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
        cosSqAlpha = 1 - sinAlpha * sinAlpha;
        cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
        C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
        double lambdaP = lambda;
        lambda = L + (1 - C) * f * sinAlpha *
            (sigma + C * sinSigma *
            (cos2SigmaM + C * cosSigma *
            (-1 + 2 * cos2SigmaM * cos2SigmaM)));
    } while (fabs(lambda - lambdaP) > 1e-12); // iterative until change is less than 1e-12
    double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
    double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
    double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
    double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 *
        (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM *
        (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
    double s = b * A * (sigma - deltaSigma);
    return s;
}

int main() {
    double lat1 = 40.7128; // latitude of point 1
    double lon1 = -74.0060; // longitude of point 1
    double lat2 = 34.0522; // latitude of point 2
    double lon2 = -118.2437; // longitude of point 2
    double distance = vincentyDistance(lat1, lon1, lat2, lon2);
    printf("Distance: %f meters\n", distance);
    return 0;
}