/// <summary>
/// Execute Dijkstra's algorithm
/// </summary>
/// <param name="start"></param>
/// <returns></returns>
private static Dictionary<Node, DijkstraEntry> ExecuteDijkstrasAlgorithm(Node start)
{
Dictionary<Node, DijkstraEntry> result = new Dictionary<Node, DijkstraEntry>();
result[start] = new DijkstraEntry { Via = start, Cost = 0f, Known = true };
Node currentNode = start;
while (currentNode != null)
{
// The best path to get to the current node is definitely known
result[currentNode].Known = true;
foreach (Node neighbor in currentNode.Neighbors)
{
// Check if it's a Node we've already analyzed
if (result.ContainsKey(neighbor) && result[neighbor].Known)
continue;
// Calculate distance from the current (known) Node
float distanceFromCurrentNode = (currentNode.Position - neighbor.Position).LengthSquared();
if (result.ContainsKey(neighbor))
{
DijkstraEntry existingEntry = result[neighbor];
// Check if a better path is available
if (result[currentNode].Cost + distanceFromCurrentNode < existingEntry.Cost)
{
existingEntry.Via = currentNode;
existingEntry.Cost = result[currentNode].Cost + distanceFromCurrentNode;
}
}
else
{
result[neighbor] = new DijkstraEntry
{
Via = currentNode,
Cost = result[currentNode].Cost + distanceFromCurrentNode,
Known = false
};
}
}
// Search for the next node to analyze
currentNode = null;
// In the set of unknown Nodes, find the one that has the least "cost" and set Known = true
KeyValuePair<Node, DijkstraEntry> nextKnownNode = result
.Where(n => !n.Value.Known)
.OrderBy(n => n.Value.Cost)
.FirstOrDefault();
if (nextKnownNode.Key != null)
currentNode = nextKnownNode.Key;
}
return result;
}
public static Dictionary<Node, Node> BuildShortestPathDictionary(Node node)
{
// Run Dijkstra's algorithm
Dictionary<Node, DijkstraEntry> dijkstrasAlgorithmResult = ExecuteDijkstrasAlgorithm(node);
// Construct result
Dictionary<Node, Node> result = new Dictionary<Node, Node>();
foreach (Node key in dijkstrasAlgorithmResult.Keys)
{
result.Add(key, dijkstrasAlgorithmResult[key].Via);
}
return result;
}
/// <summary>
/// Gets the best Node to take on the way to the goal Node
/// </summary>
/// <param name="goal"></param>
/// <returns></returns>
public Node GetShortestPathNode(Node goal)
{
return this.ShortestPathDictionary[goal];
}
/// <summary>
/// Returns the best node to travserse next in order to eventually reach the given goal Node
/// </summary>
/// <param name="goalNode"></param>
/// <returns></returns>
public Node GetNextNode(Node goalNode)
{
if (goalNode == this)
return null;
// The shortest path information is assumed to be in the ShortestPathDictionary
Node result = this.ShortestPathDictionary[goalNode];
return result;
}
/// <summary>
/// Add edge
/// </summary>
/// <param name="node1"></param>
/// <param name="node2"></param>
public void AddEdge(Node node1, Node node2)
{
node1.Neighbors.Add(node2);
node2.Neighbors.Add(node1);
}
public DijkstraEntry()
{
this.Known = false;
this.Via = null;
this.Cost = float.MaxValue;
}