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

3189人阅读 评论(1)

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

1、用A*找出一条路径

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

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

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

5、到目的地停止

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

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);
}

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.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 + 1);

//addToOpenList(node, node.X + 1, node.Y - 1);
//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;

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)
{

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.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);

}

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;
}
}
}


0
0

* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场
个人资料
• 访问：95982次
• 积分：115
• 等级：
• 排名：千里之外
• 原创：38篇
• 转载：2篇
• 译文：0篇
• 评论：33条
文章分类
评论排行
最新评论