计算两点间距离

#include <iostream>
#include <cmath>
#include <proj.h>

using namespace std;

// 圆周率
const double pi = 3.14159265358979323846;

// WGS84 中定义的常量
const double a = 6378137.0;      // 长半轴
const double b = 6356752.314245; // 短半轴
const double f = (a - b) / a;    // 扁率

// 将角度转换为弧度
double degreesToRadians(double degrees)
{
    return degrees * pi / 180.0;
}

// 计算半径曲率半径
double radiusOfCurvature(double latitude)
{
    double sinLat = sin(degreesToRadians(latitude));
    return a * (1 - pow(f, 2)) / pow(1 - pow(f * sinLat, 2), 1.5);
}

// 将 WGS84 坐标转换为 ECEF 坐标
void wgs84ToEcef(double latitude, double longitude, double altitude, double &x, double &y, double &z)
{
    double cosLat = cos(degreesToRadians(latitude));
    double sinLat = sin(degreesToRadians(latitude));
    double cosLon = cos(degreesToRadians(longitude));
    double sinLon = sin(degreesToRadians(longitude));
    double Rn = radiusOfCurvature(latitude);

    x = (Rn + altitude) * cosLat * cosLon;
    y = (Rn + altitude) * cosLat * sinLon;
    z = (Rn * (1 - pow(f, 2)) + altitude) * sinLat;
}

// 计算两点间的距离（使用 ECEF 坐标）
double distanceUsingEcef(double lat1, double lon1, double alt1, double lat2, double lon2, double alt2)
{
    double x1, y1, z1, x2, y2, z2;
    wgs84ToEcef(lat1, lon1, alt1, x1, y1, z1);
    wgs84ToEcef(lat2, lon2, alt2, x2, y2, z2);
    return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2) + pow(z1 - z2, 2));
}

// 计算两点间的距离（使用 Haversine 公式）
double distanceUsingHaversine(double lat1, double lon1, double lat2, double lon2)
{
    double R = 6371000.0; // 地球半径，单位 km
    double dLat = degreesToRadians(lat2 - lat1);
    double dLon = degreesToRadians(lon2 - lon1);
    double a = sin(dLat / 2) * sin(dLat / 2) +
               cos(degreesToRadians(lat1)) * cos(degreesToRadians(lat2)) *
                   sin(dLon / 2) * sin(dLon / 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));
    return R * c;
}

// WGS84 中定义的常量
// const double a = 6378137.0; // 长半轴
// const double b = 6356752.314245; // 短半轴
// const double f = 1 / 298.257223563; // 扁率

// 将角度转换为弧度
// double degreesToRadians(double degrees) {
//    return degrees * M_PI / 180.0;
//}

// 计算两个 WGS84 坐标点间的距离（单位：米）
double vincentyDistance(double lat1, double lon1, double lat2, double lon2)
{
    double L = degreesToRadians(lon2 - lon1);
    double U1 = atan((1 - f) * tan(degreesToRadians(lat1)));
    double U2 = atan((1 - f) * tan(degreesToRadians(lat2)));
    double sinU1 = sin(U1), cosU1 = cos(U1);
    double sinU2 = sin(U2), cosU2 = cos(U2);

    double lambda = L, lambdaP = 2 * M_PI;
    double sinLambda = sin(lambda), cosLambda = cos(lambda);
    double sinSigma = 0.0, cosSigma = 0.0, sigma = 0.0;
    double sinAlpha = 0.0, cosSqAlpha = 0.0, cos2SigmaM = 0.0;
    double C = 0.0;

    while (abs(lambda - lambdaP) > 1e-12)
    {
        sinLambda = sin(lambda), cosLambda = cos(lambda);
        sinSigma = sqrt(pow(cosU2 * sinLambda, 2) + pow(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda, 2));
        if (sinSigma == 0)
        {
            return 0.0; // 两点重合
        }
        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
        sigma = atan(sinSigma / cosSigma);
        sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
        cosSqAlpha = 1 - pow(sinAlpha, 2);
        cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
        C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
        lambdaP = lambda;
        lambda = L + (1 - C) * f * sinAlpha * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * pow(cos2SigmaM, 2))));
    }

    double uSq = cosSqAlpha * (pow(a, 2) - pow(b, 2)) / pow(b, 2);
    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 * pow(cos2SigmaM, 2)) - B / 6 * cos2SigmaM * (-3 + 4 * pow(sinSigma, 2)) * (-3 + 4 * pow(cos2SigmaM, 2))));
    return b * A * (sigma - deltaSigma);
}

int main111()
{
    double lat1 = 40.7128;  // 纬度
    double lon1 = -74.0060; // 经度
    double lat2 = 48.8566;  // 纬度
    double lon2 = 2.3522;   // 经度

    double distance = vincentyDistance(lat1, lon1, lat2, lon2);
    cout << "Vincenty distance: " << distance << " meters" << endl;

    return 0;
}

namespace ENU
{
    // WGS84 中定义的常量
    const double a = 6378137.0;         // 长半轴
    const double b = 6356752.314245;    // 短半轴
    const double f = 1 / 298.257223563; // 扁率

    // 将角度转换为弧度
    double degreesToRadians(double degrees)
    {
        return degrees * M_PI / 180.0;
    }

    // 将弧度转换为角度
    double radiansToDegrees(double radians)
    {
        return radians * 180.0 / M_PI;
    }

    // 计算两个 WGS84 坐标点间的距离（单位：米）
    // double vincentyDistance(double lat1, double lon1, double lat2, double lon2)
    // {
    //     // 略
    // }

    // 将 WGS84 坐标转换为 ECEF 坐标
    void wgs84ToEcef(double lat, double lon, double alt, double &x, double &y, double &z)
    {
        double sinLat = sin(degreesToRadians(lat));
        double cosLat = cos(degreesToRadians(lat));
        double sinLon = sin(degreesToRadians(lon));
        double cosLon = cos(degreesToRadians(lon));

        double N = a / sqrt(1 - pow(f * sinLat, 2));
        x = (N + alt) * cosLat * cosLon;
        y = (N + alt) * cosLat * sinLon;
        z = (N * (1 - pow(f, 2)) + alt) * sinLat;
    }

    // 将 ECEF 坐标转换为 ENU 坐标 double refX, double refY, double refZ,
    void ecefToEnu(double x, double y, double z,
                   double refLat, double refLon, double refAlt,
                   double &east, double &north, double &up)
    {
        double x0, y0, z0;
        wgs84ToEcef(refLat, refLon, refAlt, x0, y0, z0);

        double dx = x - x0;
        double dy = y - y0;
        double dz = z - z0;

        double sinRefLat = sin(degreesToRadians(refLat));
        double cosRefLat = cos(degreesToRadians(refLat));
        double sinRefLon = sin(degreesToRadians(refLon));
        double cosRefLon = cos(degreesToRadians(refLon));

        east = -sinRefLon * dx + cosRefLon * dy;
        north = -sinRefLat * cosRefLon * dx - sinRefLat * sinRefLon * dy + cosRefLat * dz;
        up = cosRefLat * cosRefLon * dx + cosRefLat * sinRefLon * dy + sinRefLat * dz;
    }

    // 计算两个三维点坐标之间的距离
    double distance(double x1, double y1, double z1, double x2, double y2, double z2)
    {
        return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2) + pow(z1 - z2, 2));
    }

    int calc()
    {
        double lat1 = 40.7128;  // 纬度
        double lon1 = 114.0060; // 经度
        double alt1 = 0.0;      // 海拔
        double lat2 = 40.7566;  // 纬度
        double lon2 = 114.0062; // 经度
        double alt2 = 0.0;      // 海拔

        double refLat = 40.0;
        double refLon = 114.0; // 参考点经度
        double refAlt = 0.0;   // 参考点海拔

        double x1, y1, z1, x2, y2, z2;
        wgs84ToEcef(lat1, lon1, alt1, x1, y1, z1);
        wgs84ToEcef(lat2, lon2, alt2, x2, y2, z2);

        double east, north, up;
        ecefToEnu(x1, y1, z1, refLat, refLon, refAlt, east, north, up);

        double east2, north2, up2;
        ecefToEnu(x2, y2, z2, refLat, refLon, refAlt, east2, north2, up2);

        // cout << "ENU coordinates: (" << east << ", " << north << ", " << up << ")" << endl;

        double dist = distance(east, north, up, east2, north2, up2);
        cout << "ENU distance:" << dist << " meters" << endl;

        return 0;
    }
}

namespace ProjX
{
    // #include <proj.h>

    double distance(double x1, double y1, double z1, double x2, double y2, double z2)
    {
        return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2) + pow(z1 - z2, 2));
    }

    int calc_utm_zone(double lon)
    {
        return int(lon / 6) + 31;
    }

    void ttt()
    {
        double lat1 = 40.7128;  // 纬度
        double lon1 = 114.0060; // 经度
        double alt1 = 0.0;      // 海拔
        double lat2 = 40.7566;  // 纬度
        double lon2 = 114.0062; // 经度
        double alt2 = 0.0;      // 海拔

        PJ_CONTEXT *C = proj_context_create();

        printf("Lon:%.3lf UTM Zone:%d \n", lon1, calc_utm_zone(lon1));

        PJ *P = proj_create_crs_to_crs(C,
                                       "EPSG:4326",
                                       "+proj=utm +zone=50 +datum=WGS84", /* or EPSG:32632 */
                                       NULL);

        PJ *P_for_GIS = proj_normalize_for_visualization(C, P);
        proj_destroy(P);
        P = P_for_GIS;

        PJ_COORD a = proj_coord(lon1, lat1, 0, 0);
        PJ_COORD b = proj_trans(P, PJ_FWD, a);

        PJ_COORD a2 = proj_coord(lon2, lat2, 0, 0);
        PJ_COORD b2 = proj_trans(P, PJ_FWD, a2);

        printf("east: %.3f, north: %.3f\n", b.enu.e, b.enu.n);
        printf("east: %.3f, north: %.3f\n", b2.enu.e, b2.enu.n);

        double d = distance(b.enu.e, b.enu.n, 0, b2.enu.e, b2.enu.n, 0);
        printf("distance=%.3lf \n", d);

        proj_destroy(P);
        proj_context_destroy(C);

        // auto pj_wgs84 = proj_create_crs_to_crs(C,
        //                                        "EPSG:4326",  // 源坐标系（WGS84）
        //                                        "EPSG:32618", // 目标坐标系（UTM Zone 18N）
        //                                        nullptr);

        // double x = lon, y = lat;
        // proj_transform(pj_wgs84, pj_init_plus(C, nullptr, "+units=m +no_defs"), 1, &x, &y, nullptr);

        // std::cout << "UTM coordinates: (" << x << ", " << y << ")" << std::endl;

        // proj_destroy(pj_wgs84);
    }

}

int main()
{
    double lat1 = 40.7128;  // 纬度
    double lon1 = 114.0060; // 经度
    double alt1 = 0.0;      // 海拔
    double lat2 = 40.7566;  // 纬度
    double lon2 = 114.0062; // 经度
    double alt2 = 0.0;      // 海拔

    double distance = vincentyDistance(lat1, lon1, lat2, lon2);
    cout << "Vincenty distance: " << distance << " meters" << endl;

    double distanceEcef = distanceUsingEcef(lat1, lon1, alt1, lat2, lon2, alt2);
    cout << "Distance using ECEF: " << distanceEcef << " meters" << endl;

    double distanceHaversine = distanceUsingHaversine(lat1, lon1, lat2, lon2);
    cout << "Distance using Haversine: " << distanceHaversine << " meters" << endl;

    ENU::calc();

    ProjX::ttt();

    return 0;
}

/*
sudo apt-get update
sudo apt install g++ -y
sudo apt install libproj-dev
g++ -o hello  main.cpp -lproj&& ./hello
*/