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