/// <summary>
/// xyz坐标转化为ENU坐标
/// </summary>
/// <param name="xyz">xyz坐标</param>
/// <param name="lla">经纬高</param>
/// <param name="enu">enu坐标</param>
void ecefToEnu(struct_ecef_coord xyz, struct_lla_coord lla, out struct_enu_coord enu)
{
double sinLat = 0.0, cosLat = 0.0, sinLon = 0.0, cosLon = 0.0;
/*计算ECEF直角坐标与ENU坐标系之间的转换矩阵*/
sinLat = Math.Sin(lla.lat);
cosLat = Math.Cos(lla.lat);
sinLon = Math.Sin(lla.lon);
cosLon = Math.Cos(lla.lon);
/*enu坐标等于转化矩阵乘以xyz*/
enu.e = (-sinLon) * xyz.x + cosLon * xyz.y;
enu.n = (-sinLat) * cosLon * xyz.x - sinLat * sinLon * xyz.y + cosLat * xyz.z;
enu.u = cosLat * cosLon * xyz.x + sinLon * cosLat * xyz.y + sinLat * xyz.z;
}
/// <summary>
/// ENU坐标转化为XYZ坐标
/// </summary>
/// <param name="enu">ENU坐标</param>
/// <param name="lla">经纬高</param>
/// <param name="xyz">XYZ坐标</param>
void enuToEcef(struct_enu_coord enu, struct_lla_coord lla, out struct_ecef_coord xyz)
{
double sinLat = 0.0, cosLat = 0.0, sinLon = 0.0, cosLon = 0.0;
/* ENU坐标系转化为XYZ坐标系的转化矩阵 */
sinLat = Math.Sin(lla.lat); //fai lat
cosLat = Math.Cos(lla.lat);
sinLon = Math.Sin(lla.lon); //lamda lon
cosLon = Math.Cos(lla.lon);
/* xyz坐标等于转化矩阵乘以enu */
xyz.x = -sinLon * enu.e - sinLat * cosLon * enu.n + cosLat * cosLon * enu.u;
xyz.y = cosLon * enu.e - sinLat * sinLon * enu.n + sinLon * cosLat * enu.u;
xyz.z = cosLat * enu.n + sinLat * enu.u;
}