//////////////////////////////////////////////////////////////////////
// 求解第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);
}
/**
* 求非线性方程组一组实根的拟牛顿法
*
* 调用时，须覆盖计算方程左端函数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);
}