Skip to content

Instantly share code, notes, and snippets.

@komasaru

komasaru/blh2ecef.cpp

Last active May 6, 2021
Embed
What would you like to do?
C++ source code to convert a coordinate from WGS84(BLH) to ECEF.
/***********************************************************
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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment