用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];// 计算DD = -(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;}}