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