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

blh2ecef.cpp

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

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

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

  ----------------------------------------------------------
  引数 : B L H
         (B, L, H: 元の BLH(WGS84) 座標)
  ----------------------------------------------------------
  $ g++102 -std=c++17 -Wall -O2 --pedantic-errors -o blh2ecef blh2ecef.cpp
***********************************************************/
#include <cmath>
#include <cstdlib>   // for EXIT_XXXX
#include <iomanip>
#include <iostream>

namespace blh2ecef {

// 定数
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      BLH -> ECEF
 *
 * @param[in]  BLH  座標 (CoordB)
 * @return     ECEF 座標 (CoordX)
 */
CoordX blh2ecef(CoordB c_src) {
  CoordX c_res;

  try {
    c_res.x = (n(c_src.b) + c_src.h)
            * cos(c_src.b * kPi180)
            * cos(c_src.l * kPi180);
    c_res.y = (n(c_src.b) + c_src.h)
            * cos(c_src.b * kPi180)
            * sin(c_src.l * kPi180);
    c_res.z = (n(c_src.b) * (1.0 - kE2) + c_src.h)
            * sin(c_src.b * kPi180);
  } catch (...) {
    throw;
  }

  return c_res;
}

}  // namespace blh2ecef

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

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

    // 元の BLH(WGS84) 座標取得
    c_src.b = std::stod(argv[1]);
    c_src.l = std::stod(argv[2]);
    c_src.h = std::stod(argv[3]);

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

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

  return EXIT_SUCCESS;
}