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