C++ source code to convert a coordinate from ECEF to WGS84(BLH).

ecef2blh.cpp

/***********************************************************
  ECEF -> BLH 変換
  : ECEF（Earth Centered Earth Fixed; 地球中心・地球固定直交座標系）座標を
    WGS84 の緯度(Latitude)／経度(Longitude)／楕円体高(Height)に変換する。

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

  Copyright(C) 2021 mk-mode.com All Rights Reserved.

  ----------------------------------------------------------
  引数 : X Y Z
         (X, Y, Z: 元の ECEF 座標)
  ----------------------------------------------------------
  $ g++102 -std=c++17 -Wall -O2 --pedantic-errors -o ecef2blh ecef2blh.cpp
***********************************************************/
#include <cmath>
#include <cstdlib>   // for EXIT_XXXX
#include <iomanip>
#include <iostream>

namespace ecef2blh {

// 定数
static constexpr double kPi    = atan(1.0) * 4.0;
static constexpr double kPi180 = kPi / 180;
// [ WGS84 座標パラメータ ]
// a(地球楕円体長半径(赤道面平均半径))
static constexpr double kA     = 6378137.0;
// 1 / f(地球楕円体扁平率=(a - b) / a)
static constexpr double k1F    = 298.257223563;
// b(地球楕円体短半径)
static constexpr double kB     = kA * (1.0 - 1.0 / k1F);
// e^2 = 2 * f - f * f
//     = (a^2 - b^2) / a^2
static constexpr double kE2    = (1.0 / k1F) * (2.0 - (1.0 / k1F));
// e'^2= (a^2 - b^2) / b^2
static constexpr double kEd2   = kE2 * kA * kA / (kB * kB);

// 座標
struct CoordB {
  double b;  // B(Beta)
  double l;  // L(Lambda)
  double h;  // H(Height)
};
struct CoordX {
  double x;  // X
  double y;  // Y
  double z;  // Z
};

/*
 * @brief      関数 N
 *
 * @param[in]  X (double)
 * @return     計算結果 (double)
 */
double n(double x) {
  double res;

  try {
    res = kA / sqrt(1.0 - kE2 * pow(sin(x * kPi180), 2));
  } catch (...) {
    throw;
  }

  return res;
}

/*
 * @brief      ECEF -> BLH
 *
 * @param[in]  ECEF 座標 (CoordX)
 * @return     BLH  座標 (CoordB)
 */
CoordB ecef2blh(CoordX c_src) {
  double p;
  double theta;
  CoordB c_res;

  try {
    p = sqrt(c_src.x * c_src.x + c_src.y * c_src.y);
    theta = atan2(c_src.z * kA, p * kB) / kPi180;
    c_res.b = atan2(
      c_src.z + kEd2 * kB * pow(sin(theta * kPi180), 3),
      p       - kE2  * kA * pow(cos(theta * kPi180), 3)
    ) / kPi180;                                          // Beta(Latitude)
    c_res.l = atan2(c_src.y, c_src.x) / kPi180;          // Lambda(Longitude)
    c_res.h = (p / cos(c_res.b * kPi180)) - n(c_res.b);  // Height
  } catch (...) {
    throw;
  }

  return c_res;
}

}  // namespace ecef2blh

int main(int argc, char* argv[]) {
  namespace ns = ecef2blh;
  ns::CoordX c_src;  // 元の座標
  ns::CoordB c_res;  // 変換後の座標

  try {
    // コマンドライン引数の個数チェック
    if (argc < 4) {
      std::cout << "Usage: ./ecef2blh X Y Z" << std::endl;
      return EXIT_SUCCESS;
    }

    // 元の ECEF 座標取得
    c_src.x = std::stod(argv[1]);
    c_src.y = std::stod(argv[2]);
    c_src.z = std::stod(argv[3]);

    // ECEF -> BLH
    c_res = ns::ecef2blh(c_src);

    // 結果出力
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "ECEF: X = "
              << std::setw(12) << c_src.x << " m" << std::endl;
    std::cout << "      Y = "
              << std::setw(12) << c_src.y << " m" << std::endl;
    std::cout << "      Z = "
              << std::setw(12) << c_src.z << " m" << std::endl;
    std::cout << "-->" << std::endl;
    std::cout << std::fixed << std::setprecision(8);
    std::cout << "BLH: LATITUDE(BETA) = "
              << std::setw(12) << c_res.b << " °" << std::endl;
    std::cout << "  LONGITUDE(LAMBDA) = "
              << std::setw(12) << c_res.l << " °" << std::endl;
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "             HEIGHT = "
              << std::setw(7)  << c_res.h << " m" << std::endl;
  } catch (...) {
      std::cerr << "EXCEPTION!" << std::endl;
      return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}