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());
}
