public bool Search(MapGraph mapGraph, MapNode startNode, MapNode finishNode, PathfindingInfo pathfindingInfo)
{
PriorityQueue frontier = new PriorityQueue();
Dictionary <MapNode, float> cost_of_node = new Dictionary <MapNode, float>();
frontier.Enqueue(startNode, Heuristic(startNode, finishNode));
mapGraph.AddPreviousNode(startNode, startNode);
cost_of_node.Add(startNode, 0);
while (!frontier.IsEmpty())
{
MapNode currentNode = frontier.Dequeue().Key;
pathfindingInfo.visitedByIterations.Add(currentNode.GetPosition());
if (currentNode.Equals(finishNode))
{
Debug.Log("Path found! " + this.ToString());
return(true);
}
foreach (MapNode next in mapGraph.Neighbours(currentNode))
{
float new_cost = cost_of_node[currentNode] + Heuristic(currentNode, next);
if (!cost_of_node.ContainsKey(next) || new_cost < cost_of_node[next])
{
if (!cost_of_node.ContainsKey(next))
{
cost_of_node.Add(next, new_cost);
}
else
{
cost_of_node[next] = new_cost;
}
float priority = new_cost + Heuristic(next, finishNode);
if (!frontier.Contains(next))
{
frontier.Enqueue(next, priority);
}
mapGraph.AddPreviousNode(next, currentNode);
}
}
pathfindingInfo.FrontierByOrder.Add(frontier.ReturnCurrentNodesAsArray());
pathfindingInfo.iterations++;
}
Debug.Log("Path not found!");
return(false);
}
public bool Search(MapGraph mapGraph, MapNode startNode, MapNode finishNode, PathfindingInfo pathfindingInfo)
{
Queue <MapNode> frontier = new Queue <MapNode>();
frontier.Enqueue(startNode);
mapGraph.AddPreviousNode(startNode, startNode);
while (frontier.Count > 0)
{
MapNode currentNode = frontier.Dequeue();
pathfindingInfo.visitedByIterations.Add(currentNode.GetPosition());
if (currentNode == finishNode)
{
Debug.Log("Path found!" + this.ToString());
return(true);
}
foreach (MapNode next in mapGraph.Neighbours(currentNode))
{
if (!mapGraph.IsVisited(next))
{
if (!frontier.Contains(next))
{
frontier.Enqueue(next);
}
mapGraph.AddPreviousNode(next, currentNode);
}
}
pathfindingInfo.FrontierByOrder.Add(frontier.ToArray());
pathfindingInfo.iterations++;
}
Debug.Log("Path not found!");
return(false);
}
