C++ source code to calculate geodesical values by Vincenty's formulae.(destination)

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      地点 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;
  }
}

vincenty.hpp

#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_destination.cpp

/***********************************************************
  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;
}