//////////////////////////////////////////////////////////////////////
// 求解第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);
}
