public Stack <NavArea2D> GetPath(NavArea2D start, NavArea2D goal)
{
int maxIterations = 2000;
List <NavArea2DNode> openNodes = new List <NavArea2DNode>();
List <NavArea2DNode> closedNodes = new List <NavArea2DNode>();
NavArea2DNode startNode = new NavArea2DNode(start);
NavArea2DNode goalNode = new NavArea2DNode(goal);
NavArea2DNode current = startNode;
openNodes.Add(current);
while (maxIterations > 0 && openNodes.Count > 0 && (closedNodes.Contains(goalNode) == false))
{
maxIterations--;
current = openNodes[0];
openNodes.Remove(current);
closedNodes.Add(current);
List <NavArea2DNode> neighbors = current.GetNeighbors();
foreach (NavArea2DNode neighbor in neighbors)
{
if (closedNodes.Contains(neighbor) == false)
{
if (openNodes.Contains(neighbor) == false)
{
neighbor.parent = current;
// TODO: Improve this Distance function to consider more relevant values
neighbor.distanceToTarget = neighbor.DistanceTo(goalNode);
neighbor.cost = neighbor.weight + neighbor.parent.cost;
openNodes.Add(neighbor);
openNodes = openNodes.OrderBy(node => node.F).ToList();
}
}
}
}
if (closedNodes.Contains(goalNode) == false)
{
return(null);
}
NavArea2DNode temp = current;
if (closedNodes.Contains(current) == false)
{
return(null);
}
Stack <NavArea2D> path = new Stack <NavArea2D>();
do
{
path.Push(temp.area);
temp = temp.parent;
} while (temp != startNode && temp != null);
return(path);
}
public float DistanceTo(NavArea2DNode node)
{
return(area.DistanceTo(node.area));
}
