public Tile FindClosestTile(Tile startTile, Func <Tile, bool> isValid) { // Check to see if we have a valid Tile graph if (World.Instance.TileGraph == null) { World.Instance.TileGraph = new Path_TileGraph(World.Instance); } // A dictionary of all valid, walkable nodes. var nodes = World.Instance.TileGraph.Nodes; var start = nodes[startTile]; var frontier = new SimplePriorityQueue <Path_Node <Tile> >(); frontier.Enqueue(start, 0f); var costSoFar = new Dictionary <Path_Node <Tile>, float>(); costSoFar.Add(start, 0f); Path_Node <Tile> current = null; var found = false; while (frontier.Count > 0) { current = frontier.Dequeue(); if (isValid(current.data)) { found = true; break; } foreach (var edgeNeighbour in current.edges) { var next = edgeNeighbour.node; var newCost = costSoFar[current] + next.data.MovementCost; if (costSoFar.ContainsKey(next) == false || newCost < costSoFar[next]) { costSoFar[next] = newCost; frontier.Enqueue(next, newCost); } } } if (found == false || current == null) { return(null); } return(current.data); }
private static float HeuristicCostEstimate(Path_Node <Tile> a, Path_Node <Tile> b) { if (b == null) { // We have no fixed destination, so probably just looking for an item. return(0f); } return(Mathf.Sqrt( Mathf.Pow(a.data.X - b.data.X, 2) + Mathf.Pow(a.data.Y - b.data.Y, 2) )); }
public Path_TileGraph(World world) { // Loop through all tiles of the world // For each Tile, create a node // Do we create nodes for non-floor tiles? NO! // Do we create nodes for tiles that are completely unwalkable (i.e. walls)? NO! Nodes = new Dictionary <Tile, Path_Node <Tile> >(); for (var x = 0; x < world.Width; x++) { for (var y = 0; y < world.Height; y++) { var t = world.GetTileAt(x, y); var n = new Path_Node <Tile>(); n.data = t; Nodes.Add(t, n); } } // Now loop through all nodes again, creating edges for neighbours foreach (var t in Nodes.Keys) { var n = Nodes[t]; var edges = new List <Path_Edge <Tile> >(); // Get a list of neighbours for the Tile var neighbours = t.GetNeighbours(true); // NOTE: Some of the array spots could be null. // If neighbour is walkable, create an edge to the relevant node. foreach (var neighbour in neighbours) { if (neighbour != null) { var e = new Path_Edge <Tile>(); e.cost = neighbour.MovementCost; if (IsClippingCorner(t, neighbour)) // Discourage routing through diagonal gaps by resetting the movement cost to 0. { e.cost = 0f; } e.node = Nodes[neighbour]; // Add the edge to our temporary (and growable!) list edges.Add(e); } } n.edges = edges.ToArray(); } }
private static float DistanceBetween(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 void Calculate() { if (World == null) { throw new InvalidOperationException("Invalid path calculation - no World set"); } if (Start == null) { throw new InvalidOperationException("Invalid path calculation - no start set"); } // If tileEnd is null, simply search for the nearest objectType, ignoring the A* Heuristic element. // Basically, use Dijkstra's algorithm. _reachable = true; // Check to see if we have a valid Tile graph if (this.World.TileGraph == null) { this.World.TileGraph = new Path_TileGraph(this.World); } // A dictionary of all valid, walkable nodes. var nodes = this.World.TileGraph.Nodes; if (_debugVis && End != null) { UnityEngine.Debug.DrawLine(new Vector3(Start.X, Start.Y, -2), new Vector3(End.X, End.Y, -2), Color.red, Time.deltaTime * 5, false); this.World.TileGraph.DebugVis(); } // Make sure our start/end tiles are in the list of nodes! if (nodes.ContainsKey(Start) == false) { UnityEngine.Debug.LogError("Path_AStar: The starting Tile isn't in the list of nodes!"); _reachable = false; return; } var start = nodes[Start]; Path_Node <Tile> goal = null; if (End != null) { if (nodes.ContainsKey(End) == false) { UnityEngine.Debug.LogError("Path_AStar: The ending Tile isn't in the list of nodes!"); _reachable = false; return; } goal = nodes[End]; } // Mostly following this pseusocode: // https://en.wikipedia.org/wiki/A*_search_algorithm var closedSet = new List <Path_Node <Tile> >(); var openSet = new SimplePriorityQueue <Path_Node <Tile> >(); openSet.Enqueue(start, 0); var cameFrom = new Dictionary <Path_Node <Tile>, Path_Node <Tile> >(); // Set up the array of G values var gScore = new Dictionary <Path_Node <Tile>, float>(); foreach (var n in nodes.Values) { gScore[n] = Mathf.Infinity; } gScore[start] = 0; // Set up the array of F values var fScore = new Dictionary <Path_Node <Tile>, float>(); foreach (var n in nodes.Values) { fScore[n] = Mathf.Infinity; } fScore[start] = HeuristicCostEstimate(start, goal); while (openSet.Count > 0) { var current = openSet.Dequeue(); if (goal != null) { if (current == goal) { // We have reached our goal! // Let's convert this into an actual sequene of // tiles to walk on, then end this constructor function! ReconstructPath(cameFrom, current); _reachable = true; return; } } else { // Looking for inventory if (current.data.Inventory != null && current.data.Inventory.ObjectType == ObjectType) { // Type is correct if (CanTakeFromStockpile || current.data.Furniture == null || current.data.Furniture.IsStockpile() == false) { ReconstructPath(cameFrom, current); _reachable = true; return; } } } closedSet.Add(current); foreach (var edgeNeighbor in current.edges) { var neighbor = edgeNeighbor.node; if (closedSet.Contains(neighbor)) { continue; // ignore this already completed neighbor } // If the neighbour is impassable, give this node an infinite cost var movementCostToNeighbor = Mathf.Infinity; if (!Mathf.Approximately(neighbor.data.MovementCost, 0f)) { movementCostToNeighbor = neighbor.data.MovementCost * DistanceBetween(current, neighbor); } var tentativeGScore = gScore[current] + movementCostToNeighbor; if (openSet.Contains(neighbor) && tentativeGScore >= gScore[neighbor]) { continue; } cameFrom[neighbor] = current; gScore[neighbor] = tentativeGScore; fScore[neighbor] = gScore[neighbor] + HeuristicCostEstimate(neighbor, goal); if (openSet.Contains(neighbor) == false) { openSet.Enqueue(neighbor, fScore[neighbor]); } else { openSet.UpdatePriority(neighbor, fScore[neighbor]); } } } // 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). _reachable = false; // We don't have a failure state, maybe? It's just that the // path list will be null. }
private void ReconstructPath(IDictionary <Path_Node <Tile>, Path_Node <Tile> > cameFrom, 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! var totalPath = new LinkedList <Tile>(); totalPath.AddLast(current.data); // This "final" step is the path is the goal! while (cameFrom.ContainsKey(current)) { // Came_From is a map, where the // key => value relation is real saying // some_node => we_got_there_from_this_node if (_debugVis) { UnityEngine.Debug.DrawLine(new Vector3(current.data.X, current.data.Y, -2), new Vector3(cameFrom[current].data.X, cameFrom[current].data.Y, -2), Color.green, 3f); } current = cameFrom[current]; totalPath.AddLast(current.data); } // We don't need to have the start tile in the path, because that's where we already are. totalPath.RemoveLast(); // 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 LinkedList <Tile>(totalPath.Reverse()); }