示例#1
0
        //跟随点显示
        private void RealPathReview()
        {
            //数据读取
            StreamReader pathFile = File.OpenText(RecieveRealPointInf);

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

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

            pathFile.Close();


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

            Graphics myGra = viewControl.CreateGraphics();

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

            foreach (string str in tempString)
            {
                string[] sArray = str.Split(',');
                PointF   pnt;
                pnt = ViewControl.GetPanelAxes(float.Parse(sArray[0]), float.Parse(sArray[1]));
                myGra.DrawEllipse(myPen, pnt.X, pnt.Y, 1, 1);
            }
        }
示例#2
0
        //坐标点接收函数
        private void PointRecieve(string tempStr, string DataPath)
        {
            try
            {
                StreamWriter velFile      = new StreamWriter(VelInf, true);
                StreamWriter keyPointFile = new StreamWriter(DataPath, true);

                //轨迹实时显示
                Graphics myGra = viewControl.CreateGraphics();

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


                string[] sArray = tempStr.Split('\n');


                for (int i = 0; i < sArray.Length - 1; i++)
                {
                    if (i == 0)
                    {
                        continue;
                    }
                    string[] tempArray = sArray[i].Split('\t');
                    if (tempArray.Length < 2)
                    {
                        return;
                    }


                    PointF pnt;
                    pnt = ViewControl.GetPanelAxes(float.Parse(tempArray[0]), float.Parse(tempArray[1]));
                    myGra.DrawEllipse(myPen, pnt.X, pnt.Y, 1, 1);


                    if (DataPath == RecieveRealPointInf)
                    {
                        keyPointFile.Write(tempArray[0] + "," + tempArray[1] + "\r\n");
                        velFile.WriteLine(tempArray[2] + "," + tempArray[3] + "," + tempArray[4] + "," + tempArray[5] + "," + tempArray[6] + "," +
                                          tempArray[7] + "," + tempArray[8] + "," + tempArray[9]);
                    }
                    else //示教关键点信息
                    {
                        keyPointFile.Write(tempArray[0] + "," + tempArray[1] + "," + tempArray[2] + "\r\n");
                    }
                }

                keyPointFile.Close();
                velFile.Close();
            }
            catch
            {
                return;
            }
        }
示例#3
0
        /// <summary>
        /// 首末端点有方向的B样条曲线绘制
        /// </summary>
        /// <param name="gra">外部声明调用GDI</param>
        /// <param name="point1">点1</param>
        /// <param name="point2">点2</param>
        /// <param name="angle1">点1的方向</param>
        /// <param name="angle2">点2的方向</param>
        static public void DrawBspline2(Graphics gra, PointF point1, PointF point2, float angle1, float angle2)
        {
            PointF[]    finalDataPoint   = new PointF[6];
            PointF[]    dataPoint        = new PointF[2];
            float       length           = 0.0f;
            const float CHANGE_TO_RADIAN = (3.1415927f / 180.0f);

            PointF[] lines = new PointF[2];

            length         = 0.6f * (float)Math.Sqrt((point1.X - point2.X) * (point1.X - point2.X) + (point1.Y - point2.Y) * (point1.Y - point2.Y));
            dataPoint[0].X = (int)((3 * point1.X + length * Math.Cos((angle1) * CHANGE_TO_RADIAN)) / 3.0f);
            dataPoint[0].Y = (int)((3 * point1.Y + length * Math.Sin((angle1) * CHANGE_TO_RADIAN)) / 3.0f);
            dataPoint[1].X = (int)((3 * point2.X - length * Math.Cos((angle2) * CHANGE_TO_RADIAN)) / 3.0f);
            dataPoint[1].Y = (int)((3 * point2.Y - length * Math.Sin((angle2) * CHANGE_TO_RADIAN)) / 3.0f);

            //复制最终控制点坐标
            finalDataPoint[0] = point1;
            finalDataPoint[1] = point1;
            finalDataPoint[2] = dataPoint[0];
            finalDataPoint[3] = dataPoint[1];
            finalDataPoint[4] = point2;
            finalDataPoint[5] = point2;
            //从初始点开始
            lines[0].X = (int)finalDataPoint[0].X;
            lines[0].Y = (int)finalDataPoint[0].Y;

            PointF pnt1 = new PointF();
            PointF pnt2 = new PointF();

            for (int i = 0; i < 3; 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 * finalDataPoint[i].X + b1 * finalDataPoint[i + 1].X + b2 * finalDataPoint[i + 2].X + b3 * finalDataPoint[i + 3].X);
                    lines[1].Y = (b0 * finalDataPoint[i].Y + b1 * finalDataPoint[i + 1].Y + b2 * finalDataPoint[i + 2].Y + b3 * finalDataPoint[i + 3].Y);
                    //转换到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];
                }
            }
            pnt2 = ViewControl.GetPanelAxes(point2.X, point2.Y);
            gra.DrawLine(Pens.Red, pnt1, pnt2);
        }
示例#4
0
        /// <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];
                }
            }
        }