private static void EnqueueIfPassable(ICollection <int> seen, int newCell, int cost, PriorityDictionary <int, int> queue) { if (Grid.IsValidCell(newCell) && Grid.Element[newCell]?.IsSolid == false && !seen.Contains(newCell)) { seen.Add(newCell); queue.Enqueue(cost, newCell); } }
/// <summary> /// Adds nodes to queue by priority, processing the specified node neighbors /// </summary> /// <param name="openPathsQueue">Queue</param> /// <param name="nodesData">Nodes data dictionary</param> /// <param name="currentNode">Current node</param> /// <param name="end">End node</param> /// <param name="heuristicMethod">Heuristic metod</param> /// <param name="heuristicEstimateValue">Heuristic estimate value</param> private static void ProcessNeighbors(PriorityDictionary <GridNode, float> openPathsQueue, Dictionary <GridNode, AStarQueryData> nodesData, GridNode currentNode, GridNode end, HeuristicMethods heuristicMethod, int heuristicEstimateValue) { //Search every possible direction from the current node for (int i = 1; i < currentNode.Connections.Length; i++) { var nextNode = currentNode[i]; if (nextNode == null) { continue; } if (!nodesData.ContainsKey(nextNode)) { nodesData.Add(nextNode, new AStarQueryData()); } if (nextNode.State == GridNodeStates.Closed) { //Impassable node continue; } var nextNodeData = nodesData[nextNode]; if (nextNodeData.State == GridNodeStates.Closed) { //Closed node continue; } float newGone = currentNode.TotalCost + ((int)nextNodeData.State); if (nextNodeData.State == GridNodeStates.Clear && nextNode.TotalCost < newGone) { continue; } nextNodeData.NextNode = currentNode; nextNodeData.Cost = newGone; nextNodeData.State = GridNodeStates.Clear; //Calculate priority from next to end float heuristicValue = CalcHeuristic( nextNode.Center, end.Center, heuristicMethod); openPathsQueue.Enqueue(nextNode, newGone + (heuristicEstimateValue * heuristicValue)); } }
protected override void VisualizeCells(ICollection <VisCellData> newCells) { // Rotation is only used to rotate the offset, radius is the same in all directions int startCell = RotateOffsetCell(Grid.PosToCell(gameObject), offset); if (Grid.IsValidCell(startCell) && Grid.Element[startCell]?.IsSolid == false) { var queue = new PriorityDictionary <int, int>(radius * radius); // Initial cell is seen var seen = HashSetPool <int, ElementConsumerVisualizer> .Allocate(); try { queue.Enqueue(0, startCell); seen.Add(startCell); // Dijkstra's algorithm do { queue.Dequeue(out int cost, out int newCell); if (cost < radius - 1) { // Cardinal directions EnqueueIfPassable(seen, Grid.CellLeft(newCell), cost + 1, queue); EnqueueIfPassable(seen, Grid.CellRight(newCell), cost + 1, queue); EnqueueIfPassable(seen, Grid.CellAbove(newCell), cost + 1, queue); EnqueueIfPassable(seen, Grid.CellBelow(newCell), cost + 1, queue); } } while (queue.Count > 0); // Add all cells as normal color foreach (var cell in seen) { newCells.Add(new VisCellData(cell, color)); } } finally { seen.Recycle(); } } }
/// <summary> /// Gets the path from start to end /// </summary> /// <param name="start">Start node</param> /// <param name="end">End node</param> /// <param name="heuristicMethod">Heuristic metod</param> /// <param name="heuristicEstimateValue">Heuristic estimate value</param> /// <returns>Returns the path from start to end</returns> private static Vector3[] CalcReturnPath(GridNode start, GridNode end, HeuristicMethods heuristicMethod, int heuristicEstimateValue) { //New queue PriorityDictionary <GridNode, float> openPathsQueue = new PriorityDictionary <GridNode, float>(); //Data dictionary Dictionary <GridNode, AStarQueryData> nodesData = new Dictionary <GridNode, AStarQueryData>(); //Add first node openPathsQueue.Enqueue(start, 1); nodesData.Add(start, new AStarQueryData()); bool nodeFound = false; while (openPathsQueue.Count > 0) { //Dequeue the node with lower priority var item = openPathsQueue.Dequeue(); var currentNode = item.Value; var currentNodeData = nodesData[currentNode]; //If the node is not closed to continue the process if (currentNodeData.State != GridNodeStates.Closed) { //Set the node status Closed currentNodeData.State = GridNodeStates.Closed; //If the current node is the destination node has found the way if (currentNode == end) { currentNodeData.State = GridNodeStates.Closed; nodeFound = true; break; } else { //Process neigbors ProcessNeighbors( openPathsQueue, nodesData, currentNode, end, heuristicMethod, heuristicEstimateValue); } } } if (nodeFound) { //We found a valid path List <Vector3> solvedList = new List <Vector3>(); var node = end; while (node != null) { solvedList.Insert(0, node.Center); node = nodesData[node].NextNode; } return(solvedList.ToArray()); } else { //If no result... return(new Vector3[] { }); } }