public List <Vector2> GetPath(Vector2 from, Vector2 to)
{
List <Vector2> returnList = new List <Vector2>()
{
from
};
Vector2 vectorFrom = TranslateToNavGrid(from);
NavigationGridCell cellFrom = GetCell((short)vectorFrom.X, (short)vectorFrom.Y);
Vector2 vectorTo = TranslateToNavGrid(to);
NavigationGridCell goal = GetCell((short)vectorTo.X, (short)vectorTo.Y);
if (cellFrom != null && goal != null)
{
SimplePriorityQueue <Stack <NavigationGridCell> > priorityQueue = new SimplePriorityQueue <Stack <NavigationGridCell> >();
Stack <NavigationGridCell> start = new Stack <NavigationGridCell>();
Dictionary <int, NavigationGridCell> closedList = new Dictionary <int, NavigationGridCell>();
Stack <NavigationGridCell> path = null;
start.Push(cellFrom);
priorityQueue.Enqueue(start, NavigationGridCell.Distance(cellFrom, goal));
closedList.Add(cellFrom.ID, cellFrom);
// while there are still paths to explore
while (true)
{
if (!priorityQueue.TryFirst(out _))
{
// no solution
return(null);
}
float currentCost = priorityQueue.GetPriority(priorityQueue.First);
priorityQueue.TryDequeue(out path);
NavigationGridCell cell = path.Peek();
currentCost -= (NavigationGridCell.Distance(cell, goal) + cell.Heuristic); // decrease the heuristic to get the cost
// found the min solution return it (path)
if (cell.ID == goal.ID)
{
break;
}
foreach (NavigationGridCell neighborCell in GetCellNeighbors(cell))
{
// if the neighbor in the closed list - skip
if (closedList.TryGetValue(neighborCell.ID, out _))
{
continue;
}
// not walkable - skip
if (neighborCell.HasFlag(NavigationGridCellFlags.NOT_PASSABLE) || neighborCell.HasFlag(NavigationGridCellFlags.SEE_THROUGH))
{
closedList.Add(neighborCell.ID, neighborCell);
continue;
}
// calculate the new path and cost +heuristic and add to the priority queue
Stack <NavigationGridCell> npath = new Stack <NavigationGridCell>(new Stack <NavigationGridCell>(path));
npath.Push(neighborCell);
// add 1 for every cell used
priorityQueue.Enqueue(npath, currentCost + 1 + neighborCell.Heuristic + neighborCell.ArrivalCost + neighborCell.AdditionalCost
+ NavigationGridCell.Distance(neighborCell, goal));
closedList.Add(neighborCell.ID, neighborCell);
}
}
NavigationGridCell[] pathArray = path.ToArray();
Array.Reverse(pathArray);
List <NavigationGridCell> pathList = SmoothPath(new List <NavigationGridCell>(pathArray));
pathList.RemoveAt(0); // removes the first point
foreach (NavigationGridCell navGridCell in pathList.ToArray())
{
returnList.Add(TranslateFrmNavigationGrid(navGridCell.Locator));
}
return(returnList);
}
return(null);
}
