string ConvertSearchListToString(SortedLinkedList <SearchNode <Position> > searchList)
{
StringBuilder listString = new StringBuilder();
LinkedListNode <SearchNode <Position> > currentNode = searchList.First;
while (currentNode != null)
{
listString.Append("[");
listString.Append(currentNode.Value.GraphNode.Value.PositionId + " ");
listString.Append(currentNode.Value.Distance + "] ");
currentNode = currentNode.Next;
}
return(listString.ToString());
}
public LinkedList <Position> Search(Position start, Position end, Graph <Position> graph)
{
SortedLinkedList <SearchNode <Position> > searchList = new SortedLinkedList <SearchNode <Position> >();
Dictionary <GraphNode <Position>, SearchNode <Position> > mapping =
new Dictionary <GraphNode <Position>, SearchNode <Position> >();
// save references to start and end graph nodes
GraphNode <Position> startNode = graph.Find(start);
GraphNode <Position> endNode = graph.Find(end);
// add search nodes for all graph nodes to list
foreach (GraphNode <Position> node in graph.Nodes)
{
SearchNode <Position> searchNode = new SearchNode <Position>(node);
if (node == startNode)
{
searchNode.Distance = 0;
}
searchList.Add(searchNode);
mapping.Add(node, searchNode);
}
string debug = ConvertSearchListToString(searchList);
// search until find end node or list is empty
while (searchList.Count > 0)
{
// front of search list has smallest distance
SearchNode <Position> currentSearchNode = searchList.First.Value;
searchList.RemoveFirst();
GraphNode <Position> currentGraphNode = currentSearchNode.GraphNode;
mapping.Remove(currentGraphNode);
// check for found end node
if (currentGraphNode == endNode)
{
return(BuildPath(currentSearchNode));
}
// loop through the current graph node's neighbors
foreach (GraphNode <Position> neighbor in currentGraphNode.Neighbors)
{
// only process neighbors still in the search list
if (mapping.ContainsKey(neighbor))
{
// check for new shortest distance on path from start to neighbor
double currentDistance = currentSearchNode.Distance +
currentGraphNode.GetEdgeWeight(neighbor);
SearchNode <Position> neighborSearchNode = mapping[neighbor];
if (currentDistance < neighborSearchNode.Distance)
{
// found a shorter path to the neighbor
neighborSearchNode.Distance = currentDistance;
neighborSearchNode.Previous = currentSearchNode;
searchList.Reposition(neighborSearchNode);
debug = ConvertSearchListToString(searchList);
}
}
}
}
// didn't find a path from start to end nodes
return(null);
}
