private IEnumerable <INode> GetNeighbors(INode current, SearchLayer layer) { //随机方向? foreach (var dir in dirs) { Point pos = current.Position + dir; var grid = GetNode(pos); if (grid != null && grid.InSearchLayer(layer)) { yield return(grid); } } }
public bool Search(INode start, INode goal, SearchLayer layer) { cameFrom.Clear(); cost.Clear(); frontier.Clear(); frontier.Enqueue(start, 0); cameFrom[start] = start; cost[start] = 0; bool isFound = false; while (frontier.Count > 0) { var current = frontier.Dequeue(); if (current == goal) { isFound = true; break; } foreach (var neighbor in GetNeighbors(current, layer)) { int newCost = cost[current] + 1;//所有的代价都为1 if (!cost.TryGetValue(neighbor, out int nearCost) || newCost < nearCost) { cost[neighbor] = nearCost; int priority = newCost + (neighbor.Position - goal.Position).AbsLen; frontier.Enqueue(neighbor, priority); cameFrom[neighbor] = current; } } } path.Clear(); if (isFound) { INode cur = goal; while (!cur.Equals(start)) { path.Add(cur); if (!cameFrom.TryGetValue(cur, out cur)) { break; } } path.Add(start); } return(isFound); }