Skip to content

Instantly share code, notes, and snippets.

@komasaru

komasaru/vincenty.cpp

Last active Sep 16, 2020
Embed
What would you like to do?
C++ source code to calculate geodesical values by Vincenty's formulae.(destination)
#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 地点 2 の緯度・経度、地点 2 における方位を計算
* (Vincenty 順解法)
*
* @param[in] az_1: 地点 1 における方位角(double)
* @param[in] s: 地点 1〜2 の距離(double)
* @return {
* lat_2: 地点 2 の緯度(double),
* lng_2: 地点 2 の経度(double),
* az_2: 地点 2 における方位角(double)
* }(tuple)
*/
std::tuple<double, double, double> Vincenty::calc_destination(
double az_1, double s) {
double lat_2 = 0.0; // 地点 2 の緯度
double lng_2 = 0.0; // 地点 2 の経度
double az_2 = 0.0; // 地点 2 における方位角
double alp_1;
double cos_alp_1; // cos(α_1)
double sin_alp_1; // sin(α_1)
double sgm_1;
double sin_alp; // sin(α)
double sin2_alp; // sin(α)^2
double cos2_alp; // cos(α)^2 = 1 - sin(α)^2
double u2;
double aa; // A
double bb; // B
double cc; // C
double sgm;
double sgm_prev;
double cos_sgm; // cos(σ)
double sin_sgm; // sin(σ)
double cos_2_sgm_m;
double d_sgm; // Δσ
double cos_u_1; // cos(u_1)
double sin_u_1; // sin(u_1)
double cu1_cs; // cos(u_1) * cos(σ)
double cu1_ss; // cos(u_1) * sin(σ)
double su1_cs; // sin(u_1) * cos(σ)
double su1_ss; // sin(u_1) * sin(σ)
double tmp;
double phi_2;
double lmd;
double l;
double l_2;
double alp_2;
try {
alp_1 = deg2rad(az_1);
cos_alp_1 = std::cos(alp_1);
sin_alp_1 = std::sin(alp_1);
sgm_1 = std::atan2(tan_u_1, cos_alp_1);
sin_alp = std::cos(u_1) * std::sin(alp_1);
sin2_alp = sin_alp * sin_alp;
cos2_alp = 1.0 - sin2_alp;
u2 = cos2_alp * (kA * kA - b * b) / (b * b);
aa = calc_a(u2);
bb = calc_b(u2);
sgm = s / (b * aa);
sgm_prev = kPi2;
while (std::abs(sgm - sgm_prev) > kEps) {
cos_sgm = std::cos(sgm);
sin_sgm = std::sin(sgm);
cos_2_sgm_m = std::cos(2.0 * sgm_1 + sgm);
d_sgm = calc_dlt_sgm(bb, cos_sgm, sin_sgm, cos_2_sgm_m);
sgm_prev = sgm;
sgm = s / (b * aa) + d_sgm;
}
cos_u_1 = std::cos(u_1);
sin_u_1 = std::sin(u_1);
cu1_cs = cos_u_1 * cos_sgm;
cu1_ss = cos_u_1 * sin_sgm;
su1_cs = sin_u_1 * cos_sgm;
su1_ss = sin_u_1 * sin_sgm;
tmp = su1_ss - cu1_cs * cos_alp_1;
phi_2 = std::atan2(
su1_cs + cu1_ss * cos_alp_1,
(1.0 - kF) * std::sqrt(sin_alp * sin_alp + tmp * tmp)
);
lmd = std::atan2(sin_sgm * sin_alp_1, cu1_cs - su1_ss * cos_alp_1);
cc = calc_c(cos2_alp);
l = lmd - (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)));
l_2 = l + l_1;
alp_2 = std::atan2(sin_alp, -su1_ss + cu1_cs * cos_alp_1) + kPi;
lat_2 = rad2deg(phi_2);
lng_2 = rad2deg(norm_lng(l_2));
az_2 = rad2deg(norm_az(alp_2));
return {lat_2, lng_2, 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;
}
}
#ifndef VINCENTY_CALC_DESTINATION_VINCENTY_HPP_
#define VINCENTY_CALC_DESTINATION_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_destination(double, double);
// 地点 2 の緯度・経度、地点 2 における方位を計算
// (Vincenty 順解法)
};
}
#endif
/***********************************************************
Vincenty 法で、1点から指定の方位角・距離にある点を求める
DATE AUTHOR VERSION
2020.09.15 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 az_1; // 地点 1 における方位角
double s; // 地点 1〜2 の距離
std::tuple<double, double, double> t;
// 地点 2 の緯度・経度、地点 2 における方位角
try {
// コマンドライン引数のチェック
if (argc < 5) {
std::cerr << "[ERROR] Number of arguments is wrong!\n"
<< "[USAGE] ./vincenty_destination lat_1 lng_1 az_1 s"
<< std::endl;
return EXIT_FAILURE;
}
// 地点 1, 2 の緯度・経度、地点 1 における方位角、地点 1〜2 の距離取得
lat_1 = std::stod(argv[1]);
lng_1 = std::stod(argv[2]);
az_1 = std::stod(argv[3]);
s = 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 << "AZIMUTH-1: "
<< std::setw(17) << std::right << az_1 << "°"
<< std::endl;
std::cout << " LENGTH: "
<< std::setw(17) << std::right << s << "m"
<< std::endl;
// 計算
my_lib::Vincenty v(lat_1, lng_1);
t = v.calc_destination(az_1, s);
// 結果出力
std::cout << std::fixed << std::setprecision(8);
std::cout << "-->" << std::endl;
std::cout << " POINT-2: "
<< std::setw(13) << std::right << std::get<0>(t) << "°, "
<< std::setw(13) << 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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.