A* 寻路算法简单实现

 

http://blog.csdn.net/ghgui008

基于c# framwork，自带演示界面，原理略过，只实现了最简单的功能，由于文件代码较多，只贴了关键部分，整理即可运行。

 

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace AStartTest
{
    public class SearchGrid
    {
        private Form1 mForm; // 因为form里定义了起始点等信息，所以这里需要保存一个引用，方便访问
        private SearchGrid mParent;
        public int X;
        public int Y; // 格子坐标

        public SearchGrid(Form1 form, int x, int y, SearchGrid grid = null)
        {
            mForm = form;
            X = x;
            Y = y;

            SetParent(grid);
        }

        public float G() 
        {
            return mParent == null ? 0 : mParent.G() + 1;
        }

        public float H()
        {
            return Math.Abs(X - mForm.EndX) + Math.Abs(Y - mForm.EndY);
        }

        public float F()
        {
            return G() + H();
        }

        public void SetParent(SearchGrid g)
        {
            mParent = g;
        }

        public SearchGrid GetParent()
        {
            return mParent;
        }

        public bool Same(SearchGrid g)
        {
            return Same(g.X, g.Y);
        }

        public bool Same(int x, int y)
        {
            return x == X && y == Y;
        }

        public void PaintGrid(Graphics g, Brush fillBrush, Brush textBrush, Font textFont)
        {
            var fb = fillBrush;
            if (Same((int)mForm.BeginX, (int)mForm.BeginY))
            {
                fb = new SolidBrush(Color.Green);
            }
            else if (Same((int)mForm.EndX, (int)mForm.EndY))
            {
                fb = new SolidBrush(Color.Red);
            }

            g.FillRectangle(fb, mForm.StartX + X * mForm.GridW + 2, mForm.StartY + Y * mForm.GridW + 2, mForm.GridW -3, mForm.GridW - 3 );
            if (textBrush != null && textFont != null)
            {
                g.DrawString(F().ToString(), textFont, textBrush, mForm.StartX + X * mForm.GridW + 3, mForm.StartY + Y * mForm.GridW + 3);
                g.DrawString(G().ToString(), textFont, textBrush, mForm.StartX + X * mForm.GridW + 3, mForm.StartY  + (Y + 1) * mForm.GridW - 18);
                g.DrawString(H().ToString(), textFont, textBrush, mForm.StartX + (X + 1) * mForm.GridW - 18, mForm.StartY + (Y + 1) * mForm.GridW - 18);
            }
        }
    }

    public partial class Form1 : Form
    {
        // 网格阻挡信息矩阵 0为通达，大于0为阻挡
        private int[][] mGridData = new int[][] { 
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,5,5,0,5,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,5,0,5,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,5,0,5,0,5,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,5,0,0,0,5,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,5,0,5,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,5,5,5,5,0,5,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,5,0,0,5,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,5,5,5,5,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,5,5,5,5,5,5,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
            new int[] { 5,5,5,5,5,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
            new int[] { 0,0,0,0,0,0,0,0,0,0,0,0,5,0,0,0,0,0,0,0,},
        };

        public float StartX = 20f;
        public float StartY = 20f;
        public float GridW = 45f;

        // 起始点对于格子的坐标
        public float BeginX = 1f, BeginY = 1f;
        public float EndX = 18f, EndY = 19f; 

        public Form1()
        {
            InitializeComponent();
        }

        /// <summary>
        /// 【按钮点击事件】，绘制格子基本信息
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button1_Click(object sender, EventArgs e)
        {
            Graphics g = this.CreateGraphics();
            Pen pen = new Pen(Color.Gray, 2f);

            int size = mGridData.Length;
            for (int i = 0; i < size; ++i)
            {
                // 画横线
                g.DrawLine(pen, StartX, StartY + i * GridW, StartX + size * GridW, StartY + i * GridW);

                // 画竖线
                g.DrawLine(pen, StartX + i * GridW, StartY, StartX + i * GridW, StartY + size * GridW);
            }

            // 画最后边上两条直线
            g.DrawLine(pen, StartX, StartY + size * GridW, StartX + size * GridW, StartY + size * GridW);
            g.DrawLine(pen, StartX + size * GridW, StartY, StartX + size * GridW, StartY + size * GridW);

            // 绘制表格元素
            Brush b = new SolidBrush(Color.Black);
            for (int i = 0; i < mGridData.Length; i++)
            {
                int[] d = mGridData[i];
                for (int j = 0; j < d.Length; ++j)
                {
                    int v = d[j];
                    if (v > 0)
                    {
                        // 绘制阻挡信息
                        g.FillRectangle(b, StartX + i * GridW + 2, StartY + j * GridW + 2, GridW - 3, GridW - 3);
                    }
                }
            }

            // 绘制起始点
            Brush bb = new SolidBrush(Color.Green);
            g.FillRectangle(bb, StartX + BeginX * GridW + 2, StartY + BeginY * GridW + 2, GridW - 3, GridW - 3);
            Brush eb = new SolidBrush(Color.Red);
            g.FillRectangle(eb, StartX + EndX * GridW + 2, StartY + EndY * GridW + 2, GridW - 3, GridW - 3);
        }

        /// <summary>
        /// 【按钮点击事件】开始寻路，并绘制寻路闭合列表和最优路径
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button2_Click(object sender, EventArgs e)
        {
            // 进行A* 寻路，假设相邻两个各自长度都是1
            List<SearchGrid> openList = new List<SearchGrid>();
            List<SearchGrid> closeList = new List<SearchGrid>();

            // 1. 设置当前点为起点
            SearchGrid n = new SearchGrid(this, (int)BeginX, (int)BeginY, null);
            openList.Add(n);

            SearchGrid cur = null;

            while (true)
            {
                if (openList.Count == 0)
                    break;

                // 从openlist中选取一个最近的点（F最小）作为当前节点
                float minF = -1f;
                SearchGrid minN = null;
                foreach (var on in openList)
                {
                    float f = on.F();
                    if (f < minF || minF < 0f)
                    {
                        minF = f;
                        minN = on;
                    }
                }

                if (minN != null)
                {
                    cur = minN;
                    
                    List<SearchGrid> nearList = GetNearNodes(cur, openList, closeList);
                    openList.AddRange(nearList);

                    openList.Remove(cur);
                    closeList.Add(cur);

                    if (cur.Same((int)EndX, (int)EndY))
                    {
                        break;
                    }
                }
            }


            // 绘制
            Graphics g = this.CreateGraphics();
            Brush fillCloseBrush = new SolidBrush(Color.DarkGray);
            Brush roadBrush = new SolidBrush(Color.Yellow);
            Brush textBrush = new SolidBrush(Color.Black);
            Font textFont = new Font("宋体", 12);

            foreach(var cn in closeList)
            {
                cn.PaintGrid(g, fillCloseBrush, textBrush, textFont);
            }

            // 绘制最终路径
            if (cur != null && cur.Same((int)EndX, (int)EndY))
            {
                SearchGrid nn = cur;
                while (nn != null)
                {
                    nn.PaintGrid(g, roadBrush, textBrush, textFont);

                    nn = nn.GetParent();
                }
            }
        }

        public List<SearchGrid> GetNearNodes(SearchGrid g, List<SearchGrid> openList = null, List<SearchGrid> closeList = null)
        {

            List<SearchGrid> ret = new List<SearchGrid>();
            if (g != null)
            {
                int min = 0;
                int max = mGridData.Length;

                // 左边
                int x1 = g.X - 1, y1 = g.Y;
                CheckAddGrid(g, x1, y1, openList, closeList, ref ret);

                // 右边
                int x2 = g.X + 1, y2 = g.Y;
                CheckAddGrid(g, x2, y2, openList, closeList, ref ret);

                // 上边
                int x3 = g.X, y3 = g.Y - 1;
                CheckAddGrid(g, x3, y3, openList, closeList, ref ret);

                // 下边
                int x4 = g.X, y4 = g.Y + 1;
                CheckAddGrid(g, x4, y4, openList, closeList, ref ret);
            }
            
            return ret;
        }

        public bool Crossable(int x, int y)
        {
            return mGridData[x][y] == 0;
        }


        public void CheckAddGrid(SearchGrid parent, int x, int y, List<SearchGrid> openList, List<SearchGrid> closeList, ref List<SearchGrid> result)
        {
            int min = 0;
            int max = mGridData.Length;
            // 1. 判断坐标是否越界和阻挡
            if (x >= min && x < max && y >= min && y < max && Crossable(x, y))
            {
                // 2. 如果不在闭合列表，则继续
                if (closeList == null || closeList.Find((e) => { return e.Same(x, y); }) == null)
                {
                    bool found = false;
                    if (openList != null)
                    {
                        var opn = openList.Find((e) => { return e.Same(x, y); });
                        if (opn != null)
                        {
                            found = true;
                            float oldG = opn.G();
                            float newG = parent.G() + 1;
                            if (newG < oldG)
                            {
                                opn.SetParent(parent); // 已经在openlist中，检查是否需要更新parent
                            }
                        } 
                    }
                    if (!found)
                    {
                        result.Add(new SearchGrid(this, x, y, parent));
                    }
                }
            }
        }
    }
}