public static void UpdateAllTimers(float deltaTime)
{
_time += deltaTime;
foreach (var timer in _queue)
{
timer.UpdateTime(deltaTime);
}
while (_queue.TryFirst(out var timer) && _queue.TryGetPriority(timer, out var priority) && priority <= _time)
{
timer.OnExpire(priority);
}
}
public List <Vector2Int> FindPath(Vector2Int start, Vector2Int end, Func <Node, Node, PathCost> costFunction)
{
ResetNodes();
queue.Clear();
closed.Clear();
queue = new SimplePriorityQueue <Node, float>();
closed = new HashSet <Node>();
grid[start].Cost = 0;
queue.Enqueue(grid[start], 0);
while (queue.Count > 0)
{
Node node = queue.Dequeue();
closed.Add(node);
if (node.Position == end)
{
return(ReconstructPath(node));
}
foreach (var offset in neighbors)
{
if (!grid.InBounds(node.Position + offset))
{
continue;
}
var neighbor = grid[node.Position + offset];
if (closed.Contains(neighbor))
{
continue;
}
var pathCost = costFunction(node, neighbor);
if (!pathCost.traversable)
{
continue;
}
float newCost = node.Cost + pathCost.cost;
if (newCost < neighbor.Cost)
{
neighbor.Previous = node;
neighbor.Cost = newCost;
if (queue.TryGetPriority(node, out float existingPriority))
{
queue.UpdatePriority(node, newCost);
}
else
{
queue.Enqueue(neighbor, neighbor.Cost);
}
}
}
}
return(null);
}
public Queue <GOAPAction> GetPlan(List <GOAPState> startState, List <GOAPState> goalState, List <GOAPAction> actionList)
{
Queue <GOAPAction> actionQueue = new Queue <GOAPAction>();
//Create a state comparer to compare the states
StateComparer stateComparer = new StateComparer();
//Opened List contains list of nodes that are still to be assessed
//Closed List contains list of nodes that have been processed
//previousNodes stores the links between nodes
SimplePriorityQueue <List <GOAPState> > openedList = new SimplePriorityQueue <List <GOAPState> >();
Dictionary <List <GOAPState>, bool> closedList = new Dictionary <List <GOAPState>, bool>(stateComparer);
Dictionary <List <GOAPState>, List <GOAPState> > previousNodesState = new Dictionary <List <GOAPState>, List <GOAPState> >(stateComparer);
Dictionary <List <GOAPState>, GOAPAction> previousNodesAction = new Dictionary <List <GOAPState>, GOAPAction>(stateComparer);
Dictionary <List <GOAPState>, float> costs = new Dictionary <List <GOAPState>, float>(stateComparer);
var node = goalState;
node.OrderBy(n => n.name).ToList();
openedList.Enqueue(node, 1);
costs.Add(node, 0);
while (openedList.Count > 0)
{
node = openedList.First;
//Check if we've found the solution
if (GoalNodeReached(startState, node))
{
break;
}
openedList.Dequeue();
//Add to the closedList
closedList.Add(node, true);
//Get list of neighbours
for (var i = 0; i < actionList.Count; i++)
{
var action = actionList[i];
if (action.CanResolve(node))
{
//Create a new state based on the effects and preconditions of the current action
List <GOAPState> tempState = action.UnsetStateEffects(node);
tempState = action.SetStatePrecons(tempState);
tempState.OrderBy(n => n.name).ToList();
if (closedList.ContainsKey(tempState))
{
continue;
}
float F, G, H;
if (!costs.ContainsKey(tempState))
{
G = costs[node] + StateGetDist(goalState, tempState);
costs.Add(tempState, G);
}
else
{
G = costs[tempState];
}
H = action.cost;
//Calculate the current cost of the path to the current action plus it's own cost
F = G + H;
var lastF = -1f;
if (openedList.TryGetPriority(tempState, out lastF))
{
if (F < lastF)
{
openedList.UpdatePriority(tempState, F);
//Keep track of the states and the edges (actions) that lead up to each state
if (!previousNodesState.ContainsKey(tempState))
{
previousNodesState.Add(tempState, node);
}
else
{
previousNodesState[tempState] = node;
}
if (!previousNodesAction.ContainsKey(tempState))
{
previousNodesAction.Add(tempState, action);
}
else
{
previousNodesAction[tempState] = action;
}
}
}
else
{
openedList.Enqueue(tempState, F);
//Keep track of the states and the edges (actions) that lead up to each state
if (!previousNodesState.ContainsKey(tempState))
{
previousNodesState.Add(tempState, node);
}
else
{
previousNodesState[tempState] = node;
}
if (!previousNodesAction.ContainsKey(tempState))
{
previousNodesAction.Add(tempState, action);
}
else
{
previousNodesAction[tempState] = action;
}
}
}
}
}
//If we've found the startState, our regressive search has finished
if (GoalNodeReached(startState, node))
{
//Ensure the actionQueue is clear
actionQueue.Clear();
while (previousNodesAction.ContainsKey(node))
{
//Build the new plan
var action = previousNodesAction[node];
actionQueue.Enqueue(action);
previousNodesAction.Remove(node);
node = previousNodesState[node];
}
}
//Return the plan
return(actionQueue);
}
public List <Vector3Int> FindPath(Vector3Int start, Vector3Int end, Func <Node, Node, PathCost> costFunction)
{
ResetNodes();
queue.Clear();
closed.Clear();
queue = new SimplePriorityQueue <Node, float>();
closed = new HashSet <Node>();
grid[start].Cost = 0;
queue.Enqueue(grid[start], 0);
while (queue.Count > 0)
{
Node node = queue.Dequeue();
closed.Add(node);
if (node.Position == end)
{
return(ReconstructPath(node));
}
foreach (var offset in neighbors)
{
if (!grid.InBounds(node.Position + offset))
{
continue;
}
var neighbor = grid[node.Position + offset];
if (closed.Contains(neighbor))
{
continue;
}
if (node.PreviousSet.Contains(neighbor.Position))
{
continue;
}
var pathCost = costFunction(node, neighbor);
if (!pathCost.traversable)
{
continue;
}
if (pathCost.isStairs)
{
int xDir = Mathf.Clamp(offset.x, -1, 1);
int zDir = Mathf.Clamp(offset.z, -1, 1);
Vector3Int verticalOffset = new Vector3Int(0, offset.y, 0);
Vector3Int horizontalOffset = new Vector3Int(xDir, 0, zDir);
if (node.PreviousSet.Contains(node.Position + horizontalOffset) ||
node.PreviousSet.Contains(node.Position + horizontalOffset * 2) ||
node.PreviousSet.Contains(node.Position + verticalOffset + horizontalOffset) ||
node.PreviousSet.Contains(node.Position + verticalOffset + horizontalOffset * 2))
{
continue;
}
}
float newCost = node.Cost + pathCost.cost;
if (newCost < neighbor.Cost)
{
neighbor.Previous = node;
neighbor.Cost = newCost;
if (queue.TryGetPriority(node, out float existingPriority))
{
queue.UpdatePriority(node, newCost);
}
else
{
queue.Enqueue(neighbor, neighbor.Cost);
}
neighbor.PreviousSet.Clear();
neighbor.PreviousSet.UnionWith(node.PreviousSet);
neighbor.PreviousSet.Add(node.Position);
if (pathCost.isStairs)
{
int xDir = Mathf.Clamp(offset.x, -1, 1);
int zDir = Mathf.Clamp(offset.z, -1, 1);
Vector3Int verticalOffset = new Vector3Int(0, offset.y, 0);
Vector3Int horizontalOffset = new Vector3Int(xDir, 0, zDir);
neighbor.PreviousSet.Add(node.Position + horizontalOffset);
neighbor.PreviousSet.Add(node.Position + horizontalOffset * 2);
neighbor.PreviousSet.Add(node.Position + verticalOffset + horizontalOffset);
neighbor.PreviousSet.Add(node.Position + verticalOffset + horizontalOffset * 2);
}
}
}
}
return(null);
}
