///// <summary>
///// 输出数据到Excel中
///// </summary>
///// <param name="pParameterResultToExcel">指标结果信息</param>
///// <param name="strName">文件名称</param>
///// <param name="strSavePath">保存路径</param>
///// <remarks>二维数列</remarks>
//public static void ExportDataltltToExcelSW(List<List<double>> dblDataltlt, string strName, string strSavePath)
//{
// Excel.Application pExcelAPP = new Excel.Application();
// pExcelAPP.Visible = false;
// Workbook pWorkBook = pExcelAPP.Workbooks.Add(true);
// Worksheet pWorksheet = pWorkBook.Worksheets[1] as Worksheet;
// string strfilename = System.IO.Path.GetFileNameWithoutExtension(strSavePath);
// strfilename = strfilename + strName;
// string strEntireFileName = strSavePath + "\\" + strfilename;
// pWorkBook.SaveAs(strEntireFileName, 56, Type.Missing, Type.Missing, Type.Missing, Type.Missing, XlSaveAsAccessMode.xlExclusive, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing);
// //pWorkBook
// //pWorksheet.close
// pWorkBook.Close(false, false, false);
// pExcelAPP.Quit();
// //KillExcel();
// //SaveFileDialog saveFileDialog = new SaveFileDialog();
// //saveFileDialog.FileName = strEntireFileName;
// //Stream myStream = saveFileDialog.OpenFile();
// //SaveFileDialog sfg = new SaveFileDialog();
// //sfg.FileName = strEntireFileName + ".xls";
// //Stream myStream = sfg.OpenFile();
// //StreamWriter sw = new StreamWriter(myStream, Encoding.GetEncoding(-0));
// StreamWriter sw = new StreamWriter(strEntireFileName + ".xls", false, Encoding.GetEncoding(-0));
// long lngStart = System.Environment.TickCount;
// int intRowCount = dblDataltlt.Count;
// for (int i = 0; i < intRowCount; i++)
// {
// string tempStr = "";
// int intColCount = dblDataltlt[i].Count;
// for (int j = 0; j < intColCount; j++)
// {
// if (j > 0)
// {
// tempStr += "\t";
// }
// tempStr += dblDataltlt[i][j];
// }
// //tempStr.TrimEnd(charend);
// sw.WriteLine(tempStr);
// }
// sw.Close();
// long lngEnd = System.Environment.TickCount;
// long lngTime = lngEnd - lngStart;
// //myStream.Close();
// //ofg.clo
// //pWorksheet.Columns.AutoFit();
// //pExcelAPP.DisplayAlerts = false;
// //pWorkBook.SaveAs(strSavePath + "\\" + strfilename, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing, XlSaveAsAccessMode.xlNoChange, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing);
// //pExcelAPP.Quit();
// //KillExcel();
//}
///// <summary>
///// 输出数据到Excel中
///// </summary>
///// <param name="pParameterResultToExcel">指标结果信息</param>
///// <param name="strName">文件名称</param>
///// <param name="strSavePath">保存路径</param>
///// <remarks>二维数列</remarks>
//public static void ExportColDataltltToExcel(List<List<double>> dblDataltlt, string strName, string strSavePath)
//{
// // //为应付Excel的bug，加入以下两行代码
// // System.Globalization.CultureInfo CurrentCI = System.Threading.Thread.CurrentThread.CurrentCulture;
// // System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-US");
// Excel.Application pExcelAPP = new Excel.Application();
// pExcelAPP.Visible = false;
// Workbook pWorkBook = pExcelAPP.Workbooks.Add(true);
// Worksheet pWorksheet = pWorkBook.Worksheets[1] as Worksheet;
// string strfilename = System.IO.Path.GetFileNameWithoutExtension(strSavePath);
// strfilename = strfilename + strName;
// int intRowCount = dblDataltlt[0].Count;
// int intColCount = dblDataltlt.Count;
// for (int i = 0; i < intRowCount; i++)
// {
// for (int j = 0; j < intColCount; j++)
// {
// pWorksheet.Cells[i + 1, j + 1] = dblDataltlt[j][i];
// }
// }
// pWorksheet.Columns.AutoFit();
// pExcelAPP.DisplayAlerts = false;
// pWorkBook.SaveAs(strSavePath + "\\" + strfilename, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing, XlSaveAsAccessMode.xlNoChange, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing);
// pExcelAPP.Quit();
// //KillExcel();
//}
///// <summary>
///// 输出数据到Excel中
///// </summary>
///// <param name="pParameterResultToExcel">指标结果信息</param>
///// <param name="strName">文件名称</param>
///// <param name="strSavePath">保存路径</param>
///// <remarks>二维数组</remarks>
//public static void ExportDataToExcel2(double[,] dbldata, string strName, string strSavePath)
//{
// Excel.Application pExcelAPP = new Excel.Application();
// pExcelAPP.Visible = false;
// Workbook pWorkBook = pExcelAPP.Workbooks.Add(true);
// Worksheet pWorksheet = pWorkBook.Worksheets[1] as Worksheet;
// string strfilename = System.IO.Path.GetFileNameWithoutExtension(strSavePath);
// strfilename = strfilename + strName;
// int intRowCount = dbldata.GetUpperBound(0);
// int intColCount = dbldata.GetUpperBound(1);
// for (int i = 0; i < intRowCount; i++)
// {
// for (int j = 0; j < intColCount; j++)
// {
// pWorksheet.Cells[i + 1, j + 1] = dbldata[i,j];
// }
// }
// pWorksheet.Columns.AutoFit();
// pExcelAPP.DisplayAlerts = false;
// pWorkBook.SaveAs(strSavePath + "\\" + strfilename, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing, XlSaveAsAccessMode.xlNoChange, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing);
// pExcelAPP.Quit();
// //KillExcel();
//}
/// <summary>
/// 输出数据到Excel中
/// </summary>
/// <param name="pParameterResultToExcel">指标结果信息</param>
/// <param name="strName">文件名称</param>
/// <param name="strSavePath">保存路径</param>
/// <remarks>二维矩阵</remarks>
public static void ExportDataToExcel2(VBMatrix maxData, string strName, string strSavePath)
{
//为应付Excel的bug，加入以下两行代码
//System.Globalization.CultureInfo CurrentCI = System.Threading.Thread.CurrentThread.CurrentCulture;
//System.Threading.Thread.CurrentThread.CurrentCulture = new System.Globalization.CultureInfo("en-US");
Excel.Application pExcelAPP = new Excel.Application();
pExcelAPP.Visible = false;
Workbook pWorkBook = pExcelAPP.Workbooks.Add(true);
Worksheet pWorksheet = pWorkBook.Worksheets[1] as Worksheet;
string strfilename = System.IO.Path.GetFileNameWithoutExtension(strSavePath);
strfilename = strfilename + strName;
int intRowCount = maxData.Row;
int intColCount = maxData.Col;
for (int i = 0; i < intRowCount; i++)
{
for (int j = 0; j < intColCount; j++)
{
pWorksheet.Cells[i + 1, j + 1] = maxData[i, j];
}
}
pWorksheet.Columns.AutoFit();
pExcelAPP.DisplayAlerts = false;
pWorkBook.SaveAs(strSavePath + "\\" + strfilename, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing, XlSaveAsAccessMode.xlNoChange, Type.Missing, Type.Missing, Type.Missing, Type.Missing, Type.Missing);
pExcelAPP.Quit();
//KillExcel();
}
/// <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>
/// 获取线状要素
/// </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="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];
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;
}
//计算角度初始值（全部计算）
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;
}
//生成点数组（初始值），同时计算各线段方位角混合值
//注意：默认固定第一条边
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 intKnownNum = 0; //固定点的数目
int intUnknownNum = 0; //非固定点的数目
List <int> intKnownLocationLt = new List <int>(); //记录已知点的序号
//注意：对于该循环，有一个默认条件，即FromCpl的第一个顶点只有一个对应点
for (int i = 0; i < cptlt.Count; i++)
{
if (cptlt[i].isCtrl == true)
{
intKnownLocationLt.Add(i);
intKnownNum += 1;
}
else
{
intUnknownNum += 1;
}
}
//找出长度固定的位置(如果一个线段的前后两个点都固定，则该长度固定)。另外，长度固定则该边的方位角也固定
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 = pCorrCptsLt.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 = pCorrCptsLt.Count - 2 - intKnownAngleLt.Count;
//总未知量
int intUnknownLengthAngle = intUnknownLength + intUnknownAngle;
//坐标约束个数
int intXYCst = (intKnownLocationLt.Count - 1 - intKnownLengthLt.Count) * 2; //如果两个相邻点都是控制点，则这两个相邻点之间不存在坐标约束
//夹角约束个数
int intAngleCst = intKnownLengthLt.Count - 1 - intKnownAngleLt.Count; //同坐标约束，如果有两相邻边都为已知边，则之间不存在夹角约束
//总约束个数
int intSumCst = intUnknownLengthAngle + intXYCst + intAngleCst;
//定义权重矩阵
VBMatrix P = new VBMatrix(intSumCst, intSumCst);
for (int i = 0; i < intUnknownLength; i++)
{
P[i, i] = 1;
}
for (int i = 0; i < intUnknownAngle; i++)
{
P[intUnknownLength + i, intUnknownLength + i] = 1000;
}
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] = 1;
}
//计算初始值矩阵X0（注意：此处的X0中并未包含任何坐标，而是长度和夹角的初值）
VBMatrix X0 = new VBMatrix(intPtNum * 2 - 3, 1);
for (int i = 0; i < (intPtNum - 1); i++)
{
X0[i, 0] = adblLength0[i];
}
for (int i = 0; i < (intPtNum - 2); i++)
{
X0[intPtNum - 1 + i, 0] = adblAngle0[i];
}
//Xmix里存储了XA和X0的最新混合值（此矩阵在公式推导中并不存在，只是为了方便编写代码而建立）
VBMatrix Xmix = new VBMatrix(intPtNum * 2 - 3, 1);
for (int i = 0; i < X0.Row; i++)
{
Xmix[i, 0] = X0[i, 0];
}
//定义近似值矩阵XA（注意：同上，此处的XA中并未包含任何坐标，而是长度和夹角的近似值）
VBMatrix XA = new VBMatrix(intUnknownLengthAngle, 1);
int intSumCountL = 0;
for (int i = 0; i < intUnknownLength; i++) //长度近似值部分
{
if (cptlt[intSumCountL].isCtrl == false || cptlt[intSumCountL + 1].isCtrl == false)
{
XA[i, 0] = X0[intSumCountL, 0];
}
else
{
i -= 1;
}
intSumCountL += 1;
}
int intSumCountA = 0;
for (int i = intUnknownLength; i < intUnknownLengthAngle; i++) //角度近似值部分
{
if (cptlt[intSumCountA].isCtrl == false || cptlt[intSumCountA + 1].isCtrl == false || cptlt[intSumCountA + 2].isCtrl == false)
{
XA[i, 0] = X0[intPtNum - 1 + intSumCountA, 0];
}
else
{
i -= 1;
}
intSumCountA += 1;
}
//定义系数矩阵，系数矩阵来源于四类约束方程：1、长度本身；2、角度本身；3、X、Y的闭合差；4、方位角闭合差
//此处仅给出“长度本身”和“角度本身”的系数，有关“ X、Y的闭合差”和“方位角闭合差”的系数将在循环中给出
VBMatrix A = new VBMatrix(intSumCst, intUnknownLengthAngle);
for (int i = 0; i < intUnknownLengthAngle; i++)
{
A[i, i] = 1;
}
double dblJudge1 = 0; //该值用于判断是否应该跳出循环
double dblJudge2 = 0; //该值用于判断是否应该跳出循环
int intJudgeIndex = intUnknownLength / 4;
int intIterativeCount = 0;
do
{
VBMatrix matl = new VBMatrix(intSumCst, 1);
//计算matl关于“长度本身”这部分的值
int intSumCountL1 = 0;
for (int i = 0; i < intUnknownLength; i++)
{
if (cptlt[intSumCountL1].isCtrl == false || cptlt[intSumCountL1 + 1].isCtrl == false)
{
matl[i, 0] = XA[i, 0] - X0[intSumCountL1, 0];
}
else
{
i -= 1;
}
intSumCountL1 += 1;
}
//计算matl关于“角度本身”这部分的值
int intSumCountA1 = 0;
for (int i = intUnknownLength; i < intUnknownLengthAngle; i++)
{
if (cptlt[intSumCountA1].isCtrl == false || cptlt[intSumCountA1 + 1].isCtrl == false || cptlt[intSumCountA1 + 2].isCtrl == false)
{
matl[i, 0] = XA[i, 0] - X0[intPtNum - 1 + intSumCountA1, 0];
}
else
{
i -= 1;
}
intSumCountA1 += 1;
}
//计算系数矩阵A第"intUnknownLengthAngle"行到"intUnknownLengthAngle + intXYCst - 1"行的各元素，即从由坐标约束产生的各偏导值
if (intKnownLocationLt.Count >= 2)
{
int intRow = intUnknownLengthAngle;
int intLastIsCtrl = 0;
for (int i = 0; i < cptlt.Count; i++)
{
if (cptlt[i].isCtrl == true)
{
intLastIsCtrl = i;
break;
}
}
int intKnownLength = 0;
int intKnownAngle = 0;
for (int i = intLastIsCtrl + 1; i < cptlt.Count; i++)
{
if (cptlt[i].isCtrl == true && cptlt[i - 1].isCtrl != true)
{
double dblSumDerX = new double();
double dblSumDerY = new double();
for (int j = intLastIsCtrl; j < i; j++) //注意，此处的j不可以等于0，因为等于0的时候该处不存在夹角。当然，由于之前规定前两个固定，因此j>=1
{
dblSumDerX = 0;
dblSumDerY = 0;
//关于X的约束方程，由于分别可以从左侧和右侧推算，因此有两个约束方程
A[intRow, j - intKnownLength] = Math.Cos(adblAzimuth[j]); //关于长度的偏导值
for (int k = j; k < i; k++)
{
dblSumDerX -= (adblLength0[k] * Math.Sin(adblAzimuth[k])); //关于夹角的偏导值
}
A[intRow, j - 1 - intKnownAngle + intUnknownLength] = dblSumDerX;
//关于Y的约束方程，行加1，列相同
A[intRow + 1, j - intKnownLength] = Math.Sin(adblAzimuth[j]); //关于长度的偏导值
for (int k = j; k < i; k++)
{
dblSumDerY += (adblLength0[k] * Math.Cos(adblAzimuth[k])); //关于夹角的偏导值
}
A[intRow + 1, j - 1 - intKnownAngle + intUnknownLength] = dblSumDerY;
if (j == intLastIsCtrl)
{
matl[intRow + 0, 0] = dblSumDerY - (cptlt[i].X - cptlt[intLastIsCtrl].X); //图方便，顺便计算matl（此处之所以利用之前的成果dblSumDerY，是因为数值上正好相等）
matl[intRow + 1, 0] = -dblSumDerX - (cptlt[i].Y - cptlt[intLastIsCtrl].Y); //图方便，顺便计算matl（此处之所以利用之前的成果-dblSumDerX，是因为数值上正好相等）
}
}
intRow += 2;
intLastIsCtrl = i;
}
else if (cptlt[i].isCtrl == true && cptlt[i - 1].isCtrl == true) //相邻两个点的坐标都知道，不产生约束条件
{
intKnownLength += 1;
if (i >= 2)
{
if (cptlt[i - 2].isCtrl == true)
{
intKnownAngle += 1;
}
}
intLastIsCtrl = i;
}
}
}
//计算系数矩阵A第"intUnknownLengthAngle + intXYCst"行到"intUnknownLengthAngle + intXYCst + intAngleCst - 1 (即intSumCst - 1)"行的各元素，即夹角闭合差产生的各偏导值
if (intKnownLengthLt.Count >= 2)
{
int intRow = intUnknownLengthAngle + intXYCst;
int intLastIsCtrl = 0;
for (int i = 0; i < cptlt.Count - 1; i++)
{
if (cptlt[i].isCtrl == true && cptlt[i + 1].isCtrl == true)
{
intLastIsCtrl = i;
break;
}
}
int intKnownAngle = 0;
for (int i = intLastIsCtrl + 1; i < cptlt.Count - 1; i++)
{
if (cptlt[i].isCtrl == true && cptlt[i + 1].isCtrl == true && cptlt[i - 1].isCtrl != true)
{
double dblAngleSum = 0;
for (int j = intLastIsCtrl; j < i; j++)
{
//关于夹角的约束方程
A[intRow, j - intKnownAngle + intUnknownLength] = 1;
//夹角累计值，为计算matl做准备
dblAngleSum += adblAngle0[j];
}
matl[intRow, 0] = dblAngleSum - (i - intLastIsCtrl) * Math.PI - (adblAzimuth[i] - adblAzimuth[intLastIsCtrl]); //图方便，顺便计算matl
double tt = (i - intLastIsCtrl) * Math.PI;
double ss = (adblAzimuth[i] - adblAzimuth[intLastIsCtrl]);
intRow += 1;
intLastIsCtrl = i;
}
else if (cptlt[i].isCtrl == true && cptlt[i + 1].isCtrl == true && cptlt[i - 1].isCtrl == true) //相邻三个点的坐标都知道，不产生约束条件
{
intKnownAngle += 1;
intLastIsCtrl = i;
}
}
}
//计算新的近似值
//SaveFileDialog SFD = new SaveFileDialog();
//SFD.ShowDialog();
//CHelpFuncExcel.ExportDataToExcelA(A, "maxA", _DataRecords.ParameterInitialize.strSavePath);
//CHelpFuncExcel.ExportDataToExcelP(P, "maxP", _DataRecords.ParameterInitialize.strSavePath);
//CHelpFuncExcel.ExportDataToExcel2(matl, "maxmatl", _DataRecords.ParameterInitialize.strSavePath);
//平差
VBMatrix x = _pCAL.InvAtPAAtPmatl(A, P, matl);
XA -= x;
//更新Xmix
int intSumCountL4 = 0;
for (int i = 0; i < intUnknownLength; i++) //长度近似值部分
{
if (cptlt[intSumCountL4].isCtrl == false || cptlt[intSumCountL4 + 1].isCtrl == false)
{
Xmix[intSumCountL4, 0] = XA[i, 0]; //其实此处的“intSumCountL4”等于"intPtNum-1"
}
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)
{
Xmix[intPtNum - 1 + intSumCountA4, 0] = XA[i, 0]; //其实此处的“intSumCountL4”等于"intPtNum-1"
}
else
{
i -= 1;
}
intSumCountA4 += 1;
}
//更新adblAzimuth和cptlt
for (int i = 2; i < cptlt.Count; i++)
{
if (cptlt[i].isCtrl == false)
{
adblAzimuth[i - 1] = adblAzimuth[i - 2] + Xmix[intPtNum - 1 + i - 2, 0] - Math.PI;
double dblnewX = cptlt[i - 1].X + Xmix[i - 1, 0] * Math.Cos(adblAzimuth[i - 1]);
double dblnewY = cptlt[i - 1].Y + Xmix[i - 1, 0] * Math.Sin(adblAzimuth[i - 1]);
CPoint newcpt = new CPoint(i, dblnewX, dblnewY);
//double dbloldX = cptlt[i].X;
//double dbloldY = cptlt[i].Y;
cptlt.RemoveAt(i);
cptlt.Insert(i, newcpt);
}
}
intIterativeCount += 1;
if (intIterativeCount >= 1000)
{
break;
}
dblJudge1 = Math.Abs(x[intJudgeIndex, 0]);
dblJudge2 = Math.Abs(x[3 * intJudgeIndex, 0]);
} while ((dblJudge1 > dblTX) || (dblJudge2 > dblTX));
//生成目标线段
CPolyline cpl = new CPolyline(0, cptlt);
return(cpl);
}
/// <summary>
/// 获取线状要素
/// </summary>
/// <param name="pDataRecords">数据记录</param>
/// <param name="dblProp">插值参数</param>
/// <returns>在处理面状要素时，本程序将原面状要素的边界切开，按线状要素处理，处理完后再重新生成面状要素</returns>
public CPolyline GetTargetcpl(double dblProp)
{
List <CCorrCpts> pCorrCptsLt = _DataRecords.ParameterResult.CCorrCptsLt; //读取数据后，此处ResultPtLt中的对应点为一一对应
double dblTX = _dblTX;
//统计插值点数
int intKnownNum = 0; //须固定不参与平差的点的数目
int intUnknownNum = 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);
intKnownNum += 1;
}
else
{
intUnknownNum += 1;
}
}
int intUnknownXY = intUnknownNum * 2; //每个点都有X、Y坐标
int intPtNum = pCorrCptsLt.Count;
int intXYNum = 2 * intPtNum;
//找出长度固定的位置(如果一个线段的前后两个点都固定，则该长度固定)
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 = pCorrCptsLt.Count - 1 - intKnownLengthLt.Count;
//找出角度固定的位置(如果一个固定顶点的前后两个点都固定，则该角度固定)
List <int> intKnownAngleLt = new List <int>();
for (int i = 1; i < intKnownLocationLt.Count - 1; i++)
{
if ((intKnownLocationLt[i] - intKnownLocationLt[i - 1]) == 1 && (intKnownLocationLt[i + 1] - intKnownLocationLt[i]) == 1)
{
intKnownAngleLt.Add(intKnownLocationLt[i]);
}
}
int intUnknownAngle = pCorrCptsLt.Count - 2 - intKnownAngleLt.Count;
int intUnknownXYLengthAngle = intUnknownXY + intUnknownLength + intUnknownAngle;
//定义权重矩阵
VBMatrix P = new VBMatrix(intUnknownXYLengthAngle, intUnknownXYLengthAngle);
for (int i = 0; i < intUnknownXY; i++)
{
P[i, i] = 1;
}
for (int i = 0; i < intUnknownLength; i++)
{
P[intUnknownXY + i, intUnknownXY + i] = 100;
}
for (int i = 0; i < intUnknownAngle; i++)
{
P[intUnknownXY + intUnknownLength + i, intUnknownXY + intUnknownLength + i] = 10000;
}
//计算初始值矩阵X0
VBMatrix X0 = new VBMatrix(intXYNum, 1);
int intCount = 0;
for (int i = 0; i < pCorrCptsLt.Count; i++)
{
X0[intCount, 0] = (1 - dblProp) * pCorrCptsLt[i].FrCpt.X + dblProp * pCorrCptsLt[i].ToCpt.X;
X0[intCount + 1, 0] = (1 - dblProp) * pCorrCptsLt[i].FrCpt.Y + dblProp * pCorrCptsLt[i].ToCpt.Y;
intCount += 2;
}
//计算长度初始值（全部计算）
double[] adblLength0 = 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;
}
//计算角度初始值（全部计算）
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;
}
//Xmix里存储了XA和X0的最新混合值
VBMatrix Xmix = new VBMatrix(intXYNum, 1);
for (int i = 0; i < X0.Row; i++)
{
Xmix[i, 0] = X0[i, 0];
}
//定义坐标近似值矩阵XA
VBMatrix XA = new VBMatrix(intUnknownXY, 1);
int intSumCount0 = 0;
for (int i = 0; i < intUnknownNum; i++)
{
if (pCorrCptsLt[intSumCount0].FrCpt.isCtrl == false)
{
XA[i * 2, 0] = X0[intSumCount0 * 2, 0];
XA[i * 2 + 1, 0] = X0[intSumCount0 * 2 + 1, 0];
}
else
{
i -= 1;
}
intSumCount0 += 1;
}
//定义系数矩阵（有关长度和角度的值将在循环中给定）
VBMatrix A = new VBMatrix(intUnknownXYLengthAngle, intUnknownXY);
for (int i = 0; i < intUnknownXY; i++)
{
A[i, i] = 1;
}
double dblJudge1 = 0; //该值用于判断是否应该跳出循环
double dblJudge2 = 0; //该值用于判断是否应该跳出循环
int intJudgeIndex = intUnknownXY / 4;
int intIterativeCount = 0;
double[] adblSubDis = new double[intPtNum - 1];
double[] adblAngle = new double[intPtNum - 2];
double[] adblAzimuth = new double[intPtNum - 1];
VBMatrix matl = new VBMatrix(intUnknownXYLengthAngle, 1);
do
{
int intSumCount1 = 0;
for (int i = 0; i < intUnknownNum; i++)
{
if (pCorrCptsLt[intSumCount1].FrCpt.isCtrl == false)
{
matl[2 * i, 0] = XA[2 * i, 0] - X0[intSumCount1 * 2, 0];
matl[2 * i + 1, 0] = XA[2 * i + 1, 0] - X0[intSumCount1 * 2 + 1, 0];
}
else
{
i -= 1;
}
intSumCount1 += 1;
}
//计算系数矩阵A第"intUnknownXY"行到"intUnknownXY+intUnknownLength-1"行的各元素，即线段长度对各未知数求偏导的值
//先计算各分母值（注意：分母实际上是求偏导后的一部分值，但却恰好等于两点之间距离，因此其计算公式与距离计算公式相同
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;
}
//开始计算系数矩阵第"intUnknownXY"行到"intUnknownXY+intUnknownLength-1"行的各元素
CalADevLength(pCorrCptsLt, intUnknownXY, intUnknownLength, ref A, ref matl, adblSubDis, adblAzimuth, adblLength0);
//计算系数矩阵A第"intUnknownXY+intUnknownLength"行到"intUnknownXY+intUnknownLength+intUnknownAngle"行的各元素，即角度对各未知数求偏导的值
CalADevAngle(pCorrCptsLt, intUnknownXY + intUnknownLength, intUnknownAngle, Xmix, ref A, ref matl, adblSubDis, adblAngle, adblAngle0);
////计算新的近似值
//SaveFileDialog SFD = new SaveFileDialog();
//SFD.ShowDialog();
// CHelpFuncExcel.ExportDataToExcelA(A, "maxA", SFD.FileName);
//CHelpFuncExcel.ExportDataToExcelP(P, "maxP", SFD.FileName);
//CHelpFuncExcel.ExportDataToExcel2(matl, "maxmatl", SFD.FileName);
//VBMatrix Temp =A.Trans () * P * A;
//Temp.InvertGaussJordan();
//XA -= Temp * A.Transpose() * P * matl;
//平差
VBMatrix x = InvAtPAAtPmatl(A, P, matl);
XA -= x;
//更新Xmix
int intSumCount4 = 0;
for (int i = 0; i < intUnknownNum; i++)
{
if (pCorrCptsLt[intSumCount4].FrCpt.isCtrl == false)
{
Xmix[intSumCount4 * 2, 0] = XA[i * 2, 0];
Xmix[intSumCount4 * 2 + 1, 0] = XA[i * 2 + 1, 0];
}
else
{
i -= 1;
}
intSumCount4 += 1;
}
intIterativeCount += 1;
if (intIterativeCount >= 1000)
{
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];
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);
}
public void CalADevAngle(List <CCorrCpts> pCorrCptsLt, int intBaseIndex, int intUnknownAngle, VBMatrix Xmix, ref VBMatrix A, ref VBMatrix matl, double[] adblSubDis, double[] adblAngle, double[] adblAngle0)
{
//计算系数矩阵A第"intUnknownXY+intUnknownLength"行到"intUnknownXY+intUnknownLength+intUnknownAngle"行的各元素，即角度对各未知数求偏导的值
int intKnownCount = 0;
int intUnKnownCount = 0;
for (int i = 0; i < intUnknownAngle; i++)
{
//真是太幸运了，虽然求两向量逆时针夹角时需分多种情况讨论，但各情况的导数形式却是一致的，节省了不少编程精力啊，哈哈
int intSumCount = intKnownCount + intUnKnownCount;
//常用数据准备
double dblA2 = adblSubDis[intSumCount] * adblSubDis[intSumCount];
double dblB2 = adblSubDis[intSumCount + 1] * adblSubDis[intSumCount + 1];
//开始计算系数值，由于将以下三个情况排列组合将有八种情况，因此按如下方式计算
if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 2].FrCpt.isCtrl == true)
{
intKnownCount += 1;
i -= 1;
}
else
{
matl[intBaseIndex + i, 0] = adblAngle0[intSumCount] - adblAngle[intSumCount]; //图方便，顺便计算matl
int intPreTrueNum = 0;
int intUnKnownCountorginal = intUnKnownCount;
int intKnownCountorginal = intKnownCount;
if (pCorrCptsLt[intUnKnownCountorginal + intKnownCountorginal + 0].FrCpt.isCtrl == false)
{
//X1,Y1的导数值(注意：该部分是减数，因此值为导数的负数)
A[intBaseIndex + i, 2 * intUnKnownCountorginal + 0] = -(Xmix[2 * intSumCount + 3, 0] - Xmix[2 * intSumCount + 1, 0]) / dblA2;
A[intBaseIndex + i, 2 * intUnKnownCountorginal + 1] = (Xmix[2 * intSumCount + 2, 0] - Xmix[2 * intSumCount + 0, 0]) / dblA2;
intUnKnownCount += 1;
}
else
{
intPreTrueNum += 1;
intKnownCount += 1;
}
if (pCorrCptsLt[intUnKnownCountorginal + intKnownCountorginal + 1].FrCpt.isCtrl == false)
{
//X2,Y2的导数值(注意：后半部分是由减数产生的导数，因此值为导数的负数)
A[intBaseIndex + i, 2 * (intUnKnownCountorginal - 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[intBaseIndex + i, 2 * (intUnKnownCountorginal - 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 (pCorrCptsLt[intUnKnownCountorginal + intKnownCountorginal + 2].FrCpt.isCtrl == false)
{
//X3,Y3的导数值
A[intBaseIndex + i, 2 * (intUnKnownCountorginal - intPreTrueNum) + 4] = -(Xmix[2 * intSumCount + 5, 0] - Xmix[2 * intSumCount + 3, 0]) / dblB2;
A[intBaseIndex + i, 2 * (intUnKnownCountorginal - intPreTrueNum) + 5] = (Xmix[2 * intSumCount + 4, 0] - Xmix[2 * intSumCount + 2, 0]) / dblB2;
}
}
}
}
public void CalADevLength(List <CCorrCpts> pCorrCptsLt, int intBaseIndex, int intUnknownLength, ref VBMatrix A, ref VBMatrix matl, double[] dblSubDis, double[] adblAzimuth, double[] adblLength0)
{
//计算系数矩阵中关于长度值的导数部分（注意：隐含的距离计算公式为后一个点的坐标减前一个点的坐标）
int intKnownCount = 0;
int intUnKnownCount = 0;
for (int i = 0; i < intUnknownLength; i++)
{
int intSumCount = intKnownCount + intUnKnownCount;
if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == false && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == false)
{
A[intBaseIndex + i, 2 * intUnKnownCount + 0] = -Math.Cos(adblAzimuth[intSumCount]);
A[intBaseIndex + i, 2 * intUnKnownCount + 1] = -Math.Sin(adblAzimuth[intSumCount]);
A[intBaseIndex + i, 2 * intUnKnownCount + 2] = -A[intBaseIndex + i, 2 * intUnKnownCount + 0];
A[intBaseIndex + i, 2 * intUnKnownCount + 3] = -A[intBaseIndex + i, 2 * intUnKnownCount + 1];
matl[intBaseIndex + i, 0] = adblLength0[intSumCount] - dblSubDis[intSumCount]; //图方便，顺便计算matl
intUnKnownCount += 1;
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == false && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == true)
{
A[intBaseIndex + i, 2 * intUnKnownCount + 0] = -Math.Cos(adblAzimuth[intSumCount]);
A[intBaseIndex + i, 2 * intUnKnownCount + 1] = -Math.Sin(adblAzimuth[intSumCount]);
matl[intBaseIndex + i, 0] = adblLength0[intSumCount] - dblSubDis[intSumCount]; //图方便，顺便计算matl
intUnKnownCount += 1;
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == false)
{
//注意这种情况，由于"pCorrCptsLt[intSumCount].FrCpt.isCtrl == true"不占位子（即不占列），因此列序号依然为" 2 * intUnKnownCount + 0"和" 2 * intUnKnownCount + 1"，而不是+2,+3
A[intBaseIndex + i, 2 * intUnKnownCount + 0] = Math.Cos(adblAzimuth[intSumCount]);
A[intBaseIndex + i, 2 * intUnKnownCount + 1] = Math.Sin(adblAzimuth[intSumCount]);
matl[intBaseIndex + i, 0] = adblLength0[intSumCount] - dblSubDis[intSumCount]; //图方便，顺便计算matl
intKnownCount += 1;
}
else
{
intKnownCount += 1;
i -= 1;
}
}
}
/// <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;
}
/// <summary>
/// 获取线状要素
/// </summary>
/// <param name="pDataRecords">数据记录</param>
/// <param name="dblProp">插值参数</param>
/// <returns>在处理面状要素时，本程序将原面状要素的边界切开，按线状要素处理，处理完后再重新生成面状要素</returns>
public CPolyline GetTargetcpl(double dblProp)
{
CParameterResult pParameterResult = _DataRecords.ParameterResult;
//获取周长
double dblFrLength = pParameterResult.FromCpl.pPolyline.Length;
double dblToLength = pParameterResult.ToCpl.pPolyline.Length;
//确定循环阈值（初始多边形平均边长的千分之一）
double dblTX = dblFrLength / pParameterResult.FromCpl.CptLt.Count / 1000;
List <CCorrCpts> pCorrCptsLt = pParameterResult.CCorrCptsLt; //读取数据后，此处ResultPtLt中的对应点为一一对应
//pCorrCptsLt[2].FrCpt.isCtrl = false;
//pCorrCptsLt[2].CorrespondingPtLt[0].FrCpt.isCtrl = false;
//统计插值点数
int intKnownNum = 0; //须固定不参与平差的点的数目
int intUnknownNum = 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);
intKnownNum += 1;
}
else
{
intUnknownNum += 1;
}
}
int intSumNum = intKnownNum + intUnknownNum;
int intUnknownXY = intUnknownNum * 2; //每个点都有X、Y坐标
int intSumXY = intSumNum * 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 = pCorrCptsLt.Count - 1 - intKnownLengthLt.Count;
//找出角度固定的位置(如果一个固定顶点的前后两个点都固定，则该角度固定)
List <int> intKnownAngleLt = new List <int>();
for (int i = 1; i < intKnownLocationLt.Count - 1; i++)
{
if ((intKnownLocationLt[i] - intKnownLocationLt[i - 1]) == 1 && (intKnownLocationLt[i + 1] - intKnownLocationLt[i]) == 1)
{
intKnownAngleLt.Add(intKnownLocationLt[i]);
}
}
int intUnknownAngle = pCorrCptsLt.Count - 2 - intKnownAngleLt.Count;
int intUnknownXYLengthAngle = intUnknownXY + intUnknownLength + intUnknownAngle;
//定义权重矩阵
VBMatrix P = new VBMatrix(intUnknownXYLengthAngle, intUnknownXYLengthAngle);
for (int i = 0; i < intUnknownXY; i++)
{
P[i, i] = 1;
}
for (int i = 0; i < intUnknownLength; i++)
{
P[intUnknownXY + i, intUnknownXY + i] = 10;
}
for (int i = 0; i < intUnknownAngle; i++)
{
P[intUnknownXY + intUnknownLength + i, intUnknownXY + intUnknownLength + i] = 100;
}
//计算初始值矩阵X0
VBMatrix X0 = new VBMatrix(intSumXY, 1);
int intCount = 0;
for (int i = 0; i < pCorrCptsLt.Count; i++)
{
X0[intCount, 0] = (1 - dblProp) * pCorrCptsLt[i].FrCpt.X + dblProp * pCorrCptsLt[i].ToCpt.X;
X0[intCount + 1, 0] = (1 - dblProp) * pCorrCptsLt[i].FrCpt.Y + dblProp * pCorrCptsLt[i].ToCpt.Y;
intCount += 2;
}
//计算长度初始值（全部计算）
double[] adblLength0 = new double[intSumNum - 1];
for (int i = 0; i < pCorrCptsLt.Count - 1; i++)
{
double dblfrsublength = Math.Pow((pCorrCptsLt[i + 1].ToCpt.X - pCorrCptsLt[i].ToCpt.X) * (pCorrCptsLt[i + 1].ToCpt.X - pCorrCptsLt[i].ToCpt.X)
+ (pCorrCptsLt[i + 1].ToCpt.Y - pCorrCptsLt[i].ToCpt.Y) * (pCorrCptsLt[i + 1].ToCpt.Y - pCorrCptsLt[i].ToCpt.Y), 0.5);
double dbltosublength = Math.Pow((pCorrCptsLt[i + 1].ToCpt.X - pCorrCptsLt[i].ToCpt.X) * (pCorrCptsLt[i + 1].ToCpt.X - pCorrCptsLt[i].ToCpt.X)
+ (pCorrCptsLt[i + 1].ToCpt.Y - pCorrCptsLt[i].ToCpt.Y) * (pCorrCptsLt[i + 1].ToCpt.Y - pCorrCptsLt[i].ToCpt.Y), 0.5);
adblLength0[i] = (1 - dblProp) * dblfrsublength + dblProp * dbltosublength;
}
//计算角度初始值（全部计算）
double[] adblAngle0 = new double[intSumNum - 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;
}
//Xmix里存储了XA和X0的最新混合值
VBMatrix Xmix = new VBMatrix(intSumXY, 1);
for (int i = 0; i < X0.Row; i++)
{
Xmix[i, 0] = X0[i, 0];
}
//定义坐标近似值矩阵XA
VBMatrix XA = new VBMatrix(intUnknownXY, 1);
int intKnownCount = 0;
for (int i = 0; i < intUnknownNum; i++)
{
if ((i + intKnownCount) != intKnownLocationLt[intKnownCount]) //当前遍历位置“i + intKnownCount”是否在intKnownLocationLt的位置“intKnownCount”中记录
{
XA[i * 2, 0] = X0[(i + intKnownCount) * 2, 0];
XA[i * 2 + 1, 0] = X0[(i + intKnownCount) * 2 + 1, 0];
}
else
{
intKnownCount += 1;
i -= 1;
}
}
//定义系数矩阵（有关长度和角度的值将在循环中给定）
VBMatrix A = new VBMatrix(intUnknownXYLengthAngle, intUnknownXY);
for (int i = 0; i < intUnknownXY; i++)
{
A[i, i] = 1;
}
double dblJudge = 0; //该值用于判断是否应该跳出循环
double dblOldJudege = 0;
int intIterativeCount = 0;
double[] dblSubDis = new double[intSumNum - 1];
VBMatrix matl = new VBMatrix(intUnknownXYLengthAngle, 1);
do
{
matl = new VBMatrix(intUnknownXYLengthAngle, 1);
int intSumCount1 = 0;
for (int i = 0; i < intUnknownNum; i++)
{
if (pCorrCptsLt[intSumCount1].FrCpt.isCtrl == false)
{
matl[2 * i, 0] = XA[2 * i, 0] - X0[intSumCount1 * 2, 0];
matl[2 * i + 1, 0] = XA[2 * i + 1, 0] - X0[intSumCount1 * 2 + 1, 0];
}
else
{
i -= 1;
}
intSumCount1 += 1;
}
//计算系数矩阵A第"intUnknownXY"行到"intUnknownXY+intUnknownLength-1"行的各元素，即线段长度对各未知数求偏导的值
//先计算各分母值（注意：分母实际上是求偏导后的一部分值，但却恰好等于两点之间距离，因此其计算公式与距离计算公式相同
dblSubDis = new double[intSumNum - 1];
for (int i = 0; i < intSumNum - 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);
}
//开始计算系数矩阵第"intUnknownXY"行到"intUnknownXY+intUnknownLength-1"行的各元素
int intKnownCount2 = 0;
int intUnKnownCount2 = 0;
for (int j = 0; j < intUnknownLength; j++)
{
try
{
int intSumCount = intKnownCount2 + intUnKnownCount2;
if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == false && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == false)
{
A[intUnknownXY + j, 2 * intUnKnownCount2 + 0] = (Xmix[2 * intSumCount, 0] - Xmix[2 * intSumCount + 2, 0]) / dblSubDis[intSumCount];
A[intUnknownXY + j, 2 * intUnKnownCount2 + 1] = (Xmix[2 * intSumCount + 1, 0] - Xmix[2 * intSumCount + 3, 0]) / dblSubDis[intSumCount];
A[intUnknownXY + j, 2 * intUnKnownCount2 + 2] = -A[intUnknownXY + j, 2 * intUnKnownCount2];
A[intUnknownXY + j, 2 * intUnKnownCount2 + 3] = -A[intUnknownXY + j, 2 * intUnKnownCount2 + 1];
matl[intUnknownXY + j, 0] = dblSubDis[intSumCount] - adblLength0[intSumCount]; //图方便，顺便计算matl
intUnKnownCount2 += 1;
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == false && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == true)
{
A[intUnknownXY + j, 2 * intUnKnownCount2 + 0] = (Xmix[2 * intSumCount, 0] - Xmix[2 * intSumCount + 2, 0]) / dblSubDis[intSumCount];
A[intUnknownXY + j, 2 * intUnKnownCount2 + 1] = (Xmix[2 * intSumCount + 1, 0] - Xmix[2 * intSumCount + 3, 0]) / dblSubDis[intSumCount];
matl[intUnknownXY + j, 0] = dblSubDis[intSumCount] - adblLength0[intSumCount]; //图方便，顺便计算matl
intUnKnownCount2 += 1;
}
else if (pCorrCptsLt[intSumCount].FrCpt.isCtrl == true && pCorrCptsLt[intSumCount + 1].FrCpt.isCtrl == false)
{
//注意这种情况，由于"pCorrCptsLt[intSumCount].FrCpt.isCtrl == true"不占位子（即不占列），因此列序号依然为" 2 * intUnKnownCount2 + 0"和" 2 * intUnKnownCount2 + 1"，而不是+2,+3
A[intUnknownXY + j, 2 * intUnKnownCount2 + 0] = -(Xmix[2 * intSumCount, 0] - Xmix[2 * intSumCount + 2, 0]) / dblSubDis[intSumCount];
A[intUnknownXY + j, 2 * intUnKnownCount2 + 1] = -(Xmix[2 * intSumCount + 1, 0] - Xmix[2 * intSumCount + 3, 0]) / dblSubDis[intSumCount];
matl[intUnknownXY + j, 0] = dblSubDis[intSumCount] - adblLength0[intSumCount]; //图方便，顺便计算matl
intKnownCount2 += 1;
}
else
{
intKnownCount2 += 1;
j -= 1;
}
}
catch (Exception e)
{
throw;
}
}
//计算系数矩阵A第"intUnknownXY+intUnknownLength"行到"intUnknownXY+intUnknownLength+intUnknownAngle"行的各元素，即角度对各未知数求偏导的值
int intKnownCount3 = 0;
int intUnKnownCount3 = 0;
int intUnKnownXYLength = intUnknownXY + intUnknownLength;
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 (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
{
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]);
matl[intUnKnownXYLength + j, 0] = dblNewAngle - adblAngle0[intSumCount]; //图方便，顺便计算matl
int intPreTrueNum = 0;
int intUnKnownCount3orginal = intUnKnownCount3;
int intKnownCount3orginal = intKnownCount3;
if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3 + 0].FrCpt.isCtrl == false)
{
A[intUnKnownXYLength + j, 2 * intUnKnownCount3orginal + 0] = -(Xmix[2 * intSumCount + 1, 0] - Xmix[2 * intSumCount + 3, 0]) / dblA2;
A[intUnKnownXYLength + j, 2 * intUnKnownCount3orginal + 1] = (Xmix[2 * intSumCount + 0, 0] - Xmix[2 * intSumCount + 2, 0]) / dblA2;
intUnKnownCount3 += 1;
}
else
{
intPreTrueNum += 1;
intKnownCount3 += 1;
}
if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3orginal + 1].FrCpt.isCtrl == false) //注意：对中间点的X、Y求导时，其导数由两部分组成，且为第二部分和第一部分的差
{
A[intUnKnownXYLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 2] = (Xmix[2 * intSumCount + 5, 0] - Xmix[2 * intSumCount + 3, 0]) / dblB2 - (Xmix[2 * intSumCount + 1, 0] - Xmix[2 * intSumCount + 3, 0]) / dblA2;
A[intUnKnownXYLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 3] = -(Xmix[2 * intSumCount + 4, 0] - Xmix[2 * intSumCount + 2, 0]) / dblB2 + (Xmix[2 * intSumCount + 0, 0] - Xmix[2 * intSumCount + 2, 0]) / dblA2;
}
else
{
intPreTrueNum += 1;
}
if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3orginal + 2].FrCpt.isCtrl == false)
{
A[intUnKnownXYLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 4] = -(Xmix[2 * intSumCount + 5, 0] - Xmix[2 * intSumCount + 3, 0]) / dblB2;
A[intUnKnownXYLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 5] = (Xmix[2 * intSumCount + 4, 0] - Xmix[2 * intSumCount + 2, 0]) / dblB2;
}
}
#region 用余弦值求夹角，无法判定角度方向，效果不好
//int intSumCount = intKnownCount3 + intUnKnownCount3;
////常用数据准备
//double dblA2 = dblSubDis[intSumCount] * dblSubDis[intSumCount];
//double dblB2 = dblSubDis[intSumCount + 1] * dblSubDis[intSumCount + 1];
//double dblAB = dblSubDis[intSumCount] * dblSubDis[intSumCount + 1];
//double dblC2 = (Xmix[2 * intSumCount, 0] - Xmix[2 * intSumCount + 4, 0]) * (Xmix[2 * intSumCount, 0] - Xmix[2 * intSumCount + 4, 0])
// + (Xmix[2 * intSumCount + 1, 0] - Xmix[2 * intSumCount + 5, 0]) * (Xmix[2 * intSumCount + 1, 0] - Xmix[2 * intSumCount + 5, 0]);
//double dblA2B2mC2 = dblA2 + dblB2 - dblC2;
//double dblpart = 1 / Math.Sqrt(4 * dblA2 * dblB2 - dblA2B2mC2 * dblA2B2mC2);
////开始计算系数值，由于将以下三个情况排列组合将有八种情况，因此按如下方式计算
//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
//{
// matl[intUnKnownXYLength + j, 0] = Math.Acos(dblA2B2mC2 / 2 / dblAB) - adblAngle0[intSumCount]; //图方便，顺便计算matl
// int intPreTrueNum = 0;
// int intUnKnownCount3orginal = intUnKnownCount3;
// int intKnownCount3orginal = intKnownCount3;
// if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3 + 0].FrCpt.isCtrl == false)
// {
// A[intUnKnownXYLength + j, 2 * intUnKnownCount3orginal + 0] = dblpart * (2 * dblA2 * (Xmix[2 * intSumCount + 2, 0] - Xmix[2 * intSumCount + 4, 0]) + dblA2B2mC2 * (Xmix[2 * intSumCount + 0, 0] - Xmix[2 * intSumCount + 2, 0])) / dblA2;
// A[intUnKnownXYLength + j, 2 * intUnKnownCount3orginal + 1] = dblpart * (2 * dblA2 * (Xmix[2 * intSumCount + 3, 0] - Xmix[2 * intSumCount + 5, 0]) + dblA2B2mC2 * (Xmix[2 * intSumCount + 1, 0] - Xmix[2 * intSumCount + 3, 0])) / dblA2;
// intUnKnownCount3 += 1;
// }
// else
// {
// intPreTrueNum += 1;
// intKnownCount3 += 1;
// }
// if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3orginal + 1].FrCpt.isCtrl == false)
// {
// A[intUnKnownXYLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 2] = dblpart * (2 * dblAB * (Xmix[2 * intSumCount + 4, 0] + Xmix[2 * intSumCount + 0, 0] - 2 * Xmix[2 * intSumCount + 2, 0]) + dblA2B2mC2 * (dblB2 * (Xmix[2 * intSumCount + 2, 0] - Xmix[2 * intSumCount + 0, 0]) + dblA2 * (Xmix[2 * intSumCount + 2, 0] - Xmix[2 * intSumCount + 4, 0]))) / dblAB;
// A[intUnKnownXYLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 3] = dblpart * (2 * dblAB * (Xmix[2 * intSumCount + 5, 0] + Xmix[2 * intSumCount + 1, 0] - 2 * Xmix[2 * intSumCount + 3, 0]) + dblA2B2mC2 * (dblB2 * (Xmix[2 * intSumCount + 3, 0] - Xmix[2 * intSumCount + 1, 0]) + dblA2 * (Xmix[2 * intSumCount + 3, 0] - Xmix[2 * intSumCount + 5, 0]))) / dblAB;
// }
// else
// {
// intPreTrueNum += 1;
// }
// if (pCorrCptsLt[intUnKnownCount3orginal + intKnownCount3orginal + 2].FrCpt.isCtrl == false)
// {
// A[intUnKnownXYLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 4] = dblpart * (2 * dblB2 * (Xmix[2 * intSumCount + 2, 0] - Xmix[2 * intSumCount + 0, 0]) + dblA2B2mC2 * (Xmix[2 * intSumCount + 4, 0] - Xmix[2 * intSumCount + 2, 0])) / dblB2;
// A[intUnKnownXYLength + j, 2 * (intUnKnownCount3orginal - intPreTrueNum) + 5] = dblpart * (2 * dblA2 * (Xmix[2 * intSumCount + 3, 0] - Xmix[2 * intSumCount + 1, 0]) + dblA2B2mC2 * (Xmix[2 * intSumCount + 5, 0] - Xmix[2 * intSumCount + 3, 0])) / dblB2;
// }
//}
#endregion
}
//记录一个值以协助判断是否可以退出循环
double dblLast = XA[0, 0];
////计算新的近似值
//SaveFileDialog SFD = new SaveFileDialog();
//SFD.ShowDialog();
// CHelpFuncExcel.ExportDataToExcelA(A, "maxA", SFD.FileName);
//CHelpFuncExcel.ExportDataToExcelP(P, "maxP", SFD.FileName);
//CHelpFuncExcel.ExportDataToExcel2(matl, "maxmatl", SFD.FileName);
//VBMatrix Temp =A.Trans () * P * A;
//Temp.InvertGaussJordan();
//XA -= Temp * A.Transpose() * P * matl;
XA -= InvAtPAAtPmatl(A, P, matl);
//更新Xmix和matl
int intKnownCount4 = 0;
for (int i = 0; i < intUnknownNum; i++)
{
if ((i + intKnownCount4) != intKnownLocationLt[intKnownCount4])
{
Xmix[(i + intKnownCount4) * 2, 0] = XA[i * 2, 0];
Xmix[(i + intKnownCount4) * 2 + 1, 0] = XA[i * 2 + 1, 0];
}
else
{
intKnownCount4 += 1;
i -= 1;
}
}
dblJudge = Math.Abs(dblLast - XA[0, 0]);
intIterativeCount += 1;
if (intIterativeCount >= 10000)
{
break;
}
if (dblOldJudege == dblJudge)
{
break;
}
dblOldJudege = dblJudge;
} while (dblJudge > dblTX);
//生成目标线段
List <CPoint> CTargetPtLt = new List <CPoint>();
for (int i = 0; i < intSumNum; 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);
}
