Skip to content

Instantly share code, notes, and snippets.

@komasaru

komasaru/ecef2blh.cpp

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