public Path SearchAStar(NavGraph graph, Vector3 source, Vector3 target)
{
Stopwatch sw = new Stopwatch();
sw.Start();
BinaryHeap <Node> open = new BinaryHeap <Node>(graph.Width * graph.Depth);
HashSet <Node> closed = new HashSet <Node>();
Node start = graph.GetNode(source);
if (!start.walkable)
{
start = graph.FindNearestWalkable(start);
}
Node goal = graph.GetNode(target);
if (!goal.walkable)
{
goal = graph.FindNearestWalkable(goal);
}
if (start == null || goal == null)
{
return(Path.Empty);
}
// F cost is used to prioritize
open.Insert(start, (int)start.f);
while (open.Count > 0)
{
Node current = open.Extract();
if (current == goal)
{
sw.Stop();
UnityEngine.Debug.Log("[A*]: Found path! Time: " + sw.ElapsedMilliseconds + "ms");
return(ReconstructPath(start, goal));
}
closed.Add(current);
// Evaluate neighbours
foreach (Node neighbour in current.GetNeighbours())
{
if (closed.Contains(neighbour))
{
continue;
}
// Calculate cost from source to the neighbour
float tentative_g = current.g + CalculateHeuristic(current, neighbour);
// If neighbour is already in open, and its not a better path, it does not need to be saved
if (open.Contains(neighbour) && tentative_g >= neighbour.g)
{
continue;
}
// Save this neighbours, as its a better path
neighbour.parentNode = current;
neighbour.g = tentative_g;
neighbour.h = CalculateHeuristic(neighbour, goal);
if (!open.Contains(neighbour))
{
open.Insert(neighbour, (int)neighbour.f);
}
// Because f is now changed, it should be resorted in the heap.
else
{
open.HeapifyUp(neighbour);
}
}
}
UnityEngine.Debug.LogError("[A*]: Couldnt find a path");
return(Path.Empty);
}
