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

转换 ENU 坐标系到 ECEF 坐标系：

#include <cmath>

// 定义常量
const double pi = 3.14159265358979323846;
const double a = 6378137; // WGS84 椭球体长半轴
const double f = 1.0 / 298.257223563; // WGS84 椭球体扁率
const double b = a * (1 - f); // WGS84 椭球体短半轴
const double e2 = 1 - pow((b / a), 2); // WGS84 椭球体第一偏心率的平方

// 转换函数
void enu2ecef(double& x, double& y, double& z, const double& lat0, const double& lon0, const double& h0, const double& east, const double& north, const double& up)
{
    double sinp = sin(lat0 * pi / 180);
    double cosp = cos(lat0 * pi / 180);
    double sinl = sin(lon0 * pi / 180);
    double cosl = cos(lon0 * pi / 180);

    double dx = -sinl * east - cosl * north;
    double dy = cosl * east - sinl * north;

    double dz = up;

    x = -sinl * dx + cosl * dy + cosp * dz + a * cosp * e2 * sinp;
    y = -cosl * dx - sinl * dy + cosp * dz + a * cosp * e2 * sinp;
    z =           +           sinp * dz + b * sinp * sinp * sinp + h0;
}

转换 ECEF 坐标系到经纬高坐标系：

#include <cmath>

// 定义常量
const double pi = 3.14159265358979323846;
const double a = 6378137; // WGS84 椭球体长半轴
const double f = 1.0 / 298.257223563; // WGS84 椭球体扁率
const double b = a * (1 - f); // WGS84 椭球体短半轴
const double e2 = 1 - pow((b / a), 2); // WGS84 椭球体第一偏心率的平方

// 转换函数
void ecef2lla(const double& x, const double& y, const double& z, double& lat, double& lon, double& h)
{
    double r = sqrt(x * x + y * y);
    double E = sqrt(a * a - b * b);
    double F = 54 * b * b * z * z;
    double G = r * r + (1 - e2) * z * z - e2 * E * E;
    double c = (e2 * e2 * F * r * r) / (G * G * G);
    double s = pow(1 + c + sqrt(c * c + 2 * c), 1.0 / 3.0);
    double P = F / (3 * pow((s + 1 / s + 1), 2) * G * G);
    double Q = sqrt(1 + 2 * e2 * e2 * P);
    double r0 = -(P * e2 * r) / (1 + Q) + sqrt(0.5 * a * a * (1 + 1.0 / Q) - P * (1 - e2) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
    double U = sqrt(pow((r - e2 * r0), 2) + z * z);
    double V = sqrt(pow((r - e2 * r0), 2) + (1 - e2) * z * z);
    double zo = (b * b * z) / (a * V);

    h = U * (1 - b * b / (a * V));
    lat = atan((z + e2 * zo) / r);
    lon = atan2(y, x);
}

转换经纬高坐标系到 ECEF 坐标系：

#include <cmath>

// 定义常量
const double pi = 3.14159265358979323846;
const double a = 6378137; // WGS84 椭球体长半轴
const double f = 1.0 / 298.257223563; // WGS84 椭球体扁率
const double b = a * (1 - f); // WGS84 椭球体短半轴
const double e2 = 1 - pow((b / a), 2); // WGS84 椭球体第一偏心率的平方

// 转换函数
void lla2ecef(const double& lat, const double& lon, const double& h, double& x, double& y, double& z)
{
    double sinp = sin(lat);
    double cosp = cos(lat);
    double sinl = sin(lon);
    double cosl = cos(lon);
    double N = a / sqrt(1 - e2 * sinp * sinp);

    x = (N + h) * cosp * cosl;
    y = (N + h) * cosp * sinl;
    z = (N * (1 - e2) + h) * sinp;
}

转换 ENU 坐标系到经纬高坐标系：

#include <cmath>

// 定义常量
const double pi = 3.14159265358979323846;
const double a = 6378137; // WGS84 椭球体长半轴
const double f = 1.0 / 298.257223563; // WGS84 椭球体扁率
const double b = a * (1 - f); // WGS84 椭球体短半轴
const double e2 = 1 - pow((b / a), 2); // WGS84 椭球体第一偏心率的平方

// 转换函数
void enu2lla(const double& lat0, const double& lon0, const double& h0, const double& east, const double& north, const double& up, double& lat, double& lon, double& h)
{
    double x, y, z;
    enu2ecef(x, y, z, lat0, lon0, h0, east, north, up);
    ecef2lla(x, y, z, lat, lon, h);
}

转换经纬高坐标系到 ENU 坐标系：

#include <cmath>

// 定义常量
const double pi = 3.14159265358979323846;
const double a = 6378137; // WGS84 椭球体长半轴
const double f = 1.0 / 298.257223563; // WGS84 椭球体扁率
const double b = a * (1 - f); // WGS84 椭球体短半轴
const double e2 = 1 - pow((b / a), 2); // WGS84 椭球体第一偏心率的平方

// 转换函数
void lla2enu(const double& lat0, const double& lon0, const double& h0, const double& lat, const double& lon, const double& h, double& east, double& north, double& up)
{
    double x0, y0, z0;
    double x, y, z;
    lla2ecef(lat0, lon0, h0, x0, y0, z0);
    lla2ecef(lat, lon, h, x, y, z);

    double dx = x - x0;
    double dy = y - y0;
    double dz = z - z0;

    double sinp = sin(lat0 * pi / 180);
    double cosp = cos(lat0 * pi / 180);
    double sinl = sin(lon0 * pi / 180);
    double cosl = cos(lon0 * pi / 180);

    east = -sinl * dx + cosl * dy;
    north = -sinp * cosl * dx - sinp * sinl * dy + cosp * dz;
    up = cosp * cosl * dx + cosp * sinl * dy + sinp * dz;
}