C++ - Vincenty 法による地球楕円体上の距離計算！

地球楕円体上の任意の2地点間の距離やそれぞれから見た方位角、また、1地点から見た方位角・距離にある地点の位置等を計算するために Vincenty 法なるアルゴリズムが存在します。

今回、 C++ で「地球楕円体上の任意の2地点間の距離やそれぞれから見た方位角」の計算処理を実装してみました。

過去には、 Ruby や Fortran で実装しています。

• Ruby - Vincenty 法による地球楕円体上の距離／位置計算！
• Fortran 2003 - Vincenty 法による地球楕円体上の距離／位置計算！

0. 前提条件

• Debian GNU/Linux 10.5 (64bit) での作業を想定。
• GCC 9.2.0 (G++ 9.2.0) (C++17) でのコンパイルを想定。

1. Vincenty法 (Vincenty’s formulae) について

1-1. Introduction（紹介）

Vincenty 法(Vincenty’s formulae)とは、T.Vincenty が考案した、楕円体上の2点間の距離を計算したり、1点から指定の方角・距離にある点を求めたりするのに使用する反復計算アルゴリズムである、

1-2. Notation（表記法）

以下のように定義する。

\[\begin{eqnarray*} a &:& 赤道半径（長半径） \\ b &:& 極半径（短半径） \\ f &:& 扁平率 \\ && = \frac{a - b}{a} \\ \phi_1, \phi_2 &:& 地点1,2の緯度（北緯:+, 南緯:-） \\ L_1, L_2 &:& 地点1,2の経度（東経:+, 西経:-） \\ L &:& 地点1と2の経度の差 \\ && = L_1 - L_2 \\ s &:& 地点1と2の楕円体上の距離 \\ \alpha_1, \alpha_2 &:& 地点1,2における方位角（北を基点に時計回り） \\ \alpha &:& 赤道上での方位角 \\ U_1 &:& 地点1の更成緯度 \\ && = \tan^{-1} \{(1 - f)\tan \phi_1\} \\ && (\because \tan U_1 = (1 - f)\tan \phi_1) \\ U_2 &:& 地点2の更成緯度 \\ && = \tan^{-1} \{(1 - f)\tan \phi_2\} \\ && (\because \tan U_2 = (1 - f)\tan \phi_2) \\ \lambda_1, \lambda_2 &:& 補助球上の地点1,2の経度 \\ \lambda &:& 補助球上の地点1,2の経度の差 \\ \sigma &:& 補助球上の地点1から2までの距離（弧の長さ） \\ \sigma_1 &:& 補助球上の赤道から地点1までの距離（孤の長さ） \\ \sigma_m &:& 補助球上の赤道から\sigma_1の中点までの距離（孤の長さ） \\ \end{eqnarray*}\]

1-3. Direct formula （順解法）

地点1 \((\phi_1, L_1)\) と地点1における方位角 \(\alpha_1\), 距離 \(s\) が与えられたとき、地点2 \((\phi_2, L_2)\) と方位角 \(\alpha_2\) を求める。

\[\begin{eqnarray*} \tan U_1 &=& (1 - f)\tan \phi_1 \\ U_1 &=& \tan^{-1} \{(1 - f)\tan \phi_1\} \ (= \tan^{-1} (\tan U_1)) \\ \sigma_1 &=& \tan^{-1} \frac{\tan U_1}{\cos \alpha_1} \\ \sin \alpha &=& \cos U_1 \sin \alpha_1 \\ u^2 &=& \cos^2 \alpha \left(\frac{a^2 - b^2}{b^2}\right) = (1 - \sin^2 \alpha)\left(\frac{a^2 - b^2}{b^2}\right) \\ A &=& 1 + \frac{u^2}{16384}[4096 + u^2 \{-768 + u^2 (320 - 175 u^2)\}] \\ B &=& \frac{u^2}{1024}[256 + u^2 \{-128 + u^2 (74 - 47 u^2)\}] \end{eqnarray*}\]

\(\displaystyle \sigma = \frac{s}{bA}\) で初期化後、 \(\sigma\) の値が収束する（無視できるレベルになる）まで、以下をループ処理する。

\[\begin{eqnarray*} 2\sigma_m &=& 2\sigma_1 + \sigma \\ \Delta\sigma &=& B\sin\sigma[\cos2\sigma_m + \frac{1}{4}B\{\cos\sigma(-1 + 2\cos^2 2\sigma_m) \\ && - \frac{1}{6}B\cos2\sigma_m(-3 + 4\sin^2\sigma)(-3 + 4\cos^2 2\sigma_m)\}] \\ \sigma &=& \frac{s}{bA} + \Delta\sigma \end{eqnarray*}\]

\(\sigma\)収束後、以下の処理を行なう。

\[\begin{eqnarray*} \phi_2 &=& \tan^{-1}\frac{\sin U_1\cos \sigma + \cos U_1 \sin \sigma \cos \alpha_1}{(1 - f)\sqrt{\sin^2\alpha + (\sin U_1 \sin \sigma - \cos U_1 \cos \sigma \cos \alpha_1)^2}} \\ \lambda &=& \tan^{-1}\frac{\sin \sigma \sin \alpha_1}{\cos U_1 \cos \sigma - \sin U_1 \sin \sigma \cos \alpha_1} \\ C &=& \frac{f}{16} \cos^2\alpha \{4 + f(4 - 3\cos^2\alpha)\} \\ L &=& \lambda - (1 - C)f\sin\alpha [\sigma + C\sin\sigma\{\cos 2\sigma_m + C\cos\sigma (-1 + 2\cos^2 2\sigma_m)\}] \\ L_2 &=& L + L_1 \\ \alpha_2 &=& \tan^{-1}\frac{\sin\alpha}{-\sin U_1 \sin\sigma + \cos U_1 \cos\sigma \cos\alpha_1} \end{eqnarray*}\]

1-4. Inverse formula （逆解法）

楕円体上の2地点、地点1 \((\phi_1, L_1)\), 地点2 \((\phi_2, L_2)\) が与えられたとき、地点1, 2における方位角 \(\alpha_1, \alpha_2\) と距離 \(s\) を求める。

\[\begin{eqnarray*} U_1 &=& \tan^{-1} \{(1 - f)\tan \phi_1\} \\ U_2 &=& \tan^{-1} \{(1 - f)\tan \phi_2\} \\ L &=& L_1 - L_2 \end{eqnarray*}\]

\(\lambda = L\) で初期化後、 \(\lambda\) の値が収束する（無視できるレベルになる）まで、以下をループ処理する。

\[\begin{eqnarray*} \sin\sigma &=& \sqrt{(\cos U_2 \sin\lambda)^2 + (\cos U_1 \sin U_2 - \sin U_1 \cos U_2 \cos\lambda)^2} \\ \cos\sigma &=& \sin U_1 \sin U_2 + \cos U_1 \cos U_2 \cos\lambda \\ \sigma &=& \tan^{-1}\frac{\sin\sigma}{\cos\sigma} \\ \sin\alpha &=& \frac{\cos U_1 \cos U_2 \sin\lambda}{\sin\sigma} \\ \cos^2\alpha &=& 1 - \sin^2\alpha \\ \cos2\sigma_m &=& \cos\sigma - \frac{2\sin U_1 \sin U_2}{\cos^2\alpha} \\ C &=& \frac{f}{16} \cos^2\alpha \{4 + f(4 - 3\cos^2\alpha)\} \\ L &=& \lambda - (1 - C)f\sin\alpha [\sigma + C\sin\sigma\{\cos 2\sigma_m + C\cos\sigma (-1 + 2\cos^2 2\sigma_m)\}] \end{eqnarray*}\]

\(\lambda\) 収束後、以下の処理を行なう。

\[\begin{eqnarray*} u^2 &=& \cos^2 \alpha \left(\frac{a^2 - b^2}{b^2}\right) \\ A &=& 1 + \frac{u^2}{16384}[4096 + u^2 \{-768 + u^2 (320 - 175 u^2)\}] \\ B &=& \frac{u^2}{1024}[256 + u^2 \{-128 + u^2 (74 - 47 u^2)\}] \\ \Delta\sigma &=& B\sin\sigma[\cos2\sigma_m + \frac{1}{4}B\{\cos\sigma(-1 + 2\cos^2 2\sigma_m) \\ && - \frac{1}{6}B\cos2\sigma_m(-3 + 4\sin^2\sigma)(-3 + 4\cos^2 2\sigma_m)\}] \\ s &=& bA(\sigma - \Delta\sigma) \\ \alpha_1 &=& \tan^{-1}\frac{\cos U_2 \sin\lambda}{\cos U_1 \sin U_2 - \sin U_1 \cos U_2 \cos\lambda} \\ \alpha_2 &=& \tan^{-1}\frac{\cos U_1 \sin\lambda}{-\sin U_1 \cos U_2 + \cos U_1 \sin U_2 \cos\lambda} \end{eqnarray*}\]

2. ソースコードの作成

• 今回の「地球楕円体上の任意の2地点間の距離やそれぞれから見た方位角」の計算に使用するのは「逆解法」である。

File: vincenty.hpp

#ifndef VINCENTY_CALC_DISTANCE_VINCENTY_HPP_
#define VINCENTY_CALC_DISTANCE_VINCENTY_HPP_

#include <tuple>

namespace my_lib {

class Vincenty {
  double b;                     // 極半径（短半径）
  double phi_1;                 // 地点 1 の緯度
  double l_1;                   // 地点 1 の経度
  double tan_u_1;               // tan(地点 1 の更成緯度)
  double u_1;                   // 地点 1 の更成緯度
  double deg2rad(double);       // 度 => ラジアン
  double rad2deg(double);       // ラジアン => 度
  double calc_a(double);        // A 計算
  double calc_b(double);        // B 計算
  double calc_c(double);        // C 計算
  double calc_dlt_sgm(double, double, double, double);  // Δσ 計算
  double norm_lng(double);      // 経度正規化
  double norm_az(double);       // 方位角正規化

public:
  Vincenty(double, double);  // コンストラクタ
  std::tuple<double, double, double> calc_distance(double, double);
                             // 距離と方位角 1, 2 を計算（Vincenty 逆解法）
};

}  // namespace my_lib

#endif

File: vincenty.cpp

#include "vincenty.hpp"

#include <cmath>
#include <iostream>
#include <tuple>

namespace my_lib {

// 定数
constexpr double kA        = 6378137.0;             // GRS80 長半径
constexpr double kF        = 1.0 / 298.257222101;   // GRS80 扁平率
constexpr double kPi       = std::atan(1.0) * 4.0;  // PI
constexpr double kPi2      = kPi * 2.0;             // PI * 2
constexpr double kPi180    = kPi / 180.0;           // PI / 180
constexpr double kPi180Inv = 1.0 / kPi180;          // 1 / (PI / 180)
constexpr double kEps      = 1.0e-11;               // 1^(-11) = 10^(-12) で 0.06mm の精度

Vincenty::Vincenty(double lat_1, double lng_1) {
  try {
    b = kA * (1.0 - kF);
    phi_1 = deg2rad(lat_1);
    l_1 = deg2rad(lng_1);
    tan_u_1 = (1.0 - kF) * std::tan(phi_1);
    u_1 = std::atan(tan_u_1);
  } catch (...) {
    throw;
  }
}

/**
 * @brief      距離と方位角 1, 2 を計算
 *             （Vincenty 逆解法）
 *
 * @param[in]  lat_2: 緯度(double)
 * @param[in]  lng_2: 経度(double)
 * @return     {
 *                  s: 距離と方位角1,2を計算(double),
 *               az_1: 地点 1 における方位角(double),
 *               az_2: 地点 2 における方位角(double)
 *             }(tuple)
 */
std::tuple<double, double, double> Vincenty::calc_distance(
    double lat_2, double lng_2) {
  double s    = 0.0;  // 地点 1 と 2 の距離
  double az_1 = 0.0;  // 地点 1 における方位角
  double az_2 = 0.0;  // 地点 2 における方位角
  double phi_2;
  double l_2;
  double u_2;
  double cos_u_1;  // cos(u_1)
  double cos_u_2;  // cos(u_2)
  double sin_u_1;  // sin(u_1)
  double sin_u_2;  // sin(u_2)
  double su1_su2;  // sin(u_1) * sin(u_2)
  double su1_cu2;  // sin(u_1) * cos(u_2)
  double cu1_su2;  // cos(u_1) * sin(u_2)
  double cu1_cu2;  // cos(u_1) * cos(u_2)
  double l;
  double lmd;
  double lmd_prev;
  double cos_lmd;
  double sin_lmd;
  double t_0;
  double t_1;
  double sin_sgm;
  double cos_sgm;
  double sgm;
  double sin_alp;
  double cos2_alp;
  double cos_2_sgm_m;
  double aa;     // A
  double bb;     // B
  double cc;     // C
  double u2;
  double d_sgm;  // Δσ
  double alp_1;
  double alp_2;

  try {
    phi_2 = deg2rad(lat_2);
    l_2 = deg2rad(lng_2);
    u_2 = std::atan((1.0 - kF) * std::tan(phi_2));
    cos_u_1 = std::cos(u_1);
    cos_u_2 = std::cos(u_2);
    sin_u_1 = std::sin(u_1);
    sin_u_2 = std::sin(u_2);
    su1_su2 = sin_u_1 * sin_u_2;
    su1_cu2 = sin_u_1 * cos_u_2;
    cu1_su2 = cos_u_1 * sin_u_2;
    cu1_cu2 = cos_u_1 * cos_u_2;
    l = norm_lng(l_2 - l_1);
    lmd      = l;
    lmd_prev = kPi2;
    cos_lmd = std::cos(lmd);
    sin_lmd = std::sin(lmd);

    while (std::abs(lmd - lmd_prev) > kEps) {
      t_0 = cos_u_2 * sin_lmd;
      t_1 = cu1_su2 - su1_cu2 * cos_lmd;
      sin_sgm = std::sqrt(t_0 * t_0 + t_1 * t_1);
      cos_sgm = su1_su2 + cu1_cu2 * cos_lmd;
      sgm = std::atan2(sin_sgm, cos_sgm);
      sin_alp  = cu1_cu2 * sin_lmd / sin_sgm;
      cos2_alp = 1 - sin_alp * sin_alp;
      cos_2_sgm_m = cos_sgm - 2 * su1_su2 / cos2_alp;
      cc = calc_c(cos2_alp);
      lmd_prev = lmd;
      lmd      = l + (1.0 - cc) * kF * sin_alp
               * (sgm + cc * sin_sgm
               * (cos_2_sgm_m + cc * cos_sgm
               * (-1.0 + 2.0 * cos_2_sgm_m * cos_2_sgm_m)));
      cos_lmd = std::cos(lmd);
      sin_lmd = std::sin(lmd);
      if (lmd > kPi) {
        lmd = kPi;
        break;
      }
    }

    u2 = cos2_alp * (kA * kA - b * b) / (b * b);
    aa = calc_a(u2);
    bb = calc_b(u2);
    d_sgm = calc_dlt_sgm(bb, cos_sgm, sin_sgm, cos_2_sgm_m);
    s = b * aa * (sgm - d_sgm);
    alp_1 = std::atan2(cos_u_2 * sin_lmd,  cu1_su2 - su1_cu2 * cos_lmd);
    alp_2 = std::atan2(cos_u_1 * sin_lmd, -su1_cu2 + cu1_su2 * cos_lmd) + kPi;
    az_1 = rad2deg(norm_az(alp_1));
    az_2 = rad2deg(norm_az(alp_2));

    return {s, az_1, az_2};
  } catch (...) {
    throw;
  }

  return {s, az_1, az_2};  // 計算成功
}

/**
 * @brief      度 => ラジアン
 *
 * @param[in]  deg: 度      (double)
 * @return     rad: ラジアン(double)
 */
double Vincenty::deg2rad(double deg) {
  try {
    return deg * kPi180;
  } catch (...) {
    return 0.0;
  }
}

/**
 * @brief      ラジアン => 度
 *
 * @param[in]  rad: ラジアン(double)
 * @return     deg: 度      (double)
 */
double Vincenty::rad2deg(double rad) {
  try {
    return rad * kPi180Inv;
  } catch (...) {
    return 0.0;
  }
}

/**
 * @brief  A 計算
 *
 * @param[in] u2: u^2 の値
 * @return     a: A の値(double)
 */
double Vincenty::calc_a(double u2) {
  try {
    return 1.0 + u2 / 16384.0 * (4096.0 + u2 * (-768.0 + u2 * (320.0
           - 175.0 * u2)));
  } catch (...) {
    return 0.0;
  }
}

/**
 * @brief  B 計算
 *
 * @param[in] u2: u^2 の値
 * @return     b: B の値(double)
 */
double Vincenty::calc_b(double u2) {
  try {
    return u2 / 1024.0 * (256.0 + u2 * (-128.0 + u2 * (74.0 - 47.0 * u2)));
  } catch (...) {
    return 0.0;
  }
}

/**
 * @brief  C 計算
 *
 * @param[in] cos2_alp: cos^2(α) の値
 * @return           c: C の値(double)
 */
double Vincenty::calc_c(double cos2_alp) {
  try {
    return kF * cos2_alp * (4.0 + kF * (4.0 - 3.0 * cos2_alp)) / 16.0;
  } catch (...) {
    return 0.0;
  }
}

/**
 * Δσ 計算
 *
 * @param[in]          bb: B の値
 * @param[in]     cos_sgm: cos(σ) の値
 * @param[in]     sin_sgm: sin(σ) の値
 * @param[in] cos_2_sgm_m: cos(2σ_m) の値
 * @return        dlt_sgm: Δσ の値
 */
double Vincenty::calc_dlt_sgm(
    double bb, double cos_sgm, double sin_sgm, double cos_2_sgm_m) {
  try {
    return bb * sin_sgm * (cos_2_sgm_m
           + bb / 4.0 * (cos_sgm * (-1.0 + 2.0 * cos_2_sgm_m * cos_2_sgm_m)
           - bb / 6.0 * cos_2_sgm_m * (-3.0 + 4.0 * sin_sgm * sin_sgm)
           * (-3.0 + 4.0 * cos_2_sgm_m * cos_2_sgm_m)));
  } catch (...) {
    return 0.0;
  }
}

/**
 * @brief      経度正規化
 *
 * @param[in]  lng: 正規化前の経度(rad)(double)
 * @return     lng: 正規化後の経度(rad)(double)
 */
double Vincenty::norm_lng(double lng) {
  try {
    while (lng < -kPi) lng += kPi2;
    while (lng >  kPi) lng -= kPi2;
  } catch (...) {
    return 0.0;
  }

  return lng;
}

/*
 * 方位角正規化
 *
 *  @param[in]  alp: 正規化前の方位角(rad)
 *  @return     alp: 正規化後の方位角(rad)
 */
double Vincenty::norm_az(double alp) {
  try {
    if (alp <  0.0) alp += kPi2;
    if (alp > kPi2) alp -= kPi2;
  } catch (...) {
    return 0.0;
  }

  return alp;
}

}  // namespace my_lib

File: vincenty_distance.cpp

/***********************************************************
  Vincenty 法で、楕円体上の2点間の距離、地点 1, 2 における
  方位角を計算

    DATE          AUTHOR          VERSION
    2020.09.13    mk-mode.com     1.00 新規作成

  Copyright(C) 2020 mk-mode.com All Rights Reserved.
***********************************************************/
#include "vincenty.hpp"

#include <cstdlib>   // for EXIT_XXXX
#include <iomanip>   // for setprecision
#include <iostream>
#include <string>
#include <tuple>

int main(int argc, char* argv[]) {
  double lat_1;  // 地点 1 緯度
  double lng_1;  // 地点 1 経度
  double lat_2;  // 地点 2 緯度
  double lng_2;  // 地点 2 経度
  std::tuple<double, double, double> t;  // 1-2 の距離, 1, 2 における方位角

  try {
    // コマンドライン引数のチェック
    if (argc < 5) {
      std::cerr << "[ERROR] Number of arguments is wrong!\n"
                << "[USAGE] ./vincenty_distance lat_1 long_1 lat_2 long_2"
                << std::endl;
      return EXIT_FAILURE;
    }

    // 地点 1, 2 の緯度・経度取得
    lat_1 = std::stod(argv[1]);
    lng_1 = std::stod(argv[2]);
    lat_2 = std::stod(argv[3]);
    lng_2 = std::stod(argv[4]);
    std::cout << std::fixed << std::setprecision(8);
    std::cout << "  POINT-1: "
              << std::setw(13) << std::right << lat_1 << "°, "
              << std::setw(13) << std::right << lng_1 << "°"
              << std::endl;
    std::cout << "  POINT-2: "
              << std::setw(13) << std::right << lat_2 << "°, "
              << std::setw(13) << std::right << lng_2 << "°"
              << std::endl;

    // 計算
    my_lib::Vincenty v(lat_1, lng_1);
    t = v.calc_distance(lat_2, lng_2);

    // 結果出力
    std::cout << std::fixed << std::setprecision(8);
    std::cout << "-->" << std::endl;
    std::cout << "   LENGTH: "
              << std::setw(17) << std::right << std::get<0>(t)
              << std::endl;
    std::cout << "AZIMUTH-1: "
              << std::setw(17) << std::right << std::get<1>(t)
              << std::endl;
    std::cout << "AZIMUTH-2: "
              << std::setw(17) << std::right << std::get<2>(t)
              << std::endl;
  } catch (...) {
      std::cerr << "EXCEPTION!" << std::endl;
      return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

• Gist - C++ source code to calculate geodesical values by Vincenty’s formulae.(distance)

3. ソースコードのコンパイル

まず、以下のように Makefile を作成する。（行頭のインデントはタブ文字）

File: Makefile

gcc_options = -std=c++17 -Wall -O2 --pedantic-errors

vincenty_distance: vincenty_distance.o vincenty.o
  g++ $(gcc_options) -o $@ $^

vincenty_distance.o : vincenty_distance.cpp
  g++ $(gcc_options) -c $<

calc.o : vincenty.cpp
  g++ $(gcc_options) -c $<

run : vincenty_distance
  ./vincenty_distance

clean :
  rm -f ./vincenty_distance
  rm -f ./*.o

.PHONY : run clean

そして、ビルド（コンパイル＆リンク）。

$ make

4. 動作確認

コマンドライン引数に地点 1 の緯度(°)・経度(°)、地点 2 の緯度(°)・経度(°)を指定して実行する。

$ ./vincenty_distance 35.4681 133.0486 35.472222 133.050556
  POINT-1:   35.46810000°,  133.04860000°
  POINT-2:   35.47222200°,  133.05055600°
-->
   LENGTH:      490.58216516
AZIMUTH-1:       21.21518366
AZIMUTH-2:      201.21631869

• LENGTH は地点 1 と地点 2 の距離
• AZIMUTH-1 は地点 1 から見た地点 2 の方位角
• AZIMUTH-2 は地点 2 から見た地点 1 の方位角

5. 参考

• Direct and Inverse solutions of geodesics on the ellipsoid with application of nested equations - inverse.pdf

---

以上。