地理空间坐标系统-不同椭球基准间的坐标转换-相关算法代码实现C#

在上一篇的论述中，不同椭球基准间的坐标转换除了同一个椭球基准内坐标转换算法外，我们还提到了两个算法：

1.已知两个空间直角坐标间转换的七参数△X，△Y，△Z，ωx，ωy，ωz，m的情况情况下，如何进行椭球转换。

2.在无法获取两个空间直角坐标转换所需的七参数的情况下，如何通过三个以上的坐标点对，反算七参数。

已七参数，进行椭球转换

根据上一篇中的公式，具体代码如下：

       /// <summary>
        /// 空间直角坐标之间的转换，使用布尔莎七参转换
        /// </summary>
        /// <param name="originalPoint"></param>
        /// <param name="dx">X轴平移参数，单位"米"</param>
        /// <param name="dy">Y轴平移参数，单位"米"</param>
        /// <param name="dz">Z轴平移参数，单位"米"</param>
        /// <param name="rotax">旋转参数x,单位"弧度"</param>
        /// <param name="rotay">旋转参数y,单位"弧度"</param>
        /// <param name="rotaz">旋转参数z,单位"弧度"</param>
        /// <param name="scale">尺度参数k</param>
        /// <param name="reslutPoint">返回的坐标</param>
        public static void SpaceRectangularToSpaceRectangularByBursa(
           Point3d originalPoint, 
           double dx,double dy,double dz,double rotax,double rotay,double rotaz,
           double scale, out Point3d reslutPoint)
        {
            double a1 = scale + 1;
            double a2 = a1 * rotax;
            double a3 = a1 * rotay;
            double a4 = a1 * rotaz;

            reslutPoint.X = dx + a1 * originalPoint.X 
                  - a3 * originalPoint.Z + a4 * originalPoint.Y;
            reslutPoint.Y = dy + a1 * originalPoint.Y 
                  + a2 * originalPoint.Z - a4 * originalPoint.X;  
            reslutPoint.Z = dz + a1 * originalPoint.Z 
                  - a2 * originalPoint.Y + a3 * originalPoint.X; 


            //B=AX，下面是想用矩阵计算的方式，来对结果进行验证。可以忽略

            double[,] pX = new double[7, 1];
            double[,] pA = new double[3, 7];

            pX[0, 0] = dx;
            pX[1, 0] = dy;
            pX[2, 0] = dz;
            pX[3, 0] = a1;
            pX[4, 0] = a2;
            pX[5, 0] = a3;
            pX[6, 0] = a4;

            ///初始化A矩阵第一步
            for (int i = 0; i <  3; i++)
            {
                if (i % 3 == 0)
                {
                    pA[i, 0] = 1;
                    pA[i, 1] = 0;
                    pA[i, 2] = 0;
                    pA[i, 3] = originalPoint.X;
                    pA[i, 4] = 0;
                    pA[i, 5] = -originalPoint.Z;
                    pA[i, 6] = originalPoint.Y;
                }
                else if (i % 3 == 1)
                {
                    pA[i, 0] = 0;
                    pA[i, 1] = 1;
                    pA[i, 2] = 0;
                    pA[i, 3] = originalPoint.Y;
                    pA[i, 4] = originalPoint.Z;
                    pA[i, 5] = 0;
                    pA[i, 6] = -originalPoint.X;
                }
                else if (i % 3 == 2)
                {
                    pA[i, 0] = 0;
                    pA[i, 1] = 0;
                    pA[i, 2] = 1;
                    pA[i, 3] = originalPoint.Z;
                    pA[i, 4] = -originalPoint.Y;
                    pA[i, 5] = originalPoint.X;
                    pA[i, 6] = 0;
                }
            }

            GMatrix X = new GMatrix(pX);
            GMatrix A = new GMatrix(pA);
            GMatrix B = A*X;
}

未知七参数，通过三个以上的坐标点对，反算七参数

根据上一篇的中的公式，具体代码实现如下：

 /// <summary>
        /// 七参数反算
        /// </summary>
        /// <param name="originalCooords 旧坐标"></param>
        /// <param name="targetCooords 新坐标"></param>
        /// <param name="dx">X轴平移参数，单位"米"</param>
        /// <param name="dy">Y轴平移参数，单位"米"</param>
        /// <param name="dz">Z轴平移参数，单位"米"</param>
        /// <param name="rotax">旋转参数x,单位"弧度"</param>
        /// <param name="rotay">旋转参数y,单位"弧度"</param>
        /// <param name="rotaz">旋转参数z,单位"弧度"</param>
        /// <param name="scale">尺度参数k,</param>
        public static void CalSevenParaByTwoSpaceRectangularCoords(
            Point3d[] originalCooords,Point3d[] targetCooords, 
            ref double dx, ref double dy, ref double dz, 
            ref double rotax, ref double rotay, ref double rotaz, ref double scale)
        {
         
            int pointCount = originalCooords.Length;
            if (pointCount < 3)
            {
                //坐标点数小于三。
                return;
            }
            if (targetCooords.Length < pointCount)
            {
                //新旧坐标个数不匹配
                return;
            }
            double[,] pA = new double[pointCount * 3, 7];
            double[,]