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