Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //START A* ALGORITHM
- List<Path_Node<Tile>> ClosedSet = new List<Path_Node<Tile>>();
- SimplePriorityQueue<Path_Node<Tile>> OpenSet = new SimplePriorityQueue<Path_Node<Tile>>();
- OpenSet.Enqueue(start, 0);
- Dictionary<Path_Node<Tile>, Path_Node<Tile>> Came_From = new Dictionary<Path_Node<Tile>, Path_Node<Tile>>();
- //g_score
- Dictionary<Path_Node<Tile>, float> g_score = new Dictionary<Path_Node<Tile>, float>();
- foreach (Path_Node<Tile> n in nodes.Values)
- g_score[n] = Mathf.Infinity;
- g_score[start] = 0;
- //f_score
- Dictionary<Path_Node<Tile>, float> f_score = new Dictionary<Path_Node<Tile>, float>();
- foreach (Path_Node<Tile> n in nodes.Values)
- f_score[n] = Mathf.Infinity;
- f_score[start] = CostEstimate(start, goal);
- while(OpenSet.Count > 0)
- {
- Path_Node<Tile> current = OpenSet.Dequeue();
- if(current == goal)
- {
- //Reached goal
- //Convert into a squence of tiles
- reconstruct_path(Came_From, current);
- return;
- }
- ClosedSet.Add(current);
- foreach (Path_Edge<Tile> edge_neighbor in current.edges)
- {
- Path_Node<Tile> neighbor = edge_neighbor.node;
- if (ClosedSet.Contains(neighbor) == true)
- continue;
- float movement_cost_to_neigbor = neighbor.data.movementCost * dist_between(current, neighbor);
- float tentative_g_score = g_score[current] + movement_cost_to_neigbor;
- if (OpenSet.Contains(neighbor) && tentative_g_score >= g_score[neighbor])
- continue;
- Came_From[neighbor] = current;
- g_score[neighbor] = tentative_g_score;
- f_score[neighbor] = g_score[neighbor] + CostEstimate(neighbor, goal);
- if(OpenSet.Contains(neighbor) == false)
- {
- OpenSet.Enqueue(neighbor, f_score[neighbor]);
- }
- }
- }
- //If reached here there is no path from start to goal
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement