public List<Vector2> FindFinalPath(SearchNode startNode, SearchNode endNode)
{
closedList.Add(endNode);
SearchNode parentNode = endNode.Parent;
//Trace the path back through the parent field, getting the best path and adding each node to the closed list.
while (parentNode != startNode)
{
closedList.Add(parentNode);
parentNode = parentNode.Parent;
}
//Now that the path has been traced back, reverse the path and convert it to vectors in world space
List<Vector2> finalPath = new List<Vector2>();
for (int i = closedList.Count - 1; i >= 0; i--)
{
finalPath.Add(new Vector2((closedList[i].Position.X * TileEngine.TileWidth), (closedList[i].Position.Y * TileEngine.TileHeight)));
}
return finalPath;
}
public void InitializeSearchNodes(Map map)
{
searchNodes = new SearchNode[map.MapHeight, map.MapWidth];
//For each tile in our map, create a SearchNode for it
for (int y = 0; y < levelHeight; y++)
{
for (int x = 0; x < levelWidth; x++)
{
SearchNode node = new SearchNode();
node.Position = new Point(x, y);
node.Walkable = map.mapCells[y,x].IsWalkable;
if (node.Walkable)
{
//Create the list of neighbors
node.Neighbors = new SearchNode[4];
searchNodes[y, x] = node;
}
}
}
//Now that we have created search nodes for the entire level, it is time to populate each node with its neighbors
for (int y = 0; y < levelHeight; y++)
{
for (int x = 0; x < levelWidth; x++)
{
SearchNode thisNode = searchNodes[y, x];
//Ignore any nodes that are unwalkable or don't exist.
if (thisNode == null || !thisNode.Walkable)
continue;
//A list of all possible neighbors this node can have
Point[] neighbors = new Point[]
{
new Point (x, y - 1), //The node to the left
new Point (x, y + 1), //The node to the right
new Point (x - 1, y), //The node below
new Point (x + 1, y) //The node above
};
//Now, loop through the neighbors
for (int i = 0; i < neighbors.Length; i++)
{
Point position = neighbors[i];
//First verify that the neighbor is actually part of the level
if (position.X < 0 || position.X > levelWidth - 1 || position.Y < 0 || position.Y > levelHeight - 1)
continue;
//The neighbor is part of the level; grab a reference to it from the list of nodes
SearchNode neighbor = searchNodes[position.Y, position.X];
//We will only keep a reference to search nodes that can be walked on.
if (neighbor == null || !neighbor.Walkable)
continue;
//Store the reference to the neighbor
thisNode.Neighbors[i] = neighbor;
}
}
}
}
/// <summary>
/// This Method looks at everything in the open list and chooses the next
/// path to visit based on which search type is currently selected.
/// </summary>
/// <param name="result">The node to be visited</param>
/// <returns>Whether or not SelectNodeToVisit found a node to examine
/// </returns>
private bool SelectNodeToVisit(out SearchNode result)
{
result = new SearchNode();
bool success = false;
float smallestDistance = float.PositiveInfinity;
float currentDistance = 0f;
if (openList.Count > 0)
{
switch (searchMethod)
{
// Breadth first search looks at every possible path in the
// order that we see them in.
case SearchMethod.BreadthFirst:
totalSearchSteps++;
result = openList[0];
success = true;
break;
// Best first search always looks at whatever path is closest to
// the goal regardless of how long that path is.
case SearchMethod.BestFirst:
totalSearchSteps++;
foreach (SearchNode node in openList)
{
currentDistance = node.DistanceToGoal;
if(currentDistance < smallestDistance){
success = true;
result = node;
smallestDistance = currentDistance;
}
}
break;
// A* search uses a heuristic, an estimate, to try to find the
// best path to take. As long as the heuristic is admissible,
// meaning that it never over-estimates, it will always find
// the best path.
case SearchMethod.AStar:
totalSearchSteps++;
foreach (SearchNode node in openList)
{
currentDistance = Heuristic(node);
// The heuristic value gives us our optimistic estimate
// for the path length, while any path with the same
// heuristic value is equally ‘good’ in this case we’re
// favoring paths that have the same heuristic value
// but are longer.
if (currentDistance <= smallestDistance)
{
if (currentDistance < smallestDistance)
{
success = true;
result = node;
smallestDistance = currentDistance;
}
else if (currentDistance == smallestDistance &&
node.DistanceTraveled > result.DistanceTraveled)
{
success = true;
result = node;
smallestDistance = currentDistance;
}
}
}
break;
}
}
return success;
}
/// <summary>
/// This method find the next path node to visit, puts that node on the
/// closed list and adds any nodes adjacent to the visited node to the
/// open list.
/// </summary>
private void DoSearchStep()
{
SearchNode newOpenListNode;
bool foundNewNode = SelectNodeToVisit(out newOpenListNode);
if (foundNewNode)
{
Point currentPos = newOpenListNode.Position;
foreach (Point point in board.OpenMapTiles(currentPos))
{
SearchNode mapTile = new SearchNode(point,
StepDistanceToEnd(point),
newOpenListNode.DistanceTraveled + 1);
if (!InList(openList,point) &&
!InList(closedList,point))
{
openList.Add(mapTile);
paths[point] = newOpenListNode.Position;
}
}
if (currentPos == EndTile)
{
searchStatus = SearchStatus.PathFound;
}
openList.Remove(newOpenListNode);
closedList.Add(newOpenListNode);
}
else
{
searchStatus = SearchStatus.NoPath;
}
}
/// <summary>
/// Generates an optimistic estimate of the total path length to the goal
/// from the given position.
/// </summary>
/// <param name="location">Location to examine</param>
/// <returns>Path length estimate</returns>
private static float Heuristic(SearchNode location)
{
return location.DistanceTraveled + location.DistanceToGoal;
}
/// <summary>
/// Generates an optimistic estimate of the total path length to the goal
/// from the given position.
/// </summary>
/// <param name="location">Location to examine</param>
/// <returns>Path length estimate</returns>
private static float Heuristic(SearchNode location)
{
return(location.DistanceTraveled + location.DistanceToGoal);
}
/// <summary>
/// This Method looks at everything in the open list and chooses the next
/// path to visit based on which search type is currently selected.
/// </summary>
/// <param name="result">The node to be visited</param>
/// <returns>Whether or not SelectNodeToVisit found a node to examine
/// </returns>
private bool SelectNodeToVisit(out SearchNode result)
{
result = new SearchNode();
bool success = false;
float smallestDistance = float.PositiveInfinity;
float currentDistance = 0f;
if (openList.Count > 0)
{
switch (searchMethod)
{
// Breadth first search looks at every possible path in the
// order that we see them in.
case SearchMethodEnum.BreadthFirst:
totalSearchSteps++;
result = openList[0];
success = true;
break;
//Depth first search traveses the tree by always going to the first childnode that is further
//away from the parent node. If a wall is hit or childnode is not further away than current node
//it will backtrack to the last visited node.
case SearchMethodEnum.DepthFirst:
totalSearchSteps++;
openStack = new Stack <SearchNode>(openList);
if (visitedNodesStack.Count > 0)
{
lastVisitedNode = visitedNodesStack.Peek();
}
else
{
lastVisitedNode = new SearchNode(startPosition, Map.StepDistance(map.StartTile, map.EndTile), 0);
}
foreach (SearchNode node in openStack)
{
currentDistance = node.DistanceTraveled;
if (currentDistance > lastVisitedNode.DistanceTraveled)
{
visitedNodesStack.Push(node);
result = node;
break;
}
}
if (currentDistance <= lastVisitedNode.DistanceTraveled)
{
if (visitedNodesStack.Count == 0)
{
visitedNodesStack.Push(lastVisitedNode);
}
if (visitedNodesStack.Count > 1)
{
lastVisitedNode = visitedNodesStack.Pop();
}
result = lastVisitedNode;
}
success = true;
break;
// Best first search always looks at whatever path is closest to
// the goal regardless of how long that path is.
case SearchMethodEnum.BestFirst:
totalSearchSteps++;
foreach (SearchNode node in openList)
{
currentDistance = node.DistanceToGoal;
if (currentDistance < smallestDistance)
{
success = true;
result = node;
smallestDistance = currentDistance;
}
}
break;
// A* search uses a heuristic, an estimate, to try to find the
// best path to take. As long as the heuristic is admissible,
// meaning that it never over-estimates, it will always find
// the best path.
case SearchMethodEnum.AStar:
totalSearchSteps++;
foreach (SearchNode node in openList)
{
currentDistance = Heuristic(node);
// The heuristic value gives us our optimistic estimate
// for the path length, while any path with the same
// heuristic value is equally ‘good’ in this case we’re
// favoring paths that have the same heuristic value
// but are longer.
if (currentDistance <= smallestDistance)
{
if (currentDistance < smallestDistance)
{
success = true;
result = node;
smallestDistance = currentDistance;
}
else if (currentDistance == smallestDistance &&
node.DistanceTraveled > result.DistanceTraveled)
{
success = true;
result = node;
smallestDistance = currentDistance;
}
}
}
break;
}
}
return(success);
}
