public override NavTilePath FindPath(FindPathInput inInput) { NavNodeHeap openSet = new NavNodeHeap(); HashSet <Vector2Int> closedSet = new HashSet <Vector2Int>(); PathfindingNode startNode = GetNode(inInput.StartCoordinate); openSet.Add(startNode); while (openSet.Count > 0) { PathfindingNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode.Coordinate); if (currentNode.Coordinate == inInput.TargetCoordinate) { // Path found. List <Vector2Int> pathInCoordinates = RetracePath(startNode, currentNode); #if UNITY_EDITOR NavTilePathVisualizer.SetDebugVisualization(inInput.Visualizer, closedSet.ToList(), openSet.GetCoordinateList(), pathInCoordinates); #endif return(ConvertCoordinatesToPath(pathInCoordinates, inInput.AreaMask)); } IdentifySuccessors(currentNode, openSet, closedSet, inInput); } // No path found return(null); }
/// <summary> /// Interface method to find a path using the JPS+ algorithm. /// </summary> /// <param name="inInput">Path input parameters to be used when finding a path.</param> /// <returns>A NavTilePath instance consisting of a list of nodes that represent a path.</returns> public override NavTilePath FindPath(FindPathInput inInput) { openSet = new NavNodeHeap(); closedSet = new HashSet <Vector2Int>(); // Create the first node to start the pathfinding. PathfindingNode startNode = GetNode(inInput.StartCoordinate); Vector2Int targetPosition = inInput.TargetCoordinate; // The startnode is a special case, so check all directions. for (int i = 0; i <= 3; i++) { JumpPointDirection direction = (JumpPointDirection)(1 << i); SearchInDirection(direction, startNode, inInput); } // Main loop. While there are still nodes which can be checked, continue checking them. while (openSet.Count > 0) { // Grab the node with the lowest cost of the heap and add it to the closedset. PathfindingNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode.Coordinate); // Check whether the node is the targetnode, which means the pathfinding can be stopped. if (currentNode.Coordinate == targetPosition) { // Path found List <Vector2Int> pathInCoordinates = RetracePath(startNode, currentNode); #if UNITY_EDITOR NavTilePathVisualizer.SetDebugVisualization(inInput.Visualizer, closedSet.ToList(), openSet.GetCoordinateList(), pathInCoordinates); #endif return(ConvertCoordinatesToPath(pathInCoordinates, inInput.AreaMask)); } // Get the direction of how this node was reached. JumpPointDirection parentDirection = (currentNode.AdditionalTileData as AdditionalJPSPlusData).ParentDirection; // Find the perpendicular directions based on the parent direction. JumpPointDirection firstPerpendicularDirection; JumpPointDirection secondPerpendicularDirection; GetPerpendicularDirections(parentDirection, out firstPerpendicularDirection, out secondPerpendicularDirection); // Search in all three directions for relevant nodes. SearchInDirection(parentDirection, currentNode, inInput); SearchInDirection(firstPerpendicularDirection, currentNode, inInput); SearchInDirection(secondPerpendicularDirection, currentNode, inInput); } Debug.Log("No path found from " + inInput.StartCoordinate + " to " + inInput.TargetCoordinate); return(null); }
/// <summary> /// Calculate a path using the A* algorithm and the specified input. /// </summary> /// <param name="inInput">Input used to calculate path with.</param> public override NavTilePath FindPath(FindPathInput inInput) { NavNodeHeap openSet = new NavNodeHeap(); HashSet <Vector2Int> closedSet = new HashSet <Vector2Int>(); PathfindingNode startNode = GetNode(inInput.StartCoordinate); openSet.Add(startNode); Vector2Int targetPosition = inInput.TargetCoordinate; while (openSet.Count > 0) { PathfindingNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode.Coordinate); if (currentNode.Coordinate == targetPosition) { // Path found. List <Vector2Int> pathInCoordinates = RetracePath(startNode, currentNode); #if UNITY_EDITOR NavTilePathVisualizer.SetDebugVisualization(inInput.Visualizer, closedSet.ToList(), openSet.GetCoordinateList(), pathInCoordinates); #endif return(ConvertCoordinatesToPath(pathInCoordinates, inInput.AreaMask)); } foreach (Vector2Int neighbourCoordinate in GetWalkableNeighbours(currentNode, inInput.AreaMask, inInput.DiagonalAllowed, inInput.CutCorners)) { if (inInput.PositionsToAvoid.Contains(neighbourCoordinate)) { continue; } if (closedSet.Contains(neighbourCoordinate)) { continue; } // Get node from the open set if it exists, otherwise, create one. PathfindingNode neighbourNode = openSet.GetExistingNode(neighbourCoordinate) ?? GetNode(neighbourCoordinate); int movementPenalty = inInput.IgnoreTileCost ? 0 : neighbourNode.TileCost; int newMovementCostToNeighbour = currentNode.GCost + GetAdjacentCost(neighbourCoordinate - currentNode.Coordinate) + movementPenalty; if (neighbourNode.GCost == 0 || newMovementCostToNeighbour < neighbourNode.GCost) { neighbourNode.GCost = newMovementCostToNeighbour; neighbourNode.HCost = GetHeuristicCost(neighbourCoordinate, inInput.TargetCoordinate, inInput.DiagonalAllowed); neighbourNode.ParentNode = currentNode; if (openSet.Contains(neighbourNode)) { openSet.UpdateItem(neighbourNode); } else { openSet.Add(neighbourNode); } } } } // Path not found. return(null); }