public void ExecutePathfindingStep()
{
Node <Tile> currentNode = openSet.Dequeue();
if (currentNode == EndNode)
{
ConstructPath();
return;
}
closedSet.Add(currentNode);
foreach (Edge <Tile> neighbouringEdge in currentNode.Edges)
{
Node <Tile> neighbor = neighbouringEdge.Node;
if (closedSet.Contains(neighbor))
{
continue;
}
float movementCostToNeighbor = neighbor.Data.MovementCost * DistanceBetween(currentNode, neighbor);
float tentativeGScore = currentNode.GCost + movementCostToNeighbor;
if (openSet.Contains(neighbor) && tentativeGScore >= neighbor.GCost)
{
continue;
}
cameFrom[neighbor] = currentNode;
neighbor.GCost = tentativeGScore;
neighbor.HCost = CalculateHeuristicCost(neighbor, EndNode);
openSet.EnqueueOrUpdate(neighbor, neighbor.FCost);
}
}
public void DoWork()
{
path = new Queue <T>();
HashSet <T> closedSet = new HashSet <T>();
PathfindingPriorityQueue <T> openSet = new PathfindingPriorityQueue <T>();
openSet.Enqueue(startTile, 0);
Dictionary <T, T> came_from = new Dictionary <T, T>();
Dictionary <T, float> g_score = new Dictionary <T, float>
{
{ startTile, 0 }
};
Dictionary <T, float> f_score = new Dictionary <T, float>
{
{ startTile, costEstimateFunc(startTile, endTile) }
};
while (openSet.Count > 0)
{
T current = openSet.Dequeue();
if (ReferenceEquals(current, endTile))
{
Reconstruct_path(came_from, current);
return;
}
closedSet.Add(current);
foreach (T edge_neighbor in current.GetNeighbors())
{
T neighbor = edge_neighbor;
if (closedSet.Contains(neighbor))
{
continue;
}
float total_pathfinding_cost_to_neighbor = neighbor.AggregateCostToEnter(g_score[current], current, unit);
float tentative_g_score = total_pathfinding_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] + costEstimateFunc(neighbor, endTile);
openSet.EnqueueOrUpdate(neighbor, f_score[neighbor]);
}
}
}
public Path_AStar(World world, Tile tileStart, Pathfinder.GoalEvaluator isGoal, Pathfinder.PathfindingHeuristic costEstimate)
{
float startTime = Time.realtimeSinceStartup;
// Set path to empty Queue so that there always is something to check count on
path = new Queue <Tile>();
// 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);
}
// Check to see if we have a valid tile graph
if (world.roomGraph == null)
{
world.roomGraph = new Path_RoomGraph(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)
{
UnityDebugger.Debugger.LogError("Path_AStar", "The starting tile isn't in the list of nodes!");
return;
}
Path_Node <Tile> start = nodes[tileStart];
/*
* Mostly following this pseusocode:
* https://en.wikipedia.org/wiki/A*_search_algorithm
*/
HashSet <Path_Node <Tile> > closedSet = new HashSet <Path_Node <Tile> >();
/*
* List<Path_Node<Tile>> openSet = new List<Path_Node<Tile>>();
* openSet.Add( start );
*/
PathfindingPriorityQueue <Path_Node <Tile> > openSet = new PathfindingPriorityQueue <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>();
g_score[start] = 0;
Dictionary <Path_Node <Tile>, float> f_score = new Dictionary <Path_Node <Tile>, float>();
f_score[start] = costEstimate(start.data);
while (openSet.Count > 0)
{
Path_Node <Tile> current = openSet.Dequeue();
// Check to see if we are there.
if (isGoal(current.data))
{
Duration = Time.realtimeSinceStartup - startTime;
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))
{
continue; // ignore this already completed neighbor
}
float pathfinding_cost_to_neighbor = neighbor.data.PathfindingCost * Dist_between(current, neighbor);
float tentative_g_score = g_score[current] + pathfinding_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] + costEstimate(neighbor.data);
openSet.EnqueueOrUpdate(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.
Duration = Time.realtimeSinceStartup - startTime;
}
public Path_AStar(World world, Tile tileStart, Tile tileEnd, string objectType = null, int desiredAmount = 0, bool canTakeFromStockpile = false, bool lookingForFurn = 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)
{
Debug.ULogErrorChannel("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.ULogErrorChannel("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
*/
HashSet <Path_Node <Tile> > closedSet = new HashSet <Path_Node <Tile> >();
/*
* List<Path_Node<Tile>> openSet = new List<Path_Node<Tile>>();
* openSet.Add( start );
*/
PathfindingPriorityQueue <Path_Node <Tile> > openSet = new PathfindingPriorityQueue <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>();
g_score[start] = 0;
Dictionary <Path_Node <Tile>, float> f_score = new Dictionary <Path_Node <Tile>, float>();
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 kind of inventory or furniture. Have we reached it?
if (current.data.Inventory != null && current.data.Inventory.objectType == objectType && lookingForFurn == false && current.data.Inventory.locked == false)
{
// Type is correct and we are allowed to pick it up
if (canTakeFromStockpile || current.data.Furniture == null || current.data.Furniture.IsStockpile() == false)
{
// Stockpile status is fine
Reconstruct_path(came_From, current);
return;
}
}
if (current.data.Furniture != null && current.data.Furniture.ObjectType == objectType && lookingForFurn)
{
// Type is correct
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))
{
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);
openSet.EnqueueOrUpdate(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.
}
public Path_AStar(Area area, Tile startTile, Tile endTile)
{
//if (world.path_graph == null)
// world.path_graph = new Path_TileGraph(world);
if (area.path_graph == null)
area.path_graph = new Path_TileGraph(area);
// A dictionary of Walkable Nodes (just a reference to path_graph.nodes)
Dictionary<Tile, Path_Node<Tile>> nodes = area.path_graph.nodes;
if (nodes.ContainsKey(startTile) == false)
{
Debug.LogError("PathAStar could not find starting tile in path graph!");
return;
}
// Set the starting tile's node
Path_Node<Tile> start = nodes[startTile];
// If end tile is null, keep this goal null
Path_Node<Tile> goal = null;
if (endTile != null)
{
if (nodes.ContainsKey(endTile) == false)
{
// Debug.LogError("PathAStar could not find end tile in path graph!");
return;
}
goal = nodes[endTile];
}
// Nodes already verified
List<Path_Node<Tile>> ClosedSet = new List<Path_Node<Tile>>();
//List<Path_Node<Tile>> OpenSet = new List<Path_Node<Tile>>();
//OpenSet.Add( start );
// Nodes being verified
PathfindingPriorityQueue<Path_Node<Tile>> OpenSet = new PathfindingPriorityQueue<Path_Node<Tile>>();
OpenSet.Enqueue(start, 0);
// Nodes we just came from
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>();
// Set default values to Infinity (so all nodes have a score in the Map)
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>();
// Set default values to Infinity (so all nodes have a score in the Map)
foreach (Path_Node<Tile> n in nodes.Values)
{
f_score[n] = Mathf.Infinity;
}
f_score[ start ] = HeuristicCostEstimate(start, goal);
while(OpenSet.Count > 0)
{
Path_Node<Tile> current = OpenSet.Dequeue();
if (goal != null)
{
if (current == goal)
{
ReconstructPath(Came_From, current);
return;
}
}
else
{
// TODO: goal is null, meaning the goal is to walk on a tile that contains some THING on it
}
ClosedSet.Add(current);
foreach(Path_Edge<Tile> edge_neighbor in current.edges)
{
Path_Node<Tile> neighbor = edge_neighbor.node;
if (ClosedSet.Contains(neighbor))
continue; // Ignore this already verified node
float tentative_g_score = g_score[current] + DistBetween(current, neighbor);
// Check this Node (neighbor) has not been check yet
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] + HeuristicCostEstimate(neighbor, goal);
if (OpenSet.Contains(neighbor) == false)
OpenSet.Enqueue(neighbor, f_score[neighbor]);
else
OpenSet.UpdatePriority(neighbor, f_score[neighbor]);
}
}
// No Path from Start to Goal was found if we reach here
return;
}