private Stack <Vector3> ReconstructPath()
{
Stack <Vector3> path = new Stack <Vector3>();
PathfinderNode current = targetNode;
do
{
path.Push(current.Position);
current = current.Data.Parent;
} while (current != startingNode);
return(path);
}
//A* algorithm implementation
public void StartPathfinding()
{
BinaryHeap <PathfinderNode> openSet = new BinaryHeap <PathfinderNode>(PathfindingManager.Instance.PathfinderGrid.Length);
HashSet <PathfinderNode> closedSet = new HashSet <PathfinderNode>();
List <Vector3> indexes = new List <Vector3>();
indexes.Add(startingNode.Position);
startingNode.Data.HScore = CalculateManhattanDistance(startingNode.Position);
openSet.Add(startingNode);
while (openSet.Count > 0)
{
PathfinderNode current = openSet.RemoveFirst();
if (current.Equals(targetNode))
{
break;
}
foreach (PathfinderNode neighbor in current.Neighbors)
{
if (closedSet.Contains(neighbor))
{
continue;
}
int currentGScore = current.Data.GScore + 1;
if (currentGScore < neighbor.Data.GScore || !openSet.Contains(neighbor))
{
neighbor.Data.GScore = currentGScore;
neighbor.Data.HScore = CalculateManhattanDistance(neighbor.Position);
neighbor.Data.Parent = current;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
indexes.Add(neighbor.Position);
}
else
{
openSet.UpdateItem(neighbor);
}
}
}
closedSet.Add(current);
}
Stack <Vector3> path = ReconstructPath();
PathfindingManager.Instance.PathAcquired(path, request, indexes);
}
public PathCalculator(PathRequest request)
{
this.request = request;
startingNode = VectorToNode(request.StartingPosition);
targetNode = VectorToNode(request.TargetPosition);
}
