最小二乘法拟合平面算法及C#代码

最近项目中需要用到平面拟合，本想偷懒在网上查下程序资源，发现网上资源对于平面方程为ax+by+cz+d=0，基本都是基于c=1的条件进行的推导与编程，而在实际中，c=0的特殊情况也是存在的，针对这个情况，只好自己重新推导了下平面拟合的算法：

考虑到平面方程a、b、c不可能都为0，将c=1的特定约束条件替换成通用约束条件：a+b+c=1，平面方程变为：
ax+by+(1-a-b)z+d=0
即a(x-z)+b(y-z)+d=z

上代码：

 /// <summary>
        /// 平面方程拟合，ax+by+cz+d=0,其中a=result[0],b=result[1],c=result[2],d=result[3]
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="z"></param>
        /// <returns></returns>
        private double[] PanelFit(double[] x, double[] y, double[] z)
        {
            double[] result = new double[4];
            int n = x.Length;
            double[,] A = new double[n,3];
            double[,] E = new double[n,1];
            for (int i = 0; i < n; i++)
            {
                A[i, 0] = x[i] - z[i];
                A[i, 1] = y[i] - z[i];
                A[i, 2] = 1;
                E[i, 0] = -z[i];
            }
            double[,] AT = MatrixInver(A);
            double[,] ATxA = MatrixMultiply(AT, A);
            double[,] OPPAxTA = MatrixOpp(ATxA);
            double[,] OPPATAxAT = MatrixMultiply(OPPAxTA, AT);
            double[,] DP = MatrixMultiply(OPPATAxAT, E);
            result[0] = DP[0, 0];
            result[1] = DP[1, 0];
            result[2] = 1 - result[0] - result[1];
            result[3] = DP[2, 0];
            return result;
        }
        /// <summary>
        /// 矩阵转置
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        private double[,] MatrixInver(double[,] matrix)
        {
            double[,] result = new double[matrix.GetLength(1), matrix.GetLength(0)];
            for (int i = 0; i < matrix.GetLength(1); i++)
                for (int j = 0; j < matrix.GetLength(0); j++)
                    result[i, j] = matrix[j, i];
            return result;
        }
        /// <summary>
        /// 矩阵相乘
        /// </summary>
        /// <param name="matrixA"></param>
        /// <param name="matrixB"></param>
        /// <returns></returns>
        private double[,] MatrixMultiply(double[,] matrixA,double[,] matrixB)
        {
            double[,] result = new double[matrixA.GetLength(0), matrixB.GetLength(1)];
            for (int i = 0; i < matrixA.GetLength(0); i++)
            {
                for (int j = 0; j < matrixB.GetLength(1); j++)
                {
                    result[i, j] = 0;
                    for (int k = 0; k < matrixB.GetLength(0); k++)
                    {
                        result[i, j] += matrixA[i, k] * matrixB[k, j];
                    }
                }
            }
            return result;
        }
        /// <summary>
        /// 矩阵的逆
        /// </summary>
        /// <param name="matrix"></param>
        /// <returns></returns>
        private double[,] MatrixOpp(double[,] matrix)
        {
            double X = 1 / MatrixSurplus(matrix);
            double[,] matrixB = new double[matrix.GetLength(0), matrix.GetLength(1)];
            double[,] matrixSP = new double[matrix.GetLength(0), matrix.GetLength(1)];
            double[,] matrixAB = new double[matrix.GetLength(0), matrix.GetLength(1)];

            for (int i = 0; i < matrix.GetLength(0); i++)
                for (int j = 0; j < matrix.GetLength(1); j++)
                {
                    for (int m = 0; m < matrix.GetLength(0); m++)
                        for (int n = 0; n < matrix.GetLength(1); n++)
                            matrixB[m, n] = matrix[m, n];
                    {
                        for (int x = 0; x < matrix.GetLength(1); x++)
                            matrixB[i, x] = 0;
                        for (int y = 0; y < matrix.GetLength(0); y++)
                            matrixB[y, j] = 0;
                        matrixB[i, j] = 1;
                        matrixSP[i, j] = MatrixSurplus(matrixB);
                        matrixAB[i, j] = X * matrixSP[i, j];
                    }
                }
            return MatrixInver(matrixAB);
        }
        /// <summary>
        /// 矩阵的行列式的值  
        /// </summary>
        /// <param name="A"></param>
        /// <returns></returns>
        public static double MatrixSurplus(double[,] matrix)
        {
            double X = -1;
            double[,] a = matrix;
            int i, j, k, p, r, m, n;
            m = a.GetLength(0);
            n = a.GetLength(1);
            double temp = 1, temp1 = 1, s = 0, s1 = 0;

            if (n == 2)
            {
                for (i = 0; i < m; i++)
                    for (j = 0; j < n; j++)
                        if ((i + j) % 2 > 0) temp1 *= a[i, j];
                        else temp *= a[i, j];
                X = temp - temp1;
            }
            else
            {
                for (k = 0; k < n; k++)
                {
                    for (i = 0, j = k; i < m && j < n; i++, j++)
                        temp *= a[i, j];
                    if (m - i > 0)
                    {
                        for (p = m - i, r = m - 1; p > 0; p--, r--)
                            temp *= a[r, p - 1];
                    }
                    s += temp;
                    temp = 1;
                }

                for (k = n - 1; k >= 0; k--)
                {
                    for (i = 0, j = k; i < m && j >= 0; i++, j--)
                        temp1 *= a[i, j];
                    if (m - i > 0)
                    {
                        for (p = m - 1, r = i; r < m; p--, r++)
                            temp1 *= a[r, p];
                    }
                    s1 += temp1;
                    temp1 = 1;
                }

                X = s - s1;
            }
            return X;
        }