public static Stack <TileNode> Astar(TileGraph tg, TileNode start, TileNode end) { frontier = new SimplePriorityQueue <TileNode, float>(); frontier.Enqueue(start, 0f); //node_dict = new Dictionary<TileNode, NodeInfo>(); node_dict = DijkstraInitialDictLoad(start, tg); List <TileNode> Expanded = new List <TileNode>(); TileNode v; TileNode other; float cost_so_far; float edge_weight; float dist_to_node; float manhattanDistance; while (frontier.Count > 0) { v = frontier.Dequeue(); cost_so_far = node_dict[v].dist; Expanded.Add(v); List <Edge> experiment = tg.getAdjacentEdges(v) as List <Edge>; foreach (Edge adj_edge in tg.getAdjacentEdges(v)) { other = adj_edge.getNeighbor(v); edge_weight = adj_edge.weight; dist_to_node = node_dict[other].dist; manhattanDistance = Mathf.Abs(end.transform.position.z - other.transform.position.z) + Mathf.Abs(end.transform.position.x - other.transform.position.x); if (cost_so_far + edge_weight < dist_to_node) { node_dict[other].dist = cost_so_far + edge_weight; node_dict[other].heuristic = node_dict[other].dist + manhattanDistance; node_dict[other].parent = v; } if (!Expanded.Any(node => node.isEqual(other)) && !frontier.Any(node => node.isEqual(other))) { frontier.Enqueue(other, node_dict[other].heuristic); } } } Path = NodeDictToPath(node_dict, start, end); return(Path); }
public static Stack <TileNode> Dijkstra(TileGraph tg, TileNode start, TileNode end) { frontier = new SimplePriorityQueue <TileNode, float>(); frontier.Enqueue(start, 0f); node_dict = DijkstraInitialDictLoad(start, tg); List <TileNode> Expanded = new List <TileNode>(); TileNode v; TileNode other; float edge_weight; float dist_to_node; float cost_so_far; while (frontier.Count > 0) { v = frontier.Dequeue(); cost_so_far = node_dict[v].dist; Expanded.Add(v); List <Edge> experiment = tg.getAdjacentEdges(v) as List <Edge>; foreach (Edge adj_edge in tg.getAdjacentEdges(v)) { other = adj_edge.getNeighbor(v); edge_weight = adj_edge.weight; dist_to_node = node_dict[other].dist; if (cost_so_far + edge_weight < dist_to_node) { node_dict[other].dist = cost_so_far + edge_weight; node_dict[other].parent = v; } if (!Expanded.Any(node => node.isEqual(other)) && !frontier.Any(node => node.isEqual(other))) { frontier.Enqueue(other, node_dict[other].dist); } } } Path = NodeDictToPath(node_dict, start, end); return(Path); }