// Annoyingly, C# doesn't have Priority Queues...
IEnumerator UCS(Breadcrumb root, Breadcrumb goal)
{
List <Breadcrumb> expanded = new List <Breadcrumb>();
// Organizes by Breadcrumb heuristic. The lower the number, the higher the priority
PriorityQueue <Breadcrumb> pq = new PriorityQueue <Breadcrumb>();
pq.Enqueue(root);
Breadcrumb current = root;
// Go through the nodes on the map
while (pq.Count() > 0)
{
current = pq.Dequeue();
// Each time we visit a node, leave a breadcrub
transform.position = current.transform.position;
current.SetColor(Color.blue);
numSteps++;
yield return(new WaitForSeconds((float)stepSpeed / 1000));
// If we found the goal we're done
if (current == goal)
{
print("Found Goal");
break;
}
if (!expanded.Contains(current))
{
expanded.Add(current);
// Look at north, south, east, and west nodes. Diagonals are optional
for (int x = 0; x < numDirections; x++)
{
Breadcrumb neighbor = CheckNode(current, (Direction)x);
if (neighbor != null && !expanded.Contains(neighbor) && neighbor != current && !pq.Contains(neighbor))
{
pq.Enqueue(neighbor);
yield return(new WaitForSeconds((float)stepSpeed / 1000));
}
}
}
}
searching = false;
yield return(null);
}
IEnumerator DFS(Breadcrumb root, Breadcrumb goal)
{
List <Breadcrumb> expanded = new List <Breadcrumb>();
Stack <Breadcrumb> s = new Stack <Breadcrumb>();
s.Push(root);
Breadcrumb current = root;
// Go through the nodes on the map
while (s.Count > 0)
{
current = s.Pop();
// Each time we visit a node, leave a breadcrub
transform.position = current.transform.position;
current.SetColor(Color.blue);
numSteps++;
yield return(new WaitForSeconds((float)stepSpeed / 1000));
// If we found the goal we're done
if (current.ID == goal.ID)
{
print("Found Goal");
break;
}
if (!expanded.Contains(current))
{
expanded.Add(current);
// Look at north, south, east, and west nodes. Diagonals are optional
for (int x = 0; x < numDirections; x++)
{
Breadcrumb neighbor = CheckNode(current, (Direction)x);
if (neighbor != null && !expanded.Contains(neighbor) && neighbor != current && !s.Contains(neighbor))
{
s.Push(neighbor);
yield return(new WaitForSeconds((float)stepSpeed / 1000));
}
}
}
}
searching = false;
yield return(null);
}
IEnumerator Astar(Breadcrumb root, Breadcrumb goal, bool consistentHeuristic)
{
// The set of nodes already evaluated.
// Don't used expanded if the heuristic is MONOTONIC/CONSISTENT (see Wikipedia)
// Basically, if the shortest path between two neighboring nodes is always the direct path between those nodes
// then the heuristic is consistent. There isn't another path that goes through more nodes but has a smaller heuristic
List <Breadcrumb> expanded = new List <Breadcrumb>();
PriorityQueue <Breadcrumb> pq = new PriorityQueue <Breadcrumb>();
pq.Enqueue(root);
// For each node, which node it can most efficiently be reached from.
Dictionary <Breadcrumb, Breadcrumb> cameFrom = new Dictionary <Breadcrumb, Breadcrumb>();
// For each node, the cost of getting from the start node to that node.
Dictionary <Breadcrumb, float> fromStartHeuristic = new Dictionary <Breadcrumb, float>();
fromStartHeuristic[root] = 0; // The cost of going from start to start is zero.
// For each node, the total cost of getting from the start node to the goal by passing by that node. That value is partly known, partly heuristic.
Dictionary <Breadcrumb, float> estimatedHeuristic = new Dictionary <Breadcrumb, float>();
estimatedHeuristic[root] = root.heuristic;
Breadcrumb current = null;
while (pq.Count() > 0)
{
current = pq.Dequeue();
transform.position = current.transform.position;
current.SetColor(Color.blue);
numSteps++;
yield return(new WaitForSeconds((float)stepSpeed / 1000));
if (current.ID == goal.ID)
{
print("Found Goal");
break;
}
if (!expanded.Contains(current))
{
expanded.Add(current);
// Check neighbors
for (int x = 0; x < numDirections; x++)
{
Breadcrumb neighbor = CheckNode(current, (Direction)x);
if (neighbor != null && neighbor != current)
{
// If it has consistent heuristics, then we have to check if we've already expanded the node
if (consistentHeuristic)
{
if (!expanded.Contains(neighbor))
{
float temp_gScore = fromStartHeuristic[current] + (current.heuristic - neighbor.heuristic);
if (!pq.Contains(neighbor)) // Discover a new node
{
pq.Enqueue(neighbor);
}
else if (temp_gScore >= fromStartHeuristic[neighbor])
{
continue; // This is not a better path.
}
// This path is the best until now. Record it!
cameFrom[neighbor] = current;
fromStartHeuristic[neighbor] = temp_gScore;
estimatedHeuristic[neighbor] = fromStartHeuristic[neighbor] + neighbor.heuristic;
}
}
// Otherwise, we don't need/don't want to check if we've already expanded the node, so let's check it
else
{
float tentative_gScore = fromStartHeuristic[current] + (current.heuristic - neighbor.heuristic);
if (!pq.Contains(neighbor)) // Discover a new node
{
pq.Enqueue(neighbor);
}
else if (tentative_gScore >= fromStartHeuristic[neighbor])
{
continue; // This is not a better path.
}
// This path is the best until now. Record it!
cameFrom[neighbor] = current;
fromStartHeuristic[neighbor] = tentative_gScore;
estimatedHeuristic[neighbor] = fromStartHeuristic[neighbor] + neighbor.heuristic;
}
yield return(new WaitForSeconds((float)stepSpeed / 1000));
}
}
}
}
searching = false;
yield return(null);
}