public static void FindPath(SquareGrid graph, Node startNode, Node targetNode) { Dictionary <Node, Node> nodeParent = new Dictionary <Node, Node>(); Heap <Node> frontier = new Heap <Node>(graph.gridSize.x * graph.gridSize.y); Dictionary <Node, int> costFromStartTo = new Dictionary <Node, int>(); for (int x = 0; x < graph.gridSize.x; x++) { for (int y = 0; y < graph.gridSize.y; y++) { costFromStartTo.Add(graph.grid[x, y], int.MaxValue); } } costFromStartTo[startNode] = 0; frontier.Insert(startNode, costFromStartTo[startNode]); while (frontier.itemCount > 0) { Node currentNode = frontier.Extract(); if (currentNode == targetNode) { graph.RetracePath(startNode, targetNode, nodeParent); return; } foreach (Node neighbor in graph.GetNeighbors(currentNode)) { int newCost = costFromStartTo[currentNode] + graph.edgeWeight(currentNode, neighbor); if (newCost < costFromStartTo[neighbor]) { costFromStartTo[neighbor] = newCost; nodeParent[neighbor] = currentNode; int heuristics = EuclideanDistance(neighbor, targetNode); frontier.Insert(neighbor, costFromStartTo[neighbor] + heuristics); } } } graph.path = null; }