/// <summary>
/// Using Greedy BFS algorithm finds the path from start node to the end node.
/// The "greedy" part uses Euclidian distance as the priority to explore
/// </summary>
/// <param name="start">The first node to start the search</param>
/// <param name="end">The goal node that want to be found from start</param>
/// <returns>Returns the path as an array of INodes. If there is not an available path, the array will be empty</returns>
public INode[] CalculatePath(INode start, INode end)
{
PathfinderNode startNode = PathfinderNode.GetPathfinderNodeOrCreateNew(nodes, start);
frontier.Insert(startNode);
exploration.Add(startNode, startNode);
while (frontier.Count > 0)
{
PathfinderNode exploringNode = frontier.Extract();
if (exploringNode.Node.Equals(end))
{
break;
}
for (int i = 0; i < exploringNode.Node.Neighbors.Length; i++)
{
INode neighbor = exploringNode.Node.Neighbors[i];
if (neighbor.IsVisitable)
{
PathfinderNode neighborNode = PathfinderNode.GetPathfinderNodeOrCreateNew(nodes, neighbor);
if (!exploration.ContainsKey(neighborNode)) //If is the first time to be explored
{
neighborNode.HeuristicValue = Heuristic.SquaredEuclidianDistance(neighbor, end);
frontier.Insert(neighborNode);
exploration.Add(neighborNode, exploringNode); //Add: neighborNode explored from exploringNode
}
}
}
}
//Construct path
if (nodes.ContainsKey(end)) //If the end node was added by one of its neighbors
{
//Track the path from end to start
PathfinderNode nextNode = nodes[end]; //Starts from end node
while (nextNode != startNode)
{
path.Add(nextNode.Node);
if (exploration.ContainsKey(nextNode)) //If the node was visited from other node
{
nextNode = exploration[nextNode]; //that "came from" node is the next in the path
}
else //if the node doesn't exist
{
break; //stop
}
}
path.Reverse();
}
return(path.ToArray());
}
/// <summary>
/// Using Dijkstra algorithm to find the path from start node to the end node.
/// It uses the weight of the nodes to find the best path
/// </summary>
/// <param name="start">The first node to start the search</param>
/// <param name="end">The goal node that want to be found from start</param>
/// <returns>Returns the path as an array of INodes. If there is not an available path, the array will be empty</returns>
public INode[] CalculatePath(INode start, INode end)
{
PathfinderNode startNode = PathfinderNode.GetPathfinderNodeOrCreateNew(nodes, start);
frontier.Insert(startNode);
exploration.Add(startNode, startNode);
while (frontier.Count > 0)
{
PathfinderNode exploringNode = frontier.Extract();
if (exploringNode.Node.Equals(end))
{
break;
}
for (int i = 0; i < exploringNode.Node.Neighbors.Length; i++)
{
INode neighbor = exploringNode.Node.Neighbors[i];
if (neighbor.IsVisitable)
{
PathfinderNode neighborNode = PathfinderNode.GetPathfinderNodeOrCreateNew(nodes, neighbor);
float cost = exploringNode.Cost + neighbor.Weight;
if (!exploration.ContainsKey(neighborNode)) //If is the first time to be explored
{
neighborNode.Cost = cost;
frontier.Insert(neighborNode);
exploration.Add(neighborNode, exploringNode); //Add: neighborNode explored from exploringNode
}
else //Already explored
{ //Compares if this path cost less
if (cost < exploration[neighborNode].Cost)
{
//Change the previous connection, so that neighbor now came from exploringNode
neighborNode.Cost = cost;
exploration[neighborNode] = exploringNode;
}
}
}
}
}
//Construct path
if (nodes.ContainsKey(end)) //If the end node was added by one of its neighbors
{
//Track the path from end to start
PathfinderNode nextNode = nodes[end]; //Starts from end node
while (nextNode != startNode)
{
path.Add(nextNode.Node);
if (exploration.ContainsKey(nextNode)) //If the node was visited from other node
{
nextNode = exploration[nextNode]; //that "came from" node is the next in the path
}
else //if the node doesn't exist
{
break; //stop
}
}
path.Reverse();
}
return(path.ToArray());
}
