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