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> /// Returns a path through the pipeline. /// </summary> /// <param name="inInput">Input parameters for the path.</param> public async Task <NavTilePath> GetPath(FindPathInput inInput) { if (MultiThreadingEnabled) { if (_pipelineMultiThreaded == null) { CreatePipeline(); } return(await _pipelineMultiThreaded.Execute(inInput)); } else { return(DoLinearPipeline(inInput)); } }
/// <summary> /// Executes the pipeline on the Unity main thread. /// </summary> /// <param name="inInput">Input parameters for the path.</param> private NavTilePath DoLinearPipeline(FindPathInput inInput) { if (_algorithm == null) { Type algorithm = Type.GetType(AlgorithmType); _algorithm = Activator.CreateInstance(algorithm) as IPathfindingAlgorithm; } NavTilePath path = _algorithm.FindPath(inInput); foreach (var smoothingAlgorithm in EnabledSmoothingAlgorithms) { var instance = Activator.CreateInstance(Type.GetType(smoothingAlgorithm)) as INavTilePathModifier; path = instance.ModifyPath(path); } return(path); }
public abstract NavTilePath FindPath(FindPathInput inInput);
/// <summary> /// Search nodes in a particular direction. /// </summary> private void SearchInDirection(JumpPointDirection inDirection, PathfindingNode inNode, FindPathInput inInput) { Vector2Int coordinate = inNode.Coordinate; Vector2Int goalPos = inInput.TargetCoordinate; // Get the jump distance and the absolute value of the jump distance. int jumpDistance = (inNode.AdditionalTileData as AdditionalJPSPlusData).GetJumpDistance(inDirection); int absJumpDistance = Mathf.Abs(jumpDistance); // Check whether the target node is in line with the current node. int diff; if (InLineWithTargetPosition(coordinate, goalPos, absJumpDistance, inDirection, out diff)) { // Calculate cost of next node and add to open list. int cost = inNode.GCost + GetAdditionalCost(diff); Vector2Int nextNodeCoordinate = Vector2Int.zero; switch (inDirection) { case JumpPointDirection.North: case JumpPointDirection.South: nextNodeCoordinate = new Vector2Int(coordinate.x, goalPos.y); break; case JumpPointDirection.East: case JumpPointDirection.West: nextNodeCoordinate = new Vector2Int(goalPos.x, coordinate.y); break; } AddToOpenList(nextNodeCoordinate, inNode, JumpPointDirection.North, cost, inInput); return; } // Otherwise, check if the jump distance is positive to jump to a jump point. if (jumpDistance > 0) { int cost = inNode.GCost + GetAdditionalCost(diff); AddToOpenList(GetNextPosition(coordinate, absJumpDistance, inDirection), inNode, inDirection, cost, inInput); } }
/// <summary> /// Adds the given coordinate to the open list, or updates a node already on there with new information if necessary. /// </summary> /// <param name="inNewCoordinate">The coordinate being put on the open list.</param> /// <param name="inCurrentNode">The node the new coordinate led from.</param> /// <param name="inDirection">Direction being looked at.</param> /// <param name="inCost">Cost of the new node.</param> /// <param name="inInput">Input parameters for finding a path.</param> private void AddToOpenList(Vector2Int inNewCoordinate, PathfindingNode inCurrentNode, JumpPointDirection inDirection, int inCost, FindPathInput inInput) { PathfindingNode node; if ((node = openSet.GetExistingNode(inNewCoordinate)) != null) { node.ParentNode = inCurrentNode; (node.AdditionalTileData as AdditionalJPSPlusData).ParentDirection = inDirection; node.GCost = inCost; openSet.UpdateItem(node); } else if (!closedSet.Contains(inNewCoordinate)) { int heuristicCost = GetManhattanDistance(inNewCoordinate, inInput.TargetCoordinate); node = GetNode(inNewCoordinate); node.ParentNode = inCurrentNode; (node.AdditionalTileData as AdditionalJPSPlusData).ParentDirection = inDirection; node.GCost = inCost; node.HCost = heuristicCost; openSet.Add(node); } }
private Vector2Int?Jump(Vector2Int inNeighbourCoordinate, Vector2Int inParentCoordinate, FindPathInput inInput) { if (!IsTileWalkable(inNeighbourCoordinate, inInput.AreaMask)) { return(null); } if (inNeighbourCoordinate == inInput.TargetCoordinate) { return(inNeighbourCoordinate); } if (NavTileManager.Instance.SurfaceManager.GridInfo.CellLayout == GridLayout.CellLayout.Hexagon) { // Identify forced neighbours. Direction movingDirection = HexagonGridHelper.GetDirection(inParentCoordinate, inNeighbourCoordinate); Vector2Int aboveFarCoord = HexagonGridHelper.GetNeighbourInDirection(inNeighbourCoordinate, HexagonGridHelper.GetRelativeDirection(movingDirection, 2)); Vector2Int aboveCloseCoord = HexagonGridHelper.GetNeighbourInDirection(inNeighbourCoordinate, HexagonGridHelper.GetRelativeDirection(movingDirection, 1)); if (!IsTileWalkable(aboveFarCoord, inInput.AreaMask) && IsTileWalkable(aboveCloseCoord, inInput.AreaMask)) { return(inNeighbourCoordinate); } Vector2Int belowFarCoord = HexagonGridHelper.GetNeighbourInDirection(inNeighbourCoordinate, HexagonGridHelper.GetRelativeDirection(movingDirection, -2)); Vector2Int belowCloseCoord = HexagonGridHelper.GetNeighbourInDirection(inNeighbourCoordinate, HexagonGridHelper.GetRelativeDirection(movingDirection, -1)); if (!IsTileWalkable(belowFarCoord, inInput.AreaMask) && IsTileWalkable(belowCloseCoord, inInput.AreaMask)) { return(inNeighbourCoordinate); } // No forced neighbours, jump further. if ((int)movingDirection % 2 == 0) { // Even directions have to check adjacent directions. if (Jump(aboveCloseCoord, inNeighbourCoordinate, inInput) != null || Jump(belowCloseCoord, inNeighbourCoordinate, inInput) != null) { return(inNeighbourCoordinate); } } return(Jump(HexagonGridHelper.GetNeighbourInDirection(inNeighbourCoordinate, movingDirection), inNeighbourCoordinate, inInput)); } Vector2Int horizontalDelta = new Vector2Int(inNeighbourCoordinate.x - inParentCoordinate.x, 0); Vector2Int verticalDelta = new Vector2Int(0, inNeighbourCoordinate.y - inParentCoordinate.y); if (inInput.DiagonalAllowed && horizontalDelta.x != 0 && verticalDelta.y != 0) { // Diagonal movement. if (inInput.CutCorners) { if (!IsTileWalkable(inNeighbourCoordinate - horizontalDelta, inInput.AreaMask) && IsTileWalkable(inNeighbourCoordinate - horizontalDelta + verticalDelta, inInput.AreaMask) || !IsTileWalkable(inNeighbourCoordinate - verticalDelta, inInput.AreaMask) && IsTileWalkable(inNeighbourCoordinate + horizontalDelta - verticalDelta, inInput.AreaMask)) { return(inNeighbourCoordinate); } } // Straight jumps. if (Jump(inNeighbourCoordinate + horizontalDelta, inNeighbourCoordinate, inInput) != null || Jump(inNeighbourCoordinate + verticalDelta, inNeighbourCoordinate, inInput) != null) { return(inNeighbourCoordinate); } } else { // Straight movement. if (horizontalDelta.x != 0 && CheckStraightJumpMovement(inNeighbourCoordinate, horizontalDelta, inInput)) { return(inNeighbourCoordinate); } else if (verticalDelta.y != 0) { if (CheckStraightJumpMovement(inNeighbourCoordinate, verticalDelta, inInput)) { return(inNeighbourCoordinate); } if (!inInput.DiagonalAllowed) { // Jump horizontally when moving vertically during non-diagonal search. if (Jump(inNeighbourCoordinate + Vector2Int.right, inNeighbourCoordinate, inInput) != null || Jump(inNeighbourCoordinate + Vector2Int.left, inNeighbourCoordinate, inInput) != null) { return(inNeighbourCoordinate); } } } } if (inInput.DiagonalAllowed && inInput.CutCorners) { if (IsTileWalkable(inNeighbourCoordinate + horizontalDelta, inInput.AreaMask) && IsTileWalkable(inNeighbourCoordinate + verticalDelta, inInput.AreaMask)) { return(Jump(inNeighbourCoordinate + horizontalDelta + verticalDelta, inNeighbourCoordinate, inInput)); } else { return(null); } } else { return(Jump(inNeighbourCoordinate + horizontalDelta + verticalDelta, inNeighbourCoordinate, inInput)); } }
private void IdentifySuccessors(PathfindingNode inNode, NavNodeHeap inOpenSet, HashSet <Vector2Int> inClosedSet, FindPathInput inInput) { List <Vector2Int> neighbours = FindNeighbours(inNode, inInput); foreach (Vector2Int neighbourCoord in neighbours) { Vector2Int?jumpCoordinateNullable = Jump(neighbourCoord, inNode.Coordinate, inInput); if (jumpCoordinateNullable == null || inClosedSet.Contains(jumpCoordinateNullable.GetValueOrDefault())) { continue; } PathfindingNode examinedNode = inOpenSet.GetExistingNode(jumpCoordinateNullable.Value) ?? new PathfindingNode(jumpCoordinateNullable.Value); int newMovementCost = inNode.GCost + (inInput.DiagonalAllowed ? GetDiagonalDistance(examinedNode.Coordinate, inNode.Coordinate) : GetManhattanDistance(examinedNode.Coordinate, inNode.Coordinate)); if (examinedNode.GCost == 0 || newMovementCost < examinedNode.GCost) { examinedNode.GCost = newMovementCost; examinedNode.HCost = GetHeuristicCost(examinedNode.Coordinate, inInput.TargetCoordinate, inInput.DiagonalAllowed); examinedNode.ParentNode = inNode; if (inOpenSet.Contains(examinedNode)) { inOpenSet.UpdateItem(examinedNode); } else { inOpenSet.Add(examinedNode); } } } }
private void AddStraightNeighbours(List <Vector2Int> inNeighbours, PathfindingNode inNode, Vector2Int inDelta, FindPathInput inInput) { Vector2Int swizzledDelta = new Vector2Int(inDelta.y, inDelta.x); bool nextWalkable = IsTileWalkable(inNode.Coordinate + inDelta, inInput.AreaMask); bool topWalkable = IsTileWalkable(inNode.Coordinate + swizzledDelta, inInput.AreaMask); bool bottomWalkable = IsTileWalkable(inNode.Coordinate - swizzledDelta, inInput.AreaMask); if (inInput.DiagonalAllowed && inInput.CutCorners) { if (nextWalkable) { inNeighbours.Add(inNode.Coordinate + inDelta); } if (!topWalkable) { inNeighbours.Add(inNode.Coordinate + inDelta + swizzledDelta); } if (!bottomWalkable) { inNeighbours.Add(inNode.Coordinate + inDelta - swizzledDelta); } } else { if (nextWalkable) { inNeighbours.Add(inNode.Coordinate + inDelta); if (inInput.DiagonalAllowed) { if (topWalkable) { inNeighbours.Add(inNode.Coordinate + inDelta + swizzledDelta); } if (bottomWalkable) { inNeighbours.Add(inNode.Coordinate + inDelta - swizzledDelta); } } } if (topWalkable) { inNeighbours.Add(inNode.Coordinate + swizzledDelta); } if (bottomWalkable) { inNeighbours.Add(inNode.Coordinate - swizzledDelta); } } }
private void AddHexagonalNeighbours(List <Vector2Int> inNeighbours, Vector2Int inCurrentCoord, Vector2Int inParentCoord, FindPathInput inInput) { Direction movingDirection = HexagonGridHelper.GetDirection(inParentCoord, inCurrentCoord); Vector2Int nextCoord = HexagonGridHelper.GetNeighbourInDirection(inCurrentCoord, movingDirection); if (IsTileWalkable(nextCoord, inInput.AreaMask)) { inNeighbours.Add(nextCoord); } Vector2Int aboveCoord = HexagonGridHelper.GetNeighbourInDirection(inCurrentCoord, HexagonGridHelper.GetRelativeDirection(movingDirection, 1)); if (IsTileWalkable(aboveCoord, inInput.AreaMask)) { inNeighbours.Add(aboveCoord); } Vector2Int belowCoord = HexagonGridHelper.GetNeighbourInDirection(inCurrentCoord, HexagonGridHelper.GetRelativeDirection(movingDirection, -1)); if (IsTileWalkable(belowCoord, inInput.AreaMask)) { inNeighbours.Add(belowCoord); } }
private List <Vector2Int> FindNeighbours(PathfindingNode inNode, FindPathInput inInput) { List <Vector2Int> neighbours = new List <Vector2Int>(); PathfindingNode parentNode = inNode.ParentNode; if (parentNode != null) { if (NavTileManager.Instance.SurfaceManager.GridInfo.CellLayout == GridLayout.CellLayout.Hexagon) { AddHexagonalNeighbours(neighbours, inNode.Coordinate, parentNode.Coordinate, inInput); return(neighbours); } Vector2Int difference = inNode.Coordinate - parentNode.Coordinate; Vector2Int horizontalDelta = new Vector2Int(difference.x / Mathf.Max(Mathf.Abs(difference.x), 1), 0); Vector2Int verticalDelta = new Vector2Int(0, difference.y / Mathf.Max(Mathf.Abs(difference.y), 1)); if (inInput.DiagonalAllowed && horizontalDelta.x != 0 && verticalDelta.y != 0) { // Diagonal movement. if (IsTileWalkable(inNode.Coordinate + verticalDelta, inInput.AreaMask)) { neighbours.Add(inNode.Coordinate + verticalDelta); } if (IsTileWalkable(inNode.Coordinate + horizontalDelta, inInput.AreaMask)) { neighbours.Add(inNode.Coordinate + horizontalDelta); } if (inInput.CutCorners) { if (IsTileWalkable(inNode.Coordinate + verticalDelta + horizontalDelta, inInput.AreaMask)) { neighbours.Add(inNode.Coordinate + verticalDelta + horizontalDelta); } if (!IsTileWalkable(inNode.Coordinate - horizontalDelta, inInput.AreaMask)) { neighbours.Add(inNode.Coordinate - horizontalDelta + verticalDelta); } if (!IsTileWalkable(inNode.Coordinate - verticalDelta, inInput.AreaMask)) { neighbours.Add(inNode.Coordinate + horizontalDelta - verticalDelta); } } else { if (IsTileWalkable(inNode.Coordinate + verticalDelta, inInput.AreaMask) && IsTileWalkable(inNode.Coordinate + horizontalDelta, inInput.AreaMask)) { neighbours.Add(inNode.Coordinate + verticalDelta + horizontalDelta); } } } else { // Straight movement. if (horizontalDelta.x != 0) { AddStraightNeighbours(neighbours, inNode, horizontalDelta, inInput); } else { AddStraightNeighbours(neighbours, inNode, verticalDelta, inInput); } } } else { // This is the starting node, get all neighbours. neighbours = GetWalkableNeighbours(inNode, inInput.AreaMask, inInput.DiagonalAllowed, inInput.CutCorners); } return(neighbours); }
private bool CheckStraightJumpMovement(Vector2Int inNeighbourCoordinate, Vector2Int inDelta, FindPathInput inInput) { Vector2Int swizzledDelta = new Vector2Int(inDelta.y, inDelta.x); if (inInput.DiagonalAllowed && inInput.CutCorners) { return(!IsTileWalkable(inNeighbourCoordinate + swizzledDelta, inInput.AreaMask) && IsTileWalkable(inNeighbourCoordinate + inDelta + swizzledDelta, inInput.AreaMask) || !IsTileWalkable(inNeighbourCoordinate - swizzledDelta, inInput.AreaMask) && IsTileWalkable(inNeighbourCoordinate + inDelta - swizzledDelta, inInput.AreaMask)); } return(!IsTileWalkable(inNeighbourCoordinate + swizzledDelta - inDelta, inInput.AreaMask) && IsTileWalkable(inNeighbourCoordinate + swizzledDelta, inInput.AreaMask) || !IsTileWalkable(inNeighbourCoordinate - swizzledDelta - inDelta, inInput.AreaMask) && IsTileWalkable(inNeighbourCoordinate - swizzledDelta, inInput.AreaMask)); }
/// <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); }
/// <summary> /// Moves the agent to a tile position /// </summary> /// <param name="inTargetPosition">The position to move to as a tile position.</param> public async void MoveToPosition(Vector2Int inTargetPosition, params Vector2Int[] inAvoidTiles) { // Check for early exits or possible errors first if (NavTileManager.Instance.SurfaceManager.Grid == null) { Debug.LogWarning("There is no (or multiple) grid object(s) found in the scene. Path can't be calculated without one."); OnPathNotFoundUnityEvent.Invoke(); return; } if (!NavTileManager.Instance.SurfaceManager.IsDataInitialized || NavTileManager.Instance.SurfaceManager.Data.HasNoTiles) { Debug.LogWarning("Data is not initialized for this scene. Please bake in the navigation 2D window."); OnPathNotFoundUnityEvent.Invoke(); return; } Vector2Int startCoordinate = NavTileManager.Instance.SurfaceManager.Grid.WorldToCell(this.transform.position).GetVector2Int(); if (startCoordinate == inTargetPosition) { Debug.LogWarning("Trying to find a path to the start node. Calculating a path is redundant."); OnPathNotFoundUnityEvent.Invoke(); return; } if (NavTileManager.Instance.SurfaceManager.Data.GetTileData(inTargetPosition) == null) { Debug.LogWarning(string.Format("Coordinate {0} is not a tile on the grid, can't calculate path.", inTargetPosition)); OnPathNotFoundUnityEvent.Invoke(); return; } // Start calculating a path. _pathStatus = EPathStatus.Calculating; // Intialize parameters to find a path. FindPathInput input = new FindPathInput(startCoordinate, inTargetPosition, _areaMask, this._diagonalAllowed, this._cutCorners, this._ignoreTileCost, inAvoidTiles); #if UNITY_EDITOR input.Visualizer = visualizer; #endif // Get a path based on the input and the Pipeline Settings in the Navigation2D window. _path = await NavTileManager.Instance.GetPath(input); // If no path is found, return. if (_path == null || _path.Count == 0) { Debug.LogWarning("Path could not be found.", gameObject); _pathStatus = EPathStatus.NotAvailable; OnPathNotFoundUnityEvent.Invoke(); return; } OnPathFound(); }
/// <summary> /// Calculates a path through the pipeline using the specified input. /// It's an async operation since it can be done multithreaded if specified in the Pipeline tab. /// </summary> /// <param name="inInput">Data holder for all path find variables.</param> /// <returns>A task with a NavTilePath as output when the calculation is finished.</returns> public async Task <NavTilePath> GetPath(FindPathInput inInput) { return(await PipelineManager.GetPath(inInput)); }