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

以下是经纬高（WGS84）坐标系到ECEF坐标系的转换代码：

#include <stdio.h>
#include <math.h>

#define PI 3.14159265358979323846
#define a 6378137.0        // 地球长半轴
#define b 6356752.314245   // 地球短半轴
#define e ((a*a-b*b)/(a*a)) // 地球椭球体第一偏心率

void WGS84_to_ECEF(double lat, double lon, double alt, double *x, double *y, double *z)
{
    double N = a / sqrt(1 - e*sin(lat)*sin(lat));
    *x = (N + alt) * cos(lat) * cos(lon);
    *y = (N + alt) * cos(lat) * sin(lon);
    *z = (N*(1-e) + alt) * sin(lat);
}

int main()
{
    double lat = 39.909187;
    double lon = 116.397451;
    double alt = 50.0;
    double x, y, z;
    WGS84_to_ECEF(lat*PI/180.0, lon*PI/180.0, alt, &x, &y, &z);
    printf("ECEF坐标系下的坐标为：(%lf, %lf, %lf)\n", x, y, z);
    return 0;
}

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

#include <stdio.h>
#include <math.h>

#define PI 3.14159265358979323846

void ECEF_to_ENU(double x, double y, double z, double lat0, double lon0, double alt0, double *e, double *n, double *u)
{
    double slat0 = sin(lat0);
    double clat0 = cos(lat0);
    double slon0 = sin(lon0);
    double clon0 = cos(lon0);

    double dx = x - (clon0*x + slon0*y);
    double dy = y - (-slon0*x + clon0*y);
    double dz = z - alt0;

    *e = -slon0*dx + clon0*dy;
    *n = -slat0*clon0*dx - slat0*slon0*dy + clat0*dz;
    *u = clat0*clon0*dx + clat0*slon0*dy + slat0*dz;
}

int main()
{
    double x = -2339108.3;
    double y = 5004615.5;
    double z = 3224800.9;
    double lat0 = 39.909187*PI/180.0;
    double lon0 = 116.397451*PI/180.0;
    double alt0 = 50.0;
    double e, n, u;
    ECEF_to_ENU(x, y, z, lat0, lon0, alt0, &e, &n, &u);
    printf("ENU坐标系下的坐标为：(%lf, %lf, %lf)\n", e, n, u);
    return 0;
}