经纬高坐标系-ECEF坐标系-ENU坐标系 相互转换 C/C++

以下是经纬高坐标系（LLH）到地心地固坐标系（ECEF）的转换代码示例（C++）：

#include <cmath>

// WGS84椭球参数
const double a = 6378137.0; // 长半轴
const double b = 6356752.3142; // 短半轴
const double f = (a - b) / a; // 扁率

// LLH转ECEF
void llh2ecef(const double& lat, const double& lon, const double& alt, double& x, double& y, double& z)
{
    double N = a / std::sqrt(1 - f * f * std::sin(lat) * std::sin(lat));
    x = (N + alt) * std::cos(lat) * std::cos(lon);
    y = (N + alt) * std::cos(lat) * std::sin(lon);
    z = (N * (1 - f * f) + alt) * std::sin(lat);
}

以下是地心地固坐标系（ECEF）到局部东北天坐标系（ENU）的转换代码示例（C++）：

// ECEF转ENU
void ecef2enu(const double& x, const double& y, const double& z, const double& lat0, const double& lon0, const double& alt0, double& e, double& n, double& u)
{
    double dx = x - (std::sin(lat0) * std::cos(lon0) * y - std::sin(lat0) * std::sin(lon0) * x + std::cos(lat0) * z);
    double dy = y - (-std::sin(lon0) * dx + std::cos(lon0) * y);
    double dz = z - (std::cos(lat0) * std::cos(lon0) * x + std::cos(lat0) * std::sin(lon0) * y + std::sin(lat0) * z);

    e = -std::sin(lon0) * dx + std::cos(lon0) * dy;
    n = -std::sin(lat0) * std::cos(lon0) * dx - std::sin(lat0) * std::sin(lon0) * dy + std::cos(lat0) * dz;
    u = std::cos(lat0) * std::cos(lon0) * dx + std::cos(lat0) * std::sin(lon0) * dy + std::sin(lat0) * dz;
}

需要注意的是，以上代码示例中输入参数的单位应该是弧度。