private void Check8(AStarPoint current, Point end)
{
for (int x = current.X - 1; x < current.X + 2; x++)
{
for (int y = current.Y - 1; y < current.Y + 2; y++)
{
var index0 = y;
var index1 = x;
//添加条件:
//不是当前点
//有效点
//不在开放列表
//不在封闭列表
if (x == current.X && y == current.Y ||
index0 < 0 || index0 >= _length0 || index1 < 0 || index1 >= _length1 ||
_grid[index0, index1] == 0 ||
IsInOpenList(x, y) ||
IsInCloseList(x, y))
{
}
else
{
var point = new AStarPoint(x, y, current, end);
AddToOpenList(point);
}
}
}
}
private void AddToCloseList(AStarPoint toAdd)
{
if (!IsInCloseList(toAdd.X, toAdd.Y))
{
_closeList.Add(toAdd);
}
}
private void AddToOpenList(AStarPoint toAdd)
{
if (!IsInOpenList(toAdd.X, toAdd.Y))
{
_openList.Add(toAdd);
}
}
/// <summary>
/// A* 算法点
/// </summary>
/// <param name="x0">坐标</param>
/// <param name="y0">坐标</param>
/// <param name="parent">父节点</param>
/// <param name="end">终点</param>
public AStarPoint(int x0, int y0, AStarPoint parent, Point end)
{
X = x0;
Y = y0;
Parent = parent;
GetG();
GetH(end);
}
/// <summary>
/// 寻路
/// </summary>
/// <param name="start">起点</param>
/// <param name="end">终点</param>
/// <returns></returns>
public List <Point> FindPath(Point start, Point end)
{
ServiceLocator.GlobalLogger.Debug($"FindPath(Cell Point) : {start.X} {start.Y} to {end.X} {end.Y}");
List <Point> way = new List <Point>();
var index0 = start.Y;
var index1 = start.X;
//预判断
if (index0 < 0 || index0 >= _length0 || index1 < 0 || index1 >= _length1 ||
_grid[index0, index1] == 0)
{
return(way);
}
//尝试获取一个可用的终点
if (!TryGetAnValidEndPoint(end, out end))
{
return(way);
}
//将起点加入开放列表
var startPoint = new AStarPoint(start.X, start.Y, null, end);
AddToOpenList(startPoint);
AStarPoint current = startPoint;
//从开放列表中寻路
while (!OpenListIsEmpty())
{
//获取G+H最小的节点
current = GetMinFFromOpenList();
if (current.X == end.X && current.Y == end.Y)
{
break; // Success
}
//检查并设置当前周围八个点
Check8(current, end);
RemoveFromOpenList(current);
AddToCloseList(current);
}
while (current != null)
{
way.Add(new Point(current.X, current.Y));
current = current.Parent;
}
return(way);
}
//从开启列表查找F值最小的节点
private AStarPoint GetMinFFromOpenList()
{
AStarPoint pmin = null;
foreach (AStarPoint p in _openList)
{
if (pmin == null || pmin.G + pmin.H > p.G + p.H)
{
pmin = p;
}
}
return(pmin);
}
private void RemoveFromOpenList(AStarPoint toAdd)
{
_openList.Remove(toAdd);
}
