////////////////////////////////////////////////////////////////////// // 求解第n[0,1,2]个邻域面片的交点,结果存入vIntersection // 无解及其他情况返回false! ////////////////////////////////////////////////////////////////////// protected bool Solve3NPIntersection(List <int> lstN, ref Vector3 intersection) { int npCount = mLstNeighbors.Count; //输入无效 if (lstN == null || lstN.Count < 3 || lstN[0] < 0 || lstN[0] >= npCount || lstN[1] < 0 || lstN[1] >= npCount || lstN[2] < 0 || lstN[2] >= npCount) { return(false); } //构造线性方程组 Matrix mtxCoef = new Matrix(3, 3); // 系数矩阵 Matrix mtxConst = new Matrix(3, 1); // 常数矩阵 Matrix mtxResult; // 结果(3,1) for (int i = 0; i < 3; i++) { Patch pat = mLstNeighbors[lstN[i]].NeighborPatch; Vector3 v = pat.Normal; mtxCoef.SetElement(i, 0, v.x); mtxCoef.SetElement(i, 1, v.y); mtxCoef.SetElement(i, 2, v.z); mtxConst.SetElement(i, 0, pat.Dist); } //用高斯全选主元法求解 LEquations le = new LEquations(mtxCoef, mtxConst); if (!le.GetRootsetGauss(out mtxResult)) { return(false); } intersection.x = (float)mtxResult.GetElement(0, 0); intersection.y = (float)mtxResult.GetElement(1, 0); intersection.z = (float)mtxResult.GetElement(2, 0); return(true); }
/** * 求非线性方程组最小二乘解的广义逆法 * * 调用时,1. 须覆盖计算方程左端函数f(x)值及其偏导数值的虚函数 * double Func(double[] x, double[] y) * 2. 须覆盖计算雅可比矩阵函数的虚函数 * double FuncMJ(double[] x, double[] y) * * @param m - 方程的个数 * @param n - 未知数的个数 * @param x - 一维数组,长度为n,存放一组初值x0, x1, …, xn-1,要求不全为0, * 返回时存放方程组的最小二乘解,当m=n时,即是非线性方程组的解 * @param eps1 - 最小二乘解的精度控制精度 * @param eps2 - 奇异值分解的精度控制精度 * @return bool 型,求解是否成功 */ public bool GetRootsetGinv(int m, int n, double[] x, double eps1, double eps2) { int i, j, k, l, kk, jt; double alpha, z = 0, h2, y1, y2, y3, y0, h1; double[] p, d, dx; double[] y = new double[10]; double[] b = new double[10]; // 控制参数 int ka = Math.Max(m, n) + 1; double[] w = new double[ka]; // 设定迭代次数为60,迭代求解 l = 60; alpha = 1.0; while (l > 0) { MatrixOfAlgorithm mtxP = new MatrixOfAlgorithm(m, n); MatrixOfAlgorithm mtxD = new MatrixOfAlgorithm(m, 1); p = mtxP.GetData(); d = mtxD.GetData(); Func(x, d); FuncMJ(x, p); // 构造线性方程组 LEquations leqs = new LEquations(mtxP, mtxD); // 临时矩阵 MatrixOfAlgorithm mtxAP = new MatrixOfAlgorithm(); MatrixOfAlgorithm mtxU = new MatrixOfAlgorithm(); MatrixOfAlgorithm mtxV = new MatrixOfAlgorithm(); // 解矩阵 MatrixOfAlgorithm mtxDX = new MatrixOfAlgorithm(); // 基于广义逆的最小二乘解 if (!leqs.GetRootsetGinv(mtxDX, mtxAP, mtxU, mtxV, eps2)) { return(false); } dx = mtxDX.GetData(); j = 0; jt = 1; h2 = 0.0; while (jt == 1) { jt = 0; if (j <= 2) { z = alpha + 0.01 * j; } else { z = h2; } for (i = 0; i <= n - 1; i++) { w[i] = x[i] - z * dx[i]; } Func(w, d); y1 = 0.0; for (i = 0; i <= m - 1; i++) { y1 = y1 + d[i] * d[i]; } for (i = 0; i <= n - 1; i++) { w[i] = x[i] - (z + 0.00001) * dx[i]; } Func(w, d); y2 = 0.0; for (i = 0; i <= m - 1; i++) { y2 = y2 + d[i] * d[i]; } y0 = (y2 - y1) / 0.00001; if (Math.Abs(y0) > 1.0e-10) { h1 = y0; h2 = z; if (j == 0) { y[0] = h1; b[0] = h2; } else { y[j] = h1; kk = 0; k = 0; while ((kk == 0) && (k <= j - 1)) { y3 = h2 - b[k]; if (Math.Abs(y3) + 1.0 == 1.0) { kk = 1; } else { h2 = (h1 - y[k]) / y3; } k = k + 1; } b[j] = h2; if (kk != 0) { b[j] = 1.0e+35; } h2 = 0.0; for (k = j - 1; k >= 0; k--) { h2 = -y[k] / (b[k + 1] + h2); } h2 = h2 + b[0]; } j = j + 1; if (j <= 7) { jt = 1; } else { z = h2; } } } alpha = z; y1 = 0.0; y2 = 0.0; for (i = 0; i <= n - 1; i++) { dx[i] = -alpha * dx[i]; x[i] = x[i] + dx[i]; y1 = y1 + Math.Abs(dx[i]); y2 = y2 + Math.Abs(x[i]); } // 求解成功 if (y1 < eps1 * y2) { return(true); } l = l - 1; } // 求解失败 return(false); }
/** * 求非线性方程组一组实根的拟牛顿法 * * 调用时,须覆盖计算方程左端函数f(x)值及其偏导数值的虚函数 * double Func(double[] x, double[] y) * * @param n - 方程的个数,也是未知数的个数 * @param x - 一维数组,长度为n,存放一组初值x0, x1, …, xn-1, * 返回时存放方程组的一组实根 * @param t - 控制h大小的变量,0<t<1 * @param h - 增量初值 * @param nMaxIt - 迭代次数 * @param eps - 控制精度 * @return bool 型,求解是否成功 */ public bool GetRootsetNewton(int n, double[] x, double t, double h, int nMaxIt, double eps) { int i, j, l; double am, z, beta, d; double[] y = new double[n]; // 构造矩阵 MatrixOfAlgorithm mtxCoef = new MatrixOfAlgorithm(n, n); MatrixOfAlgorithm mtxConst = new MatrixOfAlgorithm(n, 1); double[] a = mtxCoef.GetData(); double[] b = mtxConst.GetData(); // 迭代求解 l = nMaxIt; am = 1.0 + eps; while (am >= eps) { Func(x, b); am = 0.0; for (i = 0; i <= n - 1; i++) { z = Math.Abs(b[i]); if (z > am) { am = z; } } if (am >= eps) { l = l - 1; if (l == 0) { return(false); } for (j = 0; j <= n - 1; j++) { z = x[j]; x[j] = x[j] + h; Func(x, y); for (i = 0; i <= n - 1; i++) { a[i * n + j] = y[i]; } x[j] = z; } // 调用全选主元高斯消元法 LEquations leqs = new LEquations(mtxCoef, mtxConst); MatrixOfAlgorithm mtxResult = new MatrixOfAlgorithm(); if (!leqs.GetRootsetGauss(mtxResult)) { return(false); } mtxConst.SetValue(mtxResult); b = mtxConst.GetData(); beta = 1.0; for (i = 0; i <= n - 1; i++) { beta = beta - b[i]; } if (beta == 0.0) { return(false); } d = h / beta; for (i = 0; i <= n - 1; i++) { x[i] = x[i] - d * b[i]; } h = t * h; } } // 是否在有效迭代次数内达到精度 return(nMaxIt > l); }