A*尋路算法——多人尋路虱肄、實(shí)時(shí)碰撞尋路、最近目的地

A* 路算法原理可以參考這個(gè)文章交煞，已經(jīng)寫的很詳細(xì)了http://www.cppblog.com/mythit/archive/2009/04/19/80492.aspx
這篇文章主要寫寫多人尋路的實(shí)時(shí)碰撞
先說說無法尋路的情況下咏窿，如何移動(dòng)的離目的地最近的點(diǎn)
其實(shí)所有能到達(dá)的點(diǎn)都在"關(guān)閉列表中"，當(dāng)“開啟列表”中所有的點(diǎn)都遍歷完后错敢，如果還未找到終點(diǎn)翰灾，則視為路徑不通
這時(shí)候遍歷“關(guān)閉列表”，找出其中離終點(diǎn)直線距離最短的點(diǎn)即可稚茅，見下面代碼中findNearPointFromList函數(shù)
多人碰撞的大體思路就是

  1. 用A*找出一條路徑
  2. 按該路徑走纸淮，沒移動(dòng)一格檢測(cè)是否發(fā)生碰撞
  3. 如果碰撞，調(diào)用A*重新尋路
  4. 如果未碰撞亚享，按原來路徑繼續(xù)走
  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*尋路算法（動(dòng)態(tài)碰撞與尋路演示）A:增加 D:減少 左鍵:障礙設(shè)置 右鍵+數(shù)字鍵:對(duì)應(yīng)編號(hào)物體的尋路";  
            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;  
        }  
    }  
}
最后編輯于 ：
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請(qǐng)聯(lián)系作者
  • 人面猴
    序言：七十年代末咽块，一起剝皮案震驚了整個(gè)濱河市，隨后出現(xiàn)的幾起案子欺税，更是在濱河造成了極大的恐慌侈沪，老刑警劉巖，帶你破解...
    沈念sama閱讀 218,858評(píng)論 6贊 508
  • 死咒
    序言：濱河連續(xù)發(fā)生了三起死亡事件晚凿，死亡現(xiàn)場(chǎng)離奇詭異亭罪，居然都是意外死亡，警方通過查閱死者的電腦和手機(jī)歼秽，發(fā)現(xiàn)死者居然都...
    沈念sama閱讀 93,372評(píng)論 3贊 395
  • 救了他兩次的神仙讓他今天三更去死
    文/潘曉璐 我一進(jìn)店門应役，熙熙樓的掌柜王于貴愁眉苦臉地迎上來，“玉大人燥筷，你說我怎么就攤上這事箩祥。” “怎么了肆氓？”我有些...
    開封第一講書人閱讀 165,282評(píng)論 0贊 356
  • 道士緝兇錄：失蹤的賣姜人
    文/不壞的土叔 我叫張陵袍祖，是天一觀的道長(zhǎng)。 經(jīng)常有香客問我谢揪，道長(zhǎng)蕉陋，這世上最難降的妖魔是什么？ 我笑而不...
    開封第一講書人閱讀 58,842評(píng)論 1贊 295
  • ?港島之戀（遺憾婚禮）
    正文 為了忘掉前任键耕，我火速辦了婚禮寺滚，結(jié)果婚禮上，老公的妹妹穿的比我還像新娘屈雄。我一直安慰自己村视，他們只是感情好，可當(dāng)我...
    茶點(diǎn)故事閱讀 67,857評(píng)論 6贊 392
  • 惡毒庶女頂嫁案：這布局不是一般人想出來的
    文/花漫 我一把揭開白布酒奶。 她就那樣靜靜地躺著蚁孔，像睡著了一般奶赔。 火紅的嫁衣襯著肌膚如雪。 梳的紋絲不亂的頭發(fā)上杠氢，一...
    開封第一講書人閱讀 51,679評(píng)論 1贊 305
  • 城市分裂傳說
    那天站刑，我揣著相機(jī)與錄音，去河邊找鬼鼻百。 笑死绞旅，一個(gè)胖子當(dāng)著我的面吹牛，可吹牛的內(nèi)容都是我干的温艇。 我是一名探鬼主播因悲，決...
    沈念sama閱讀 40,406評(píng)論 3贊 418
  • 雙鴛鴦連環(huán)套：你想象不到人心有多黑
    文/蒼蘭香墨 我猛地睜開眼，長(zhǎng)吁一口氣：“原來是場(chǎng)噩夢(mèng)啊……” “哼勺爱！你這毒婦竟也來了晃琳？” 一聲冷哼從身側(cè)響起，我...
    開封第一講書人閱讀 39,311評(píng)論 0贊 276
  • 萬榮殺人案實(shí)錄
    序言：老撾萬榮一對(duì)情侶失蹤琐鲁，失蹤者是張志新（化名）和其女友劉穎卫旱，沒想到半個(gè)月后，有當(dāng)?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體围段，經(jīng)...
    沈念sama閱讀 45,767評(píng)論 1贊 315
  • ?護(hù)林員之死
    正文 獨(dú)居荒郊野嶺守林人離奇死亡顾翼，尸身上長(zhǎng)有42處帶血的膿包…… 初始之章·張勛 以下內(nèi)容為張勛視角 年9月15日...
    茶點(diǎn)故事閱讀 37,945評(píng)論 3贊 336
  • ?白月光啟示錄
    正文 我和宋清朗相戀三年，在試婚紗的時(shí)候發(fā)現(xiàn)自己被綠了奈泪。 大學(xué)時(shí)的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片暴构。...
    茶點(diǎn)故事閱讀 40,090評(píng)論 1贊 350
  • 活死人
    序言：一個(gè)原本活蹦亂跳的男人離奇死亡，死狀恐怖段磨，靈堂內(nèi)的尸體忽然破棺而出，到底是詐尸還是另有隱情耗绿，我是刑警寧澤苹支，帶...
    沈念sama閱讀 35,785評(píng)論 5贊 346
  • ?日本核電站爆炸內(nèi)幕
    正文 年R本政府宣布，位于F島的核電站误阻，受9級(jí)特大地震影響债蜜，放射性物質(zhì)發(fā)生泄漏。R本人自食惡果不足惜究反，卻給世界環(huán)境...
    茶點(diǎn)故事閱讀 41,420評(píng)論 3贊 331
  • 男人毒藥：我在死后第九天來索命
    文/蒙蒙 一寻定、第九天 我趴在偏房一處隱蔽的房頂上張望。 院中可真熱鬧精耐，春花似錦狼速、人聲如沸。這莊子的主人今日做“春日...
    開封第一講書人閱讀 31,988評(píng)論 0贊 22
  • 一樁弒父案向胡，背后竟有這般陰謀
    文/蒼蘭香墨 我抬頭看了看天上的太陽恼蓬。三九已至，卻和暖如春僵芹，著一層夾襖步出監(jiān)牢的瞬間处硬，已是汗流浹背。 一陣腳步聲響...
    開封第一講書人閱讀 33,101評(píng)論 1贊 271
  • 情欲美人皮
    我被黑心中介騙來泰國(guó)打工拇派， 沒想到剛下飛機(jī)就差點(diǎn)兒被人妖公主榨干…… 1. 我叫王不留荷辕，地道東北人。 一個(gè)月前我還...
    沈念sama閱讀 48,298評(píng)論 3贊 372
  • 代替公主和親
    正文 我出身青樓件豌，卻偏偏與公主長(zhǎng)得像疮方，于是被迫代替她去往敵國(guó)和親。 傳聞我的和親對(duì)象是個(gè)殘疾皇子苟径，可洞房花燭夜當(dāng)晚...
    茶點(diǎn)故事閱讀 45,033評(píng)論 2贊 355

推薦閱讀更多精彩內(nèi)容

  • 文章轉(zhuǎn)自http://www.cnblogs.com/technology/archive/2011/05/26/...
    MrPurussaurus閱讀 1,201評(píng)論 0贊 3
  • 當(dāng)然尋路算法不止 A* 這一種, 還有遞歸, 非遞歸, 廣度優(yōu)先, 深度優(yōu)先, 使用堆棧等等, 有興趣的可以研究研...
    胤醚貔貅閱讀 583評(píng)論 1贊 1
  • 秋信 “樹葉拎著信案站，匆匆地追趕秋天，來自天堂的慈祥棘街，沒有彷徨……” 恐懼 “生活的恐懼就是越來越難看……” 說種 ...
    通靈石閱讀 273評(píng)論 0贊 1
  • 1蟆盐、添加鏡像文件 2、查看是否添加成功 3遭殉、安裝cocoaPods并初始化 –––––––––––那么問題來了——...
    會(huì)編程的男神俊閱讀 604評(píng)論 0贊 0
  • 八月的天空似乎沒有了浪漫 沒有了秋氣宜人的溫柔 門口的花花草草奄奄一息 鳥鳴也不再清脆 季節(jié)誠(chéng)然忘記歸途 找不到來...
    唐春元ok閱讀 410評(píng)論 28贊 22