Exemple #1
0
        //通过配置的轮子最大加速度进行降速
        //适当比例的降速,算完后记得把最新速度数据放在ringbuffer里
        //wheelOne 一号轮速度数组首地址
        //wheelTwo  二号轮速度数组首地址
        //wheelThree 三号轮速度数组首地址
        static public void DynamicalAjusting(float[] wheelOne, float[] wheelTwo, float[] wheelThree)
        {
            float time    = 0.0f;
            int   n       = PointsInfo.pnts.Count();
            float tempAcc = 0.0f;

            //每次加速度降低至上次的百分值
            float percent = 0.9f;

            //先正向削减速度
            for (int i = 2; i < n + 1; i++)
            {
                //粗略计算每两示教点之间的运动的时间
                time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2;
                //轮1
                //只处理速度同向的情况
                if (wheelOne[i - 1] * wheelOne[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelOne[i - 1]) - Math.Abs(wheelOne[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc > MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelOne[i - 1] = wheelOne[i - 1] > 0 ? wheelOne[i - 2] + tempAcc * percent * time : wheelOne[i - 2] - tempAcc * percent * time;
                }
                //轮2
                //只处理速度同向的情况
                if (wheelTwo[i - 1] * wheelTwo[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelTwo[i - 1]) - Math.Abs(wheelTwo[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc > MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelTwo[i - 1] = wheelTwo[i - 1] > 0 ? wheelTwo[i - 2] + tempAcc * percent * time : wheelTwo[i - 2] - tempAcc * percent * time;
                }

                //轮3
                //只处理速度同向的情况
                if (wheelThree[i - 1] * wheelThree[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelThree[i - 1]) - Math.Abs(wheelThree[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc > MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelThree[i - 1] = wheelThree[i - 1] > 0 ? wheelThree[i - 2] + tempAcc * percent * time : wheelThree[i - 2] - tempAcc * percent * time;
                }
            }

            //反向削减速度
            for (int i = n; i > 1; i--)
            {
                //粗略计算每两示教点之间的运动的时间
                time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2;
                //轮1
                //只处理速度同向的情况
                if (wheelOne[i - 1] * wheelOne[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelOne[i - 1]) - Math.Abs(wheelOne[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc < -MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelOne[i - 2] = wheelOne[i - 2] > 0 ? wheelOne[i - 1] - tempAcc * percent * time : wheelOne[i - 1] + tempAcc * percent * time;
                }


                //轮2
                //只处理速度同向的情况
                if (wheelTwo[i - 1] * wheelTwo[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelTwo[i - 1]) - Math.Abs(wheelTwo[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc < -MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelTwo[i - 2] = wheelTwo[i - 2] > 0 ? wheelTwo[i - 1] - tempAcc * percent * time : wheelTwo[i - 1] + tempAcc * percent * time;
                }


                //轮3
                //只处理速度同向的情况
                if (wheelThree[i - 1] * wheelThree[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelThree[i - 1]) - Math.Abs(wheelThree[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc < -MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelThree[i - 2] = wheelThree[i - 2] > 0 ? wheelThree[i - 1] - tempAcc * percent * time : wheelThree[i - 1] + tempAcc * percent * time;
                }
            }



            for (int i = 0; i < n - 1; i++)
            {
                Calculate.TriWheelVel2_t tempVel2;

                tempVel2 = Calculate.TriWheelVel2ResultantVel(wheelOne[i], wheelTwo[i], wheelThree[i], PointsInfo.pnts[i].posAngle);

                //存入新计算的速度
                PointsInfo.pnts[i].velMax = tempVel2.speed;
            }
        }
        //绘制合速度图像
        void ResultantVelCurveDrawing()
        {
            //数据读取
            StreamReader pathFile = File.OpenText(VelInf);

            List <string> tempString = new List <string>();

            while (!pathFile.EndOfStream)
            {
                tempString.Add(pathFile.ReadLine());
            }

            pathFile.Close();


            //如果点数小于2,不能规划出轨迹
            if (tempString.Count() < 2)
            {
                return;
            }

            Graphics myGra = VelDisplay.CreateGraphics();

            Pen myPen = new Pen(Color.Red, 1);

            PointF[] velWheel1       = new PointF[tempString.Count()];
            PointF[] velWheel2       = new PointF[tempString.Count()];
            PointF[] velWheel3       = new PointF[tempString.Count()];
            PointF[] targetVelWheel1 = new PointF[tempString.Count()];
            PointF[] targetVelWheel2 = new PointF[tempString.Count()];
            PointF[] targetVelWheel3 = new PointF[tempString.Count()];

            float[] zAngle = new float[tempString.Count()];
            int     i      = 0;

            foreach (string str in tempString)
            {
                string[] sArray = str.Split(',');

                velWheel1[i].X = velWheel2[i].X = velWheel3[i].X
                                                      = targetVelWheel1[i].X = targetVelWheel2[i].X = targetVelWheel3[i].X
                                                                                                          = float.Parse(sArray[0]);

                velWheel1[i].Y = float.Parse(sArray[1]);
                velWheel2[i].Y = float.Parse(sArray[2]);
                velWheel3[i].Y = float.Parse(sArray[3]);

                targetVelWheel1[i].Y = float.Parse(sArray[4]);
                targetVelWheel2[i].Y = float.Parse(sArray[5]);
                targetVelWheel3[i].Y = float.Parse(sArray[6]);
                i++;
            }

            PointF[] trueVel         = new PointF[tempString.Count()];
            PointF[] targetVel       = new PointF[tempString.Count()];
            PointF[] trueDir         = new PointF[tempString.Count()];
            PointF[] targetDir       = new PointF[tempString.Count()];
            PointF[] trueRotateVel   = new PointF[tempString.Count()];
            PointF[] targetRotateVel = new PointF[tempString.Count()];
            PointF[] DirectionErr    = new PointF[tempString.Count()];
            PointF[] binaryzation    = new PointF[tempString.Count()];



            //由三个轮子的速度计算合成速度
            for (int j = 0; j < targetVelWheel1.Count(); j++)
            {
                Calculate.TriWheelVel2_t tempVel;
                tempVel = Calculate.TriWheelVel2ResultantVel(velWheel1[j].Y, velWheel2[j].Y, velWheel3[j].Y, zAngle[j]);

                trueVel[j].Y       = tempVel.speed;
                trueVel[j].X       = velWheel1[j].X;
                trueDir[j].Y       = tempVel.direction;
                trueDir[j].X       = velWheel1[j].X;
                trueRotateVel[j].Y = tempVel.rotationVel;
                trueRotateVel[j].X = velWheel1[j].X;


                tempVel              = Calculate.TriWheelVel2ResultantVel(targetVelWheel1[j].Y, targetVelWheel2[j].Y, targetVelWheel3[j].Y, zAngle[j]);
                targetVel[j].Y       = tempVel.speed;
                targetVel[j].X       = targetVelWheel1[j].X;
                targetDir[j].Y       = tempVel.direction;
                targetDir[j].X       = targetVelWheel1[j].X;
                targetRotateVel[j].Y = tempVel.rotationVel;
                targetRotateVel[j].X = targetVelWheel1[j].X;
            }


            pathMax = FindMaxAbs(targetDir).X;
            zoom    = (float)(VelDisplay.Bottom / 2) / FindMaxAbs(targetDir).Y;


            //设置滚动条最大值
            hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;



            for (int j = 0; j < targetVelWheel1.Count(); j++)
            {
                DirectionErr[j].X = targetDir[j].X;
                DirectionErr[j].Y = Math.Abs(targetDir[j].Y - trueDir[j].Y);
                if (DirectionErr[j].Y > 180)
                {
                    DirectionErr[j].Y = 360 - DirectionErr[j].Y;
                }
                else if (DirectionErr[j].Y < -180)
                {
                    DirectionErr[j].Y = 360 + DirectionErr[j].Y;
                }
            }


            //进行滤波处理
            for (int m = 0; m < 4; m++)
            {
                for (int j = 1; j < DirectionErr.Count() - 1; j++)
                {
                    DirectionErr[j].Y = (DirectionErr[j - 1].Y + DirectionErr[j + 1].Y) / 2;
                }
            }



            //进行阈值判断,二值化
            for (int j = 0; j < DirectionErr.Count(); j++)
            {
                if (DirectionErr[j].Y < 8)
                {
                    binaryzation[j].Y = 0;
                    binaryzation[j].X = DirectionErr[j].X;
                }
                else
                {
                    binaryzation[j].Y = DirectionErr[j].Y;
                    binaryzation[j].X = DirectionErr[j].X;
                }
            }



            //将二值化的文件存入txt文档中
            StreamWriter SpeedFile = new StreamWriter(SpeedReplanInfo, false);

            for (int m = 0; m < binaryzation.Count(); m++)
            {
                SpeedFile.WriteLine("{0:f0},{1:f0}", binaryzation[m].X, binaryzation[m].Y);
            }

            SpeedFile.Close();


            //进行阈值判断,二值化
            for (int j = 0; j < DirectionErr.Count(); j++)
            {
                binaryzation[j] = GetPanelAxes(binaryzation[j].X, binaryzation[j].Y);
            }



            myPen.Color = Color.Black;
            myGra.DrawLines(myPen, binaryzation);
        }
        //计算所绘制B样条曲线关键点路径长度,并存在txt文件中,用于切分姿态角度所用
        static public void CalculateBsplineInfo(string txtAdress)
        {
            //创建临时文件
            StreamWriter tempFile = new StreamWriter(Directory.GetCurrentDirectory() + "\\eeww.txt", true);

            //////////////////////////////////从TXT文档读取数据点/////////////////////////////////////
            //数据读取
            StreamReader  pathFile   = File.OpenText(txtAdress);
            List <string> tempString = new List <string>();

            while (!pathFile.EndOfStream)
            {
                tempString.Add(pathFile.ReadLine());
            }

            pathFile.Close();

            //如果点数小于2,不能规划出轨迹
            if (tempString.Count() < 2)
            {
                return;
            }

            List <PointF> tempPnts = new List <PointF>();

            Pen myPen = new Pen(Color.Black, 1);

            foreach (string str in tempString)
            {
                string[] sArray = str.Split(',');
                tempPnts.Add(new PointF(float.Parse(sArray[0]), float.Parse(sArray[1])));
            }

            System.Drawing.PointF[] tempPoint = tempPnts.ToArray();
            //////////////////////////////////////////////////////////////////////////////////////////////


            int num = tempPoint.Count();

            float[] pntLen = new float[num];
            //系数矩阵对角列
            float[] a = new float[num];
            //系数矩阵对角上列
            float[] b = new float[num];
            //系数矩阵对角下列
            float[] c = new float[num];

            //定义soluctionX、soluctionY为线性方程的解
            float[] soluctionX = new float[num];
            float[] soluctionY = new float[num];
            //定义dataX和dataY,用来存放inPoint里的X和Y坐标
            float[] dataX = new float[num];
            float[] dataY = new float[num];
            //定义controlPoint用来存放控制点
            PointF[] controlPoint = new PointF[num + 4];
            //存放画线的两个使用点
            PointF[] lines = new PointF[2];

            //初始化 a,b,c
            a[0]       = 18;
            a[num - 1] = 18;

            for (int i = 1; i < num - 1; i++)
            {
                a[i] = 4;
            }

            for (int i = 1; i < num - 1; i++)
            {
                b[i] = 1;
                c[i] = 1;
            }

            c[num - 1] = -9;
            b[0]       = -9;



            for (int i = 0; i < num; i++)
            {
                dataX[i] = 6.0f * tempPnts[i].X;
                dataY[i] = 6.0f * tempPnts[i].Y;
            }

            dataX[0]       *= 1.5f;
            dataY[0]       *= 1.5f;
            dataX[num - 1] *= 1.5f;
            dataY[num - 1] *= 1.5f;



            //计算outdataX,outdataY;

            Calculate math = new Calculate();

            //调用Matrix用追赶法求解线性方程
            math.Matrix(dataX, num, ref a, ref b, ref c, ref soluctionX);
            math.Matrix(dataY, num, ref a, ref b, ref c, ref soluctionY);



            controlPoint[num + 3].X = dataX[num - 1] / 9;
            controlPoint[num + 2].X = dataX[num - 1] / 9;
            controlPoint[0].X       = dataX[0] / 9;
            controlPoint[1].X       = dataX[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].X = soluctionX[i];
            }

            controlPoint[num + 3].Y = dataY[num - 1] / 9;
            controlPoint[num + 2].Y = dataY[num - 1] / 9;
            controlPoint[0].Y       = dataY[0] / 9;
            controlPoint[1].Y       = dataY[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].Y = soluctionY[i];
            }



            float threshold = 1.0f;

            while (true)
            {
                //计算型值点,画出曲线
                //从初始点开始
                lines[0] = tempPoint[0];

                pntLen[0] = 0.0f;

                int count = 1;

                float length = 0.0f;
                for (int i = 0; i < num + 1; i++)
                {
                    for (float u = 0.01f; u <= 1; u += 0.01f)
                    {
                        float b0 = 1.0f / 6 * (1 - u) * (1 - u) * (1 - u);
                        float b1 = 1.0f / 6 * (3 * u * u * u - 6 * u * u + 4);
                        float b2 = 1.0f / 6 * (-3 * u * u * u + 3 * u * u + 3 * u + 1);
                        float b3 = 1.0f / 6 * u * u * u;

                        lines[1].X = (b0 * controlPoint[i].X + b1 * controlPoint[i + 1].X + b2 * controlPoint[i + 2].X + b3 * controlPoint[i + 3].X);
                        lines[1].Y = (b0 * controlPoint[i].Y + b1 * controlPoint[i + 1].Y + b2 * controlPoint[i + 2].Y + b3 * controlPoint[i + 3].Y);

                        length += Bspline.CalculatePnt2Pnt(lines[0], lines[1]);

                        if (Bspline.CalculatePnt2Pnt(lines[1], tempPnts[count]) < threshold)
                        {
                            pntLen[count++] = length;
                        }



                        lines[0] = lines[1];
                    }
                }
                if (count == num - 1)
                {
                    length += Bspline.CalculatePnt2Pnt(lines[0], tempPoint[num - 1]);

                    pntLen[num - 1] = length;

                    break;
                }
                else
                {
                    threshold *= 2.0f;
                }
            }



            int ii = 0;

            foreach (string str in tempString)
            {
                tempFile.WriteLine(str + "," + pntLen[ii++].ToString());
            }

            tempFile.Close();

            //清空txt文档
            FileStream fs = new FileStream(txtAdress, FileMode.Create, FileAccess.Write);

            fs.Close();

            FileTxt.FileTxtCopy(Directory.GetCurrentDirectory() + "\\eeww.txt", txtAdress);

            //删除临时文件
            System.IO.File.Delete(Directory.GetCurrentDirectory() + "\\eeww.txt");
        }
        //将姿态角度没15cm计算一个值
        static public void Segment2(PointF[] inPoint, StreamWriter file, StreamReader lengthSegmentFileR)
        {
            int num = inPoint.Count();

            //系数矩阵对角列
            float[] a = new float[num];
            //系数矩阵对角上列
            float[] b = new float[num];
            //系数矩阵对角下列
            float[] c = new float[num];

            //定义soluctionX、soluctionY为线性方程的解
            float[] soluctionX = new float[num];
            float[] soluctionY = new float[num];
            //定义dataX和dataY,用来存放inPoint里的X和Y坐标
            float[] dataX = new float[num];
            float[] dataY = new float[num];
            //定义controlPoint用来存放控制点
            PointF[] controlPoint = new PointF[num + 4];
            //存放画线的两个使用点
            PointF[] lines = new PointF[2];

            //初始化 a,b,c
            a[0]       = 18;
            a[num - 1] = 18;

            for (int i = 1; i < num - 1; i++)
            {
                a[i] = 4;
            }

            for (int i = 1; i < num - 1; i++)
            {
                b[i] = 1;
                c[i] = 1;
            }

            c[num - 1] = -9;
            b[0]       = -9;



            for (int i = 0; i < num; i++)
            {
                dataX[i] = 6.0f * inPoint[i].X;
                dataY[i] = 6.0f * inPoint[i].Y;
            }

            dataX[0]       *= 1.5f;
            dataY[0]       *= 1.5f;
            dataX[num - 1] *= 1.5f;
            dataY[num - 1] *= 1.5f;



            //计算outdataX,outdataY;

            Calculate math = new Calculate();

            //调用Matrix用追赶法求解线性方程
            math.Matrix(dataX, num, ref a, ref b, ref c, ref soluctionX);
            math.Matrix(dataY, num, ref a, ref b, ref c, ref soluctionY);



            controlPoint[num + 3].X = dataX[num - 1] / 9;
            controlPoint[num + 2].X = dataX[num - 1] / 9;
            controlPoint[0].X       = dataX[0] / 9;
            controlPoint[1].X       = dataX[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].X = soluctionX[i];
            }

            controlPoint[num + 3].Y = dataY[num - 1] / 9;
            controlPoint[num + 2].Y = dataY[num - 1] / 9;
            controlPoint[0].Y       = dataY[0] / 9;
            controlPoint[1].Y       = dataY[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].Y = soluctionY[i];
            }


            List <string> segmentStr = new List <string>();

            while (!lengthSegmentFileR.EndOfStream)
            {
                segmentStr.Add(lengthSegmentFileR.ReadLine());
            }


            List <float> lenthSegments = new List <float>();

            foreach (string str in segmentStr)
            {
                string[] sArray = str.Split(',');
                lenthSegments.Add(float.Parse(sArray[0]));
            }


            int count = 0;

            file.WriteLine(inPoint[0].Y);

            for (int i = 0; i < num + 1; i++)
            {
                for (float u = 0.00001f; u <= 1; u += 0.00001f)
                {
                    float b0 = 1.0f / 6 * (1 - u) * (1 - u) * (1 - u);
                    float b1 = 1.0f / 6 * (3 * u * u * u - 6 * u * u + 4);
                    float b2 = 1.0f / 6 * (-3 * u * u * u + 3 * u * u + 3 * u + 1);
                    float b3 = 1.0f / 6 * u * u * u;

                    lines[1].X = (b0 * controlPoint[i].X + b1 * controlPoint[i + 1].X + b2 * controlPoint[i + 2].X + b3 * controlPoint[i + 3].X);
                    lines[1].Y = (b0 * controlPoint[i].Y + b1 * controlPoint[i + 1].Y + b2 * controlPoint[i + 2].Y + b3 * controlPoint[i + 3].Y);


                    float temp = 0.0f;

                    if (lines[1].X > lenthSegments[count])
                    {
                        temp = lines[1].Y;
                        if (lines[1].Y < 0.0f)
                        {
                            int tempI = -1;
                            while (true)
                            {
                                tempI++;
                                if (temp > tempI * (-360) - 180)
                                {
                                    break;
                                }
                            }
                            temp += tempI * 360;
                            file.WriteLine(temp);
                        }
                        else if (lines[1].Y > 0.0f)
                        {
                            int tempI = -1;
                            while (true)
                            {
                                tempI++;
                                if (temp < tempI * (360) + 180)
                                {
                                    break;
                                }
                            }
                            temp -= tempI * 360;
                            file.WriteLine(temp);
                        }
                        else
                        {
                            file.WriteLine(lines[1].Y);
                        }

                        count++;

                        if (count == lenthSegments.Count())
                        {
                            break;
                        }
                    }
                }

                if (count == lenthSegments.Count())
                {
                    break;
                }
            }



            //对最后一点进行判断
            if (inPoint[num - 1].Y < 0.0f)
            {
                float temp1 = inPoint[num - 1].Y;
                int   tempI = -1;
                while (true)
                {
                    tempI++;
                    if (temp1 > tempI * (-360) - 180)
                    {
                        break;
                    }
                }
                temp1 += tempI * 360;
                file.WriteLine(temp1);
            }
            else if (lines[1].Y > 0.0f)
            {
                float temp1 = inPoint[num - 1].Y;
                int   tempI = -1;
                while (true)
                {
                    tempI++;
                    if (temp1 < tempI * (360) + 180)
                    {
                        break;
                    }
                }
                temp1 -= tempI * 360;
                file.WriteLine(temp1);
            }
            else
            {
                file.WriteLine(lines[1].Y);
            }
        }
        //进行整体分段处理,仅用于BsplineSegment,计算每段15cm处切线方向
        static public float Segment(PointF[] inPoint, string BsplineWithDirTxt, StreamWriter lengthSegmentFile)
        {
            StreamWriter file = new StreamWriter(BsplineWithDirTxt, false);

            int num = inPoint.Count();

            //系数矩阵对角列
            float[] a = new float[num];
            //系数矩阵对角上列
            float[] b = new float[num];
            //系数矩阵对角下列
            float[] c = new float[num];

            //定义soluctionX、soluctionY为线性方程的解
            float[] soluctionX = new float[num];
            float[] soluctionY = new float[num];
            //定义dataX和dataY,用来存放inPoint里的X和Y坐标
            float[] dataX = new float[num];
            float[] dataY = new float[num];
            //定义controlPoint用来存放控制点
            PointF[] controlPoint = new PointF[num + 4];
            //存放画线的两个使用点
            PointF[] lines = new PointF[2];

            //初始化 a,b,c
            a[0]       = 18;
            a[num - 1] = 18;

            for (int i = 1; i < num - 1; i++)
            {
                a[i] = 4;
            }

            for (int i = 1; i < num - 1; i++)
            {
                b[i] = 1;
                c[i] = 1;
            }

            c[num - 1] = -9;
            b[0]       = -9;



            for (int i = 0; i < num; i++)
            {
                dataX[i] = 6.0f * inPoint[i].X;
                dataY[i] = 6.0f * inPoint[i].Y;
            }

            dataX[0]       *= 1.5f;
            dataY[0]       *= 1.5f;
            dataX[num - 1] *= 1.5f;
            dataY[num - 1] *= 1.5f;



            //计算outdataX,outdataY;

            Calculate math = new Calculate();

            //调用Matrix用追赶法求解线性方程
            math.Matrix(dataX, num, ref a, ref b, ref c, ref soluctionX);
            math.Matrix(dataY, num, ref a, ref b, ref c, ref soluctionY);



            controlPoint[num + 3].X = dataX[num - 1] / 9;
            controlPoint[num + 2].X = dataX[num - 1] / 9;
            controlPoint[0].X       = dataX[0] / 9;
            controlPoint[1].X       = dataX[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].X = soluctionX[i];
            }

            controlPoint[num + 3].Y = dataY[num - 1] / 9;
            controlPoint[num + 2].Y = dataY[num - 1] / 9;
            controlPoint[0].Y       = dataY[0] / 9;
            controlPoint[1].Y       = dataY[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].Y = soluctionY[i];
            }



            //计算型值点,画出曲线
            ////从初始点开始
            //lines[0].X = controlPoint[0].X;
            //lines[0].Y = controlPoint[0].Y;


            float length = 0.0f;

            float lengthOrigin = 0.0f;

            const float CHANGE_TO_ANGLE = (180.0f / 3.1415927f);

            int count = 0;

            for (int i = 0; i < num + 1; i++)
            {
                for (float u = 0.01f; u <= 1; u += 0.01f)
                {
                    float b0 = 1.0f / 6 * (1 - u) * (1 - u) * (1 - u);
                    float b1 = 1.0f / 6 * (3 * u * u * u - 6 * u * u + 4);
                    float b2 = 1.0f / 6 * (-3 * u * u * u + 3 * u * u + 3 * u + 1);
                    float b3 = 1.0f / 6 * u * u * u;

                    lines[1].X = (b0 * controlPoint[i].X + b1 * controlPoint[i + 1].X + b2 * controlPoint[i + 2].X + b3 * controlPoint[i + 3].X);
                    lines[1].Y = (b0 * controlPoint[i].Y + b1 * controlPoint[i + 1].Y + b2 * controlPoint[i + 2].Y + b3 * controlPoint[i + 3].Y);

                    lengthOrigin += Bspline.CalculatePnt2Pnt(lines[0], lines[1]);
                    length       += Bspline.CalculatePnt2Pnt(lines[0], lines[1]);


                    if (length > 300.0)
                    {
                        length = 0;
                        float Db0 = -1.0f / 2 * (u - 1) * (u - 1);
                        float Db1 = 1.0f / 2 * (3 * u * u - 4 * u);
                        float Db2 = 1.0f / 2 * (-3 * u * u + u * 2 + 1);
                        float Db3 = 1.0f / 2 * u * u;

                        float angle = CHANGE_TO_ANGLE * (float)Math.Atan2((Db0 * controlPoint[i].Y + Db1 * controlPoint[i + 1].Y
                                                                           + Db2 * controlPoint[i + 2].Y + Db3 * controlPoint[i + 3].Y),
                                                                          (Db0 * controlPoint[i].X + Db1 * controlPoint[i + 1].X
                                                                           + Db2 * controlPoint[i + 2].X + Db3 * controlPoint[i + 3].X));

                        if (count == 0)
                        {
                            file.Write(inPoint[0].X.ToString() + "," + inPoint[0].Y.ToString() + "," + (Math.Atan2(lines[1].Y - inPoint[0].Y, lines[1].X - inPoint[0].X) * 180.0f / 3.14159f).ToString() + "\r\n");

                            file.Write(lines[1].X.ToString() + "," + lines[1].Y.ToString() + "," + angle.ToString() + "\r\n");
                        }
                        else
                        {
                            file.Write(lines[1].X.ToString() + "," + lines[1].Y.ToString() + "," + angle.ToString() + "\r\n");
                        }
                        count++;

                        //并没有记录第一点和最后一点
                        lengthSegmentFile.WriteLine(lengthOrigin);
                    }

                    lines[0] = lines[1];
                }
            }

            lengthOrigin += Bspline.CalculatePnt2Pnt(inPoint[num - 1], lines[1]);

            file.Write(inPoint[num - 1].X.ToString() + "," + inPoint[num - 1].Y.ToString() + "," + (Math.Atan2(inPoint[num - 1].Y - lines[1].Y, inPoint[num - 1].X - lines[1].X) * 180.0f / 3.14159f).ToString() + "\r\n");

            file.Close();

            return(lengthOrigin);
        }
        /// <summary>
        /// @name DrowBspline1
        /// @brief 首末端点为自由条件的B样条曲线绘制
        /// @inPoint 示教点
        /// </summary>
        static public void DrawBspline1(int num, Graphics gra, Pen pen, PointF[] inPoint)
        {
            //系数矩阵对角列
            float[] a = new float[num];
            //系数矩阵对角上列
            float[] b = new float[num];
            //系数矩阵对角下列
            float[] c = new float[num];

            //定义soluctionX、soluctionY为线性方程的解
            float[] soluctionX = new float[num];
            float[] soluctionY = new float[num];
            //定义dataX和dataY,用来存放inPoint里的X和Y坐标
            float[] dataX = new float[num];
            float[] dataY = new float[num];
            //定义controlPoint用来存放控制点
            PointF[] controlPoint = new PointF[num + 4];
            //存放画线的两个使用点
            PointF[] lines = new PointF[2];

            //初始化 a,b,c
            a[0]       = 18;
            a[num - 1] = 18;

            for (int i = 1; i < num - 1; i++)
            {
                a[i] = 4;
            }

            for (int i = 1; i < num - 1; i++)
            {
                b[i] = 1;
                c[i] = 1;
            }

            c[num - 1] = -9;
            b[0]       = -9;



            for (int i = 0; i < num; i++)
            {
                dataX[i] = 6.0f * inPoint[i].X;
                dataY[i] = 6.0f * inPoint[i].Y;
            }

            dataX[0]       *= 1.5f;
            dataY[0]       *= 1.5f;
            dataX[num - 1] *= 1.5f;
            dataY[num - 1] *= 1.5f;



            //计算outdataX,outdataY;

            Calculate math = new Calculate();

            //调用Matrix用追赶法求解线性方程
            math.Matrix(dataX, num, ref a, ref b, ref c, ref soluctionX);
            math.Matrix(dataY, num, ref a, ref b, ref c, ref soluctionY);



            controlPoint[num + 3].X = dataX[num - 1] / 9;
            controlPoint[num + 2].X = dataX[num - 1] / 9;
            controlPoint[0].X       = dataX[0] / 9;
            controlPoint[1].X       = dataX[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].X = soluctionX[i];
            }

            controlPoint[num + 3].Y = dataY[num - 1] / 9;
            controlPoint[num + 2].Y = dataY[num - 1] / 9;
            controlPoint[0].Y       = dataY[0] / 9;
            controlPoint[1].Y       = dataY[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].Y = soluctionY[i];
            }



            //计算型值点,画出曲线
            //从初始点开始
            lines[0].X = (int)controlPoint[0].X;
            lines[0].Y = (int)controlPoint[0].Y;


            for (int i = 0; i < num + 1; i++)
            {
                for (float u = 0.01f; u <= 1; u += 0.01f)
                {
                    float b0 = 1.0f / 6 * (1 - u) * (1 - u) * (1 - u);
                    float b1 = 1.0f / 6 * (3 * u * u * u - 6 * u * u + 4);
                    float b2 = 1.0f / 6 * (-3 * u * u * u + 3 * u * u + 3 * u + 1);
                    float b3 = 1.0f / 6 * u * u * u;

                    lines[1].X = (b0 * controlPoint[i].X + b1 * controlPoint[i + 1].X + b2 * controlPoint[i + 2].X + b3 * controlPoint[i + 3].X);
                    lines[1].Y = (b0 * controlPoint[i].Y + b1 * controlPoint[i + 1].Y + b2 * controlPoint[i + 2].Y + b3 * controlPoint[i + 3].Y);
                    PointF pnt1 = new PointF();
                    PointF pnt2 = new PointF();


                    //转换到panel像素坐标系
                    pnt1 = ViewControl.GetPanelAxes(lines[0].X, lines[0].Y);
                    pnt2 = ViewControl.GetPanelAxes(lines[1].X, lines[1].Y);

                    gra.DrawLine(Pens.Yellow, pnt1, pnt2);


                    lines[0] = lines[1];
                }
            }
        }
        //计算b样条长度,端点条件为自由端点条件
        static public float CalculateBsplineLenWithoutDir(PointF[] inPoint)
        {
            int num = inPoint.Count();

            //系数矩阵对角列
            float[] a = new float[num];
            //系数矩阵对角上列
            float[] b = new float[num];
            //系数矩阵对角下列
            float[] c = new float[num];

            //定义soluctionX、soluctionY为线性方程的解
            float[] soluctionX = new float[num];
            float[] soluctionY = new float[num];
            //定义dataX和dataY,用来存放inPoint里的X和Y坐标
            float[] dataX = new float[num];
            float[] dataY = new float[num];
            //定义controlPoint用来存放控制点
            PointF[] controlPoint = new PointF[num + 4];
            //存放画线的两个使用点
            PointF[] lines = new PointF[2];

            //初始化 a,b,c
            a[0]       = 18;
            a[num - 1] = 18;

            for (int i = 1; i < num - 1; i++)
            {
                a[i] = 4;
            }

            for (int i = 1; i < num - 1; i++)
            {
                b[i] = 1;
                c[i] = 1;
            }

            c[num - 1] = -9;
            b[0]       = -9;



            for (int i = 0; i < num; i++)
            {
                dataX[i] = 6.0f * inPoint[i].X;
                dataY[i] = 6.0f * inPoint[i].Y;
            }

            dataX[0]       *= 1.5f;
            dataY[0]       *= 1.5f;
            dataX[num - 1] *= 1.5f;
            dataY[num - 1] *= 1.5f;



            //计算outdataX,outdataY;

            Calculate math = new Calculate();

            //调用Matrix用追赶法求解线性方程
            math.Matrix(dataX, num, ref a, ref b, ref c, ref soluctionX);
            math.Matrix(dataY, num, ref a, ref b, ref c, ref soluctionY);



            controlPoint[num + 3].X = dataX[num - 1] / 9;
            controlPoint[num + 2].X = dataX[num - 1] / 9;
            controlPoint[0].X       = dataX[0] / 9;
            controlPoint[1].X       = dataX[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].X = soluctionX[i];
            }

            controlPoint[num + 3].Y = dataY[num - 1] / 9;
            controlPoint[num + 2].Y = dataY[num - 1] / 9;
            controlPoint[0].Y       = dataY[0] / 9;
            controlPoint[1].Y       = dataY[0] / 9;


            for (int i = 0; i < num; i++)
            {
                controlPoint[i + 2].Y = soluctionY[i];
            }



            //计算型值点,画出曲线
            //从初始点开始
            lines[0] = inPoint[0];

            float       length          = 0.0f;
            const float CHANGE_TO_ANGLE = (180.0f / 3.1415927f);

            for (int i = 0; i < num + 1; i++)
            {
                for (float u = 0.01f; u <= 1; u += 0.01f)
                {
                    float b0 = 1.0f / 6 * (1 - u) * (1 - u) * (1 - u);
                    float b1 = 1.0f / 6 * (3 * u * u * u - 6 * u * u + 4);
                    float b2 = 1.0f / 6 * (-3 * u * u * u + 3 * u * u + 3 * u + 1);
                    float b3 = 1.0f / 6 * u * u * u;

                    lines[1].X = (b0 * controlPoint[i].X + b1 * controlPoint[i + 1].X + b2 * controlPoint[i + 2].X + b3 * controlPoint[i + 3].X);
                    lines[1].Y = (b0 * controlPoint[i].Y + b1 * controlPoint[i + 1].Y + b2 * controlPoint[i + 2].Y + b3 * controlPoint[i + 3].Y);
                    length    += Bspline.CalculatePnt2Pnt(lines[0], lines[1]);

                    float Db0 = -1.0f / 2 * (u - 1) * (u - 1);
                    float Db1 = 1.0f / 2 * (3 * u * u - 4 * u);
                    float Db2 = 1.0f / 2 * (-3 * u * u + u * 2 + 1);
                    float Db3 = 1.0f / 2 * u * u;

                    float angle = CHANGE_TO_ANGLE * (float)Math.Atan2((Db0 * controlPoint[i].Y + Db1 * controlPoint[i + 1].Y
                                                                       + Db2 * controlPoint[i + 2].Y + Db3 * controlPoint[i + 3].Y),
                                                                      (Db0 * controlPoint[i].X + Db1 * controlPoint[i + 1].X
                                                                       + Db2 * controlPoint[i + 2].X + Db3 * controlPoint[i + 3].X));

                    lines[0] = lines[1];
                }
            }

            length += Bspline.CalculatePnt2Pnt(lines[0], inPoint[num - 1]);

            return(length);
        }