经过测试，效率还可以
使用
public void Test()
{
var pathGrid = new MapGrid(100, 100, new List<Point2>());
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
var path = pathGrid.FindPath(new Point2(0, 0), new Point2(99, 99));
sw.Stop();
Debug.LogError("total second:" + sw.ElapsedMilliseconds);
}
Point2
namespace AStar
{
/// <summary>
/// 坐标数据
/// </summary>
public class Point2
{
public Point2(int x, int y)
{
this.x = x;
this.y = y;
}
public int x { get; set; }
public int y { get; set; }
public override bool Equals(object obj)
{
return this.x == (obj as Point2).x && this.y == (obj as Point2).y;
}
public override int GetHashCode()
{
return x ^ (y * 256);
}
public override string ToString()
{
return x + "," + y;
}
public static bool operator ==(Point2 a, Point2 b)
{
return a.Equals(b);
}
public static bool operator !=(Point2 a, Point2 b)
{
return !a.Equals(b);
}
}
}
PathNode
namespace AStar
{
/// <summary>
/// 节点数据
/// </summary>
public class PathNode
{
public PathNode(bool isWall, Point2 position)
{
this.isWall = isWall;
this.position = position;
}
public readonly Point2 position;
public bool isWall { get; set; }
public PathNode parent { get; set; }
public int gCost { get; set; }
public int hCost { get; set; }
public int fCost
{
get
{
return gCost + hCost;
}
}
public override bool Equals(object obj)
{
PathNode node = obj as PathNode;
return node.isWall == this.isWall && node.gCost == this.gCost && node.hCost == this.hCost && node.parent == this.parent && this.position == node.position;
}
public override int GetHashCode()
{
return gCost ^ (hCost * 128) + position.GetHashCode();
}
public static bool operator ==(PathNode a, PathNode b)
{
return a.Equals(b);
}
public static bool operator !=(PathNode a, PathNode b)
{
return !a.Equals(b);
}
}
}
MapGrid
using System.Collections.Generic;
using System.Linq;
using System;
namespace AStar
{
/// <summary>
/// 地图数据
/// </summary>
public class MapGrid
{
/// <summary>
/// 开启列表树，key为FCose
/// </summary>
private SortedDictionary<int, List<Point2>> openTree = new SortedDictionary<int, List<Point2>>();
/// <summary>
/// 开启列表
/// </summary>
private HashSet<Point2> openSet = new HashSet<Point2>();
/// <summary>
/// 关闭列表
/// </summary>
private HashSet<Point2> closeSet = new HashSet<Point2>();
/// <summary>
/// 所有的节点
/// </summary>
private Dictionary<Point2, PathNode> allNodes = new Dictionary<Point2, PathNode>();
/// <summary>
/// 寻路终点
/// </summary>
private Point2 endPos;
/// <summary>
/// 网格大小
/// </summary>
private Point2 gridSize;
/// <summary>
/// 当前寻路数据
/// </summary>
private List<Point2> currentPath;
/// <summary>
/// 新建一个PathGrid，包含了网格大小和障碍物信息
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="walls"></param>
public MapGrid(int x, int y, List<Point2> walls)
{
gridSize = new Point2(x, y);
for (int i = 0; i < x; i++)
{
for (int j = 0; j < y; j++)
{
Point2 newPos = new Point2(i, j);
allNodes.Add(newPos, new PathNode(walls.Contains(newPos), newPos));
}
}
}
/// <summary>
/// 寻路主要逻辑，通过调用该方法来获取路径信息，由一串Point2代表
/// </summary>
/// <param name="beginPos"></param>
/// <param name="endPos"></param>
/// <returns></returns>
public List<Point2> FindPath(Point2 beginPos, Point2 endPos)
{
List<Point2> result = new List<Point2>();
this.endPos = endPos;
Point2 currentPos = beginPos;
//把起点加入开启列表
openSet.Add(currentPos);
while (!currentPos.Equals(this.endPos))
{
UpdatePath(currentPos);
//未找到路径
if (openSet.Count == 0) return null;
currentPos = openTree.First().Value.First();
}
//找到了路径，返回路径
Point2 path = currentPos;
while (!path.Equals(beginPos))
{
result.Add(path);
path = allNodes[path].parent.position;
currentPath = result;
}
result.Add(beginPos);
return result;
}
/// <summary>
/// 寻路
/// </summary>
/// <param name="currentPos"></param>
private void UpdatePath(Point2 currentPos)
{
//关闭该节点
closeSet.Add(currentPos);
RemoveOpen(currentPos, allNodes[currentPos]);
//继续寻找周围节点
List<Point2> neighborNodes = FindNeighbor(currentPos);
foreach (Point2 nodePos in neighborNodes)
{
PathNode newNode = new PathNode(false, nodePos);
newNode.parent = allNodes[currentPos];
int g;
int h;
//计算当前代价gCost，距离起点的距离
{
//直行移动耗费设为10，斜行移动耗费设为14
g = currentPos.x == nodePos.x || currentPos.y == nodePos.y ? 10 : 14;
newNode.gCost = g + newNode.parent.gCost;
}
//计算预估代价hCost，预估终点的距离
{
int xMoves = Math.Abs(nodePos.x - endPos.x);
int yMoves = Math.Abs(nodePos.y - endPos.y);
int min = Math.Min(xMoves, yMoves);
int max = Math.Max(xMoves, yMoves);
h = min * 14 + (max - min) * 10;
//欧拉距离
//h = (int)Math.Pow(nodePos.x - endPos.x, 2) + (int)Math.Pow(nodePos.y - endPos.y, 2);
//曼哈顿距离
//h = Math.Abs(nodePos.x - endPos.x) + Math.Abs(nodePos.y - endPos.y);
newNode.hCost = h;
}
PathNode originNode = allNodes[nodePos];
if (openSet.Contains(nodePos))
{
//如果新节点的f值小于老节点的f值，用新节点替换老节点
if (newNode.fCost < originNode.fCost)
{
UpdateNode(newNode, originNode);
}
}
else
{
allNodes[nodePos] = newNode;
AddOpen(nodePos, newNode);
}
}
}
/// <summary>
/// 将旧节点更新为新节点
/// </summary>
/// <param name="newNode"></param>
/// <param name="oldNode"></param>
private void UpdateNode(PathNode newNode, PathNode oldNode)
{
Point2 nodePos = newNode.position;
int oldCost = oldNode.fCost;
allNodes[nodePos] = newNode;
List<Point2> sameCost;
if (openTree.TryGetValue(oldCost, out sameCost))
{
sameCost.Remove(nodePos);
if (sameCost.Count == 0) openTree.Remove(oldCost);
}
if (openTree.TryGetValue(newNode.fCost, out sameCost))
{
sameCost.Add(nodePos);
}
else
{
sameCost = new List<Point2> { nodePos };
openTree.Add(newNode.fCost, sameCost);
}
}
/// <summary>
/// 将目标节点移出开启列表
/// </summary>
/// <param name="pos"></param>
/// <param name="node"></param>
private void RemoveOpen(Point2 pos, PathNode node)
{
openSet.Remove(pos);
List<Point2> sameCost;
if (openTree.TryGetValue(node.fCost, out sameCost))
{
sameCost.Remove(pos);
if (sameCost.Count == 0) openTree.Remove(node.fCost);
}
}
/// <summary>
/// 将目标节点加入开启列表
/// </summary>
/// <param name="pos"></param>
/// <param name="node"></param>
private void AddOpen(Point2 pos, PathNode node)
{
openSet.Add(pos);
List<Point2> sameCost;
if (openTree.TryGetValue(node.fCost, out sameCost))
{
sameCost.Add(pos);
}
else
{
sameCost = new List<Point2> { pos };
openTree.Add(node.fCost, sameCost);
}
}
/// <summary>
/// 找到某节点的所有相邻节点
/// </summary>
/// <param name="nodePos"></param>
/// <returns></returns>
private List<Point2> FindNeighbor(Point2 nodePos)
{
List<Point2> result = new List<Point2>();
for (int x = -1; x < 2; x++)
{
for (int y = -1; y < 2; y++)
{
if (x == 0 && y == 0) continue;
Point2 currentPos = new Point2(nodePos.x + x, nodePos.y + y);
if (currentPos.x >= gridSize.x || currentPos.y >= gridSize.y || currentPos.x < 0 || currentPos.y < 0) continue; //out of bondary
if (closeSet.Contains(currentPos)) continue; // already in the close list
if (allNodes[currentPos].isWall) continue; // the node is a wall
result.Add(currentPos);
}
}
return result;
}
}
}
