void reconstruct_path( Dictionary <Path_Node <Tile>, Path_Node <Tile> > Came_From, Path_Node <Tile> current ) { // So at this point, current IS the goal. // So what we want to do is walk backwards through the Came_From // map, until we reach the "end" of that map...which will be // our starting node! Queue <Tile> total_path = new Queue <Tile>(); total_path.Enqueue(current.data); // This "final" step is the path is the goal! while (Came_From.ContainsKey(current)) { // Came_From is a map, where the // key => value relation is real saying // some_node => we_got_there_from_this_node current = Came_From[current]; total_path.Enqueue(current.data); } // At this point, total_path is a queue that is running // backwards from the END tile to the START tile, so let's reverse it. path = new Queue <Tile>(total_path.Reverse()); }
float heuristic_cost_estimate(Path_Node <Tile> a, Path_Node <Tile> b) { if (b == null) { // We have no fixed destination (i.e. probably looking for an inventory item) // so just return 0 for the cost estimate (i.e. all directions as just as good) return(0f); } return(Mathf.Sqrt( Mathf.Pow(a.data.X - b.data.X, 2) + Mathf.Pow(a.data.Y - b.data.Y, 2) )); }
float dist_between(Path_Node <Tile> a, Path_Node <Tile> b) { // We can make assumptions because we know we're working // on a grid at this point. // Hori/Vert neighbours have a distance of 1 if (Mathf.Abs(a.data.X - b.data.X) + Mathf.Abs(a.data.Y - b.data.Y) == 1) { return(1f); } // Diag neighbours have a distance of 1.41421356237 if (Mathf.Abs(a.data.X - b.data.X) == 1 && Mathf.Abs(a.data.Y - b.data.Y) == 1) { return(1.41421356237f); } // Otherwise, do the actual math. return(Mathf.Sqrt( Mathf.Pow(a.data.X - b.data.X, 2) + Mathf.Pow(a.data.Y - b.data.Y, 2) )); }
public Path_AStar(World world, Tile tileStart, Tile tileEnd, string objectType = null, int desiredAmount = 0, bool canTakeFromStockpile = false) { // if tileEnd is null, then we are simply scanning for the nearest objectType. // We can do this by ignoring the heuristic component of AStar, which basically // just turns this into an over-engineered Dijkstra's algo // Check to see if we have a valid tile graph if (world.tileGraph == null) { world.tileGraph = new Path_TileGraph(world); } // A dictionary of all valid, walkable nodes. Dictionary <Tile, Path_Node <Tile> > nodes = world.tileGraph.nodes; // Make sure our start/end tiles are in the list of nodes! if (nodes.ContainsKey(tileStart) == false) { Utils.LogError("Path_AStar: The starting tile isn't in the list of nodes!"); return; } Path_Node <Tile> start = nodes[tileStart]; // if tileEnd is null, then we are simply looking for an inventory object // so just set goal to null. Path_Node <Tile> goal = null; if (tileEnd != null) { if (nodes.ContainsKey(tileEnd) == false) { Debug.LogError("Path_AStar: The ending tile isn't in the list of nodes!"); return; } goal = nodes[tileEnd]; } // Mostly following this pseusocode: // https://en.wikipedia.org/wiki/A*_search_algorithm List <Path_Node <Tile> > ClosedSet = new List <Path_Node <Tile> >(); /* List<Path_Node<Tile>> OpenSet = new List<Path_Node<Tile>>(); * OpenSet.Add( start ); */ 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> >(); 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; 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] = heuristic_cost_estimate(start, goal); while (OpenSet.Count > 0) { Path_Node <Tile> current = OpenSet.Dequeue(); // If we have a POSITIONAL goal, check to see if we are there. if (goal != null) { if (current == goal) { reconstruct_path(Came_From, current); return; } } else { // We don't have a POSITIONAL goal, we're just trying to find // some king of inventory. Have we reached it? //TODO: verificar isso /*if (current.data.inventory != null && current.data.inventory.objectType == objectType) * { * // Type is correct * if (canTakeFromStockpile || current.data.furniture == null || current.data.furniture.IsStockpile() == false) * { * // Stockpile status is fine * 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; // ignore this already completed neighbor } float movement_cost_to_neighbor = neighbor.data.movementCost * dist_between(current, neighbor); float tentative_g_score = g_score[current] + movement_cost_to_neighbor; 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] + heuristic_cost_estimate(neighbor, goal); if (OpenSet.Contains(neighbor) == false) { OpenSet.Enqueue(neighbor, f_score[neighbor]); } else { OpenSet.UpdatePriority(neighbor, f_score[neighbor]); } } // foreach neighbour } // while // If we reached here, it means that we've burned through the entire // OpenSet without ever reaching a point where current == goal. // This happens when there is no path from start to goal // (so there's a wall or missing floor or something). // We don't have a failure state, maybe? It's just that the // path list will be null. }