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

3736人阅读 评论(1) 收藏 举报
分类:

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<AstarNode> openList = new List<AstarNode>();
        private List<AstarNode> closeList = new List<AstarNode>();
        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<AstarNode> 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<AstarNode> 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<AstarNode> findPath(int _startX, int _startY, int _endX, int _endY)
        {
            this.endX = _endX;
            this.endY = _endY;
            this.openList.Clear();
            this.closeList.Clear();
            List<AstarNode> result = new List<AstarNode>();
            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<AstarNode> 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<AstarNode> 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<AstarNode> 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<Player> players = new List<Player>();

        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


查看评论

动态寻路

动态寻路 目标        在一个所有物体都在动态移动的场景中,调用物理引擎现有的能力让一个实体从任意一点走到另一点的同时能够逼真地躲开所有可能与此物相撞的物体。 在这个话题上可说的很多。当你看下面...
  • yvoliuone
  • yvoliuone
  • 2015-07-08 14:27:59
  • 827

unity寻路——一劳永逸地解决寻路问题

一劳永逸地解决寻路问题 作者:PaulT 译者:trcj 原文:http://www.ai-blog.net/archives/000152.html            通常我都会尽量避...
  • tchenjiant
  • tchenjiant
  • 2015-10-30 14:02:23
  • 15689

A星寻路算法之RVO动态寻路

A星寻路算法之RVO动态寻路A星寻路算法之RVO动态寻路A星寻路算法之RVO动态寻路 未完待续...
  • qq563129582
  • qq563129582
  • 2015-12-30 19:15:59
  • 4201

unity3d多人寻路问题方案

转自:http://tieba.baidu.com/p/3397037553 相信大家用unity3d自带navmeshagent寻路的时候,一定会碰到多人寻路相互挤压的问题。 这个我已经解决了,...
  • u011926026
  • u011926026
  • 2017-03-19 10:59:51
  • 829

Unity A*寻路算法(人物移动AI)

最近闲来无事,从网上看了一些大神的心得,再融合自己的体会,写了一个简单点的寻路算法,废话不多说,直接上代码 usingUnityEngine; using System.Colle...
  • qq_38813600
  • qq_38813600
  • 2017-12-11 14:04:12
  • 196

深入理解游戏中寻路算法

摘要: 看似寻常的路径行走,在程序看来就需要一定的寻路算法来解决,如何在最短时间内找到一条路径最短的路线,这是我们首要考虑的问题。 如果你玩过MMOARPG游戏,比如魔兽,你会发现人物行走会...
  • zerokkqq
  • zerokkqq
  • 2017-07-28 20:49:08
  • 4529

Unity3d 自动寻路系统Navigation实现人物上楼梯、走斜坡、攀爬、跳跃

参考文章:Unity3D深入浅出 - 导航网格自动寻路(Navigation Mesh) http://blog.csdn.net/yuxikuo_1/article/details/4497473...
  • q764424567
  • q764424567
  • 2017-10-21 17:29:39
  • 2592

【Unity学习笔记】——使用unity自带寻路系统进行寻路

自动寻路步骤: ①  把场景中不动的物体勾选static ②  烘焙寻路网格 ③  添加NavMeshAgent组件 ④  给需要寻路的物体添加脚本 实现: ① 搭一个简易场景 ...
  • wwanrong
  • wwanrong
  • 2017-07-27 15:50:43
  • 3073

带地形惩罚和碰撞半径的A*寻路算法

这是之前写的带地形惩罚的A*寻路算法:http://blog.csdn.net/clb929/article/details/54745742 前言 在实际开发过程中,发现碰撞计算始终是无法回避的...
  • clb929
  • clb929
  • 2017-05-12 22:10:32
  • 789

寻路算法A*, JPS(跳点搜索)的一些杂谈

A*是一个比较经典的启发式寻路算法。是基于dijkstra算法,但是加入了启发函数,使路径搜索效率更高。实现起来很简单。不过要做到通用性高,比如支持各种不同类型的地图,甚至不仅仅是地图,而是个图结构如...
  • czh3642210
  • czh3642210
  • 2016-09-19 16:48:16
  • 1789
    个人资料
    持之以恒
    等级:
    访问量: 10万+
    积分: 187
    排名: 99万+
    文章分类
    最新评论