计算两点间距离

#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
*/

本栏目推荐文章
• word或文档中英文间距问题
• 纯CSS实现可自定义间距虚线边框
• 防御式CSS—设置组件间距
• html字间距怎么设置？
• Android Recyclerview的item间距实现
• ggplot画组合热图，设置面板间距
• js 计算两个地点坐标之间的间距
• 连载随笔：第一个VBA工具的诞生-站间距计算工具（二）
• 连载随笔：第一个VBA工具的诞生-站间距计算工具（一）
• ITK 实例5 定义图像原点和间距

间距间距html 工具 间距 随笔vba 虚线 间距 边框css 间距 中英 文档 问题 间距 组件css 铜皮 间距 实际 规则 间距 图标 桌面 问题 间距 原点 实例 图像 间距 方式video img 间距 两端 一行 字符