public MySortedListElement Add(Vertex key, int value) { double Value = value; MySortedListElement element = new MySortedListElement(); element.Key = key; element.Value = Value; element.next = null; element.previous = null; if (first == null) { first = element; } else { MySortedListElement current = first; if (current.Value > Value) { element.next = first; first.previous = element; first = element; } else if (current.next == null) { first.next = element; element.previous = first; } else { while (current.next.Value < Value) { current = current.next; if (current.next == null) { break; } } element.next = current.next; current.next = element; if (element.next != null) { element.next.previous = element; } } } return(element); }
public (Vertex, int) First() { if (first == null) { return(null, 0); } else { MySortedListElement current = first; first = first.next; return(current.Key, (int)current.Value); } }
public void Remove(MySortedListElement element) { if (element.next != null) { element.next.previous = element.previous; } if (element.previous != null) { element.previous.next = element.next; } if (element == first) { first = element.next; } }
public Dictionary <Vertex, Vertex> AStarAlgotihm(Vertex startVertex, Vertex endVertex, Func <Point, Point, int> heuristicFunction) { Dictionary <Vertex, int> Distance = new Dictionary <Vertex, int>(); Dictionary <Vertex, Vertex> Previous = new Dictionary <Vertex, Vertex>(); HashSet <Vertex> Close = new HashSet <Vertex>(); Dictionary <Vertex, MySortedListElement> OpenDictionary = new Dictionary <Vertex, MySortedListElement>(); MySortedList OpenList = new MySortedList(); Distance.Add(startVertex, 0); Previous.Add(startVertex, null); MySortedListElement element = OpenList.Add(startVertex, heuristicFunction(startVertex.point, endVertex.point)); OpenDictionary.Add(startVertex, element); while (OpenDictionary.Count != 0) { Vertex u; int uValue; (u, uValue) = OpenList.First(); OpenDictionary.Remove(u); Close.Add(u); if (u == endVertex) { break; } foreach (Vertex w in u.Vertices.Keys) { int weight = u.Vertices[w]; if (!Close.Contains(w)) { if (!OpenDictionary.ContainsKey(w)) { MySortedListElement currentElement = OpenList.Add(w, weight + Distance[u]); OpenDictionary.Add(w, currentElement); //Distance.Add(w, weight + Distance[u]); Distance.Add(w, int.MaxValue); } if (Distance[w] > Distance[u] + weight) { Distance[w] = Distance[u] + weight; MySortedListElement currentElement = OpenDictionary[w]; OpenList.Remove(currentElement); OpenList.Add(currentElement.Key, Distance[w] + heuristicFunction(w.point, endVertex.point)); if (Previous.ContainsKey(w)) { Previous[w] = u; } else { Previous.Add(w, u); } } } } } return(Previous); }