A*寻路算法不需要多言=w=
直接上成型代码
因为是很早编写的代码 只用到了.net2.0的语法 算是很合用的
首先是一系列抽象泛型类
保留了一些关键函数作为抽象方法被继承
比如获取相邻格子 获取格子的实际消耗 计算启发距离
注意一点 这里对T类型实现了字典类
所以自定义T类型的时候 如果是引用类型 确保你引用的是同一个实例
否则务必对T类型重写GetHashCode和Equals方法
public abstract class AStar<T>
{
public AStar()
{
}
protected abstract int GetCost(T Start, T end);
protected abstract int GetHeuristicCost(T start, T end);
protected abstract T[] GetNeighbors(T data);
private AStarNode<T>[] CreateNeighborArray(T[] neighbors)
{
if (neighbors == null || neighbors.Length == 0)
return new AStarNode<T>[0];
AStarNode<T>[] neighborArray = new AStarNode<T>[neighbors.Length];
for (int i = 0; i < neighbors.Length; i++)
{
neighborArray[i] = new AStarNode<T>(neighbors[i]);
}
return neighborArray;
}
public virtual T[] Search(T start, T end)
{
AStartNodeDictionary<T> closed = new AStartNodeDictionary<T>();
AStartNodeDictionary<T> open = new AStartNodeDictionary<T>();
AStarNode<T> startNode = new AStarNode<T>(start);
startNode.Goal = 0;
startNode.Heuristic = GetHeuristicCost(startNode.Data, end);
open.Add(startNode);
while (open.Count > 0)
{
AStarNode<T> current = null;
int f = Int32.MaxValue;
foreach (AStarNode<T> node in open)
{
if (node.Fitness < f)
{
current = node;
f = current.Fitness;
}
}
if (current.Data.Equals(end))
{
return ReconstructPath(current);
}
open.Remove(current.Data);
closed.Add(current);
if (current.Neighbors == null)
{
current.Neighbors = CreateNeighborArray(GetNeighbors(current.Data));
}
foreach (AStarNode<T> neighbor in current.Neighbors)
{
if (closed.Contains(neighbor.Data))
continue;
int tentative_goal = current.Goal + GetCost(current.Data, neighbor.Data);
bool openedContains = open.Contains(neighbor.Data);
if (!openedContains || tentative_goal < neighbor.Goal)
{
if (!openedContains)
{
open.Add(neighbor);
}
neighbor.CameFrom = current;
neighbor.Goal = tentative_goal;
neighbor.Heuristic = GetHeuristicCost(current.Data, end);
}
}
}
return null;
}
public T[] ReconstructPath(AStarNode<T> node)
{
if (node == null)
return null;
Stack<T> stack = new Stack<T>();
while (node != null)
{
stack.Push(node.Data);
node = node.CameFrom;
}
return stack.ToArray();
}
}
public class AStartNodeDictionary<T> : System.Collections.ObjectModel.KeyedCollection<T, AStarNode<T>>
{
public AStartNodeDictionary()
: base()
{
}
protected override T GetKeyForItem(AStarNode<T> item)
{
return item.Data;
}
}
public class AStarNode<T>
{
public AStarNode()
: this(default(T))
{
}
public AStarNode(T data)
{
this.data = data;
this.g = Int32.MaxValue;
}
private int g;
private int h;
private T data;
private AStarNode<T> cameFrom;
private AStarNode<T>[] neighbors;
public int Goal
{
get { return g; }
set { g = value; }
}
public int Heuristic
{
get { return h; }
set { h = value; }
}
public int Fitness
{
get { return g + h; }
}
public T Data
{
get { return data; }
set { data = value; }
}
public AStarNode<T> CameFrom
{
get { return cameFrom; }
set { cameFrom = value; }
}
public AStarNode<T>[] Neighbors
{
get { return neighbors; }
set { neighbors = value; }
}
}
下面是一个实际应用场合
声明了三个类 分别作为搜索地图 地图的单元格 与特定算法的A*实现
class SlgGrid
{
public SlgGrid(int x, int y)
: this(x, y, 1)
{
}
public SlgGrid(int x, int y, int cost)
{
this.x = x;
this.y = y;
this.cost = cost;
}
public int x;
public int y;
public int cost;
public static int Distance(SlgGrid grid1, SlgGrid grid2)
{
if (grid1 == null || grid2 == null)
return 0xff;
return Math.Abs(grid1.x - grid2.x) + Math.Abs(grid1.y - grid2.y);
}
public override string ToString()
{
return string.Format("x:{0} y:{1} cost:{2}", x, y, cost);
}
}
class SlgMapData
{
public SlgMapData(int width, int height)
{
this.width = width;
this.height = height;
this.grids = new SlgGrid[width][];
for (int i = 0; i < width; i++)
{
this.grids[i]=new SlgGrid[height];
for (int j = 0; j < height; j++)
{
this.grids[i][j] = new SlgGrid(i, j, 1);
}
}
}
int width;
int height;
SlgGrid[][] grids;
public SlgGrid GetGrid(int x, int y)
{
if (x < 0 || x >= width || y < 0 || y >= height)
return null;
return grids[x][y];
}
public void SetGridCost(int x, int y, int cost)
{
SlgGrid grid = GetGrid(x,y);
if (grid!=null) grid.cost = cost;
}
}
class SlgAStar : AStar<SlgGrid>
{
public SlgAStar(SlgMapData map)
{
this.map = map;
}
SlgMapData map;
protected override int GetCost(SlgGrid start, SlgGrid end)
{
if (start == null || end == null || SlgGrid.Distance(start, end) != 1)
return 0xff;
return end.cost;
}
protected override int GetHeuristicCost(SlgGrid start, SlgGrid end)
{
if (start == null || end == null)
return 0;
return SlgGrid.Distance(start, end);
}
protected override SlgGrid[] GetNeighbors(SlgGrid data)
{
if (data == null)
return null;
List<SlgGrid> neighbors = new List<SlgGrid>(4);
SlgGrid grid;
if ((grid = map.GetGrid(data.x, data.y - 1)) != null)
neighbors.Add(grid);
if ((grid = map.GetGrid(data.x, data.y + 1)) != null)
neighbors.Add(grid);
if ((grid = map.GetGrid(data.x - 1, data.y)) != null)
neighbors.Add(grid);
if ((grid = map.GetGrid(data.x + 1, data.y)) != null)
neighbors.Add(grid);
return neighbors.ToArray();
}
}
下面是一个测试用例....
void Main()
{
SlgMapData map = new SlgMapData(7,7);
int[,] matrix = new int[7,7] {
{6,3,2,3,4,5,9},
{3,2,4,1,5,7,3},
{3,4,6,3,2,4,4},
{5,4,7,9,8,5,2},
{1,2,3,4,5,6,2},
{4,5,4,5,7,8,4},
{3,4,5,7,2,5,3}
};
for(int i=0;i<7;i++){
for(int j=0;j<7;j++){
map.SetGridCost(i,j,matrix[i,j]);
}
}
AStar<SlgGrid> astar = new SlgAStar(map);
var path = astar.Search(map.GetGrid(3,3), map.GetGrid(6,6));
for(int i=0;i<path.Length;i++)
Console.WriteLine("{0},{1} cost:{2}",path[i].x, path[i].y, path[i].cost);
Console.WriteLine(path.Sum(grid=>grid.cost));
}
返回结果:
3,3 cost:9
4,3 cost:4
4,4 cost:5
4,5 cost:6
4,6 cost:2
5,6 cost:4
6,6 cost:3
33