计算地图上两点距离

参考：https://blog.csdn.net/pyx6119822/article/details/52298037

# coding: utf-8

import math

# 参看：http://blog.csdn.net/zengmingen/article/details/68490497

# 大地坐标系资料WGS-84 长半径a=6378137 短半径b=6356752.3142 扁率f=1/298.2572236
# 长半径a=6378137
a = 6378137
# 短半径b=6356752.3142
b = 6356752.3142
# 扁率f=1/298.2572236
f = 1 / 298.257223563


#  角度转弧度
def rad(d):
    return d * math.pi / 180.0


# 弧度转角度
def deg(x):
    return x * 180.0 / math.pi


def computerThatLonLat(lon, lat, brng, dist):
    """
    取边界值请注意，程序未验证。实际情况出现请人工验证边界值的情况。

    :param lon: 经度, 正东为正方向。取值：[0, 360)
    :param lat: 纬度，正北为正方向。取值：[-90, 90]
    :param brng: 方位角，和真北方向的夹角，顺时针方向是正方向。取值[0, 360]，正北：0，正东：90，正南：180，正西：270
    :param dist: 距离。单位：米
    :return: 一个数组：[经度，纬度]
    """

    alpha1 = rad(brng)
    sinAlpha1 = math.sin(alpha1)
    cosAlpha1 = math.cos(alpha1)
    # print("alpha1:{1}, sinAlpha1:{1}, cosAlpha1:{2}".format(alpha1, sinAlpha1, cosAlpha1))

    tanU1 = (1 - f) * math.tan(rad(lat))
    cosU1 = 1 / math.sqrt((1 + tanU1 * tanU1))
    sinU1 = tanU1 * cosU1
    sigma1 = math.atan2(tanU1, cosAlpha1)
    sinAlpha = cosU1 * sinAlpha1
    cosSqAlpha = 1 - sinAlpha * sinAlpha
    uSq = cosSqAlpha * (a * a - b * b) / (b * b)
    A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)))
    B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)))
    # print("tanU1:{0}, cosU1:{1}, sinU1:{2}, sigma1:{3}, sinAlpha:{4}, cosSqAlpha{5}, uSq:{6}, A:{7}, B:{8}".format(tanU1, cosU1, sinU1, sigma1, sinAlpha, cosSqAlpha, uSq, A, B))

    cos2SigmaM = 0
    sinSigma = 0
    cosSigma = 0
    sigma = dist / (b * A)
    sigmaP = 2 * math.pi

    while math.fabs(sigma - sigmaP) > 1e-12:
        cos2SigmaM = math.cos(2 * sigma1 + sigma)
        sinSigma = math.sin(sigma)
        cosSigma = math.cos(sigma)

        deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (
            cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (
                -3 + 4 * cos2SigmaM * cos2SigmaM)))

        sigmaP = sigma
        sigma = dist / (b * A) + deltaSigma

    tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1
    # print("sinU1:{0}, tmp:{1}".format(sinU1, tmp))
    lat2 = math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
                      (1 - f) * math.sqrt(sinAlpha * sinAlpha + tmp * tmp))

    lambdas = math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1)
    C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha))
    L = lambdas - (1 - C) * f * sinAlpha * (
        sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)))
    revAz = math.atan2(sinAlpha, -tmp)

    # print("revAz:{0}.".format(revAz))
    # print("lon:{0}, lat:{1}.".format(lon+deg(L), deg(lat2) ))
    return [lon + deg(L), deg(lat2)]


"""
lon = 0
lat = 0
brng = 90
dist = 2000000

print(computerThatLonLat(lon, lat, brng, dist))
"""


def getRightUp(lon, lat, dist):
    res = computerThatLonLat(lon, lat, 90, dist)
    res = computerThatLonLat(res[0], res[1], 0, dist)
    return res


def getLeftBottom(lon, lat, dist):
    res = computerThatLonLat(lon, lat, 270, dist)
    res = computerThatLonLat(res[0], res[1], 180, dist)
    return res


def getRectangle(clon, clat, n, m):
    ru = getRightUp(clon, clat, 1.0 * n * m / 2.0)
    lb = getLeftBottom(clon, clat, 1.0 * n * m / 2.0)
    return [lb, ru]


clon = 0
clat = 0
n = 100
m = 100

# print(getRectangle(clon, clat, n, m))

"""
if __name__ == "__main__":
    if len(sys.argv) != 5:
        print("please input 4 parameter: centerlon centerlat n(一行格子个数) m(单个格子的宽度,单位:米).")
    else:
        print(getRectangle(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3]), float(sys.argv[4])))
"""


# 参考：http://blog.csdn.net/zhuqiuhui/article/details/53180395

# 计算方位角
def getDegree(latA, lonA, latB, lonB):
    """
    Args:
        point p1(latA, lonA)
        point p2(latB, lonB)
    Returns:
        bearing between the two GPS points,
        default: the basis of heading direction is north
    """
    radLatA = rad(latA)
    radLonA = rad(lonA)
    radLatB = rad(latB)
    radLonB = rad(lonB)
    dLon = radLonB - radLonA
    y = math.sin(dLon) * math.cos(radLatB)
    x = math.cos(radLatA) * math.sin(radLatB) - math.sin(radLatA) * math.cos(radLatB) * math.cos(dLon)
    brng = deg(math.atan2(y, x))
    brng = (brng + 360) % 360
    return brng


# print(getDegree(39.997559, 116.537327, 39.944033, 116.474661))


# 计算距离
def getDistance(latA, lonA, latB, lonB):
    ra = 6378140  # radius of equator: meter
    rb = 6356755  # radius of polar: meter
    flatten = (ra - rb) / ra  # Partial rate of the earth
    # change angle to radians
    radLatA = rad(latA)
    radLonA = rad(lonA)
    radLatB = rad(latB)
    radLonB = rad(lonB)

    pA = math.atan(rb / ra * math.tan(radLatA))
    pB = math.atan(rb / ra * math.tan(radLatB))
    x = math.acos(math.sin(pA) * math.sin(pB) + math.cos(pA) * math.cos(pB) * math.cos(radLonA - radLonB))
    c1 = (math.sin(x) - x) * (math.sin(pA) + math.sin(pB)) ** 2 / math.cos(x / 2) ** 2
    c2 = (math.sin(x) + x) * (math.sin(pA) - math.sin(pB)) ** 2 / math.sin(x / 2) ** 2
    dr = flatten / 8 * (c1 - c2)
    distance = ra * (x + dr)
    return distance


# print(getDistance(-0.0452184737581234, -0.04491576420631404, 0.04521847375812339, 0.044915764206314046))

"""
inputf = open("outputj.txt")


exps = 1e-10

for i in range(0,360):
    for j in range(-90,91):
        res = computerThatLonLat(i,j,brng, dist)
        line = inputf.readline().lower().strip().split(",")

        if math.fabs(res[0] - float(line[0])) > exps or math.fabs(res[1] - float(line[1])) > exps or math.fabs(res[2] - float(line[2])) > exps :
            print(i,j)
            print(res)
            print(line)

        if math.fabs(getDegree(j,i, res[2], res[1]) - 63) > 2:
            print(i,j)
        if math.fabs(getDistance(j,i,res[2],res[1]) - 47 ) > 2:
            print("aaaa",i,j)

"""