/// <summary>
/// Function to find a path between two positions
/// </summary>
/// <param name="startPos">starting position</param>
/// <param name="targetPos">target position</param>
/// <returns>Queue containing the nodes an entity shall traverse</returns>
public Queue <IPath> FindPath(Vector3 startPos, Vector3 targetPos)
{
IPath startCel = m_SquareGrid.WorldPosToGrid(startPos);
IPath targetCel = m_SquareGrid.WorldPosToGrid(targetPos);
if (targetCel == null || startCel == null)
{
return(null);
}
Heap <IPath> openSet = new Heap <IPath>(m_SquareGrid.CelCount);
HashSet <IPath> closedSet = new HashSet <IPath>();
openSet.Add(startCel);
while (openSet.Count > 0)
{
IPath currentCel = openSet.RemoveFirst();
closedSet.Add(currentCel);
if (currentCel == targetCel)
{
var path = RetracePath(startCel, targetCel);
return(path);
}
foreach (IPath neighbour in GetNeighbours(currentCel))
{
// TODO: Consider neighour state
if (closedSet.Contains(neighbour))
{
continue;
}
int newCostToNeighbour = currentCel.GCost + GetDistance(currentCel, neighbour);
if (newCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour))
{
neighbour.GCost = newCostToNeighbour;
neighbour.HCost = GetDistance(neighbour, targetCel);
neighbour.Parent = currentCel;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
return(null);
}