C语言写出站心坐标系转换的代码

以下是C语言写出站心坐标系转换的代码：

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

void xyz_to_enu(double x, double y, double z, double lat0, double lon0, double h0,
				double* E, double* N, double* U)
{
	// WGS84椭球参数
	double a = 6378137.0;
	double b = 6356752.3142;
	double e = sqrt(1 - (b / a) * (b / a));
	// 弧度转换
	double lat0_rad = lat0 / 180.0 * M_PI;
	double lon0_rad = lon0 / 180.0 * M_PI;
	// ECEF -> ENU 转换矩阵
	double R[3][3];
	R[0][0] = -sin(lon0_rad);
	R[0][1] = cos(lon0_rad);
	R[0][2] = 0;
	R[1][0] = -sin(lat0_rad) * cos(lon0_rad);
	R[1][1] = -sin(lat0_rad) * sin(lon0_rad);
	R[1][2] = cos(lat0_rad);
	R[2][0] = cos(lat0_rad) * cos(lon0_rad);
	R[2][1] = cos(lat0_rad) * sin(lon0_rad);
	R[2][2] = sin(lat0_rad);
	// XYZ相对于基准点的差值
	double dx = x - (a * cos(lat0_rad) * cos(lon0_rad));
	double dy = y - (a * cos(lat0_rad) * sin(lon0_rad));
	double dz = z - (a * sin(lat0_rad));
	// 因旋转而产生的位移
	*E = R[0][0] * dx + R[0][1] * dy + R[0][2] * dz;
	*N = R[1][0] * dx + R[1][1] * dy + R[1][2] * dz;
	*U = R[2][0] * dx + R[2][1] * dy + R[2][2] * dz + h0;
}

int main()
{
	double x = 3590454.931, y = 2721456.484, z = 4365849.481;
	double lat0 = 31.238514, lon0 = 121.492845, h0 = 4.36;
	double E, N, U;
	xyz_to_enu(x, y, z, lat0, lon0, h0, &E, &N, &U);
	printf("E = %f m, N = %f m, U = %f m\n", E, N, U);
	return 0;
}

此代码实现了从地心空间直角坐标系(XYZ)转换到局部东北天坐标系(ENU)的坐标转换，其中的站心点使用的是经纬高坐标(lat0, lon0, h0)。