用C#完成最小二乘法拟合平面方程，再计算点到面的距离

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace ConsoleApp2
{
    internal class Program
    {
        static void Main(string[] args)
        {

            double[] Xs =
                { 1, 2, 3, 4,
                1, 2, 3, 4,
                1, 2, 3, 4,
                1, 2, 3, 4 };

            double[] Ys =
                { 1, 1, 1, 1,
                2, 2, 2, 2,
                3, 3, 3, 3,
                4, 4, 4, 4};

            double[] Zs =
                {0, 0, 0, 0,
                0, 1, 1, 0,
                0, 1, 1, 0,
                0, 0, 0, 0 };

            double A, B, C, D;
            string strMsg = string.Empty;
            Operator3D.FitPlane(Xs, Ys, Zs, out A, out B, out C, out D, out strMsg);


            double dist = Operator3D.DistanceP2Plane(0, 0, 0, A, B, C, D, out strMsg);
        }
    }

    class Operator3D
    {
        /// <summary>
        /// 最小二乘法拟合平面方程,该方法通过计算协方差矩阵，并使用雅可比（Jacobi）方法求解其特征向量，从而得到最佳拟合平面的参数A\B\C\D
        /// </summary>
        /// <param name="Xs">三维点X坐标</param>
        /// <param name="Ys">三维点Y坐标</param>
        /// <param name="Zs">三维点Z坐标</param>
        /// <param name="A">平面方程一般式参数A</param>
        /// <param name="B">平面方程一般式参数B</param>
        /// <param name="C">平面方程一般式参数C</param>
        /// <param name="D">平面方程一般式参数D</param>
        /// <param name="errorMsg"></param>
        /// <returns></returns>
        public static bool FitPlane(double[] Xs, double[] Ys, double[] Zs, out double A, out double B, out double C, out double D, out string error)
        {
            error = string.Empty;
            A = B = C = D = 0;

            try 
            {
                // 验证输入数据有效性：保证输入数据不为空，数据长度一致，且至少有3个点
                if (Xs == null || Ys == null || Zs == null)
                {
                    error = "输入Xs、Ys、Zs数据为null";
                    return false;
                }
                else if (Xs.Length != Ys.Length || Xs.Length != Zs.Length)
                {
                    error = "输入Xs、Ys、Zs数据长度不同";
                    return false;
                }
                else if (Xs.Length < 3)
                {
                    error = "输入数据点数小于3";
                    return false;
                }

                int n = Xs.Length;

                // 计算质心
                double x0 = Average(Xs);
                double y0 = Average(Ys);
                double z0 = Average(Zs);

                // 计算协方差矩阵
                double[,] cov = new double[3, 3];
                for (int i = 0; i < n; i++)
                {
                    double dx = Xs[i] - x0;
                    double dy = Ys[i] - y0;
                    double dz = Zs[i] - z0;

                    cov[0, 0] += dx * dx;
                    cov[0, 1] += dx * dy;
                    cov[0, 2] += dx * dz;

                    cov[1, 1] += dy * dy;
                    cov[1, 2] += dy * dz;

                    cov[2, 2] += dz * dz;
                }
                // 填充对称元素
                cov[1, 0] = cov[0, 1];
                cov[2, 0] = cov[0, 2];
                cov[2, 1] = cov[1, 2];

                // 计算特征值和特征向量
                if (!JacobiEigenvalue(cov, out double[] eigenvalues, out double[,] eigenvectors))
                {
                    error = "雅可比特征值分解失败";
                    return false;
                }


                // 找到最小特征值的索引
                int minIndex = 0;
                for (int i = 1; i < 3; i++)
                    if (eigenvalues[i] < eigenvalues[minIndex])
                        minIndex = i;

                // 获取法向量
                A = eigenvectors[0, minIndex];
                B = eigenvectors[1, minIndex];
                C = eigenvectors[2, minIndex];

                // 计算D
                D = -(A * x0 + B * y0 + C * z0);

                // 归一化平面方程（可选）
                double norm = Math.Sqrt(A * A + B * B + C * C);
                if (norm < 1e-12) // 避免除以零
                {
                    error = "除数不能为零";
                    return false;
                }

                A /= norm;
                B /= norm;
                C /= norm;
                D /= norm;

                return true;
            }
            catch 
            {
                error = "拟合平面失败";
                return false;
            }
        }

        //计算平均数
        private static double Average(double[] arr)
        {
            double sum = 0;
            foreach (double d in arr)
                sum += d;
            return sum / arr.Length;
        }

        //雅可比特征值分解
        private static bool JacobiEigenvalue(double[,] matrix, out double[] eigenvalues, out double[,] eigenvectors)
        {
            int n = 3;
            eigenvalues = new double[n];
            eigenvectors = new double[n, n];
            for (int i = 0; i < n; i++)
            {
                eigenvectors[i, i] = 1.0;
                for (int j = 0; j < n; j++)
                    if (i != j)
                        eigenvectors[i, j] = 0.0;
            }

            const int maxIterations = 50;
            const double epsilon = 1e-12;

            for (int k = 0; k < maxIterations; k++)
            {
                // 找到最大的非对角元素
                int p = 0, q = 0;
                double maxVal = 0;
                for (int i = 0; i < n; i++)
                    for (int j = i + 1; j < n; j++)
                        if (Math.Abs(matrix[i, j]) > maxVal)
                        {
                            maxVal = Math.Abs(matrix[i, j]);
                            p = i;
                            q = j;
                        }

                if (maxVal < epsilon)
                    break;

                // 计算旋转角度
                double theta = 0.5 * Math.Atan2(2 * matrix[p, q], matrix[q, q] - matrix[p, p]);
                double c = Math.Cos(theta);
                double s = Math.Sin(theta);

                // 更新矩阵
                double new_pp = c * c * matrix[p, p] - 2 * c * s * matrix[p, q] + s * s * matrix[q, q];
                double new_qq = s * s * matrix[p, p] + 2 * c * s * matrix[p, q] + c * c * matrix[q, q];
                double new_pq = (c * c - s * s) * matrix[p, q] + c * s * (matrix[p, p] - matrix[q, q]);

                matrix[p, p] = new_pp;
                matrix[q, q] = new_qq;
                matrix[p, q] = matrix[q, p] = new_pq;

                // 更新其他行和列
                for (int i = 0; i < n; i++)
                {
                    if (i != p && i != q)
                    {
                        double new_ip = c * matrix[i, p] - s * matrix[i, q];
                        double new_iq = s * matrix[i, p] + c * matrix[i, q];
                        matrix[i, p] = matrix[p, i] = new_ip;
                        matrix[i, q] = matrix[q, i] = new_iq;
                    }
                }

                // 更新特征向量
                for (int i = 0; i < n; i++)
                {
                    double temp = eigenvectors[i, p];
                    eigenvectors[i, p] = c * temp - s * eigenvectors[i, q];
                    eigenvectors[i, q] = s * temp + c * eigenvectors[i, q];
                }
            }

            // 提取特征值
            for (int i = 0; i < n; i++)
                eigenvalues[i] = matrix[i, i];

            return true;
        }

        /// <summary>
        /// 计算点到平面的带符号距离
        /// </summary>
        /// <param name="X">点的X坐标</param>
        /// <param name="Y">点的Y坐标</param>
        /// <param name="Z">点的Z坐标</param>
        /// <param name="A">平面方程系数A</param>
        /// <param name="B">平面方程系数B</param>
        /// <param name="C">平面方程系数C</param>
        /// <param name="D">平面方程系数D</param>
        /// <returns>
        /// 带符号的距离值：
        /// - 正值表示点在法向量指向的一侧
        /// - 负值表示点在法向量反向的一侧
        /// - 零表示点在平面上
        /// </returns>
        public static double DistanceP2Plane(double X, double Y, double Z, double A, double B, double C, double D, out string error)
        {
            error = string.Empty;
            try
            {
                // 计算公式：(A*X + B*Y + C*Z + D) / √(A² + B² + C²)
                double numerator = A * X + B * Y + C * Z + D;
                double denominator = Math.Sqrt(A * A + B * B + C * C);

                // 处理极小值防止除以零
                if (Math.Abs(denominator) < 1e-12)
                {
                    error = "非法平面参数";
                    return double.NaN; // 非法平面参数时返回NaN
                }
                return numerator / denominator;
            }
            catch (Exception)
            {
                error = "非法平面参数,计算失败";
                throw;
            }
        }