ENU坐标系-ECEF坐标系-经纬高坐标系转换 C

以下是ENU坐标系到ECEF坐标系的转换代码：

#include <math.h>

// WGS84椭球体参数
const double a = 6378137.0; // 地球长半轴
const double b = 6356752.314245; // 地球短半轴
const double f = 1 / 298.257223563; // 地球扁率
const double e = sqrt(2 * f - f * f); // 地球第一偏心率
const double e2 = e * e; // 地球第二偏心率

// ENU坐标系到ECEF坐标系的转换函数
void enu2ecef(double enu[3], double lat, double lon, double alt, double ecef[3])
{
    // 计算ENU坐标系到NED坐标系的转换矩阵
    double cosLat = cos(lat);
    double sinLat = sin(lat);
    double cosLon = cos(lon);
    double sinLon = sin(lon);
    double R_ENU2NED[3][3] = {
        {-sinLon, cosLon, 0},
        {-sinLat * cosLon, -sinLat * sinLon, cosLat},
        {cosLat * cosLon, cosLat * sinLon, sinLat}
    };
    
    // 计算ENU坐标系到ECEF坐标系的转换矩阵
    double R_NED2ECEF[3][3] = {
        {-sinLon, -sinLat * cosLon, cosLat * cosLon},
        {cosLon, -sinLat * sinLon, cosLat * sinLon},
        {0, cosLat, sinLat}
    };
    
    // 计算ENU坐标系到ECEF坐标系的转换矩阵
    double R_ENU2ECEF[3][3];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            R_ENU2ECEF[i][j] = 0;
            for (int k = 0; k < 3; k++) {
                R_ENU2ECEF[i][j] += R_NED2ECEF[i][k] * R_ENU2NED[k][j];
            }
        }
    }
    
    // 计算ENU坐标系到ECEF坐标系的转换向量
    double ECEF_Origin[3], NED_Origin[3];
    llh2ecef(lat, lon, alt, ECEF_Origin);
    llh2ned(lat, lon, alt, enu[0], enu[1], enu[2], NED_Origin);
    double ENU_Origin[3] = {NED_Origin[1], NED_Origin[0], -NED_Origin[2]};
    double ECEF_enu[3];
    for (int i = 0; i < 3; i++) {
        ECEF_enu[i] = 0;
        for (int j = 0; j < 3; j++) {
            ECEF_enu[i] += R_ENU2ECEF[i][j] * ENU_Origin[j];
        }
    }
    
    // 计算ENU坐标点在ECEF坐标系中的位置
    for (int i = 0; i < 3; i++) {
        ecef[i] = ECEF_Origin[i] + ECEF_enu[i];
    }
}

// 经纬高坐标系到ECEF坐标系的转换函数
void llh2ecef(double lat, double lon, double alt, double ecef[3])
{
    double N = a / sqrt(1 - e2 * sin(lat) * sin(lat));
    ecef[0] = (N + alt) * cos(lat) * cos(lon);
    ecef[1] = (N + alt) * cos(lat) * sin(lon);
    ecef[2] = (N * (1 - e2) + alt) * sin(lat);
}

// 经纬高坐标系到NED坐标系的转换函数
void llh2ned(double lat, double lon, double alt, double lat0, double lon0, double alt0, double ned[3])
{
    double cosLat = cos(lat);
    double sinLat = sin(lat);
    double cosLon = cos(lon);
    double sinLon = sin(lon);
    double cosLat0 = cos(lat0);
    double sinLat0 = sin(lat0);
    double cosLon0 = cos(lon0);
    double sinLon0 = sin(lon0);
    double R_ENU2NED[3][3] = {
        {-sinLon0 * cosLat0, -sinLat0 * cosLon0, cosLat0},
        {-sinLon0 * sinLat0, cosLat0 * cosLon0, sinLat0},
        {cosLon0, sinLon0, 0}
    };
    
    double ecef[3], ecef0[3];
    llh2ecef(lat, lon, alt, ecef);
    llh2ecef(lat0, lon0, alt0, ecef0);
    
    for (int i = 0; i < 3; i++) {
        ned[i] = 0;
        for (int j = 0; j < 3; j++) {
            ned[i] += R_ENU2NED[i][j] * (ecef[i] - ecef0[j]);
        }
    }
}

以上是ENU坐标系到ECEF坐标系的转换代码，其中 `enu` 表示ENU坐标系下的坐标，`lat`、`lon`、`alt` 分别表示参考点的纬度、经度和高度，`ecef` 表示转换后的ECEF坐标系下的坐标。

如果需要进行其他坐标系之间的转换，可以根据上面的代码进行修改。