예제 #1
0
        /// <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;
        }
예제 #2
0
        /// <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;
        }
예제 #3
0
        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;
        }
예제 #4
0
        /// <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;
        }