基于经纬度的转弯半径插值点计算函数开发需求

针对需求：已知WGS84经纬度点A、B，转弯半径r（球面距离，单位米），根据进度值x∈[0,1]计算逆时针转弯路径上的插值点P（x=0对应A，x=1对应B），以下是完整的实现方案：

核心思路 #

球面转弯路径本质是球面上的一段小圆弧（区别于大圆路径），实现步骤分为：

1. 经纬度与地心地固坐标系（ECEF）的转换，便于向量运算
2. 计算A、B两点的大圆距离与地心角
3. 确定转弯小圆的圆心C（球面上的点，A、B到C的球面距离均为r）
4. 通过向量旋转，根据进度x在小圆弧上插值得到P点
5. 将P点的ECEF坐标转回经纬度

必要结构体与常量定义 #

#include <cmath>

// 经纬度结构体（弧度）
struct LatLon {
    double lat; // 纬度范围：[-π/2, π/2]
    double lon; // 经度范围：[-π, π]
};

// ECEF地心地固坐标系结构体
struct ECEF {
    double x, y, z;
};

// WGS84地球参数
const double WGS84_R = 6378137.0;          // 地球半长轴（米）
const double WGS84_E2 = 0.00669437999014; // 第一偏心率平方

辅助工具函数 #

坐标系转换 #

// 经纬度转ECEF坐标
ECEF latLonToECEF(const LatLon& ll) {
    double N = WGS84_R / sqrt(1 - WGS84_E2 * sin(ll.lat) * sin(ll.lat));
    return {
        N * cos(ll.lat) * cos(ll.lon),
        N * cos(ll.lat) * sin(ll.lon),
        N * (1 - WGS84_E2) * sin(ll.lat)
    };
}

// ECEF坐标转经纬度（迭代修正精度）
LatLon ecefToLatLon(const ECEF& e) {
    double p = sqrt(e.x*e.x + e.y*e.y);
    double lat = atan2(e.z, p * (1 - WGS84_E2));
    double lon = atan2(e.y, e.x);
    
    // 迭代3次修正纬度，提升精度
    for (int i = 0; i < 3; ++i) {
        double N = WGS84_R / sqrt(1 - WGS84_E2 * sin(lat) * sin(lat));
        lat = atan2(e.z + N * WGS84_E2 * sin(lat), p);
    }
    return {lat, lon};
}

向量运算工具 #

// 向量单位化
ECEF normalize(const ECEF& v) {
    double mag = sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
    return {v.x/mag, v.y/mag, v.z/mag};
}

// 向量叉乘
ECEF cross(const ECEF& a, const ECEF& b) {
    return {
        a.y*b.z - a.z*b.y,
        a.z*b.x - a.x*b.z,
        a.x*b.y - a.y*b.x
    };
}

// 向量点乘
double dot(const ECEF& a, const ECEF& b) {
    return a.x*b.x + a.y*b.y + a.z*b.z;
}

// 罗德里格斯向量旋转（绕指定轴旋转指定角度）
ECEF rotateVector(const ECEF& v, const ECEF& axis, double angle) {
    double cosθ = cos(angle);
    double sinθ = sin(angle);
    double dotProd = dot(v, axis);
    return {
        v.x*cosθ + (axis.y*v.z - axis.z*v.y)*sinθ + axis.x*dotProd*(1 - cosθ),
        v.y*cosθ + (axis.z*v.x - axis.x*v.z)*sinθ + axis.y*dotProd*(1 - cosθ),
        v.z*cosθ + (axis.x*v.y - axis.y*v.x)*sinθ + axis.z*dotProd*(1 - cosθ)
    };
}

大圆距离计算 #

// 计算两点间的大圆距离（米），使用haversine公式
double haversineDistance(const LatLon& a, const LatLon& b) {
    double dLat = b.lat - a.lat;
    double dLon = b.lon - a.lon;
    double a_val = sin(dLat/2)*sin(dLat/2) + cos(a.lat)*cos(b.lat)*sin(dLon/2)*sin(dLon/2);
    double c = 2 * atan2(sqrt(a_val), sqrt(1 - a_val));
    return WGS84_R * c;
}

核心插值函数实现 #

// 计算转弯路径插值点
// 参数说明：
//   A/B: 起点/终点的经纬度（弧度）
//   r: 转弯半径（球面距离，单位米）
//   x: 进度值，范围[0,1]，0对应A，1对应B
// 返回：插值点P的经纬度（弧度）
LatLon getTurnInterpolation(const LatLon& A, const LatLon& B, double r, double x) {
    // 边界情况直接返回
    if (x <= 0.0) return A;
    if (x >= 1.0) return B;

    // 转换为ECEF单位向量
    ECEF ecefA = latLonToECEF(A);
    ECEF ecefB = latLonToECEF(B);
    ECEF uA = normalize(ecefA);
    ECEF uB = normalize(ecefB);

    // 计算A、B的大圆距离与地心角
    double d = haversineDistance(A, B);
    double theta_AB = d / WGS84_R; // A、B对应的地心角（弧度）

    // 计算转弯小圆上A、B的夹角α
    double theta_r = r / WGS84_R; // 转弯半径对应的地心角
    double cos_alpha = (cos(theta_AB) - cos(theta_r)*cos(theta_r)) / (sin(theta_r)*sin(theta_r));
    // 修正浮点误差导致的超出范围问题
    cos_alpha = std::max(-1.0, std::min(1.0, cos_alpha));
    double alpha = acos(cos_alpha);

    // 确定逆时针转弯的小圆圆心C的单位向量
    ECEF uPerp = normalize(cross(uA, uB)); // 逆时针方向的垂直向量
    ECEF uC = {
        cos(theta_r)*uA.x + sin(theta_r)*uPerp.x,
        cos(theta_r)*uA.y + sin(theta_r)*uPerp.y,
        cos(theta_r)*uA.z + sin(theta_r)*uPerp.z
    };
    uC = normalize(uC);

    // 绕C点的径向向量旋转x*alpha角度，得到P点的单位向量
    ECEF uP = rotateVector(uA, uC, x*alpha);

    // 转换回经纬度
    ECEF ecefP = {uP.x*WGS84_R, uP.y*WGS84_R, uP.z*WGS84_R};
    return ecefToLatLon(ecefP);
}

使用注意事项 #

1. 输入单位：所有经纬度参数必须是弧度，如果是角度，需先转换：rad = deg * M_PI / 180.0
2. 参数合法性：转弯半径r必须满足 r >= d/(2*sin(theta_AB/2))（即A、B能在半径为r的小圆上），否则函数会因浮点误差返回异常结果，建议添加参数校验逻辑
3. 方向切换：若需要顺时针转弯，只需将uPerp = normalize(cross(uB, uA))即可
4. 精度控制：ECEF转经纬度的迭代次数可根据需求调整，3次迭代足以满足大多数场景的精度要求

内容的提问来源于stack exchange，提问作者uray