Advertisement
Guest User

Untitled

a guest
Jul 22nd, 2018
73
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C# 2.21 KB | None | 0 0
  1. //START A* ALGORITHM
  2.         List<Path_Node<Tile>> ClosedSet = new List<Path_Node<Tile>>();
  3.  
  4.         SimplePriorityQueue<Path_Node<Tile>> OpenSet = new SimplePriorityQueue<Path_Node<Tile>>();
  5.         OpenSet.Enqueue(start, 0);
  6.  
  7.         Dictionary<Path_Node<Tile>, Path_Node<Tile>> Came_From = new Dictionary<Path_Node<Tile>, Path_Node<Tile>>();
  8.  
  9.         //g_score
  10.         Dictionary<Path_Node<Tile>, float> g_score = new Dictionary<Path_Node<Tile>, float>();
  11.  
  12.         foreach (Path_Node<Tile> n in nodes.Values)
  13.             g_score[n] = Mathf.Infinity;
  14.  
  15.         g_score[start] = 0;
  16.  
  17.         //f_score
  18.         Dictionary<Path_Node<Tile>, float> f_score = new Dictionary<Path_Node<Tile>, float>();
  19.  
  20.         foreach (Path_Node<Tile> n in nodes.Values)
  21.             f_score[n] = Mathf.Infinity;
  22.  
  23.         f_score[start] = CostEstimate(start, goal);
  24.  
  25.         while(OpenSet.Count > 0)
  26.         {
  27.             Path_Node<Tile> current = OpenSet.Dequeue();
  28.  
  29.             if(current == goal)
  30.             {
  31.                 //Reached goal
  32.                 //Convert into a squence of tiles
  33.                 reconstruct_path(Came_From, current);
  34.                 return;
  35.             }
  36.  
  37.             ClosedSet.Add(current);
  38.  
  39.             foreach (Path_Edge<Tile> edge_neighbor in current.edges)
  40.             {
  41.                 Path_Node<Tile> neighbor = edge_neighbor.node;
  42.  
  43.                 if (ClosedSet.Contains(neighbor) == true)
  44.                     continue;
  45.  
  46.                 float movement_cost_to_neigbor = neighbor.data.movementCost * dist_between(current, neighbor);
  47.  
  48.                 float tentative_g_score = g_score[current] + movement_cost_to_neigbor;
  49.  
  50.                 if (OpenSet.Contains(neighbor) && tentative_g_score >= g_score[neighbor])
  51.                     continue;
  52.  
  53.                 Came_From[neighbor] = current;
  54.                 g_score[neighbor] = tentative_g_score;
  55.                 f_score[neighbor] = g_score[neighbor] + CostEstimate(neighbor, goal);
  56.  
  57.                 if(OpenSet.Contains(neighbor) == false)
  58.                 {
  59.                     OpenSet.Enqueue(neighbor, f_score[neighbor]);
  60.                 }
  61.             }
  62.  
  63.         }
  64.         //If reached here there is no path from start to goal
  65.  
  66.     }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement