[Python]始点の緯度経度と方位角と距離から、終点の緯度経度と方位角を求める

## はじめに
以前、[A地点とB地点の緯度経度から距離と方位角を計算する方法](99ca549b963cedc106ab)について記事を書きましたが、
今回は、**A地点の緯度経度と距離と方位角からB地点の緯度経度と方位角を求める方法** について書いていこうと思います。

前回と同様、計算式は[Wikipedia](https://ja.wikipedia.org/wiki/Vincenty%E6%B3%95#%E9%A0%86%E8%A7%A3%E6%B3%95)を参考にしています。

## 作ってみた
```python
# -*- coding: utf-8 -*-
from math import *

# 楕円体
ELLIPSOID_GRS80 = 1 # GRS80
ELLIPSOID_WGS84 = 2 # WGS84

# 楕円体別の長軸半径と扁平率
GEODETIC_DATUM = {
    ELLIPSOID_GRS80: [
        6378137.0,         # [GRS80]長軸半径
        1 / 298.257222101, # [GRS80]扁平率
    ],
    ELLIPSOID_WGS84: [
        6378137.0,         # [WGS84]長軸半径
        1 / 298.257223563, # [WGS84]扁平率
    ],
}

# 反復計算の上限回数
ITERATION_LIMIT = 1000

'''
Vincenty法(順解法)
始点の座標(緯度経度)と方位角と距離から、終点の座標と方位角を求める
:param lat: 緯度
:param lon: 経度
:param azimuth: 方位角
:param distance: 距離
:param ellipsoid: 楕円体
:return: 終点の座標、方位角
'''
def vincenty_direct(lat, lon, azimuth, distance, ellipsoid=None):

    # 計算時に必要な長軸半径(a)と扁平率(ƒ)を定数から取得し、短軸半径(b)を算出する
    # 楕円体が未指定の場合はGRS80の値を用いる
    a, ƒ = GEODETIC_DATUM.get(ellipsoid, GEODETIC_DATUM.get(ELLIPSOID_GRS80))
    b = (1 - ƒ) * a

    # ラジアンに変換する(距離以外)
    φ1 = radians(lat)
    λ1 = radians(lon)
    α1 = radians(azimuth)
    s = distance

    sinα1 = sin(α1)
    cosα1 = cos(α1)

    # 更成緯度(補助球上の緯度)
    U1 = atan((1 - ƒ) * tan(φ1))

    sinU1 = sin(U1)
    cosU1 = cos(U1)
    tanU1 = tan(U1)

    σ1 = atan2(tanU1, cosα1)
    sinα = cosU1 * sinα1
    cos2α = 1 - sinα ** 2
    u2 = cos2α * (a ** 2 - b ** 2) / (b ** 2)
    A = 1 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2)))
    B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2)))

    # σをs/(b*A)で初期化
    σ = s / (b * A)

    # 以下の計算をσが収束するまで反復する
    # 地点によっては収束しないことがあり得るため、反復回数に上限を設ける
    for i in range(ITERATION_LIMIT):
        cos2σm = cos(2 * σ1 + σ)
        sinσ = sin(σ)
        cosσ = cos(σ)
        Δσ = B * sinσ * (cos2σm + B / 4 * (cosσ * (-1 + 2 * cos2σm ** 2) - B / 6 * cos2σm * (-3 + 4 * sinσ ** 2) * (-3 + 4 * cos2σm ** 2)))
        σʹ = σ
        σ = s / (b * A) + Δσ

        # 偏差が.000000000001以下ならbreak
        if abs(σ - σʹ) <= 1e-12:
            break
    else:
        # 計算が収束しなかった場合はNoneを返す
        return None

    # σが所望の精度まで収束したら以下の計算を行う
    x = sinU1 * sinσ - cosU1 * cosσ * cosα1
    φ2 = atan2(sinU1 * cosσ + cosU1 * sinσ * cosα1, (1 - ƒ) * sqrt(sinα ** 2 + x ** 2))
    λ = atan2(sinσ * sinα1, cosU1 * cosσ - sinU1 * sinσ * cosα1)
    C = ƒ / 16 * cos2α * (4 + ƒ * (4 - 3 * cos2α))
    L = λ - (1 - C) * ƒ * sinα * (σ + C * sinσ * (cos2σm + C * cosσ * (-1 + 2 * cos2σm ** 2)))
    λ2 = L + λ1

    α2 = atan2(sinα, -x) + pi

    return {
        'lat': degrees(φ2),     # 緯度
        'lon': degrees(λ2),     # 経度
        'azimuth': degrees(α2), # 方位角
    }
```

## 試してみた
始点の緯度軽度を「南鳥島」に、方位角を「276.8697566783211」に、距離を「3143772m」に設定して実行しました。

```python

lat = 24.288472   # 南鳥島の緯度
lon = 153.9707894 # 南鳥島の経度

result = vincenty_direct(lat, lon, 276.8697566783211, 3143772, 1)
if result:
    print('緯度：%s' % result['lat'])
    print('軽度：%s' % result['lon'])
    print('方位角：%s' % result['azimuth'])
```
結果は以下の通りです。

`緯度：24.455922367465213`
`軽度：122.91876257345254`
`方位角：83.78819260393011`

出力結果の緯度軽度はどこを指すのか、国土地理院の [**距離と方位角を計算するサービス**](https://vldb.gsi.go.jp/sokuchi/surveycalc/surveycalc/bl2stf.html) で確認したところ、「与那国島」であることが分かりました。

##おわりに
日常生活で**Vincenty法**を用いて何かをすることはまず無いかもしれませんが、どうやって距離や方位角を計算するのかを知ることができて、自分にとってはいい経験になりました。