Пример #1
0
        /// <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);
        }
Пример #2
0
        /// <summary>
        /// 计算Inv(A'*P*A)*A'*P*matl,时间复杂度(2n)^2,n为顶点个数
        /// </summary>
        /// <param name="A">系数矩阵:该矩阵从第0行到第i行为对角矩阵,i行之后为全矩阵</param>
        /// <param name="P">权重矩阵:对角矩阵</param>
        /// <param name="matl">初值矩阵:列向量</param>
        /// <returns>鉴于本实验矩阵的特殊性,可以使用该函数进行计算。</returns>
        public VBMatrix InvAtPAAtPmatl(VBMatrix A, VBMatrix P, VBMatrix matl)
        {
            int intRowA        = A.Row;
            int intColA        = A.Col;
            int intDiffRowColA = intRowA - intColA;

            //计算A'*P
            VBMatrix MatrixAtP = new VBMatrix(intColA, intRowA);

            for (int i = 0; i < intColA; i++)
            {
                //第i行i列的值
                MatrixAtP[i, i] = A[i, i] * P[i, i];
                //第i行最后intDiffRowColA列的值
                for (int j = intColA; j < intRowA; j++)
                {
                    MatrixAtP[i, j] = A[j, i] * P[j, j];
                }
            }

            //计算A'*P*A
            VBMatrix MatrixAtPA = new VBMatrix(intColA, intColA);

            for (int i = 0; i < intColA; i++)
            {
                //第i行i列的值(即对角线上的值)
                double dblValue = MatrixAtP[i, i] * A[i, i];
                for (int j = intColA; j < intRowA; j++)
                {
                    dblValue += MatrixAtP[i, j] * A[j, i];
                }
                MatrixAtPA[i, i] = dblValue;

                //第i行j列和j行i列的值(注意:MatrixAtPA[i, j] == MatrixAtPA[j, i])
                for (int j = i + 1; j < intColA; j++)
                {
                    for (int k = 0; k < intDiffRowColA; k++)
                    {
                        MatrixAtPA[i, j] += MatrixAtP[i, intColA + k] * A[intColA + k, j];
                    }
                    MatrixAtPA[j, i] = MatrixAtPA[i, j];
                }
            }

            //计算A'*P*matl
            VBMatrix MatrixAtPmatl = new VBMatrix(intColA, 1);

            for (int i = 0; i < intColA; i++)
            {
                double dblValue = MatrixAtP[i, i] * matl[i, 0];
                for (int j = intColA; j < intRowA; j++)
                {
                    dblValue += MatrixAtP[i, j] * matl[j, 0];
                }
                MatrixAtPmatl[i, 0] = dblValue;
            }

            //计算Inv(A'*P*A)*A'*P*matl
            MatrixAtPA.Inv(MatrixAtPA);
            VBMatrix MatrixResult = MatrixAtPA * MatrixAtPmatl;

            return(MatrixResult);
        }
Пример #3
0
        /// <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);
        }
Пример #4
0
        /// <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;
        }