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

更新日時:


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

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

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

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(表記法)

以下のように定義する。

1-3. Direct formula (順解法)

地点1 と地点1における方位角 , 距離 が与えられたとき、地点2 と方位角 を求める。

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

収束後、以下の処理を行なう。

1-4. Inverse formula (逆解法)

楕円体上の2地点、地点1 , 地点2 が与えられたとき、地点1, 2における方位角 と距離 を求める。

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

収束後、以下の処理を行なう。

2. ソースコードの作成

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

File: vincenty.hpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
#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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
#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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
/***********************************************************
  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;
}

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. 参考


以上。

 

Sponsored Link

 

コメントする