private void InitializeSearchNodes(Map map)
{
searchNodes = new SearchNode[mapWidth, mapHeight];
//For each of these nodes we will create, from a tile, the proper
//search node for it.
for (int y = 0; y < mapHeight; y++)
{
for (int x = 0; x < mapWidth; x++)
{
//Create the search node and set its position
SearchNode node = new SearchNode();
node.Position = new Point(x * numberNodesInTile, y * numberNodesInTile);
//Only store nodes that are walkable
foreach (Tile tile in Map.BlockedTiles)
{
if (tile.PositionRectangle.Contains(node.Position))
{
node.Walkable = false;
}
}
if (node.Walkable)
{
node.Walkable = true;
node.Neighbors = new SearchNode[8];
searchNodes[x, y] = node;
}
}
}
//Now we connect each node to its neighbors
for (int y = 0; y < mapHeight; y++)
{
for (int x = 0; x < mapWidth; x++)
{
SearchNode node = searchNodes[x, y];
if (node == null || node.Walkable == false)
{
continue;
}
//Create an array of all possible neighbors
Point[] neighbors = new Point[]
{
new Point(x, y - 1),
new Point(x, y + 1),
new Point(x - 1, y),
new Point(x + 1, y),
new Point(x - 1, y - 1),
new Point(x - 1, y + 1),
new Point(x + 1, y - 1),
new Point(x + 1, y + 1)
};
//Loop through the neighbors and keep them if they are walkable, discard them otherwise
for (int i = 0; i < neighbors.Length; i++)
{
Point position = neighbors[i];
//Make sure this is a part of the map
if (position.X < 0 || position.X > mapWidth - 1 || position.Y < 0 || position.Y > mapHeight - 1)
{
continue;
}
SearchNode neighbor = searchNodes[position.X, position.Y];
if (neighbor == null || neighbor.Walkable == false)
{
continue;
}
//Finally store the reference to the neighbor
node.Neighbors[i] = neighbor;
}
}
}
}
//Finds the optimal path from one point to another
public List <Vector2> FindPath(Point startPoint, Point endPoint)
{
//Only try to find a path if the start and end are different
if (startPoint == endPoint)
{
return(new List <Vector2>());
}
//Step 1: Clear everything from last time
ResetSearchNodes();
//Store reference to start and end nodes for convenience
SearchNode startNode = searchNodes[startPoint.X, startPoint.Y];
SearchNode endNode = searchNodes[endPoint.X, endPoint.Y];
//Step 2: Set start node's G to 0 and its F value to H.
startNode.InOpenList = true;
startNode.DistanceToGoal = Heuristic(startPoint, endPoint);
startNode.DistanceTraveled = 0;
openList.Add(startNode);
//Step 3: While there are still nodes to look at in the open list:
while (openList.Count > 0)
{
//a: Loop through the open list and find the node that has the smallest F value
SearchNode currentNode = FindBestNode();
//b: If the open list is empty or no node can be found, no path can be found so algorithm terminates.
if (currentNode == null)
{
break;
}
//c: If the active node is the goal node, we will find and return the final path.
if (currentNode == endNode)
{
//Trace our path back to the start.
return(FindFinalPath(startNode, endNode));
}
//d: Else, for each of the Active Node's neighbors :
for (int i = 0; i < currentNode.Neighbors.Length; i++)
{
SearchNode neighbor = currentNode.Neighbors[i];
//i: Make sure the neighbor node can be walked across.
if (neighbor == null || neighbor.Walkable == false)
{
continue;
}
//ii: Calculate a new G value for the neighboring node.
float distanceTraveled = currentNode.DistanceTraveled + 1;
//An estimate of the distance from this node to the end node.
float heuristic = Heuristic(neighbor.Position, endPoint);
//iii: If the neighboring node is not in either the open list or the closed list.
if (neighbor.InOpenList == false && neighbor.InClosedList == false)
{
//1: Set the neighboring G value to the one just calculated.
neighbor.DistanceTraveled = distanceTraveled;
//2: Set the neighboring F value to the new G value + the estimated value.
neighbor.DistanceToGoal = distanceTraveled + heuristic;
//3: Set the neighboring node's parent property to point at the active node.
neighbor.Parent = currentNode;
//4: Add the neighboring node to the open list.
neighbor.InOpenList = true;
openList.Add(neighbor);
}
//iv: if the neighboring node is in either the open or closed list
else if (neighbor.InOpenList || neighbor.InClosedList)
{
//1: If our new G value is less than the neighboring node's G value, we basically do exactly the same steps as if the node is not in the open and closed lists except we do not need to add this node to the open list again
if (neighbor.DistanceTraveled > distanceTraveled)
{
neighbor.DistanceTraveled = distanceTraveled;
neighbor.DistanceToGoal = distanceTraveled + heuristic;
neighbor.Parent = currentNode;
}
}
}
//e: Remove the active node from the open list and add it to the closed list
openList.Remove(currentNode);
currentNode.InClosedList = true;
}
//No path could be found.
return(new List <Vector2>());
}