c#路径规划实例及算法

路径规划实例，绘图及算法实例

public void DrawPath()
        {
           
            AStarRoutePlanner aStarRoutePlanner = new AStarRoutePlanner();
            IList<Point> obstaclePoints = new List<Point>();
           
            for (int Y = 0; Y < Bmp.Height; Y )
                for (int X = 0; X < Bmp.Width; X )
                    if (Bmp.GetPixel(X, Y).R == 0)
                        obstaclePoints.Add(new Point(X, Y));//添加障碍物位置
            /*
            obstaclePoints.Add(new Point(2, 4));
            obstaclePoints.Add(new Point(3, 4));
            obstaclePoints.Add(new Point(4, 4));
            obstaclePoints.Add(new Point(5, 4));
            obstaclePoints.Add(new Point(6, 4));
            */
            aStarRoutePlanner.Initialize(obstaclePoints);
            IList<Point> route = aStarRoutePlanner.Plan(
                new Point(Convert.ToInt32(textBox_StartP_X.Text), Convert.ToInt32(textBox_StartP_Y.Text)),
                new Point(Convert.ToInt32(textBox_EndP_X.Text), Convert.ToInt32(textBox_EndP_Y.Text)));
            IList<Point> route1 = aStarRoutePlanner.Plan(new Point(400,10),new Point(300,20));            int r_num = 0,min=0, max = 0;
            if (route.Count > route1.Count)
            {
                max = route.Count;
                min = route1.Count;
            }
            else
            {
                max = route1.Count;
                min = route.Count;
            }            while (true)
            {
                if (planning && (r_num < max) )
                {
                    if (r_num < min)
                    {
                        Bmp.SetPixel(route[r_num].X, route[r_num].Y, Color.Green);
                        Bmp.SetPixel(route1[r_num].X, route1[r_num].Y, Color.Green);
                    }
                    else if(route.Count > route1.Count)
                        Bmp.SetPixel(route[r_num].X, route[r_num].Y, Color.Green);
                    else Bmp.SetPixel(route1[r_num].X, route1[r_num].Y, Color.Green);
                                       
                    pictureBox1.Image = Bmp;
                    // g.DrawLine(new Pen(Color.Red), new Point (route[r_num].X, route[r_num].Y), new Point(route[r_num 1].X,route[r_num 1].Y) );
                    // pictureBox1.Image = Bmp;
                    r_num ;
                    if (r_num == route.Count)
                    {
                        MessageBox.Show("结束");
                        g.Dispose();//释放画布资源
                    }
                    planning = false;
                }
               
            }           
        }