经纬度距离计算Vincenty's formulae算法

维基百科公式

原始论文

import math
from geopy.distance import geodesic

# 计算两点之间的椭球面距离
# 使用Vincent算法，使用WGS84参考椭球体。
# 输入两个点的经纬度坐标，输出距离，单位为米
def distance(lat1, lon1, lat2, lon2):
    # WGS84 参考椭球体参数 Ellipsoid Parameters
    a = 6378137.000  # 长半轴 semi-major axis
    b = 6356752.3142  # 短半轴 semi-minor axis
    f = 1 / 298.257223563  # 扁率 flattening
    L = math.radians(lon2 - lon1)
    U1 = math.atan((1 - f) * math.tan(math.radians(lat1)))
    U2 = math.atan((1 - f) * math.tan(math.radians(lat2)))
    sinU1, cosU1, sinU2, cosU2 = math.sin(U1), math.cos(U1), math.sin(U2), math.cos(U2)
    lam = L

    for i in range(100):
        sinLam, cosLam = math.sin(lam), math.cos(lam)
        sinSigma = math.sqrt((cosU2 * sinLam) ** 2 +
                             (cosU1 * sinU2 - sinU1 * cosU2 * cosLam) ** 2)
        if sinSigma == 0:
            return 0
        cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLam
        sigma = math.atan2(sinSigma, cosSigma)
        sinAlpha = cosU1 * cosU2 * sinLam / sinSigma
        cosSqAlpha = 1 - sinAlpha ** 2
        cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha
        if math.isnan(cos2SigmaM):
            cos2SigmaM = 0  # equatorial line
        C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
        LP = lam
        lam = L + (1 - C) * f * sinAlpha * (
                sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)))
        if not abs(lam - LP) > 1e-12:
            break

    uSq = cosSqAlpha * (a ** 2 - b ** 2) / b ** 2
    A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
    B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))
    deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 *
                                 (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                                  B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) *
                                  (-3 + 4 * cos2SigmaM * cos2SigmaM)))
    s = b * A * (sigma - deltaSigma)
    return s

e = [63, 12]
f = [63, -12]
dis = geodesic(e, f).km
print(dis)
dispaper = distance(63, 12, 63, -12) / 1000
print(dispaper)