/// <summary>
/// 获取线状要素
/// </summary>
/// <param name="pDataRecords">数据记录</param>
/// <param name="dblProp">插值参数</param>
/// <returns>在处理面状要素时，本程序将原面状要素的边界切开，按线状要素处理，处理完后再重新生成面状要素</returns>
public CPolyline GetTargetcpl(double dblProp)
{
List <CCorrCpts> pCorrCptsLt = _DataRecords.ParameterResult.CCorrCptsLt; //Read Datasets后，此处ResultPtLt中的对应点为一一对应
double dblTX = _dblTX;
int intPtNum = pCorrCptsLt.Count;
//计算长度初始值（全部计算）
double[] adblLength0 = new double[intPtNum - 1];
double[] adblLength = new double[intPtNum - 1]; //可以变化的值
for (int i = 0; i < pCorrCptsLt.Count - 1; i++)
{
double dblfrsublength = CGeoFunc.CalDis(pCorrCptsLt[i + 1].FrCpt, pCorrCptsLt[i].FrCpt);
double dbltosublength = CGeoFunc.CalDis(pCorrCptsLt[i + 1].ToCpt, pCorrCptsLt[i].ToCpt);
adblLength0[i] = (1 - dblProp) * dblfrsublength + dblProp * dbltosublength;
adblLength[i] = adblLength0[i];
}
//计算角度初始值（全部计算）
double[] adblAngle0 = new double[intPtNum - 2];
double[] adblAngle = new double[intPtNum - 2]; //可以变化的值
for (int i = 0; i < pCorrCptsLt.Count - 2; i++)
{
//较大比例尺线状要素上的夹角
double dblfrAngle = CGeoFunc.CalAngle_Counterclockwise(pCorrCptsLt[i].FrCpt, pCorrCptsLt[i + 1].FrCpt, pCorrCptsLt[i + 2].FrCpt);
//较小比例尺线状要素上的夹角
double dbltoAngle = CGeoFunc.CalAngle_Counterclockwise(pCorrCptsLt[i].ToCpt, pCorrCptsLt[i + 1].ToCpt, pCorrCptsLt[i + 2].ToCpt);
//角度初始值
adblAngle0[i] = (1 - dblProp) * dblfrAngle + dblProp * dbltoAngle;
adblAngle[i] = adblAngle0[i];
}
//生成点数组（初始值），同时计算各线段方位角混合值
//注意：默认固定第一条边
List <CPoint> cptlt = new List <CPoint>();
double[] adblAzimuth = new double[intPtNum - 1];
//计算第一个点和第二个点
double dblnewX0 = (1 - dblProp) * pCorrCptsLt[0].FrCpt.X + dblProp * pCorrCptsLt[0].ToCpt.X;
double dblnewY0 = (1 - dblProp) * pCorrCptsLt[0].FrCpt.Y + dblProp * pCorrCptsLt[0].ToCpt.Y;
CPoint newcpt0 = new CPoint(0, dblnewX0, dblnewY0);
newcpt0.isCtrl = true;
cptlt.Add(newcpt0);
double dblnewX1 = (1 - dblProp) * pCorrCptsLt[1].FrCpt.X + dblProp * pCorrCptsLt[1].ToCpt.X;
double dblnewY1 = (1 - dblProp) * pCorrCptsLt[1].FrCpt.Y + dblProp * pCorrCptsLt[1].ToCpt.Y;
CPoint newcpt1 = new CPoint(1, dblnewX1, dblnewY1);
newcpt1.isCtrl = true;
cptlt.Add(newcpt1);
adblAzimuth[0] = CGeoFunc.CalAxisAngle(newcpt0, newcpt1);
////后面的其它点（不固定最后两个点）
////pCorrCptsLt[intPtNum - 2].FrCpt.isCtrl = false;
////pCorrCptsLt[intPtNum - 1].FrCpt.isCtrl = false;
//for (int i = 2; i < pCorrCptsLt.Count; i++)
//{
// CPoint newcpt = new CPoint();
// if (pCorrCptsLt[i].FrCpt.isCtrl == false)
// {
// adblAzimuth[i - 1] = adblAzimuth[i - 2] + adblAngle0[i - 2] - Math.PI;
// double dblnewX = cptlt[i - 1].X + adblLength0[i - 1] * Math.Cos(adblAzimuth[i - 1]);
// double dblnewY = cptlt[i - 1].Y + adblLength0[i - 1] * Math.Sin(adblAzimuth[i - 1]);
// newcpt = new CPoint(i, dblnewX, dblnewY);
// }
// else
// {
// double dblnewX = (1 - dblProp) * pCorrCptsLt[i].FrCpt.X + dblProp * pCorrCptsLt[i].ToCpt.X;
// double dblnewY = (1 - dblProp) * pCorrCptsLt[i].FrCpt.Y + dblProp * pCorrCptsLt[i].ToCpt.Y;
// newcpt = new CPoint(i, dblnewX, dblnewY);
// newcpt.isCtrl = true;
// //计算角度：不能采用“CGeoFunc.CalAxisAngle”，因为此处的方位角不一定在0到2Pi之间，采用重新连接法
// double dblAngle = CGeoFunc.CalAngle_Counterclockwise(cptlt[cptlt.Count - 2], cptlt[cptlt.Count - 1], newcpt); //计算实际夹角
// adblAzimuth[i - 1] = adblAzimuth[i - 2] + dblAngle - Math.PI;
// }
// cptlt.Add(newcpt);
//}
//后面的其它点（固定最后两个点）
for (int i = 2; i < pCorrCptsLt.Count - 2; i++)
{
CPoint newcpt = new CPoint();
if (pCorrCptsLt[i].FrCpt.isCtrl == false)
{
adblAzimuth[i - 1] = adblAzimuth[i - 2] + adblAngle0[i - 2] - Math.PI;
double dblnewX = cptlt[i - 1].X + adblLength0[i - 1] * Math.Cos(adblAzimuth[i - 1]);
double dblnewY = cptlt[i - 1].Y + adblLength0[i - 1] * Math.Sin(adblAzimuth[i - 1]);
newcpt = new CPoint(i, dblnewX, dblnewY);
}
else
{
double dblnewX = (1 - dblProp) * pCorrCptsLt[i].FrCpt.X + dblProp * pCorrCptsLt[i].ToCpt.X;
double dblnewY = (1 - dblProp) * pCorrCptsLt[i].FrCpt.Y + dblProp * pCorrCptsLt[i].ToCpt.Y;
newcpt = new CPoint(i, dblnewX, dblnewY);
newcpt.isCtrl = true;
//计算角度：不能采用“CGeoFunc.CalAxisAngle”，因为此处的方位角不一定在0到2Pi之间，采用重新连接法
double dblAngle = CGeoFunc.CalAngle_Counterclockwise(cptlt[cptlt.Count - 2], cptlt[cptlt.Count - 1], newcpt); //计算实际夹角
adblAzimuth[i - 1] = adblAzimuth[i - 2] + dblAngle - Math.PI;
}
cptlt.Add(newcpt);
}
//计算最后两个点
double dblnewXlast1 = (1 - dblProp) * pCorrCptsLt[pCorrCptsLt.Count - 2].FrCpt.X + dblProp * pCorrCptsLt[pCorrCptsLt.Count - 2].ToCpt.X;
double dblnewYlast1 = (1 - dblProp) * pCorrCptsLt[pCorrCptsLt.Count - 2].FrCpt.Y + dblProp * pCorrCptsLt[pCorrCptsLt.Count - 2].ToCpt.Y;
CPoint newcptlast1 = new CPoint(pCorrCptsLt.Count - 2, dblnewXlast1, dblnewYlast1);
newcptlast1.isCtrl = true;
cptlt.Add(newcptlast1);
double dblAnglelast1 = CGeoFunc.CalAngle_Counterclockwise(cptlt[cptlt.Count - 3], cptlt[cptlt.Count - 2], cptlt[cptlt.Count - 1]); //计算实际夹角
adblAzimuth[pCorrCptsLt.Count - 3] = adblAzimuth[pCorrCptsLt.Count - 4] + dblAnglelast1 - Math.PI;
double dblnewXlast0 = (1 - dblProp) * pCorrCptsLt[pCorrCptsLt.Count - 1].FrCpt.X + dblProp * pCorrCptsLt[pCorrCptsLt.Count - 1].ToCpt.X;
double dblnewYlast0 = (1 - dblProp) * pCorrCptsLt[pCorrCptsLt.Count - 1].FrCpt.Y + dblProp * pCorrCptsLt[pCorrCptsLt.Count - 1].ToCpt.Y;
CPoint newcptlast0 = new CPoint(pCorrCptsLt.Count - 1, dblnewXlast0, dblnewYlast0);
newcptlast0.isCtrl = true;
cptlt.Add(newcptlast0);
double dblAnglelast0 = CGeoFunc.CalAngle_Counterclockwise(cptlt[cptlt.Count - 3], cptlt[cptlt.Count - 2], cptlt[cptlt.Count - 1]); //计算实际夹角
adblAzimuth[pCorrCptsLt.Count - 2] = adblAzimuth[pCorrCptsLt.Count - 3] + dblAnglelast0 - Math.PI;
//统计插值点数
int intKnownPt = 0; //固定点的数目
int intUnknownPt = 0; //非固定点的数目
List <int> intKnownLocationLt = new List <int>(); //记录已知点的序号
//注意：对于该循环，有一个默认条件，即FromCpl的第一个顶点只有一个对应点
for (int i = 0; i < cptlt.Count; i++)
{
if (cptlt[i].isCtrl == true)
{
intKnownLocationLt.Add(i);
intKnownPt += 1;
}
else
{
intUnknownPt += 1;
}
}
int intUnknownXY = intUnknownPt * 2; //每个点都有X、Y坐标
//找出长度固定的位置(如果一个线段的前后两个点都固定，则该长度固定)。另外，长度固定则该边的方位角也固定
List <int> intKnownLengthLt = new List <int>();
for (int i = 0; i < intKnownLocationLt.Count - 1; i++)
{
if ((intKnownLocationLt[i + 1] - intKnownLocationLt[i]) == 1)
{
intKnownLengthLt.Add(intKnownLocationLt[i]);
}
}
int intUnknownLength = cptlt.Count - 1 - intKnownLengthLt.Count;
//找出角度固定的位置(如果一个固定顶点的前后两个点都固定，则该角度固定)
List <int> intKnownAngleLt = new List <int>();
for (int i = 0; i < intKnownLocationLt.Count - 2; i++)
{
if ((intKnownLocationLt[i + 1] - intKnownLocationLt[i]) == 1 && (intKnownLocationLt[i + 2] - intKnownLocationLt[i + 1]) == 1)
{
intKnownAngleLt.Add(intKnownLocationLt[i]);
}
}
int intUnknownAngle = cptlt.Count - 2 - intKnownAngleLt.Count;
//总未知量
int intUnknownLengthAngle = intUnknownLength + intUnknownAngle;
//定义权重矩阵
VBMatrix P = new VBMatrix(intUnknownLengthAngle, intUnknownLengthAngle);
for (int i = 0; i < intUnknownLength; i++)
{
P[i, i] = 1;
}
for (int i = 0; i < intUnknownAngle; i++)
{
P[intUnknownLength + i, intUnknownLength + i] = 1;
}
//定义权重矩阵倒数
VBMatrix QLL = new VBMatrix(intUnknownLengthAngle, intUnknownLengthAngle);
for (int i = 0; i < intUnknownLength; i++)
{
QLL[i, i] = 1;
}
for (int i = 0; i < intUnknownAngle; i++)
{
QLL[intUnknownLength + i, intUnknownLength + i] = 1;
}
//for (int i = 0; i < intXYCst; i++)
//{
// P[intUnknownLengthAngle + i, intUnknownLengthAngle + i] = 1;
//}
//for (int i = 0; i < intAngleCst; i++)
//{
// P[intUnknownLengthAngle + intXYCst + i, intUnknownLengthAngle + intXYCst + i] =100000;
//}
//计算初始值矩阵X0
VBMatrix X0 = new VBMatrix(2 * intPtNum, 1);
for (int i = 0; i < intPtNum; i++)
{
X0[2 * i, 0] = cptlt[i].X;
X0[2 * i + 1, 0] = cptlt[i].Y;
}
//Xmix里存储了XA和X0的最新混合值（此矩阵在公式推导中并不存在，只是为了方便编写代码而建立）
VBMatrix Xmix = new VBMatrix(2 * intPtNum, 1);
for (int i = 0; i < (2 * intPtNum); i++)
{
Xmix[i, 0] = X0[i, 0];
}
//定义系数矩阵B(各方程对长度、角度的导数值)
VBMatrix B = new VBMatrix(intUnknownLengthAngle, intUnknownLengthAngle);
for (int i = 0; i < intUnknownLengthAngle; i++)
{
B[i, i] = -1;
}
//定义系数矩阵A(各方程对坐标的导数值)，A的导数值将在循环中给出
VBMatrix A = new VBMatrix(intUnknownLengthAngle, intUnknownXY);
double dblJudge1 = 0; //该值用于判断是否应该跳出循环
double dblJudge2 = 0; //该值用于判断是否应该跳出循环
int intJudgeIndex = intUnknownLength / 4;
int intIterativeCount = 0;
double[] dblSubDis = new double[intPtNum - 1];
do
{
VBMatrix w = new VBMatrix(intUnknownLengthAngle, 1);
//计算系数矩阵A第0行到"intUnknownLength"行的各元素，即线段长度对各未知数求偏导的值
//先计算各分母值（注意：分母实际上是求偏导后的一部分值，但却恰好等于两点之间距离，因此其计算公式与距离计算公式相同
dblSubDis = new double[intPtNum - 1];
for (int i = 0; i < intPtNum - 1; i++)
{
dblSubDis[i] = Math.Pow((Xmix[2 * i, 0] - Xmix[2 * i + 2, 0]) * (Xmix[2 * i, 0] - Xmix[2 * i + 2, 0]) + (Xmix[2 * i + 1, 0] - Xmix[2 * i + 3, 0]) * (Xmix[2 * i + 1, 0] - Xmix[2 * i + 3, 0]), 0.5);
}
//计算新的方位角
adblAzimuth[0] = CGeoFunc.CalAxisAngle(Xmix[0, 0], Xmix[1, 0], Xmix[2, 0], Xmix[3, 0]);
for (int i = 1; i < intPtNum - 1; i++)
{
double dblAngle = CGeoFunc.CalAngle_Counterclockwise(Xmix[i * 2 - 2, 0], Xmix[i * 2 - 1, 0], Xmix[i * 2, 0], Xmix[i * 2 + 1, 0], Xmix[i * 2 + 2, 0], Xmix[i * 2 + 3, 0]);
adblAzimuth[i] = adblAzimuth[i - 1] + dblAngle - Math.PI;
}
//开始计算系数矩阵第"intUnknownXY"行到"intUnknownXY+intUnknownLength-1"行的各元素
int intKnownCount2 = 0;
int intUnKnownCount2 = 0;
for (int j = 0; j < intUnknownLength; j++)
{
int intSumCount = intKnownCount2 + intUnKnownCount2;
if (cptlt[intSumCount].isCtrl == false && cptlt[intSumCount + 1].isCtrl == false)
{
A[j, 2 * intUnKnownCount2 + 0] = -Math.Cos(adblAzimuth[intSumCount]);
A[j, 2 * intUnKnownCount2 + 1] = -Math.Sin(adblAzimuth[intSumCount]);
A[j, 2 * intUnKnownCount2 + 2] = -A[j, 2 * intUnKnownCount2 + 0];
A[j, 2 * intUnKnownCount2 + 3] = -A[j, 2 * intUnKnownCount2 + 1];
w[j, 0] = dblSubDis[intSumCount] - adblLength[intSumCount]; //图方便，顺便计算matl
intUnKnownCount2 += 1;
}
else if (cptlt[intSumCount].isCtrl == false && cptlt[intSumCount + 1].isCtrl == true)
{
A[j, 2 * intUnKnownCount2 + 0] = -Math.Cos(adblAzimuth[intSumCount]);
A[j, 2 * intUnKnownCount2 + 1] = -Math.Sin(adblAzimuth[intSumCount]);
w[j, 0] = dblSubDis[intSumCount] - adblLength[intSumCount]; //图方便，顺便计算matl
intUnKnownCount2 += 1;
}
else if (cptlt[intSumCount].isCtrl == true && cptlt[intSumCount + 1].isCtrl == false)
{
//注意这种情况，由于"pCorrCptsLt[intSumCount].FrCpt.isCtrl == true"不占位子（即不占列），因此列序号依然为" 2 * intUnKnownCount2 + 0"和" 2 * intUnKnownCount2 + 1"，而不是+2,+3
A[j, 2 * intUnKnownCount2 + 0] = Math.Cos(adblAzimuth[intSumCount]);
A[j, 2 * intUnKnownCount2 + 1] = Math.Sin(adblAzimuth[intSumCount]);
w[j, 0] = dblSubDis[intSumCount] - adblLength[intSumCount]; //图方便，顺便计算matl
intKnownCount2 += 1;
}
else
{
intKnownCount2 += 1;
j -= 1;
}
}
//计算系数矩阵A第"intUnknownXY+intUnknownLength"行到"intUnknownXY+intUnknownLength+intUnknownAngle"行的各元素，即角度对各未知数求偏导的值
int intKnownCount3 = 0;
int intUnKnownCount3 = 0;
for (int j = 0; j < intUnknownAngle; j++)
{
//真是太幸运了，虽然求两向量逆时针夹角时需分多种情况讨论，但各情况的导数形式却是一致的，节省了不少编程精力啊，哈哈
int intSumCount = intKnownCount3 + intUnKnownCount3;
//常用数据准备
double dblA2 = dblSubDis[intSumCount] * dblSubDis[intSumCount];
double dblB2 = dblSubDis[intSumCount + 1] * dblSubDis[intSumCount + 1];
//开始计算系数值，由于将以下三个情况排列组合将有八种情况，因此按如下方式计算
if (cptlt[intUnKnownCount3 + intKnownCount3].isCtrl == true && cptlt[intUnKnownCount3 + intKnownCount3 + 1].isCtrl == true && cptlt[intUnKnownCount3 + intKnownCount3 + 2].isCtrl == true)
{
intKnownCount3 += 1;
j -= 1;
}
else
{
double dblNewAngle = CGeoFunc.CalAngle_Counterclockwise(Xmix[2 * intSumCount + 0, 0], Xmix[2 * intSumCount + 1, 0],
Xmix[2 * intSumCount + 2, 0], Xmix[2 * intSumCount + 3, 0],
Xmix[2 * intSumCount + 4, 0], Xmix[2 * intSumCount + 5, 0]);
w[intUnknownLength + j, 0] = dblNewAngle - adblAngle[intSumCount]; //图方便，顺便计算matl
int intPreTrueNum = 0;
int intUnKnownCount3orginal = intUnKnownCount3;
int intKnownCount3orginal = intKnownCount3;
if (cptlt[intUnKnownCount3orginal + intKnownCount3orginal + 0].isCtrl == false)
{
//X1,Y1的导数值(注意：该部分是减数，因此值为导数的负数)
A[intUnknownLength + j, 2 * intUnKnownCount3orginal + 0] = -(Xmix[2 * intSumCount + 3, 0] - Xmix[2 * intSumCount + 1, 0]) / dblA2;
A[intUnknownLength + j, 2 * intUnKnownCount3orginal + 1] = (Xmix[2 * intSumCount + 2, 0] - Xmix[2 * intSumCount + 0, 0]) / dblA2;
intUnKnownCount3 += 1;
}
else
{
intPreTrueNum += 1;
intKnownCount3 += 1;
}
if (cptlt[intUnKnownCount3orginal + intKnownCount3orginal + 1].isCtrl == false)
{
//X2,Y2的导数值
//A[intUnknownLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 2] = -(Xmix[2 * intSumCount + 5, 0] - Xmix[2 * intSumCount + 3, 0]) / dblB2 - (Xmix[2 * intSumCount + 3, 0] - Xmix[2 * intSumCount + 1, 0]) / dblA2;
//A[intUnknownLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 3] = (Xmix[2 * intSumCount + 4, 0] - Xmix[2 * intSumCount + 2, 0]) / dblB2 + (Xmix[2 * intSumCount + 2, 0] - Xmix[2 * intSumCount + 0, 0]) / dblA2;
A[intUnknownLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 2] = (Xmix[2 * intSumCount + 5, 0] - Xmix[2 * intSumCount + 3, 0]) / dblB2 + (Xmix[2 * intSumCount + 3, 0] - Xmix[2 * intSumCount + 1, 0]) / dblA2;
A[intUnknownLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 3] = -(Xmix[2 * intSumCount + 4, 0] - Xmix[2 * intSumCount + 2, 0]) / dblB2 - (Xmix[2 * intSumCount + 2, 0] - Xmix[2 * intSumCount + 0, 0]) / dblA2;
}
else
{
intPreTrueNum += 1;
}
if (cptlt[intUnKnownCount3orginal + intKnownCount3orginal + 2].isCtrl == false)
{
//X3,Y3的导数值
A[intUnknownLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 4] = -(Xmix[2 * intSumCount + 5, 0] - Xmix[2 * intSumCount + 3, 0]) / dblB2;
A[intUnknownLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 5] = (Xmix[2 * intSumCount + 4, 0] - Xmix[2 * intSumCount + 2, 0]) / dblB2;
}
}
}
//一个中间值
VBMatrix InvBQLLBt = new VBMatrix(intUnknownLengthAngle, intUnknownLengthAngle);
for (int i = 0; i < intUnknownLengthAngle; i++) //注意：此处之所以可以这样计算是因为B矩阵是元素为-1的对角矩阵，且QLL是对角矩阵
{
InvBQLLBt[i, i] = 1 / (QLL[i, i] * QLL[i, i]);
}
//计算Q22
VBMatrix TempQ22 = A.Trans() * InvBQLLBt * A;
VBMatrix NegQ22 = TempQ22.Inv(TempQ22);
VBMatrix Q22 = new VBMatrix(intUnknownXY, intUnknownXY);
for (int i = 0; i < intUnknownXY; i++)
{
for (int j = 0; j < intUnknownXY; j++)
{
Q22[i, j] = -NegQ22[i, j];
}
}
//计算Q12
VBMatrix NegQ12 = InvBQLLBt * A * Q22;
VBMatrix Q12 = new VBMatrix(intUnknownLengthAngle, intUnknownXY);
for (int i = 0; i < intUnknownLengthAngle; i++)
{
for (int j = 0; j < intUnknownXY; j++)
{
Q12[i, j] = -NegQ12[i, j];
}
}
//计算Q21
VBMatrix Q21 = Q12.Trans();
//计算Q11********************************************************************
VBMatrix I = new VBMatrix(intUnknownLengthAngle, intUnknownLengthAngle);
for (int i = 0; i < intUnknownLengthAngle; i++)
{
I[i, i] = 1;
}
VBMatrix Q11 = InvBQLLBt * (I - A * Q21);
//计算负v
VBMatrix Negv = QLL * B.Trans() * Q11 * w;
//计算负x
VBMatrix Negx = Q21 * w;
//对观测值进行改正
int intSumCountL4 = 0;
for (int i = 0; i < intUnknownLength; i++) //长度近似值部分
{
if (cptlt[intSumCountL4].isCtrl == false || cptlt[intSumCountL4 + 1].isCtrl == false)
{
adblLength[intSumCountL4] = adblLength[intSumCountL4] - Negv[i, 0];
}
else
{
i -= 1;
}
intSumCountL4 += 1;
}
int intSumCountA4 = 0;
for (int i = intUnknownLength; i < intUnknownLengthAngle; i++) //角度近似值部分
{
if (cptlt[intSumCountA4].isCtrl == false || cptlt[intSumCountA4 + 1].isCtrl == false || cptlt[intSumCountA4 + 2].isCtrl == false)
{
adblAngle[intSumCountA4] = adblAngle[intSumCountA4] - Negv[i, 0];
}
else
{
i -= 1;
}
intSumCountA4 += 1;
}
//对坐标值进行改正
int intSumCount5 = 0;
for (int i = 0; i < intUnknownPt; i++)
{
if (cptlt[intSumCount5].isCtrl == false)
{
Xmix[intSumCount5 * 2, 0] = Xmix[intSumCount5 * 2, 0] - Negx[i * 2, 0];
Xmix[intSumCount5 * 2 + 1, 0] = Xmix[intSumCount5 * 2 + 1, 0] - Negx[i * 2 + 1, 0];
}
else
{
i -= 1;
}
intSumCount5 += 1;
}
if (intIterativeCount == 50)
{
int kk = 5;
}
intIterativeCount += 1;
if (intIterativeCount >= 1000)
{
break;
}
//这里只是随便取两个中间值以观测是否收敛
dblJudge1 = Math.Abs(Negx[intJudgeIndex, 0]);
dblJudge2 = Math.Abs(Negx[3 * intJudgeIndex, 0]);
} while ((dblJudge1 > dblTX) || (dblJudge2 > dblTX));
//生成目标线段
List <CPoint> CTargetPtLt = new List <CPoint>();
for (int i = 0; i < intPtNum; i++)
{
CPoint cpt = new CPoint(i);
cpt.X = Xmix[2 * i, 0];
cpt.Y = Xmix[2 * i + 1, 0];
CTargetPtLt.Add(cpt);
}
CPolyline cpl = new CPolyline(0, CTargetPtLt);
return(cpl);
}
/// <summary>
/// 计算Inv(A'*P*A)*A'*P*matl，时间复杂度（2n）^2，n为顶点个数
/// </summary>
/// <param name="A">系数矩阵：该矩阵从第0行到第i行为对角矩阵，i行之后为全矩阵</param>
/// <param name="P">权重矩阵：对角矩阵</param>
/// <param name="matl">初值矩阵：列向量</param>
/// <returns>鉴于本实验矩阵的特殊性，可以使用该函数进行计算。</returns>
public VBMatrix InvAtPAAtPmatl(VBMatrix A, VBMatrix P, VBMatrix matl)
{
int intRowA = A.Row;
int intColA = A.Col;
int intDiffRowColA = intRowA - intColA;
//计算A'*P
VBMatrix MatrixAtP = new VBMatrix(intColA, intRowA);
for (int i = 0; i < intColA; i++)
{
//第i行i列的值
MatrixAtP[i, i] = A[i, i] * P[i, i];
//第i行最后intDiffRowColA列的值
for (int j = intColA; j < intRowA; j++)
{
MatrixAtP[i, j] = A[j, i] * P[j, j];
}
}
//计算A'*P*A
VBMatrix MatrixAtPA = new VBMatrix(intColA, intColA);
for (int i = 0; i < intColA; i++)
{
//第i行i列的值(即对角线上的值)
double dblValue = MatrixAtP[i, i] * A[i, i];
for (int j = intColA; j < intRowA; j++)
{
dblValue += MatrixAtP[i, j] * A[j, i];
}
MatrixAtPA[i, i] = dblValue;
//第i行j列和j行i列的值(注意：MatrixAtPA[i, j] == MatrixAtPA[j, i])
for (int j = i + 1; j < intColA; j++)
{
for (int k = 0; k < intDiffRowColA; k++)
{
MatrixAtPA[i, j] += MatrixAtP[i, intColA + k] * A[intColA + k, j];
}
MatrixAtPA[j, i] = MatrixAtPA[i, j];
}
}
//计算A'*P*matl
VBMatrix MatrixAtPmatl = new VBMatrix(intColA, 1);
for (int i = 0; i < intColA; i++)
{
double dblValue = MatrixAtP[i, i] * matl[i, 0];
for (int j = intColA; j < intRowA; j++)
{
dblValue += MatrixAtP[i, j] * matl[j, 0];
}
MatrixAtPmatl[i, 0] = dblValue;
}
//计算Inv(A'*P*A)*A'*P*matl
MatrixAtPA.Inv(MatrixAtPA);
VBMatrix MatrixResult = MatrixAtPA * MatrixAtPmatl;
return(MatrixResult);
}
/// <summary>
/// 获取线状要素
/// </summary>
/// <param name="pDataRecords">数据记录</param>
/// <param name="dblProp">插值参数</param>
/// <returns>在处理面状要素时，本程序将原面状要素的边界切开，按线状要素处理，处理完后再重新生成面状要素</returns>
public CPolyline GetTargetcpl(double dblProp, CPolyline lastcpl)
{
if (dblProp == 0)
{
int aa = 5;
}
List <CCorrCpts> pCorrCptsLt = _DataRecords.ParameterResult.CCorrCptsLt; //读取数据后，此处ResultPtLt中的对应点为一一对应
double dblTX = _dblTX;
int intPtNum = pCorrCptsLt.Count;
int intXYNum = 2 * intPtNum;
//计算长度初始值（全部计算）
double[] adblLength0 = new double[intPtNum - 1];
double[] adblFrLength0 = new double[intPtNum - 1];
double[] adblToLength0 = new double[intPtNum - 1];
for (int i = 0; i < pCorrCptsLt.Count - 1; i++)
{
double dblfrsublength = CGeoFunc.CalDis(pCorrCptsLt[i + 1].FrCpt, pCorrCptsLt[i].FrCpt);
adblFrLength0[i] = dblfrsublength;
double dbltosublength = CGeoFunc.CalDis(pCorrCptsLt[i + 1].ToCpt, pCorrCptsLt[i].ToCpt);
adblToLength0[i] = dbltosublength;
adblLength0[i] = (1 - dblProp) * dblfrsublength + dblProp * dbltosublength;
}
//计算角度初始值（全部计算）
double[] adblAngle0 = new double[intPtNum - 2];
for (int i = 0; i < pCorrCptsLt.Count - 2; i++)
{
//较大比例尺线状要素上的夹角
double dblfrAngle = CGeoFunc.CalAngle_Counterclockwise(pCorrCptsLt[i].FrCpt, pCorrCptsLt[i + 1].FrCpt, pCorrCptsLt[i + 2].FrCpt);
//较小比例尺线状要素上的夹角
double dbltoAngle = CGeoFunc.CalAngle_Counterclockwise(pCorrCptsLt[i].ToCpt, pCorrCptsLt[i + 1].ToCpt, pCorrCptsLt[i + 2].ToCpt);
//角度初始值
adblAngle0[i] = (1 - dblProp) * dblfrAngle + dblProp * dbltoAngle;
}
//计算坐标初始值，以及各线段方位角初始值
//注意：默认固定第一条边
pCorrCptsLt[0].FrCpt.isCtrl = true;
pCorrCptsLt[1].FrCpt.isCtrl = true;
//固定最后两条边
pCorrCptsLt[intPtNum - 1].FrCpt.isCtrl = true;
pCorrCptsLt[intPtNum - 2].FrCpt.isCtrl = true;
VBMatrix X0 = new VBMatrix(intXYNum, 1);
//以上一次结果的值作为新的估算值
List <CPoint> lastcptlt = lastcpl.CptLt;
for (int i = 0; i < intPtNum; i++)
{
if (pCorrCptsLt[i].FrCpt.isCtrl == false)
{
X0[2 * i + 0, 0] = lastcptlt[i].X;
X0[2 * i + 1, 0] = lastcptlt[i].Y;
}
else
{
X0[2 * i + 0, 0] = (1 - dblProp) * pCorrCptsLt[i].FrCpt.X + dblProp * pCorrCptsLt[i].ToCpt.X;
X0[2 * i + 1, 0] = (1 - dblProp) * pCorrCptsLt[i].FrCpt.Y + dblProp * pCorrCptsLt[i].ToCpt.Y;
}
}
//统计插值点数
int intKnownPt = 0; //固定点的数目
int intUnknownPt = 0; //非固定点的数目
List <int> intKnownLocationLt = new List <int>(); //记录已知点的序号
//注意：对于该循环，有一个默认条件，即FromCpl的第一个顶点只有一个对应点
for (int i = 0; i < pCorrCptsLt.Count; i++)
{
if (pCorrCptsLt[i].FrCpt.isCtrl == true)
{
intKnownLocationLt.Add(i);
intKnownPt += 1;
}
else
{
intUnknownPt += 1;
}
}
int intUnknownXY = intUnknownPt * 2; //每个点都有X、Y坐标
//找出长度固定的位置(如果一个线段的前后两个点都固定，则该长度固定)。另外，长度固定则该边的方位角也固定
List <int> intKnownLengthLt = new List <int>();
for (int i = 0; i < intKnownLocationLt.Count - 1; i++)
{
if ((intKnownLocationLt[i + 1] - intKnownLocationLt[i]) == 1)
{
intKnownLengthLt.Add(intKnownLocationLt[i]);
}
}
int intUnknownLength = intPtNum - 1 - intKnownLengthLt.Count;
//找出角度固定的位置(如果一个固定顶点的前后两个点都固定，则该角度固定)
List <int> intKnownAngleLt = new List <int>();
for (int i = 0; i < intKnownLocationLt.Count - 2; i++)
{
if ((intKnownLocationLt[i + 1] - intKnownLocationLt[i]) == 1 && (intKnownLocationLt[i + 2] - intKnownLocationLt[i + 1]) == 1)
{
intKnownAngleLt.Add(intKnownLocationLt[i]);
}
}
int intUnknownAngle = intPtNum - 2 - intKnownAngleLt.Count;
//长度及角度未知量（方便使用）
int intUnknownLengthAngle = intUnknownLength + intUnknownAngle;
//总未知量
int intUnknownLengthAnglePt = intUnknownLength + intUnknownAngle + intUnknownPt;
//定义权重矩阵
int intKnownCount = 0;
int intUnKnownCount = 0;
VBMatrix P = new VBMatrix(intUnknownLengthAnglePt, intUnknownLengthAnglePt);
for (int i = 0; i < intUnknownLength; i++)
{
P[i, i] = 1;
}
for (int i = 0; i < intUnknownAngle; i++)
{
int intSumCount = intKnownCount + intUnKnownCount;
//开始计算系数值，由于将以下三个情况排列组合将有八种情况，因此按如下方式计算
if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 2].FrCpt.isCtrl == true)
{
i -= 1;
}
else
{
double dblWeight = 0;
if (pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == false)
{
dblWeight = adblFrLength0[intSumCount] + adblFrLength0[intSumCount + 1] + adblToLength0[intSumCount] + adblToLength0[intSumCount + 1];
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == false && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 2].FrCpt.isCtrl == false)
{
dblWeight = adblFrLength0[intSumCount] + adblFrLength0[intSumCount + 1] + adblToLength0[intSumCount] + adblToLength0[intSumCount + 1];
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 2].FrCpt.isCtrl == false)
{
dblWeight = adblFrLength0[intSumCount + 1] + adblToLength0[intSumCount + 1];
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == false && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 2].FrCpt.isCtrl == true)
{
dblWeight = adblFrLength0[intSumCount] + adblToLength0[intSumCount];
}
P[intUnknownLength + i, intUnknownLength + i] = dblWeight;
}
if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == true)
{
intKnownCount += 1;
}
else
{
intUnKnownCount += 1;
}
}
for (int i = 0; i < intUnknownPt; i++)
{
P[intUnknownLengthAngle + i, intUnknownLengthAngle + i] = 0.000000000000001;
}
//定义坐标近似值矩阵XA
VBMatrix XA = new VBMatrix(intUnknownXY, 1);
VBMatrix XA0 = new VBMatrix(intUnknownXY, 1);
int intSumCount0 = 0;
for (int i = 0; i < intUnknownPt; i++)
{
if (pCorrCptsLt[intSumCount0].FrCpt.isCtrl == false)
{
XA0[i * 2 + 0, 0] = X0[intSumCount0 * 2 + 0, 0];
XA0[i * 2 + 1, 0] = X0[intSumCount0 * 2 + 1, 0];
XA[i * 2 + 0, 0] = XA0[i * 2 + 0, 0] - 0.0000000001;
XA[i * 2 + 1, 0] = XA0[i * 2 + 1, 0] - 0.0000000001;
}
else
{
i -= 1;
}
intSumCount0 += 1;
}
//Xmix里存储了XA和X0的最新混合值（此矩阵在公式推导中并不存在，只是为了方便编写代码而建立）
VBMatrix Xmix = new VBMatrix(intXYNum, 1);
for (int i = 0; i < intXYNum; i++)
{
Xmix[i, 0] = X0[i, 0];
}
//近似值与观测值之差matl，平差中的-l
VBMatrix matl = new VBMatrix(intUnknownLengthAnglePt, 1);
//定义系数矩阵A(各方程对坐标的导数值)，A的导数值将在循环中给出
VBMatrix A = new VBMatrix(intUnknownLengthAnglePt, intUnknownXY);
double dblJudge1 = 0; //该值用于判断是否应该跳出循环
double dblJudge2 = 0; //该值用于判断是否应该跳出循环
int intJudgeIndex = intUnknownLength / 4;
int intIterativeCount = 0;
double[] adblSubDis = new double[intPtNum - 1];
double[] adblAngle = new double[intPtNum - 2];
double[] adblAzimuth = new double[intPtNum - 1];
do
{
//计算系数矩阵A第0行到"intUnknownLength"行的各元素，即线段长度对各未知数求偏导的值
for (int i = 0; i < intPtNum - 1; i++)
{
adblSubDis[i] = Math.Pow((Xmix[2 * i, 0] - Xmix[2 * i + 2, 0]) * (Xmix[2 * i, 0] - Xmix[2 * i + 2, 0]) + (Xmix[2 * i + 1, 0] - Xmix[2 * i + 3, 0]) * (Xmix[2 * i + 1, 0] - Xmix[2 * i + 3, 0]), 0.5);
}
//计算新的方位角
adblAzimuth[0] = CGeoFunc.CalAxisAngle(Xmix[0, 0], Xmix[1, 0], Xmix[2, 0], Xmix[3, 0]);
for (int i = 1; i < intPtNum - 1; i++)
{
adblAngle[i - 1] = CGeoFunc.CalAngle_Counterclockwise(Xmix[i * 2 - 2, 0], Xmix[i * 2 - 1, 0], Xmix[i * 2, 0], Xmix[i * 2 + 1, 0], Xmix[i * 2 + 2, 0], Xmix[i * 2 + 3, 0]);
adblAzimuth[i] = adblAzimuth[i - 1] + adblAngle[i - 1] - Math.PI;
}
//计算系数矩阵中关于长度值的导数部分
_pCAL.CalADevLength(pCorrCptsLt, 0, intUnknownLength, ref A, ref matl, adblSubDis, adblAzimuth, adblLength0);
//计算系数矩阵中关于夹角值的导数部分
_pCAL.CalADevAngle(pCorrCptsLt, intUnknownLength, intUnknownAngle, Xmix, ref A, ref matl, adblSubDis, adblAngle, adblAngle0);
//计算系数矩阵中关于与上一个LSA结果坐标距离的导数部分
for (int i = 0; i < intUnknownPt; i++)
{
//平差后的点与估计值点之间的距离
double dblDis = Math.Pow((XA[2 * i, 0] - XA0[2 * i, 0]) * (XA[2 * i, 0] - XA0[2 * i, 0]) + (XA[2 * i + 1, 0] - XA0[2 * i + 1, 0]) * (XA[2 * i + 1, 0] - XA0[2 * i + 1, 0]), 0.5);
A[intUnknownLengthAngle + i, i * 2 + 0] = (XA[2 * i + 0, 0] - XA0[2 * i + 0, 0]) / dblDis;
A[intUnknownLengthAngle + i, i * 2 + 1] = (XA[2 * i + 1, 0] - XA0[2 * i + 1, 0]) / dblDis;
//if (dblDis==0)
//{
// //导数值
// A[intUnknownLengthAngle + i, i * 2 + 0] = 0;
// A[intUnknownLengthAngle + i, i * 2 + 1] = 0;
//}
//else
//{
// //导数值
// A[intUnknownLengthAngle + i, i * 2 + 0] = (XA[2 * i + 0, 0] - XA0[2 * i + 0, 0]) / dblDis;
// A[intUnknownLengthAngle + i, i * 2 + 1] = (XA[2 * i + 1, 0] - XA0[2 * i + 1, 0]) / dblDis;
//}
matl[intUnknownLengthAngle + i, 0] = -dblDis; //图方便，顺便计算matl
}
//CHelpFuncExcel.ExportDataToExcel2(A, "maxA", _DataRecords.ParameterInitialize.strSavePath);
//CHelpFuncExcel.ExportDataToExcelP(P, "maxP", _DataRecords.ParameterInitialize.strSavePath);
//CHelpFuncExcel.ExportDataToExcel2(matl, "maxmatl", _DataRecords.ParameterInitialize.strSavePath);
//平差（阻尼最小二乘法）
VBMatrix Temp = A.Trans() * P * A;
//VBMatrix E = new VBMatrix(intUnknownXY, intUnknownXY); //单位矩阵
//for (int i = 0; i < intUnknownXY; i++)
//{
// E[i, i] = 1;
//}
//Temp = Temp + E;
VBMatrix InvTemp = Temp.Inv(Temp);
VBMatrix x = InvTemp * A.Trans() * P * matl;
//CHelpFuncExcel.ExportDataToExcel2(x, "maxX", _DataRecords.ParameterInitialize.strSavePath);
//CHelpFuncExcel.ExportDataToExcel2(XA, "maxXA", _DataRecords.ParameterInitialize.strSavePath);
XA += x;
//对坐标值进行改正
int intSumCount5 = 0;
for (int i = 0; i < intUnknownPt; i++)
{
if (pCorrCptsLt[intSumCount5].FrCpt.isCtrl == false)
{
Xmix[intSumCount5 * 2 + 0, 0] = XA[i * 2 + 0, 0];
Xmix[intSumCount5 * 2 + 1, 0] = XA[i * 2 + 1, 0];
}
else
{
i -= 1;
}
intSumCount5 += 1;
}
if (intIterativeCount == 50)
{
int kk = 5;
}
intIterativeCount += 1;
if (intIterativeCount >= 20)
{
break;
}
//这里只是随便取两个中间值以观测是否收敛
dblJudge1 = Math.Abs(x[intJudgeIndex, 0]);
dblJudge2 = Math.Abs(x[3 * intJudgeIndex, 0]);
} while ((dblJudge1 > dblTX) || (dblJudge2 > dblTX));
//生成目标线段
List <CPoint> CTargetPtLt = new List <CPoint>();
for (int i = 0; i < intPtNum; i++)
{
CPoint cpt = new CPoint(i);
cpt.X = Xmix[2 * i, 0];
cpt.Y = Xmix[2 * i + 1, 0];
if (pCorrCptsLt[i].FrCpt.isCtrl == true)
{
cpt.isCtrl = true;
}
else
{
cpt.isCtrl = false;
}
CTargetPtLt.Add(cpt);
}
CPolyline cpl = new CPolyline(0, CTargetPtLt);
cpl.CreateSubPllt();
//记录各平差成果
//坐标改正值
VBMatrix Xc = XA - XA0;
//观测值改正值矩阵V
VBMatrix V = A * Xc + matl;
//VtPV值
cpl.dblVtPV = (V.Trans() * P * V).MatData[0, 0];
int intUnKnownCountL6 = 0;
for (int i = 0; i < intPtNum - 1; i++)
{
if (pCorrCptsLt[i].FrCpt.isCtrl == false || pCorrCptsLt[i + 1].FrCpt.isCtrl == false)
{
cpl.SubCPlLt[i].dblLengthV = cpl.SubCPlLt[i].pPolyline.Length - adblLength0[i];
//double dblLength = cpl.SubCPlLt[i].Length;
//double dblLength0 = adblLength0[i];
intUnKnownCountL6 += 1;
}
else
{
cpl.SubCPlLt[i].dblLengthV = 0;
}
}
int intUnKnownCountA6 = 0;
for (int i = 0; i < intPtNum - 2; i++)
{
if (pCorrCptsLt[i].FrCpt.isCtrl == false || pCorrCptsLt[i + 1].FrCpt.isCtrl == false || pCorrCptsLt[i + 2].FrCpt.isCtrl == false)
{
double dblAngle = CGeoFunc.CalAngle_Counterclockwise(cpl.CptLt[i], cpl.CptLt[i + 1], cpl.CptLt[i + 2]);
cpl.CptLt[i + 1].dblAngleV = dblAngle - adblAngle0[i];
//double dblAngle = CGeoFunc.CalAngle_Counterclockwise(cpl.CptLt[i], cpl.CptLt[i + 1], cpl.CptLt[i + 2]);
//cpl.CptLt[i + 1].dblAngleV = V[intUnknownLength + intUnKnownCountA6, 0];
intUnKnownCountA6 += 1;
}
else
{
cpl.CptLt[i + 1].dblAngleV = 0;
}
}
return(cpl);
}
/// <summary>
/// 获取线状要素
/// </summary>
/// <param name="intInterNum">Inter: Interpolation</param>
/// <returns>在处理面状要素时，本程序将原面状要素的边界切开，按线状要素处理，处理完后再重新生成面状要素</returns>
public List<CPolyline> GetTargetcpllt()
{
int intIterationNum = Convert.ToInt32(_DataRecords.ParameterInitialize.txtIterationNum.Text);
int intInterNum = Convert.ToInt32(_DataRecords.ParameterInitialize.txtInterpolationNum.Text);
List<CCorrCpts> pCorrCptsLt = _DataRecords.ParameterResult.CCorrCptsLt; //Read Datasets后，此处ResultPtLt中的对应点为一一对应
double dblTX = _dblTX;
double dblInterval = 1 / Convert.ToDouble(intInterNum + 1);
int intPtNum = pCorrCptsLt.Count;
int intXYNum = intPtNum * 2;
int intMultiXYNum = intInterNum * intXYNum;
//计算长度初始值（全部计算）
double[,] adblLength0 = new double[intInterNum, intPtNum - 1];
double[,] adblLength = new double[intInterNum, intPtNum - 1]; //顺便定义中间值数组
for (int j = 0; j < pCorrCptsLt.Count - 1; j++)
{
double dblfrsublength = CGeoFunc.CalDis(pCorrCptsLt[j + 1].FrCpt, pCorrCptsLt[j].FrCpt);
double dbltosublength = CGeoFunc.CalDis(pCorrCptsLt[j + 1].ToCpt, pCorrCptsLt[j].ToCpt);
for (int i = 0; i < intInterNum; i++)
{
double dblProp = (i+1) * dblInterval;
adblLength0[i, j] = (1 - dblProp) * dblfrsublength + dblProp * dbltosublength;
}
//pCorrCptsLt[j].FrCpt.isCtrl = false; //仅以最开始两对和最终两队对应点为固定点，故此先设置为false
}
//计算角度初始值（全部计算）
double[,] adblAngle0 = new double[intInterNum, intPtNum - 2];
double[,] adblAngle = new double[intInterNum, intPtNum - 2];
for (int j = 0; j < pCorrCptsLt.Count - 2; j++)
{
//较大比例尺线状要素上的夹角
double dblfrAngle = CGeoFunc.CalAngle_Counterclockwise(pCorrCptsLt[j].FrCpt, pCorrCptsLt[j + 1].FrCpt, pCorrCptsLt[j + 2].FrCpt);
//较小比例尺线状要素上的夹角
double dbltoAngle = CGeoFunc.CalAngle_Counterclockwise(pCorrCptsLt[j].ToCpt, pCorrCptsLt[j + 1].ToCpt, pCorrCptsLt[j + 2].ToCpt);
//角度初始值
for (int i = 0; i < intInterNum; i++)
{
double dblProp = (i + 1) * dblInterval;
adblAngle0[i, j] = (1 - dblProp) * dblfrAngle + dblProp * dbltoAngle;
}
}
//多线段间对应顶点距离初始值（全部计算），全部设置为0
//目标值也设置为0
double[,] adblIntervalDis0 = new double[intInterNum + 1, intPtNum];
double[,] adblIntervalDis = new double[intInterNum + 1, intPtNum]; //顺便定义中间值数组
for (int j = 0; j < pCorrCptsLt.Count; j++)
{
//长度初始值
for (int i = 0; i <= intInterNum; i++)
{
adblIntervalDis0[i, j] = 0;
adblIntervalDis[i, j] = 0;
}
}
//计算坐标初始值，以及各线段方位角初始值
//注意：默认固定第一条边
pCorrCptsLt[0].FrCpt.isCtrl = true;
pCorrCptsLt[1].FrCpt.isCtrl = true;
VBMatrix X0 = new VBMatrix(intMultiXYNum, 1);
double[,] adblAzimuth = new double[intInterNum, intPtNum - 1];
for (int i = 0; i < intInterNum; i++)
{
double dblProp = (i + 1) * dblInterval;
double dblnewX0 = (1 - dblProp) * pCorrCptsLt[0].FrCpt.X + dblProp * pCorrCptsLt[0].ToCpt.X;
double dblnewY0 = (1 - dblProp) * pCorrCptsLt[0].FrCpt.Y + dblProp * pCorrCptsLt[0].ToCpt.Y;
double dblnewX1 = (1 - dblProp) * pCorrCptsLt[1].FrCpt.X + dblProp * pCorrCptsLt[1].ToCpt.X;
double dblnewY1 = (1 - dblProp) * pCorrCptsLt[1].FrCpt.Y + dblProp * pCorrCptsLt[1].ToCpt.Y;
adblAzimuth[i, 0] = CGeoFunc.CalAxisAngle(dblnewX0, dblnewY0, dblnewX1, dblnewY1);
int intBasicIndex = i * intXYNum;
X0[intBasicIndex + 0, 0] = dblnewX0;
X0[intBasicIndex + 1, 0] = dblnewY0;
X0[intBasicIndex + 2, 0] = dblnewX1;
X0[intBasicIndex + 3, 0] = dblnewY1;
}
//其它点
//是否固定最后两个点
pCorrCptsLt[pCorrCptsLt.Count - 2].FrCpt.isCtrl = true;
pCorrCptsLt[pCorrCptsLt.Count - 1].FrCpt.isCtrl = true;
for (int j = 2; j < intPtNum; j++)
{
for (int i = 0; i < intInterNum; i++)
{
int intBasicIndexIJ = i * intXYNum + 2 * j;
double dblProp = (i + 1) * dblInterval;
X0[intBasicIndexIJ + 0, 0] = (1 - dblProp) * pCorrCptsLt[j].FrCpt.X + dblProp * pCorrCptsLt[j].ToCpt.X;
X0[intBasicIndexIJ + 1, 0] = (1 - dblProp) * pCorrCptsLt[j].FrCpt.Y + dblProp * pCorrCptsLt[j].ToCpt.Y;
double dblAngle = CGeoFunc.CalAngle_Counterclockwise(X0[intBasicIndexIJ - 4, 0], X0[intBasicIndexIJ - 3, 0], X0[intBasicIndexIJ - 2, 0], X0[intBasicIndexIJ - 1, 0], X0[intBasicIndexIJ - 0, 0], X0[intBasicIndexIJ + 1, 0]); //计算实际夹角
adblAzimuth[i, j - 1] = adblAzimuth[i, j - 2] + dblAngle - Math.PI;
}
}
//for (int j = 2; j < intPtNum; j++)
//{
// if (pCorrCptsLt[j].FrCpt.isCtrl == false)
// {
// for (int i = 0; i < intInterNum; i++)
// {
// int intBasicIndexI = i * intXYNum;
// adblAzimuth[i, j - 1] = adblAzimuth[i, j - 2] + adblAngle0[i, j - 2] - Math.PI;
// X0[intBasicIndexI + 2 * j + 0, 0] = X0[intBasicIndexI + 2 * (j - 1) + 0, 0] + adblLength0[i, j - 1] * Math.Cos(adblAzimuth[i, j - 1]);
// X0[intBasicIndexI + 2 * j + 1, 0] = X0[intBasicIndexI + 2 * (j - 1) + 1, 0] + adblLength0[i, j - 1] * Math.Sin(adblAzimuth[i, j - 1]);
// }
// }
// else
// {
// for (int i = 0; i < intInterNum; i++)
// {
// int intBasicIndexIJ = i * intXYNum + 2 * j;
// double dblProp = (i + 1) * dblInterval;
// X0[intBasicIndexIJ + 0, 0] = (1 - dblProp) * pCorrCptsLt[j].FrCpt.X + dblProp * pCorrCptsLt[j].ToCpt.X;
// X0[intBasicIndexIJ + 1, 0] = (1 - dblProp) * pCorrCptsLt[j].FrCpt.Y + dblProp * pCorrCptsLt[j].ToCpt.Y;
// double dblAngle = CGeoFunc.CalAngle_Counterclockwise(X0[intBasicIndexIJ - 4, 0], X0[intBasicIndexIJ - 3, 0], X0[intBasicIndexIJ - 2, 0], X0[intBasicIndexIJ - 1, 0], X0[intBasicIndexIJ - 0, 0], X0[intBasicIndexIJ + 1, 0]); //计算实际夹角
// adblAzimuth[i, j - 1] = adblAzimuth[i, j - 2] + dblAngle - Math.PI;
// }
// }
//}
//统计插值点数
int intKnownPt = 0; //固定点的数目
int intUnknownPt = 0; //非固定点的数目
List<int> intKnownLocationLt = new List<int>(); //记录已知点的序号
//注意：对于该循环，有一个默认条件，即FromCpl的第一个顶点只有一个对应点
for (int i = 0; i < intPtNum; i++)
{
if (pCorrCptsLt[i].FrCpt.isCtrl == true)
{
intKnownLocationLt.Add(i);
intKnownPt += 1;
}
else
{
intUnknownPt += 1;
}
}
int intUnknownXY = intUnknownPt * 2; //每个点都有X、Y坐标
int intMultiUnknownXY = intInterNum * intUnknownXY;
//找出长度固定的位置(如果一个线段的前后两个点都固定，则该长度固定)。另外，长度固定则该边的方位角也固定
List<int> intKnownLengthLt = new List<int>();
for (int i = 0; i < intKnownLocationLt.Count - 1; i++)
{
if ((intKnownLocationLt[i + 1] - intKnownLocationLt[i]) == 1)
{
intKnownLengthLt.Add(intKnownLocationLt[i]);
}
}
int intUnknownLength = intPtNum - 1 - intKnownLengthLt.Count;
//找出角度固定的位置(如果一个固定顶点的前后两个点都固定，则该角度固定)
List<int> intKnownAngleLt = new List<int>();
for (int i = 0; i < intKnownLocationLt.Count - 2; i++)
{
if ((intKnownLocationLt[i + 1] - intKnownLocationLt[i]) == 1 && (intKnownLocationLt[i + 2] - intKnownLocationLt[i + 1]) == 1)
{
intKnownAngleLt.Add(intKnownLocationLt[i]);
}
}
int intUnknownAngle = intPtNum - 2 - intKnownAngleLt.Count;
//长度角度未知量
int intUnknownLengthAngle = intUnknownLength + intUnknownAngle;
//各未知量个数
int intMultiUnknownLength = intInterNum * intUnknownLength;
int intMultiUnknownAngle = intInterNum * intUnknownAngle;
int intMultiUnknownLengthAngle = intMultiUnknownLength + intMultiUnknownAngle;
int intMultiUnknownInterval = (intInterNum + 1) * intUnknownPt;
int intSumConstraints = intMultiUnknownLength + intMultiUnknownAngle + intMultiUnknownInterval;
//定义权重矩阵
VBMatrix P = new VBMatrix(intSumConstraints, intSumConstraints);
for (int i = 0; i < intMultiUnknownLength; i++) //长度权重
{
P[i, i] = 1;
}
for (int i = 0; i < intMultiUnknownAngle; i++) //角度权重
{
P[intMultiUnknownLength + i, intMultiUnknownLength + i] = 40;
}
for (int i = 0; i < intMultiUnknownInterval; i++) //角度权重
{
P[intMultiUnknownLengthAngle + i, intMultiUnknownLengthAngle + i] = 10000000000000;
}
//for (int i = 0; i < intInterNum; i++)
//{
// int intBasicIndex2 = i * intUnknownLengthAngle;
// for (int j = 0; j < intUnknownLength; j++)
// {
// P[intBasicIndex2 + j, intBasicIndex2 + j] = 1;
// }
// for (int j = 0; j < intUnknownAngle; j++)
// {
// P[intBasicIndex2 + intUnknownLength + j, intBasicIndex2 + intUnknownLength + j] = 1;
// }
//}
//Xmix里存储了XA和X0的最新混合值（此矩阵在公式推导中并不存在，只是为了方便编写代码而建立）
VBMatrix Xmix = new VBMatrix(intMultiXYNum, 1);
for (int i = 0; i < intMultiXYNum; i++)
{
Xmix[i, 0] = X0[i, 0];
}
//定义坐标近似值矩阵XA
VBMatrix XA = new VBMatrix(intMultiUnknownXY, 1);
VBMatrix XA0 = new VBMatrix(intMultiUnknownXY, 1);
int intSumCount0 = 0;
for (int j = 0; j < intUnknownPt; j++)
{
if (pCorrCptsLt[intSumCount0].FrCpt.isCtrl == false)
{
for (int i = 0; i < intInterNum; i++)
{
XA[i * intUnknownXY + j * 2 + 0, 0] = X0[i * intXYNum + intSumCount0 * 2 + 0, 0];
XA[i * intUnknownXY + j * 2 + 1, 0] = X0[i * intXYNum + intSumCount0 * 2 + 1, 0];
XA0[i * intUnknownXY + j * 2 + 0, 0] = XA[i * intUnknownXY + j * 2 + 0, 0];
XA0[i * intUnknownXY + j * 2 + 1, 0] = XA[i * intUnknownXY + j * 2 + 1, 0];
}
}
else
{
j -= 1;
}
intSumCount0 += 1;
}
//定义系数矩阵A(各方程对坐标的导数值)，A的导数值将在循环中给出
VBMatrix A = new VBMatrix(intSumConstraints, intMultiUnknownXY);
double dblJudge1 = 0; //该值用于判断是否应该跳出循环
double dblJudge2 = 0; //该值用于判断是否应该跳出循环
double dblJudge3 = 0; //该值用于判断是否应该跳出循环
int intJudgeIndex = intMultiUnknownXY / 4;
int intIterativeCount = 0;
for (int k = 0; k < 2; k++)
{
//break;
do
{
if (intIterativeCount >= intIterationNum)
{
break;
}
intIterativeCount += 1;
VBMatrix matl = new VBMatrix(intSumConstraints, 1);
//计算系数矩阵A第0行到"intUnknownLength"行的各元素，即线段长度对各未知数求偏导的值
//先计算各分母值（注意：分母实际上是求偏导后的一部分值，但却恰好等于两点之间距离，因此其计算公式与距离计算公式相同
for (int i = 0; i < intInterNum; i++)
{
int intBasicIndexS1 = i * intXYNum;
for (int j = 0; j < intPtNum - 1; j++)
{
int intBasicIndexIJS1 = intBasicIndexS1 + 2 * j;
adblLength[i, j] = Math.Sqrt((Xmix[intBasicIndexIJS1 + 0, 0] - Xmix[intBasicIndexIJS1 + 2, 0]) * (Xmix[intBasicIndexIJS1 + 0, 0] - Xmix[intBasicIndexIJS1 + 2, 0]) +
(Xmix[intBasicIndexIJS1 + 1, 0] - Xmix[intBasicIndexIJS1 + 3, 0]) * (Xmix[intBasicIndexIJS1 + 1, 0] - Xmix[intBasicIndexIJS1 + 3, 0]));
}
}
//计算新的方位角
for (int i = 0; i < intInterNum; i++)
{
int intBasicIndexA1 = i * intXYNum;
//第一条线段的方位角
adblAzimuth[i, 0] = CGeoFunc.CalAxisAngle(Xmix[intBasicIndexA1 + 0, 0], Xmix[intBasicIndexA1 + 1, 0], Xmix[intBasicIndexA1 + 2, 0], Xmix[intBasicIndexA1 + 3, 0]);
//后面线段的方位角
for (int j = 1; j < intPtNum - 1; j++)
{
int intBasicIndexIJA1 = intBasicIndexA1 + 2 * j;
double dblAngle = CGeoFunc.CalAngle_Counterclockwise(Xmix[intBasicIndexIJA1 - 2, 0], Xmix[intBasicIndexIJA1 - 1, 0],
Xmix[intBasicIndexIJA1 + 0, 0], Xmix[intBasicIndexIJA1 + 1, 0],
Xmix[intBasicIndexIJA1 + 2, 0], Xmix[intBasicIndexIJA1 + 3, 0]);
adblAzimuth[i, j] = adblAzimuth[i, j - 1] + dblAngle - Math.PI;
}
}
//计算新的多线段间对应顶点距离
for (int j = 0; j < intPtNum; j++)
{
int int2J = 2 * j;
//源线段与第一生成线段间对应点距离
adblIntervalDis[0, j] = Math.Sqrt((pCorrCptsLt[j].FrCpt.X - Xmix[int2J + 0, 0]) * (pCorrCptsLt[j].FrCpt.X - Xmix[int2J + 0, 0]) +
(pCorrCptsLt[j].FrCpt.Y - Xmix[int2J + 1, 0]) * (pCorrCptsLt[j].FrCpt.Y - Xmix[int2J + 1, 0]));
//目标线段与最后生成线段间对应点距离
adblIntervalDis[intInterNum, j] = Math.Sqrt((pCorrCptsLt[j].ToCpt.X - Xmix[(intInterNum - 1) * intXYNum + int2J + 0, 0]) * (pCorrCptsLt[j].ToCpt.X - Xmix[(intInterNum - 1) * intXYNum + int2J + 0, 0]) +
(pCorrCptsLt[j].ToCpt.Y - Xmix[(intInterNum - 1) * intXYNum + int2J + 1, 0]) * (pCorrCptsLt[j].ToCpt.Y - Xmix[(intInterNum - 1) * intXYNum + int2J + 1, 0]));
//各生成线段间对应点距离
for (int i = 1; i < intInterNum; i++)
{
adblIntervalDis[i, j] = Math.Sqrt((Xmix[(i - 1) * intXYNum + int2J + 0, 0] - Xmix[i * intXYNum + int2J + 0, 0]) * (Xmix[(i - 1) * intXYNum + int2J + 0, 0] - Xmix[i * intXYNum + int2J + 0, 0]) +
(Xmix[(i - 1) * intXYNum + int2J + 1, 0] - Xmix[i * intXYNum + int2J + 1, 0]) * (Xmix[(i - 1) * intXYNum + int2J + 1, 0] - Xmix[i * intXYNum + int2J + 1, 0]));
}
}
//计算系数矩阵中关于长度值的导数部分（注意：隐含的距离计算公式为后一个点的坐标减前一个点的坐标）
int intKnownCount2 = 0;
int intUnKnownCount2 = 0;
for (int j = 0; j < intUnknownLength; j++)
{
int intSumCount = intKnownCount2 + intUnKnownCount2;
int intBasicIndexL2 = 2 * intUnKnownCount2;
if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == false && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == false)
{
for (int i = 0; i < intInterNum; i++)
{
A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 0] = -Math.Cos(adblAzimuth[i, intSumCount]);
A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 1] = -Math.Sin(adblAzimuth[i, intSumCount]);
A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 2] = -A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 0];
A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 3] = -A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 1];
matl[i * intUnknownLength + j, 0] =adblLength0[i, intSumCount]- adblLength[i, intSumCount]; //图方便，顺便计算matl
}
intUnKnownCount2 += 1;
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == false && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == true)
{
for (int i = 0; i < intInterNum; i++)
{
A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 0] = -Math.Cos(adblAzimuth[i, intSumCount]);
A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 1] = -Math.Sin(adblAzimuth[i, intSumCount]);
matl[i * intUnknownLength + j, 0] = adblLength0[i, intSumCount]- adblLength[i, intSumCount]; //图方便，顺便计算matl
}
intUnKnownCount2 += 1;
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == false)
{
for (int i = 0; i < intInterNum; i++)
{
//注意这种情况，由于"pCorrCptsLt[intSumCount].FrCpt.isCtrl == true"不占位子（即不占列），因此列序号依然为" 2 * intUnKnownCount + 0"和" 2 * intUnKnownCount + 1"，而不是+2,+3
A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 0] = Math.Cos(adblAzimuth[i, intSumCount]);
A[i * intUnknownLength + j, i * intUnknownXY + intBasicIndexL2 + 1] = Math.Sin(adblAzimuth[i, intSumCount]);
matl[i * intUnknownLength + j, 0] =adblLength0[i, intSumCount]- adblLength[i, intSumCount]; //图方便，顺便计算matl
}
intKnownCount2 += 1;
}
else
{
intKnownCount2 += 1;
j -= 1;
}
}
//计算系数矩阵中关于夹角值的导数部分
int intKnownCount3 = 0;
int intUnKnownCount3 = 0;
for (int j = 0; j < intUnknownAngle; j++)
{
//真是太幸运了，虽然求两向量逆时针夹角时需分多种情况讨论，但各情况的导数形式却是一致的，节省了不少编程精力啊，哈哈
int intSumCount = intKnownCount3 + intUnKnownCount3;
//常用数据准备
double[] adblA2 = new double[intInterNum];
double[] adblB2 = new double[intInterNum];
for (int i = 0; i < intInterNum; i++)
{
adblA2[i] = adblLength[i, intSumCount + 0] * adblLength[i, intSumCount + 0];
adblB2[i] = adblLength[i, intSumCount + 1] * adblLength[i, intSumCount + 1];
}
//开始计算系数值，由于将以下三个情况排列组合将有八种情况，因此按如下方式计算
if (pCorrCptsLt[intUnKnownCount3 + intKnownCount3].FrCpt.isCtrl == true && pCorrCptsLt[intUnKnownCount3 + intKnownCount3 + 1].FrCpt.isCtrl == true && pCorrCptsLt[intUnKnownCount3 + intKnownCount3 + 2].FrCpt.isCtrl == true)
{
intKnownCount3 += 1;
j -= 1;
}
else
{
for (int i = 0; i < intInterNum; i++)
{
int intBasicIndexFi3 = i * intXYNum + 2 * intSumCount;
double dblNewAngle = CGeoFunc.CalAngle_Counterclockwise(Xmix[intBasicIndexFi3 + 0, 0], Xmix[intBasicIndexFi3 + 1, 0],
Xmix[intBasicIndexFi3 + 2, 0], Xmix[intBasicIndexFi3 + 3, 0],
Xmix[intBasicIndexFi3 + 4, 0], Xmix[intBasicIndexFi3 + 5, 0]);
adblAngle[i, j] = dblNewAngle;
matl[intMultiUnknownLength + i * intUnknownAngle + j, 0] =adblAngle0[i, intSumCount]- dblNewAngle; //图方便，顺便计算matl
}
int intPreTrueNum = 0;
int intUnKnownCount3orginal = intUnKnownCount3;
int intKnownCount3orginal = intKnownCount3;
if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3orginal + 0].FrCpt.isCtrl == false)
{
//X1,Y1的导数值(注意：该部分是减数，因此值为导数的负数)
for (int i = 0; i < intInterNum; i++)
{
A[intMultiUnknownLength + i * intUnknownAngle + j, i * intUnknownXY + 2 * intUnKnownCount3orginal + 0] = -(Xmix[i * intXYNum + 2 * intSumCount + 3, 0] - Xmix[i * intXYNum + 2 * intSumCount + 1, 0]) / adblA2[i];
A[intMultiUnknownLength + i * intUnknownAngle + j, i * intUnknownXY + 2 * intUnKnownCount3orginal + 1] = (Xmix[i * intXYNum + 2 * intSumCount + 2, 0] - Xmix[i * intXYNum + 2 * intSumCount + 0, 0]) / adblA2[i];
}
intUnKnownCount3 += 1;
}
else
{
intPreTrueNum += 1;
intKnownCount3 += 1;
}
if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3orginal + 1].FrCpt.isCtrl == false)
{
//X2,Y2的导数值
for (int i = 0; i < intInterNum; i++)
{
A[intMultiUnknownLength + i * intUnknownAngle + j, i * intUnknownXY + 2 * (intUnKnownCount3orginal - intPreTrueNum) + 2] = (Xmix[i * intXYNum + 2 * intSumCount + 5, 0] - Xmix[i * intXYNum + 2 * intSumCount + 3, 0]) / adblB2[i]
+ (Xmix[i * intXYNum + 2 * intSumCount + 3, 0] - Xmix[i * intXYNum + 2 * intSumCount + 1, 0]) / adblA2[i];
A[intMultiUnknownLength + i * intUnknownAngle + j, i * intUnknownXY + 2 * (intUnKnownCount3orginal - intPreTrueNum) + 3] = -(Xmix[i * intXYNum + 2 * intSumCount + 4, 0] - Xmix[i * intXYNum + 2 * intSumCount + 2, 0]) / adblB2[i]
- (Xmix[i * intXYNum + 2 * intSumCount + 2, 0] - Xmix[i * intXYNum + 2 * intSumCount + 0, 0]) / adblA2[i];
}
}
else
{
intPreTrueNum += 1;
}
if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3orginal + 2].FrCpt.isCtrl == false)
{
//X3,Y3的导数值
for (int i = 0; i < intInterNum; i++)
{
A[intMultiUnknownLength + i * intUnknownAngle + j, i * intUnknownXY + 2 * (intUnKnownCount3orginal - intPreTrueNum) + 4] = -(Xmix[i * intXYNum + 2 * intSumCount + 5, 0] - Xmix[i * intXYNum + 2 * intSumCount + 3, 0]) / adblB2[i];
A[intMultiUnknownLength + i * intUnknownAngle + j, i * intUnknownXY + 2 * (intUnKnownCount3orginal - intPreTrueNum) + 5] = (Xmix[i * intXYNum + 2 * intSumCount + 4, 0] - Xmix[i * intXYNum + 2 * intSumCount + 2, 0]) / adblB2[i];
}
}
}
}
//计算系数矩阵中关于多线段间对应顶点距离的导数部分（注意：隐含的距离计算公式为后一个点的坐标减前一个点的坐标）
int intKnownCount4 = 0;
int intUnKnownCount4 = 0;
for (int j = 0; j < intUnknownPt; j++)
{
int intSumCount4 = intKnownCount4 + intUnKnownCount4;
int intBasicIndexL4 = 2 * intUnKnownCount4;
if (pCorrCptsLt[intSumCount4].FrCpt.isCtrl == false)
{
int l = 0;
//源线段与第一生成线段间对应点距离导数
A[intMultiUnknownLengthAngle + l * intUnknownPt + j, (l - 0) * intUnknownXY + intBasicIndexL4 + 0] = (Xmix[l * intXYNum + 2 * intSumCount4 + 0, 0] - pCorrCptsLt[intSumCount4].FrCpt.X) / adblIntervalDis[l, intSumCount4];
A[intMultiUnknownLengthAngle + l * intUnknownPt + j, (l - 0) * intUnknownXY + intBasicIndexL4 + 1] = (Xmix[l * intXYNum + 2 * intSumCount4 + 1, 0] - pCorrCptsLt[intSumCount4].FrCpt.Y) / adblIntervalDis[l, intSumCount4];
matl[intMultiUnknownLengthAngle + l * intUnknownPt + j, 0] =adblIntervalDis0[l, intSumCount4]- adblIntervalDis[l, intSumCount4]; //图方便，顺便计算matl
//最后生成线段与目标线段间对应点距离导数
l = intInterNum;
A[intMultiUnknownLengthAngle + l * intUnknownPt + j, (l - 1) * intUnknownXY + intBasicIndexL4 + 0] = -(pCorrCptsLt[intSumCount4].ToCpt.X - Xmix[(l - 1) * intXYNum + 2 * intSumCount4 + 0, 0]) / adblIntervalDis[l , intSumCount4];
A[intMultiUnknownLengthAngle + l * intUnknownPt + j, (l - 1) * intUnknownXY + intBasicIndexL4 + 1] = -(pCorrCptsLt[intSumCount4].ToCpt.Y - Xmix[(l - 1) * intXYNum + 2 * intSumCount4 + 1, 0]) / adblIntervalDis[l , intSumCount4];
matl[intMultiUnknownLengthAngle + l * intUnknownPt + j, 0] =adblIntervalDis0[l , intSumCount4]- adblIntervalDis[l , intSumCount4]; //图方便，顺便计算matl
//各生成线段间对应点距离导数
for (int i = 1; i < intInterNum; i++)
{
A[intMultiUnknownLengthAngle + i * intUnknownPt + j, (i - 1) * intUnknownXY + intBasicIndexL4 + 0] = -(Xmix[i * intXYNum + 2 * intSumCount4 + 0, 0] - Xmix[(i - 1) * intXYNum + 2 * intSumCount4 + 0, 0]) / adblIntervalDis[i, intSumCount4];
A[intMultiUnknownLengthAngle + i * intUnknownPt + j, (i - 1) * intUnknownXY + intBasicIndexL4 + 1] = -(Xmix[i * intXYNum + 2 * intSumCount4 + 1, 0] - Xmix[(i - 1) * intXYNum + 2 * intSumCount4 + 1, 0]) / adblIntervalDis[i, intSumCount4];
A[intMultiUnknownLengthAngle + i * intUnknownPt + j, (i - 0) * intUnknownXY + intBasicIndexL4 + 0] = -A[intMultiUnknownLengthAngle + i * intUnknownPt + j, (i - 1) * intUnknownXY + +intBasicIndexL4 + 0];
A[intMultiUnknownLengthAngle + i * intUnknownPt + j, (i - 0) * intUnknownXY + intBasicIndexL4 + 1] = -A[intMultiUnknownLengthAngle + i * intUnknownPt + j, (i - 1) * intUnknownXY + +intBasicIndexL4 + 1];
matl[intMultiUnknownLengthAngle + i * intUnknownPt + j, 0] =adblIntervalDis0[i, intSumCount4]- adblIntervalDis[i, intSumCount4]; //图方便，顺便计算matl
//double dblIntervalDis = adblIntervalDis[i, intSumCount4];
//double dblIntervalDis0 = adblIntervalDis0[i, intSumCount4];
//double dblmatl = dblIntervalDis - dblIntervalDis0;
//double ttt = 6;
}
intUnKnownCount4 += 1;
}
else
{
intKnownCount4 += 1;
j -= 1;
}
}
int tt = 5;
//CHelpFuncExcel.ExportDataToExcel2(A, "matA", _DataRecords.ParameterInitialize.strSavePath);
//CHelpFuncExcel.ExportDataToExcelP(P, "matP", _DataRecords.ParameterInitialize.strSavePath);
//CHelpFuncExcel.ExportDataToExcel2(matl, "matmatl", _DataRecords.ParameterInitialize.strSavePath);
//平差
VBMatrix Temp = A.Trans() * P * A;
VBMatrix InvTemp = Temp.Inv(Temp);
VBMatrix x = InvTemp * A.Trans() * P * matl;
XA += x;
//CHelpFuncExcel.ExportDataToExcel2(XA, "matXA", _DataRecords.ParameterInitialize.strSavePath);
//记录各平差成果
//坐标改正值
//VBMatrix Xc = XA - XA0;
//观测值改正值矩阵V
VBMatrix V = A * x - matl;
//VtPV值
double dblVtPV = (V.Trans() * P * V).MatData[0, 0];
_DataRecords.ParameterInitialize.txtVtPV.Text = " VtPV = " + dblVtPV.ToString();
//VBMatrix L = new VBMatrix(intSumConstraints, 1);
//for (int i = 0; i < intInterNum; i++)
//{
// for (int j = 0; j < intUnknownLength; j++)
// {
// L[i * intUnknownLength + j, 0] = adblLength[i, j];
// }
// for (int j = 0; j < intUnknownAngle; j++)
// {
// L[intInterNum * intUnknownLength + i * intUnknownAngle + j, 0] = adblAngle[i, j];
// }
//}
//for (int i = 0; i <= intInterNum; i++)
//{
// for (int j = 0; j < intUnknownPt; j++)
// {
// L[intInterNum * intUnknownLengthAngle + i * intUnknownPt + j, 0] = adblIntervalDis[i, j];
// }
//}
//VBMatrix LPlusV = L + V;
//VBMatrix AX = A * XA;
//CHelpFuncExcel.ExportDataToExcel2(LPlusV, "matLPlusV", _DataRecords.ParameterInitialize.strSavePath);
//CHelpFuncExcel.ExportDataToExcel2(AX, "matAX", _DataRecords.ParameterInitialize.strSavePath);
//更新Xmix
int intSumCount5 = 0;
for (int j = 0; j < intUnknownPt; j++)
{
if (pCorrCptsLt[intSumCount5].FrCpt.isCtrl == false)
{
for (int i = 0; i < intInterNum; i++)
{
Xmix[i * intXYNum + intSumCount5 * 2 + 0, 0] = XA[i * intUnknownXY + j * 2 + 0, 0];
Xmix[i * intXYNum + intSumCount5 * 2 + 1, 0] = XA[i * intUnknownXY + j * 2 + 1, 0];
}
}
else
{
j -= 1;
}
intSumCount5 += 1;
}
//这里只是随便取两个中间值以观测是否收敛
dblJudge1 = Math.Abs(x[1 * intJudgeIndex, 0]);
dblJudge2 = Math.Abs(x[2 * intJudgeIndex, 0]);
dblJudge3 = Math.Abs(x[3 * intJudgeIndex, 0]);
//} while ((dblJudge1 > dblTX) || (dblJudge2 > dblTX) || (dblJudge3 > dblTX));
}while ((dblJudge1 >= 0) || (dblJudge2 >= 0) || (dblJudge3 >= 0)) ;
break;
for (int i = 0; i < intMultiUnknownLength; i++) //长度权重
{
P[i, i] = 1;
}
for (int i = 0; i < intMultiUnknownAngle; i++) //角度权重
{
P[intMultiUnknownLength + i, intMultiUnknownLength + i] = 39.48;
}
for (int i = 0; i < intMultiUnknownInterval; i++) //角度权重
{
P[intMultiUnknownLengthAngle + i, intMultiUnknownLengthAngle + i] = 1;
}
}
//生成目标线段
List<CPolyline> cpllt = new List<CPolyline>();
for (int i = 0; i < intInterNum; i++)
{
List<CPoint> cptlt = new List<CPoint>();
for (int j = 0; j < intPtNum; j++)
{
CPoint cpt = new CPoint(j, Xmix[i * intXYNum + j * 2, 0], Xmix[i * intXYNum + j * 2 + 1, 0]);
cptlt.Add(cpt);
}
CPolyline cpl = new CPolyline(i, cptlt);
cpllt.Add(cpl);
}
return cpllt;
}