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