/// <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;
}
/// <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;
}
const double WGS_E2_SQRT = 0.0067394967422751; // second numerical eccentricity
/// <summary>
/// 经纬高转换为XYZ
/// </summary>
/// <param name="lla">经纬高</param>
/// <param name="ecef">XYZ坐标</param>
void llaToEcef(struct_lla_coord lla, out struct_ecef_coord ecef)
{
double N = 0.0;
double sinLat, cosLat;
double sinLon, cosLon;
sinLat = Math.Sin(lla.lat);
cosLat = Math.Cos(lla.lat);
sinLon = Math.Sin(lla.lon);
cosLon = Math.Cos(lla.lon);
N = WGS_MAJOR / Math.Sqrt(1.0 - WGS_E1_SQRT * sinLat * sinLat);
ecef.x = (N + lla.alt) * cosLat * cosLon;
ecef.y = (N + lla.alt) * cosLat * sinLon;
ecef.z = (N * (1.0 - WGS_E1_SQRT) + lla.alt) * sinLat;
}
/// <summary>
/// XYZ坐标转换为经纬高
/// </summary>
/// <param name="ecef">ecef坐标系的xyz </param>
/// <param name="lla">经纬高</param>
void ecefToLla(struct_ecef_coord ecef, out struct_lla_coord lla)
{
double p = 0.0, theta = 0.0, sinTheta = 0.0, cosTheta = 0.0, temp = 0.0, temp1 = 0.0, sinLat = 0.0;
double up_lat = 0.0, low_lat = 0.0;
double x = 0.0, y = 0.0, z = 0.0, r = 0.0;
lla.lat = 0;
lla.lon = 0;
lla.alt = 0;
x = ecef.x;
y = ecef.y;
z = ecef.z;
r = x * x + y * y;
if (Math.Abs(r) < 1e-12)
{
return;
}
p = Math.Sqrt(r);
temp = (z * WGS_MAJOR) / (p * WGS_MINOR);
theta = Math.Atan(temp);
sinTheta = Math.Sin(theta);
cosTheta = Math.Cos(theta);
up_lat = z + WGS_E2_SQRT * WGS_MINOR * sinTheta * sinTheta * sinTheta;
low_lat = p - WGS_E1_SQRT * WGS_MAJOR * cosTheta * cosTheta * cosTheta;
lla.lat = Math.Atan2(up_lat, low_lat);
lla.lon = Math.Atan2(y, x);
sinLat = Math.Sin(lla.lat);
temp = WGS_MAJOR / Math.Sqrt(1.0 - WGS_E1_SQRT * sinLat * sinLat);
temp1 = Math.Cos(lla.lat);
if (Math.Abs(r) < 1e-12)
{
return;
}
lla.alt = (p / temp1) - temp;
}
