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); }