/// <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; }