private static double GetDividedMinAngle(CEdge newCEdge,
CEdge firstCEdge, CEdge secondCEdge, CEdge thirdCEdge, CEdge fourthCEdge)
{
var dblAngle1 = CGeoFunc.CalAngle_Counterclockwise(firstCEdge.dblAxisAngle, newCEdge.dblAxisAngle);
var dblAngle2 = CGeoFunc.CalAngle_Counterclockwise(newCEdge.dblAxisAngle, secondCEdge.dblAxisAngle);
var dblAngle3 = Math.PI - CGeoFunc.CalAngle_Counterclockwise(newCEdge.dblAxisAngle, thirdCEdge.dblAxisAngle);
var dblAngle4 = Math.PI - CGeoFunc.CalAngle_Counterclockwise(fourthCEdge.dblAxisAngle, newCEdge.dblAxisAngle);
return(Math.Min(dblAngle1, Math.Min(dblAngle2, Math.Min(dblAngle3, dblAngle4))));
}
private void DetermineOuterOrHole(List <CPolygon> rawfaceLt)
{
foreach (CPolygon cpg in rawfaceLt)
{
var AxisAngleCEdgeLt = cpg.LeftMostCpt.AxisAngleCEdgeLt;
//find UpperCorrectAxisAngleCEdge and LowerCorrectAxisAngleCEdge
//UpperCorrectAxisAngleCEdge has angle most close to 90 degrees
//LowerCorrectAxisAngleCEdge has angle most close to 270 degrees
var UpperCorrectAxisAngleCEdge = AxisAngleCEdgeLt[0];
var dblUpperAngleToHalfPI = MakeAngleCorrect(
CGeoFunc.CalAngle_Counterclockwise(AxisAngleCEdgeLt[0].dblAxisAngle, CConstants.dblHalfPI));
var LowerCorrectAxisAngleCEdge = AxisAngleCEdgeLt[0];
var dblLowerAngleToHalfPI = dblUpperAngleToHalfPI;
for (int i = 1; i < AxisAngleCEdgeLt.Count; i++)
{
double dblAngleToHalfPI = MakeAngleCorrect(
CGeoFunc.CalAngle_Counterclockwise(AxisAngleCEdgeLt[i].dblAxisAngle, CConstants.dblHalfPI));
if (dblAngleToHalfPI <= dblUpperAngleToHalfPI)
{
dblUpperAngleToHalfPI = dblAngleToHalfPI;
UpperCorrectAxisAngleCEdge = AxisAngleCEdgeLt[i];
}
if (dblAngleToHalfPI > dblLowerAngleToHalfPI)
{
dblLowerAngleToHalfPI = dblAngleToHalfPI;
LowerCorrectAxisAngleCEdge = AxisAngleCEdgeLt[i];
}
}
cpg.IsHole = false; //a hole means that the direction is clockwise
var TestCEdgeStartAtExtreme = cpg.cedgeStartAtLeftMost;
do
{
if (TestCEdgeStartAtExtreme.cpgIncidentFace.GID == cpg.GID) //TestCEdgeStartAtExtreme belongs to face cpg
{
//if the component is a hole, the following must hold
if (TestCEdgeStartAtExtreme.GID == UpperCorrectAxisAngleCEdge.GID &&
TestCEdgeStartAtExtreme.cedgePrev.GID == LowerCorrectAxisAngleCEdge.cedgeTwin.GID)
{
cpg.IsHole = true; //inner boundary
break;
}
}
TestCEdgeStartAtExtreme = TestCEdgeStartAtExtreme.GetLargerAxisAngleCEdge();
} while (TestCEdgeStartAtExtreme.GID != cpg.cedgeStartAtLeftMost.GID);
}
}
private static bool IsBaselineRightHandSide(CEdge CEdgeBaseline, CEdge CEdgeRef)
{
double dblAngle = CGeoFunc.CalAngle_Counterclockwise(CEdgeBaseline, CEdgeRef);
if (dblAngle < Math.PI) //it doesn't matter we are simplifying a exterior ring or a interior ring (hole)
{
return(true);
}
else
{
return(false);
}
}
public override void btnRun_Click(object sender, EventArgs e)
{
List <int> intSetoff = new List <int>();
for (int i = 0; i < _CPolylineLt.Count; i++)
{
if (_CPolylineLt[i].CptLt[0].Z > _CPolylineLt[i].CptLt[_CPolylineLt[i].CptLt.Count - 1].Z)
{
CDrawInActiveView.ViewPolyline(_DataRecords.ParameterInitialize.m_mapControl, _CPolylineLt[i]);
}
else
{
intSetoff.Add(_CPolylineLt[i].ID);
}
}
CDrawInActiveView.ViewPolyline(_DataRecords.ParameterInitialize.m_mapControl, _CPolylineLt[intCount]);
intCount += 1;
int kk = 5;
//以最后两个点为基点
//_CPolyline = _CPolylineLt[1];
for (int i = 0; i < _CPolylineLt.Count; i++)
{
if (_CPolylineLt[i].ID == 10)
{
_CPolyline = _CPolylineLt[i];
break;
}
}
CPolyline cpl = _CPolyline;
CDrawInActiveView.ViewPolyline(_DataRecords.ParameterInitialize.m_mapControl, cpl);
int intPtCount = cpl.CptLt.Count;
//计算长度初始值（全部计算）
_adblLength0 = new double[intPtCount - 2];
for (int i = 0; i < intPtCount - 2; i++)
{
_adblLength0[i] = CGeoFunc.CalDis(cpl.CptLt[i], cpl.CptLt[i + 1]);
}
//计算角度初始值
_adblAngle0 = new double[intPtCount - 2];
for (int i = 0; i < intPtCount - 2; i++)
{
_adblAngle0[i] = CGeoFunc.CalAngle_Counterclockwise(cpl.CptLt[i], cpl.CptLt[i + 1], cpl.CptLt[i + 2]);
}
}
protected static void SetCpbAngleDiffLt(CPolyBase cpb)
{
var cedgelt = cpb.CEdgeLt;
var dblAngleDiffLt = new List <double>(cedgelt.Count);
dblAngleDiffLt.Add(CGeoFunc.CalAngle_Counterclockwise
(cedgelt.Last().dblAxisAngle, cedgelt[0].dblAxisAngle));
for (int i = 0; i < cedgelt.Count - 1; i++)
{
dblAngleDiffLt.Add(CGeoFunc.CalAngle_Counterclockwise
(cedgelt[i].dblAxisAngle, cedgelt[i + 1].dblAxisAngle));
}
cpb.dblAngleDiffLt = dblAngleDiffLt;
}
public void UnifyDirectionsPolylines()
{
var ParameterInitialize = _ParameterInitialize;
//List<CPolyline> pCPlLt = this.ObjCGeoLtLt[0].AsExpectedClassEb<CPolyline, CGeoBase>().ToList();
List <CPolyline> LSCPlLt = this.ObjCGeoLtLt[0].AsExpectedClassEb <CPolyline, CGeoBase>().ToList();
List <CPolyline> SSCPlLt = this.ObjCGeoLtLt[1].AsExpectedClassEb <CPolyline, CGeoBase>().ToList();
for (int i = 0; i < LSCPlLt.Count; i++)
{
double dblAngleDiff = CGeoFunc.CalAngle_Counterclockwise(LSCPlLt[i].FrCpt, LSCPlLt[i].ToCpt, SSCPlLt[i].FrCpt, SSCPlLt[i].ToCpt);
if ((Math.Abs(dblAngleDiff) > (Math.PI / 2) && Math.Abs(dblAngleDiff) < (3 * Math.PI / 2)))
{
LSCPlLt[i].ReverseCpl();
}
}
CSaveFeature.SaveCGeoEb(LSCPlLt, esriGeometryType.esriGeometryPolyline, ParameterInitialize.pFLayerLt[0].Name + "UnifiedDirections");
}
private void SubRecursivelyConfirmMoveInfo(CPolyline cpl, CVirtualPolyline pVtPl, double dblTDis)
{
if (pVtPl.CLeftPolyline == null)
{
return;
}
cpl.CptLt[pVtPl.intMaxDisID].isMoveable = true;
CPoint frcpt = cpl.CptLt[pVtPl.intFrID];
CPoint tocpt = cpl.CptLt[pVtPl.intToID];
CPoint cpt = cpl.CptLt[pVtPl.intMaxDisID];
double dblPreDis = frcpt.DistanceTo(cpt);
double dblSucDis = tocpt.DistanceTo(cpt);
double dblTotalDis = dblPreDis + dblSucDis;
double dblRatio = 0;
if (dblTotalDis != 0)
{
dblRatio = dblPreDis / dblTotalDis;
}
//double dblTargetX = (1 - dblRatio) * frcpt.X + dblRatio * tocpt.X;
//double dblTargetY = (1 - dblRatio) * frcpt.Y + dblRatio * tocpt.Y;
//CPoint targetcpt = new CPoint(-1, dblTargetX,dblTargetY);
CPoint targetcpt = CGeoFunc.GetInbetweenCpt(frcpt, tocpt, dblRatio);
pVtPl.dblRatioforMovePt = dblRatio;
pVtPl.dblLengthforMovePt = CGeoFunc.CalDis(targetcpt, cpt);
pVtPl.dblAngleDiffforMovePt = CGeoFunc.CalAngle_Counterclockwise(cpt, targetcpt, tocpt);
//pVtPl.dblDifffromMovePtX = cpt.X - dblTargetX;
//pVtPl.dblDifffromMovePtY = cpt.Y - dblTargetY;
SubRecursivelyConfirmMoveInfo(cpl, pVtPl.CLeftPolyline, dblTDis);
SubRecursivelyConfirmMoveInfo(cpl, pVtPl.CRightPolyline, dblTDis);
}
/// <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, CPolyline lastcpl)
{
//this.intIterationNum = Convert.ToInt32(_DataRecords.ParameterInitialize .txtIterationNum .Text );
if (dblProp == 0)
{
int aa = 5;
}
//intIterationNum = Convert.ToInt32(_DataRecords.ParameterInitialize.txtIterationNum.Text); //the maximum itrative times
List <CCorrCpts> pCorrCptsLt = _DataRecords.ParameterResult.CCorrCptsLt; //读取数据后，此处ResultPtLt中的对应点为一一对应
double dblTX = _dblTX;
int intPtNum = pCorrCptsLt.Count;
int intXYNum = 2 * intPtNum;
////generate the list of vertices based on linear interpolation
//List<CPoint> newcptlt = new List<CPoint>();
//for (int i = 0; i < pCorrCptsLt.Count ; i++)
//{
// 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;
// CPoint newcpt = new CPoint(i, dblnewx, dblnewy);
// newcptlt.Add(newcpt);
//}
//计算长度初始值（全部计算）
double dblTune = 3;
double[] adblLength0 = new double[intPtNum - 1];
double dblSumFrLength0 = 0;
double dblSumToLength0 = 0;
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);
//double dble=Math .E;
//double dbla = (dblfrsublength - dbltosublength) * dble / (dble - 1);
//double dblb = (dble * dbltosublength - dblfrsublength) / (dble - 1);
//adblLength0[i] = dbla * Math.Pow(dble, -dblProp) + dblb;
adblLength0[i] = (1 - dblProp) * dblfrsublength + dblProp * dbltosublength;
//adblLength0[i] = (1 - Math.Pow(dblProp, 1 / dblTune)) * dblfrsublength + Math.Pow(dblProp, 1 / dblTune) * dbltosublength;
//dblSumFrLength0 = dblSumFrLength0 + dblfrsublength;
//dblSumToLength0 = dblSumToLength0 + dbltosublength;
//pCorrCptsLt[i].FrCpt.isCtrl = false; //仅以最开始两对和最终两队对应点为固定点，故此先设置为false
}
//double dblFrRealSumLength = dblSumFrLength0 - CGeoFunc.CalDis(pCorrCptsLt[0].FrCpt, pCorrCptsLt[1].FrCpt) - CGeoFunc.CalDis(pCorrCptsLt[intPtNum - 2].FrCpt, pCorrCptsLt[intPtNum - 1].FrCpt);
//double dblToRealSumLength = dblSumToLength0 - CGeoFunc.CalDis(pCorrCptsLt[0].ToCpt, pCorrCptsLt[1].ToCpt) - CGeoFunc.CalDis(pCorrCptsLt[intPtNum - 2].ToCpt, pCorrCptsLt[intPtNum - 1].ToCpt);
//计算角度初始值（全部计算）
double[] adblAngle0 = new double[intPtNum - 2];
double dblSumFrAngle0 = 0;
double dblSumToAngle0 = 0;
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);
//double dble = Math.E;
//double dbla = (dbltoAngle - dblfrAngle) * dble / (dble - 1);
//double dblb = (dble * dblfrAngle - dbltoAngle) / (dble - 1);
////角度初始值
//adblAngle0[i] = dbla * Math.Pow(dble, dblProp-1) + dblb;
adblAngle0[i] = (1 - dblProp) * dblfrAngle + dblProp * dbltoAngle;
//adblAngle0[i] = (1 - Math.Pow(dblProp, dblTune)) * dblfrAngle + Math.Pow(dblProp, dblTune) * dbltoAngle;
//dblSumFrAngle0 = dblSumFrAngle0 + dblfrAngle;
//dblSumToAngle0 = dblSumToAngle0 + dbltoAngle;
}
//double dblLengthAngleRatio = (dblSumFrToLength0 / (pCorrCptsLt.Count - 1)) / (dblSumFrToAngle0 / (pCorrCptsLt.Count - 2));
//for (int i = 0; i < pCorrCptsLt.Count - 2; i++)
//{
// adblAngle0[i] = CGeoFunc.CalAngle_Counterclockwise(newcptlt[i], newcptlt[i + 1], newcptlt[i + 2]);
//}
//计算坐标初始值，以及各线段方位角初始值
//for (int i = 0; i < pCorrCptsLt.Count; i++)
//{
// pCorrCptsLt[i].FrCpt.isCtrl = false;
//}
//注意：默认固定第一条边
pCorrCptsLt[0].FrCpt.isCtrl = true;
pCorrCptsLt[1].FrCpt.isCtrl = true;
//固定最后两条边
pCorrCptsLt[intPtNum - 2].FrCpt.isCtrl = true;
pCorrCptsLt[intPtNum - 1].FrCpt.isCtrl = true;
//统计插值点数
int intKnownPt = 0; //固定点的数目
int intUnknownPt = 0; //非固定点的数目
bool[] isFixed = new bool[intPtNum]; // is vertex i fixed?
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;
isFixed[i] = true;
}
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;
List <CPoint> lastcptlt = lastcpl.CptLt;
#region new codes
////以上一次结果的值作为新的估算值
double[] adblx = new double[intPtNum];
double[] adbly = new double[intPtNum];
for (int i = 0; i < intPtNum; i++)
{
if (pCorrCptsLt[i].FrCpt.isCtrl == false)
{
adblx[i] = lastcptlt[i].X;
adbly[i] = lastcptlt[i].Y;
}
else
{
adblx[i] = (1 - dblProp) * pCorrCptsLt[i].FrCpt.X + dblProp * pCorrCptsLt[i].ToCpt.X;
adbly[i] = (1 - dblProp) * pCorrCptsLt[i].FrCpt.Y + dblProp * pCorrCptsLt[i].ToCpt.Y;
}
}
double[] weight = new double[intUnknownLengthAngle];
for (int i = 0; i < intUnknownLength; i++)
{
weight[i] = 1;
//weight[i] = 0.0036;
}
for (int i = 0; i < intUnknownAngle; i++)
{
weight[intUnknownLength + i] = 39.48;
}
double[] xout = new double[intPtNum];
double[] yout = new double[intPtNum];
//CPlusClass.Matrix.leastSquaresAdjust(intPtNum, adblx, adbly, isFixed,
//adblLength0, adblAngle0, weight, dblTX, intIterationNum, xout, yout);
//MessageBox.Show("" + xout[5]);
//生成目标线段
List <CPoint> CTargetPtLt = new List <CPoint>();
for (int i = 0; i < intPtNum; i++)
{
CPoint cpt = new CPoint(i);
cpt.X = xout[i];
cpt.Y = yout[i];
if (pCorrCptsLt[i].FrCpt.isCtrl == true)
{
cpt.isCtrl = true;
}
else
{
cpt.isCtrl = false;
}
CTargetPtLt.Add(cpt);
}
CPolyline cpl = new CPolyline(0, CTargetPtLt);
cpl.CreateSubPllt();
#endregion
#region old codes
////定义权重矩阵***************************************************************************************定义权重矩阵************************************************************************************************定义权重矩阵//
//VBMatrix P = new VBMatrix(intUnknownLengthAngle, intUnknownLengthAngle);
//for (int i = 0; i < intUnknownLength; i++)
//{
// P[i, i] = 0.0036;
//}
//for (int i = 0; i < intUnknownAngle; i++)
//{
// P[intUnknownLength + i, intUnknownLength + i] = 40;
// //P[intUnknownLength + i, intUnknownLength + i] = 0.0002;
//}
////以上一次结果的值作为新的估算值
//VBMatrix X0 = new VBMatrix(intXYNum, 1);
//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;
// }
//}
////定义坐标近似值矩阵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)
// {
// XA[i * 2 + 0, 0] = X0[intSumCount0 * 2, 0];
// XA[i * 2 + 1, 0] = X0[intSumCount0 * 2 + 1, 0];
// XA0[i * 2 + 0, 0] = XA[i * 2 + 0, 0];
// XA0[i * 2 + 1, 0] = XA[i * 2 + 1, 0];
// }
// 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(intUnknownLengthAngle, 1);
////定义系数矩阵A(各方程对坐标的导数值)，A的导数值将在循环中给出
//VBMatrix A = new VBMatrix(intUnknownLengthAngle, intUnknownXY);
//VBMatrix V = new VBMatrix();
//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]; ;
//VBMatrix LPlusV = new VBMatrix(intUnknownLengthAngle, 1);
//do
//{
// //计算系数矩阵A第0行到"intUnknownLength"行的各元素，即线段长度对各未知数求偏导的值
// //先计算各分母值（注意：分母实际上是求偏导后的一部分值，但却恰好等于两点之间距离，因此其计算公式与距离计算公式相同
// adblSubDis = new double[intPtNum - 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;
// }
// //计算系数矩阵中关于长度值的导数部分
// _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);
// //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 InvTemp = Temp.Inv(Temp);
// VBMatrix x = InvTemp * A.Trans() * P * matl;
// //CHelpFuncExcel.ExportDataToExcel2(x, "maxX", _DataRecords.ParameterInitialize.strSavePath);
// XA += x;
// //CHelpFuncExcel.ExportDataToExcel2(x, "maxX", _DataRecords.ParameterInitialize.strSavePath);
// //CHelpFuncExcel.ExportDataToExcel2(XA, "maxXA", _DataRecords.ParameterInitialize.strSavePath);
// V = A * x - matl;
// //VtPV值
// double dblVtPV = (V.Trans() * P * V).MatData[0, 0];
// //XA -= x;
// _DataRecords.ParameterInitialize.txtVtPV.Text = " VtPV = " + dblVtPV.ToString();
// //VBMatrix L = new VBMatrix(intUnknownLengthAngle, 1);
// //for (int j = 0; j < intUnknownLength; j++)
// //{
// // L[j, 0] = adblSubDis[j];
// //}
// //for (int j = 0; j < intUnknownAngle; j++)
// //{
// // L[intUnknownLength+ j, 0] = adblAngle[j];
// //}
// //LPlusV = L + V;
// //对坐标值进行改正
// 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;
// }
// ////计算系数矩阵A第0行到"intUnknownLength"行的各元素，即线段长度对各未知数求偏导的值
// ////先计算各分母值（注意：分母实际上是求偏导后的一部分值，但却恰好等于两点之间距离，因此其计算公式与距离计算公式相同
// //for (int i = 0; i < intPtNum - 1; i++)
// //{
// // L[i,0] = 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++)
// //{
// // L[intUnknownLength + i -1, 0] = 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]);
// //}
// //CHelpFuncExcel.ExportDataToExcel2(LPlusV, "matLPlusV", _DataRecords.ParameterInitialize.strSavePath);
// //CHelpFuncExcel.ExportDataToExcel2(L, "matL", _DataRecords.ParameterInitialize.strSavePath);
// if (intIterativeCount == 50)
// {
// int kk = 5;
// }
// intIterativeCount += 1;
// if (intIterativeCount >= intIterationNum)
// {
// break;
// }
// //这里只是随便取两个中间值以观测是否收敛
// double dblLength0 = CGeoFunc.CalLengthofVector(x);
// if (dblLength0 <= dblTX)
// {
// break;
// }
//} while (true);
////生成目标线段
//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();
#endregion
////记录各平差成果
////坐标改正值
//VBMatrix Xc = XA-XA0;
////观测值改正值矩阵V
//VBMatrix V = A * Xc + matl;
////VtPV值
//cpl.dblVtPV = (V.Trans() * P * V).MatData[0, 0];
//VBMatrix VLength = V.GetSubMatrix(0, intUnknownLength, 0, 1);
//VBMatrix VAngle = V.GetSubMatrix(intUnknownLength, intUnknownAngle, 0, 1);
//VBMatrix PLength = P.GetSubMatrix(0, intUnknownLength, 0, intUnknownLength);
//VBMatrix PAngle = P.GetSubMatrix(intUnknownLength, intUnknownAngle, intUnknownLength, intUnknownAngle);
//VBMatrix VtPVLength = VLength.Trans() * PLength * VLength;
//VBMatrix VtPVAngle = VAngle.Trans() * PAngle * VAngle;
//double dblVtPVLength = VtPVLength[0, 0];
//double dblVtPVAngle = VtPVAngle[0, 0];
//double pdblVtPV = (V.Trans() * P * V)[0, 0];
//double ss = dblProp;
////
//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].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;
// }
//}
//intIterationNum++;
return(cpl);
//return null;
}
/// <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>
/// 获取线状要素
/// </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;
}
private double CalSimilarity(List <CCorrCpts> pCorrCptsLt, CPolyline frcpl, CPolyline tocpl)
{
//数据准备
//X、Y的差值
double[] dblFrDiffX = new double[pCorrCptsLt.Count - 1];
double[] dblFrDiffY = new double[pCorrCptsLt.Count - 1];
double[] dblToDiffX = new double[pCorrCptsLt.Count - 1];
double[] dblToDiffY = new double[pCorrCptsLt.Count - 1];
for (int i = 0; i < pCorrCptsLt.Count - 1; i++)
{
dblFrDiffX[i] = pCorrCptsLt[i + 1].FrCpt.X - pCorrCptsLt[i].FrCpt.X;
dblFrDiffY[i] = pCorrCptsLt[i + 1].FrCpt.Y - pCorrCptsLt[i].FrCpt.Y;
dblToDiffX[i] = pCorrCptsLt[i + 1].ToCpt.X - pCorrCptsLt[i].ToCpt.X;
dblToDiffY[i] = pCorrCptsLt[i + 1].ToCpt.Y - pCorrCptsLt[i].ToCpt.Y;
}
//各线段长度
double[] dblFrDis = new double[pCorrCptsLt.Count - 1];
double[] dblToDis = new double[pCorrCptsLt.Count - 1];
for (int i = 0; i < pCorrCptsLt.Count - 1; i++)
{
dblFrDis[i] = Math.Sqrt(dblFrDiffX[i] * dblFrDiffX[i] + dblFrDiffY[i] * dblFrDiffY[i]);
dblToDis[i] = Math.Sqrt(dblToDiffX[i] * dblToDiffX[i] + dblToDiffY[i] * dblToDiffY[i]);
}
//各线段累积方位角（IAzimuth: Integral Azimuth）
double[] adblFrIAzimuth = new double[pCorrCptsLt.Count - 1];
double[] adblToIAzimuth = new double[pCorrCptsLt.Count - 1];
adblFrIAzimuth[0] = CGeoFunc.CalAxisAngle(dblFrDiffX[0], dblFrDiffY[0]);
adblToIAzimuth[0] = CGeoFunc.CalAxisAngle(dblToDiffX[0], dblToDiffY[0]);
for (int i = 1; i < pCorrCptsLt.Count - 1; i++)
{
adblFrIAzimuth[i] = adblFrIAzimuth[i - 1] + CGeoFunc.CalAngle_Counterclockwise(pCorrCptsLt[i - 1].FrCpt, pCorrCptsLt[i].FrCpt, pCorrCptsLt[i + 1].FrCpt) - Math.PI;
adblToIAzimuth[i] = adblToIAzimuth[i - 1] + CGeoFunc.CalAngle_Counterclockwise(pCorrCptsLt[i - 1].ToCpt, pCorrCptsLt[i].ToCpt, pCorrCptsLt[i + 1].ToCpt) - Math.PI;
}
double dblAreaFr = 0;
double dblAreaTo = 0;
double dblAreaDiff = 0;
for (int i = 0; i < pCorrCptsLt.Count - 1; i++)
{
double dblRatioFr = dblFrDis[i] / frcpl.pPolyline.Length;
double dblRatioTo = dblToDis[i] / tocpl.pPolyline.Length;
double dblRatioAv = (dblFrDis[i] + dblToDis[i]) / (frcpl.pPolyline.Length + tocpl.pPolyline.Length);
dblAreaFr += (Math.Abs(adblFrIAzimuth[i]) * dblFrDis[i]);
dblAreaTo += (Math.Abs(adblToIAzimuth[i]) * dblToDis[i]);
dblAreaDiff += (Math.Abs((adblFrIAzimuth[i] - adblToIAzimuth[i])) * (dblFrDis[i] + dblToDis[i]));
}
dblAreaFr = dblAreaFr / frcpl.pPolyline.Length;
dblAreaTo = dblAreaTo / tocpl.pPolyline.Length;
dblAreaDiff = dblAreaDiff / (frcpl.pPolyline.Length + tocpl.pPolyline.Length);
double dblDis = 0;
if (dblAreaFr >= dblAreaTo)
{
dblDis = dblAreaDiff / dblAreaFr;
}
else
{
dblDis = dblAreaDiff / dblAreaTo;
}
double dblSimilarity = 1 - dblDis;
return(dblSimilarity);
}
/// <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);
}
