A*寻路算法——多人寻路、实时碰撞寻路、最近目的地

A* 路算法原理可以参考这个文章，已经写的很详细了http://www.cppblog.com/mythit/archive/2009/04/19/80492.aspx

这篇文章主要写写多人寻路的实时碰撞

先说说无法寻路的情况下，如何移动的离目的地最近的点

其实所有能到达的点都在"关闭列表中"，当“开启列表”中所有的点都遍历完后，如果还未找到终点，则视为路径不通

这时候遍历“关闭列表”，找出其中离终点直线距离最短的点即可，见下面代码中findNearPointFromList函数

多人碰撞的大体思路就是

1、用A*找出一条路径

2、按该路径走，没移动一格检测是否发生碰撞

3、如果碰撞，调用A*重新寻路

4、如果未碰撞，按原来路径继续走

5、到目的地停止

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace testAstar
{
    public class AstarNode
    {
        private AstarNode parent = null;
        private int g;
        private int h;
        private int x;
        private int y;

        public AstarNode Parent
        {
            get
            {
                return parent;
            }
            set
            {
                parent = value;
            }
        }

        public int G
        {
            get
            {
                return g;
            }
            set
            {
                g = value;
            }
        }

        public int H
        {
            get
            {
                return h;
            }
            set
            {
                h = value;
            }
        }

        public int F
        {
            get
            {
                return g + h;
            }
        }

        public int X
        {
            get
            {
                return x;
            }
            set
            {
                x = value;
            }
        }

        public int Y
        {
            get
            {
                return y;
            }
            set
            {
                y = value;
            }
        }

        public AstarNode(int _x, int _y)
        {
            this.x = _x;
            this.y = _y;
            this.parent = null;
            this.g = 0;
            this.h = 0;
        }
    }

    public class Astar
    {
        private List openList = new List();
        private List closeList = new List();
        private bool[,] mapData = null;
        private int pixelFormat = 16;
        private int mapWidth = 0;
        private int mapHeight = 0;
        private int endX = 0;
        private int endY = 0;

        public bool[,] MapData
        {
            get
            {
                return mapData;
            }
        }

        public int PixelFormat
        {
            get
            {
                return pixelFormat;
            }
        }

        public int MapWidth
        {
            get
            {
                return mapWidth;
            }
        }

        public int MapHeight
        {
            get
            {
                return mapHeight;
            }
        }

        public Astar()
        {
        }

        private bool isValid(int x, int y)
        {
            if (x < 0 || x >= mapWidth)
            {
                return false;
            }

            if (y < 0 || y >= mapHeight)
            {
                return false;
            }

            return true;
        }

        private bool inList(List list, int x, int y)
        {
            foreach (AstarNode node in list)
            {
                if (node.X == x && node.Y == y)
                {
                    return true;
                }
            }

            return false;
        }

        private bool inOpenList(int x, int y)
        {
            return inList(openList, x, y);
        }

        private bool inCloseList(int x, int y)
        {
            return inList(closeList, x, y);
        }

        private AstarNode getBestNodeFromOpenList()
        {
            if (openList.Count == 0)
            {
                return null;
            }

            return openList[0];
        }

        private void openToClose(AstarNode node)
        {
            openList.Remove(node);
            closeList.Add(node);
        }

        private AstarNode openToCloseWithBest()
        {
            AstarNode node = getBestNodeFromOpenList();

            if (node == null)
            {
                return null;
            }

            openToClose(node);
            return node;
        }

        private void addToOpenList(AstarNode parent, int x, int y)
        {
            if (!isValid(x, y))
            {
                return;
            }

            if (inOpenList(x, y) || inCloseList(x, y))
            {
                return;
            }

            if (!canWalk(x, y) && parent != null)
            {
                return;
            }

            AstarNode node = new AstarNode(x, y);
            node.Parent = parent;

            if (parent == null)
            {
                node.G = 0;
                node.H = 0;
            }
            else
            {
                if (Math.Abs(parent.X - x) + Math.Abs(parent.Y - y) == 2)
                {
                    node.G = 14;
                }
                else
                {
                    node.G = 10;
                }

                node.H = Math.Abs(endX - x) * 10 + Math.Abs(endY - y) * 10;
            }

            openList.Add(node);
            openList.Sort(delegate(AstarNode lhs, AstarNode rhs)
            {
                if (lhs.F < rhs.F)
                {
                    return -1;
                }
                else if (lhs.F > rhs.F)
                {
                    return 1;
                }
                return 0;
            });
        }

        private void genAroundNode(AstarNode node)
        {
            //addToOpenList(node, node.X - 1, node.Y - 1);
            addToOpenList(node, node.X - 1, node.Y);
            //addToOpenList(node, node.X - 1, node.Y + 1);

            addToOpenList(node, node.X, node.Y - 1);
            addToOpenList(node, node.X, node.Y + 1);

            //addToOpenList(node, node.X + 1, node.Y - 1);
            addToOpenList(node, node.X + 1, node.Y);
            //addToOpenList(node, node.X + 1, node.Y + 1);
        }

        private AstarNode findNearPointFromList(List list, int x, int y)
        {
            AstarNode result = null;
            int minDistance = int.MaxValue;

            foreach (AstarNode node in list)
            {
                int dist = (int)Math.Sqrt(Math.Abs(node.X - x) * Math.Abs(node.X - x) + Math.Abs(node.Y - y) * Math.Abs(node.Y - y));

                if (dist < minDistance)
                {
                    minDistance = dist;
                    result = node;
                }
            }

            return result;
        }

        public bool canWalk(int x, int y)
        {
            return mapData[x, y];
        }

        public bool canWalkPixel(int x, int y)
        {
            int px = x / pixelFormat;
            int py = y / pixelFormat;

            return canWalk(px, py);
        }

        public List findPath(int _startX, int _startY, int _endX, int _endY)
        {
            this.endX = _endX;
            this.endY = _endY;
            this.openList.Clear();
            this.closeList.Clear();
            List result = new List();
            AstarNode currNode = null;
            bool findPathFlag = false;

            addToOpenList(null, _startX, _startY);

            while (openList.Count > 0)
            {
                currNode = openToCloseWithBest();

                if (currNode == null)
                {
                    break;
                }

                if (currNode.X == _endX && currNode.Y == _endY)
                {
                    findPathFlag = true;
                    break;
                }

                genAroundNode(currNode);
            }

            if (!findPathFlag)
            {
                currNode = findNearPointFromList(closeList, endX, endY);
            }

            if (currNode == null)
            {
                return null;
            }

            while (true)
            {
                result.Add(currNode);

                if (currNode.X == _startX && currNode.Y == _startY)
                {
                    break;
                }

                currNode = currNode.Parent;
            }

            result.Reverse();

            return result;
        }

        public List findPathPixel(int startX, int startY, int endX, int endY)
        {
            int sx = startX / pixelFormat;
            int sy = startY / pixelFormat;
            int ex = endX / pixelFormat;
            int ey = endY / pixelFormat;

            List result = findPath(sx, sy, ex, ey);

            if (result == null)
            {
                return null;
            }

            for (int i = 0; i < result.Count; ++i)
            {
                result[i].X *= pixelFormat;
                result[i].Y *= pixelFormat;
            }

            return result;
        }

        public void enableMapData(int x, int y, bool value)
        {
            mapData[x, y] = value;
        }

        public void enableMapDataPixel(int x, int y, bool value)
        {
            int px = x / pixelFormat;
            int py = y / pixelFormat;

            enableMapData(px, py, value);
        }

        public void syncMapData(int x, int y)
        {
            mapData[x, y] = !mapData[x, y];
        }

        public void syncMapDataPixel(int x, int y)
        {
            int px = x / pixelFormat;
            int py = y / pixelFormat;

            syncMapData(px, py);
        }

        public void enableMapDataAll(bool value)
        {
            for (int w = 0; w < mapWidth; ++w)
            {
                for (int h = 0; h < mapHeight; ++h)
                {
                    mapData[w, h] = value;
                }
            }
        }

        public void initMapData(int _widthPixel, int _heightPixel, int _pixelFormat)
        {
            int width = _widthPixel / _pixelFormat;
            int height = _heightPixel / _pixelFormat;

            pixelFormat = _pixelFormat;
            mapData = new bool[width, height];
            mapWidth = width;
            mapHeight = height;

            enableMapDataAll(true);
        }
    }
}

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 testAstar
{
    public class Player
    {
        public int ID;
        public int X;
        public int Y;
        public int TargetX;
        public int TargetY;
        public List Paths;
        public int PathIndex;
        public Color PathColor;
        public Color ShowColor;

        public void delete(Astar astar)
        {
            astar.enableMapDataPixel(X, Y, true);
        }

        public void update(Astar astar)
        {
            if (Paths != null)
            {
                if (PathIndex < Paths.Count)
                {
                    int tx = Paths[PathIndex].X;
                    int ty = Paths[PathIndex].Y;

                    if (astar.canWalkPixel(tx, ty))
                    {
                        astar.enableMapDataPixel(X, Y, true);
                        X = tx;
                        Y = ty;
                        astar.enableMapDataPixel(X, Y, false);
                        PathIndex++;
                    }
                    else
                    {
                        astar.enableMapDataPixel(X, Y, true);
                        Paths = astar.findPathPixel(X, Y, TargetX, TargetY);
                        PathIndex = 0;
                    }
                }
                else
                {
                    astar.enableMapDataPixel(X, Y, true);
                    Paths = null;
                    PathIndex = 0;
                }
            }
            else
            {
                int x = Form1.rand.Next(0, Form1.MapWidth);
                int y = Form1.rand.Next(0, Form1.MapHeight);

                if (astar.canWalkPixel(x, y))
                {
                    TargetX = x;
                    TargetY = y;
                    Paths = astar.findPathPixel(X, Y, x, y);
                    PathIndex = 0;
                }
            }
        }

        public void render(Astar astar, Graphics g)
        {
            if (Paths != null)
            {
                for (int i = PathIndex; i < Paths.Count; ++i)
                {
                    g.FillRectangle(new SolidBrush(PathColor), new Rectangle(Paths[i].X, Paths[i].Y, astar.PixelFormat, astar.PixelFormat));
                }
            }

            g.FillRectangle(new SolidBrush(ShowColor), new Rectangle(X, Y, astar.PixelFormat, astar.PixelFormat));

            g.DrawString(ID.ToString(), new Font("楷体", 14, FontStyle.Bold), Brushes.Black, X, Y);
        }
    }

    public partial class Form1 : Form
    {
        public static int MapWidth = 640;
        public static int MapHeight = 480;

        public static Random rand = new Random((int)DateTime.Now.Ticks);

        private Astar astar = new Astar();
        private Bitmap surface = null;
        private Graphics g = null;

        private List players = new List();

        private bool[] keys = new bool[256];

        private void init()
        {
            pictureBox1.Location = Point.Empty;
            pictureBox1.ClientSize = new System.Drawing.Size(MapWidth, MapHeight);

            surface = new Bitmap(MapWidth, MapHeight);
            g = Graphics.FromImage(surface);

            astar.initMapData(MapWidth, MapHeight, 16);

            for (int i = 0; i < keys.Length; ++i)
            {
                keys[i] = false;
            }
        }

        private void update()
        {
            foreach (Player p in players)
            {
                p.update(astar);
            }
        }

        private void render()
        {
            g.Clear(Color.White);

            bool[,] mapData = astar.MapData;

            for (int w = 0; w < astar.MapWidth; ++w)
            {
                for (int h = 0; h < astar.MapHeight; ++h)
                {
                    if (!mapData[w, h])
                    {
                        g.FillRectangle(Brushes.Black, new Rectangle(w * astar.PixelFormat, h * astar.PixelFormat, astar.PixelFormat, astar.PixelFormat));
                    }
                }
            }

            foreach (Player p in players)
            {
                p.render(astar, g);
            }

            pictureBox1.Image = surface;
        }

        public Form1()
        {
            InitializeComponent();
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            this.Text = "A*寻路算法（动态碰撞与寻路演示）A:增加 D:减少 左键:障碍设置 右键+数字键:对应编号物体的寻路";
            init();

            Timer gameTimer = new Timer();
            gameTimer.Tick += gameTimer_Tick;
            gameTimer.Interval = 100;
            gameTimer.Start();
        }

        void gameTimer_Tick(object sender, EventArgs e)
        {
            update();
            render();
        }

        private void pictureBox1_MouseDown(object sender, MouseEventArgs e)
        {
            if (e.Button == System.Windows.Forms.MouseButtons.Left)
            {
                astar.syncMapDataPixel(e.X, e.Y);
            }
            else if (e.Button == System.Windows.Forms.MouseButtons.Right)
            {
                /*endX = e.X;
                endY = e.Y;
                paths = astar.findPathPixel(px, py, endX, endY);
                pathIndex = 0;*/

                int pi = 0;
                for (int i = 0; i < 256; ++i)
                {
                    if (keys[i])
                    {
                        pi = i - (int)Keys.D1;

                        if (pi < 0 || pi >= players.Count)
                        {
                            return;
                        }

                        Player p = players[pi];

                        p.TargetX = e.X;
                        p.TargetY = e.Y;
                        p.Paths = astar.findPathPixel(players[pi].X, players[pi].Y, e.X, e.Y);
                        p.PathIndex = 0;

                        players[pi] = p;

                        return;
                    }
                }
            }
        }

        private void Form1_KeyDown(object sender, KeyEventArgs e)
        {
            keys[e.KeyValue] = true;

            if (e.KeyCode == Keys.A)
            {
                Player p = new Player();
                p.ID = players.Count + 1;
                p.X = 0;
                p.Y = 0;
                p.TargetX = 0;
                p.TargetY = 0;
                p.Paths = null;
                p.PathIndex = 0;
                p.ShowColor = Color.FromArgb(rand.Next(0, 255), rand.Next(0, 255), rand.Next(0, 255));
                p.PathColor = Color.FromArgb(64, p.ShowColor);

                players.Add(p);
            }

            if (e.KeyCode == Keys.D)
            {
                if (players.Count > 0)
                {
                    players[players.Count - 1].delete(astar);
                    players.RemoveAt(players.Count - 1);
                }
            }
        }

        private void Form1_KeyUp(object sender, KeyEventArgs e)
        {
            keys[e.KeyValue] = false;
        }
    }
}

工程文件：http://pan.baidu.com/s/1mg1tnKk