A*寻路算法
namespace S
{
public class ANode
{
public ANode priviour;//上一个节点
public int x;
public int y;
public int f => h + g;//预估代价
public int h;//预估到终点的代价
public int g;//从开始节点到此节点的代价
public ANode(int x,int y) {
this.x = x;
this.y = y;
}
}
class AStar
{
private List<ANode> openList = new List<ANode>();
private List<ANode> closeList = new List<ANode>();
private ANode startNode;
private ANode endNode;
private bool canCorner;//可以走斜角
private Func<int,int, bool> onWalkNode;
/// <summary>
/// 获取从开始点到结束点的路径
/// </summary>
/// <param name="startNode">开始点</param>
/// <param name="endNode">结束点</param>
/// <param name="canCorner">是否可以斜着走</param>
/// <returns></returns>
public ANode GetPath(ANode startNode,ANode endNode,bool canCorner) {
if (openList == null) openList = new List<ANode>();
else openList.Clear();
if (closeList == null) closeList = new List<ANode>();
else closeList.Clear();
this.startNode = startNode;
this.endNode = endNode;
this.canCorner = canCorner;
openList.Add(startNode);
return SearchOpenList();
}
/// <summary>
/// 注册位置点的可走状态
/// </summary>
/// <param name="onWalkNode">（位置点x坐标，位置点y坐标，返回值是否可走）</param>
/// <returns></returns>
public AStar OnWalkNode(Func<int, int, bool> onWalkNode)
{
this.onWalkNode = onWalkNode;
return this;
}
/// <summary>
/// 遍历openList
/// </summary>
ANode SearchOpenList() {
while (openList.Count>0) {
ANode bestNode = GetBestNodeOfOpenList();
openList.Remove(bestNode);
closeList.Add(bestNode);
List<ANode> rangeList = GetRangeNodes(bestNode);
ANode node = null;
ANode tryNodeOfOpenList = null;
for (int i = 0; i < rangeList.Count; i++)
{
node = rangeList[i];
tryNodeOfOpenList = GetNodeOfOpenList(node.x, node.y);//试图从openList中得到这个Node
if (tryNodeOfOpenList==null)//这个Node不在openList中
{
ConnectNode(bestNode,node);
openList.Add(node);
}
else { //此节点已经存在于OpenList中
int tryG = bestNode.g + GetDistance(bestNode,node);
int tryH = GetDistance(node,endNode);
int tryF = tryG + tryH;
if (tryF<tryNodeOfOpenList.f)//从bestNode去往当前点的代价更低
{
ConnectNode(bestNode,node);
}
}
}
ANode tryEndNode = GetNodeOfOpenList(endNode.x,endNode.y);
if (tryEndNode != null) return tryEndNode;//如果结束点已经在openList中则直接返回结束点
}
return null;
}
/// <summary>
/// 在OpenList中找到代价最小的Node
/// </summary>
ANode GetBestNodeOfOpenList() {
if (openList.Count == 0) return null;
ANode bestNode = null;
ANode curNode = null;
for (int i = openList.Count-1; i >=0; i--)
{
curNode = openList[i];
if (bestNode==null)
{
bestNode = curNode;
continue;
}
if (curNode.f > bestNode.f) continue;
if (curNode.f < bestNode.f) bestNode = curNode;
else if (curNode.h < bestNode.h) bestNode = curNode;
}
return bestNode;
}
/// <summary>
/// 获取Node周围不在CloseList中的所有Node
/// </summary>
List<ANode> GetRangeNodes(ANode node) {
if (node == null) return null;
List<ANode> list = new List<ANode>();
ANode newNode = null;
for (int i = node.x-1; i <=node.x+1; i++)
{
for (int j = node.y-1; j <=node.y+1; j++)
{
if (i == node.x && j == node.y) continue;//node本身
if (!CanWalk(node, i, j)) continue;//不可行至的节点
if (HasNodeOfCloseList(i,j)) continue;//已经在CloseList中
newNode = GetNodeOfOpenList(i,j);
if (newNode == null) newNode=new ANode(i,j);
list.Add(newNode);
}
}
return list;
}
/// <summary>
/// 连接Node
/// </summary>
/// <param name="begainNode">开始Node</param>
/// <param name="targetNode">目标Node</param>
void ConnectNode(ANode begainNode,ANode targetNode) {
targetNode.priviour = begainNode;
targetNode.g = begainNode.g+GetDistance(begainNode, targetNode);
targetNode.h = GetDistance(targetNode, endNode);
}
/// <summary>
/// 获取两个Node之间的移动代价
/// </summary>
/// <param name="begainNode">开始Node</param>
/// <param name="targetNode">目标Node</param>
/// <returns></returns>
int GetDistance(ANode begainNode, ANode targetNode) {
// 直着走代价为10 斜着走代价为14 预判代价时不考虑斜着走
int disX = Math.Abs(begainNode.x-targetNode.x);
int disY = Math.Abs(begainNode.y-targetNode.y);
int step = disX + disY;
int dis = 0;
if (step == 2)
{
if (canCorner)//可以走斜角
{
if (disX == disY) dis = 14;//斜对角
}
else dis = step * 10;
}
else dis=step * 10;
return dis;
}
/// <summary>
/// 从CloseList中寻找指定位置点的ANode
/// </summary>
ANode GetNodeOfCloseList(int x,int y) {
if (closeList == null || closeList.Count == 0) return null;
ANode node = null;
for (int i = closeList.Count-1; i >=0; i--)
{
node = closeList[i];
if (x == node.x && y == node.y) return node;
}
return null;
}
/// <summary>
/// 从OpenList中寻找指定位置点的ANode
/// </summary>
ANode GetNodeOfOpenList(int x,int y) {
if (openList == null || openList.Count == 0) return null;
ANode node = null;
for (int i = openList.Count - 1; i >= 0; i--)
{
node = openList[i];
if (x == node.x && y == node.y) return node;
}
return null;
}
/// <summary>
/// CloseList中是否包含指定位置点的ANode
/// </summary>
bool HasNodeOfCloseList(int x, int y) {
if (closeList == null || closeList.Count == 0) return false;
ANode node = null;
for (int i = closeList.Count - 1; i >= 0; i--)
{
node = closeList[i];
if (x == node.x && y == node.y) return true;
}
return false;
}
/// <summary>
/// OpenList中是否包含指定位置点的ANode
/// </summary>
bool HasNodeOfOpenList(int x, int y)
{
if (openList == null || openList.Count == 0) return false;
ANode node = null;
for (int i = openList.Count - 1; i >= 0; i--)
{
node = openList[i];
if (x == node.x && y == node.y) return true;
}
return false;
}
/// <summary>
/// 从指定Node能否走到目标坐标
/// </summary>
/// <param name="begainNode">指定Node</param>
/// <param name="endX">目标坐标X</param>
/// <param name="endY">目标坐标Y</param>
/// <returns></returns>
bool CanWalk(ANode begainNode,int endX,int endY) {
bool canWalk = onWalkNode == null ? true : onWalkNode(endX,endY);
if (canWalk)
{
int disX = Math.Abs(begainNode.x-endX);
int disY = Math.Abs(begainNode.y-endY);
int dis = disX + disY;
if (dis < 2) canWalk = true;
else if (dis == 2)
{
if (canCorner) canWalk = disX == disY;
else canWalk = false;
}
else if (dis > 2) canWalk = false;
}
return canWalk;
}
}
}
示例
namespace S
{
class Program
{
static void Main(string[] args)
{
AStar aStar = new AStar().OnWalkNode(WalkNode);
ANode node=aStar.GetPath(new ANode(2,2),new ANode(6,0),false);
while (node!=null)
{
Console.WriteLine(node.x + "," + node.y);
node = node.priviour;
}
Console.ReadKey();
}
static bool WalkNode(int x,int y) {
if (x < 0 || x > 6) return false;
if (y < 0 || y > 6) return false;
if (x == 3 && y == 0) return false;
if (x == 3 && y == 1) return false;
if (x == 3 && y == 2) return false;
if (x == 3 && y == 3) return false;
if (x == 3 && y == 4) return false;
if (x == 3 && y == 5) return false;
// if (x == 3 && y == 6) return false;
return true;
}
}
}