// Returns true when pathing is complete
public void IteratePath()
{
if (frontier.Count <= 0)
{
Debug.Log("Frontier is empty");
complete = true;
return;
}
MoveCostNode curr = frontier[0];
frontier.RemoveAt(0);
if (curr.node == to)
{
Debug.Log($"Found: {curr.node.name}");
TerrainNode currNode = curr.node;
finalPath.Add(currNode);
while (currNode != from)
{
currNode = currPath[currNode.GetInstanceID()].node;
finalPath.Add(currNode);
}
finalPath.Reverse();
complete = true;
return;
}
for (int i = 0; i < curr.node.paths.Count; i++)
{
TerrainNode path = curr.node.paths[i];
if (path == null || path.type == TerrainType.Blocked)
{
continue;
}
float newCost = curr.totalMoveCost + path.moveCost;
// Current paths either don't have the path node, or they do but the new move cost is better than existing move cost
if (visited.Contains(path))
{
continue;
}
if (currPath.ContainsKey(path.GetInstanceID()) && newCost >= currPath[path.GetInstanceID()].totalMoveCost)
{
continue;
}
currPath.Remove(path.GetInstanceID());
currPath.Add(path.GetInstanceID(), new MoveCostNode(curr.node, newCost));
visited.Add(path);
frontier.Add(new MoveCostNode(path, newCost));
frontier.Sort((a, b) => a.totalMoveCost.CompareTo(b.totalMoveCost));
}
}
// Returns true when pathing is complete
public void IteratePath()
{
if (frontier.Count <= 0)
{
Debug.Log("Frontier is empty");
complete = true;
return;
}
TerrainNode currNode = frontier.Dequeue();
if (currNode == to)
{
Debug.Log($"Found: {currNode.name}");
// Use cameFrom to find path back
finalPath.Add(currNode);
while (currNode != from)
{
currNode = cameFrom[currNode.GetInstanceID()];
finalPath.Add(currNode);
}
finalPath.Reverse();
complete = true;
return;
}
for (int i = 0; i < currNode.paths.Count; i++)
{
TerrainNode path = currNode.paths[i];
if (path == null || path.type == TerrainType.Blocked || visited.Contains(path))
{
continue;
}
visited.Add(path);
frontier.Enqueue(path);
cameFrom.Add(path.GetInstanceID(), currNode);
}
}
