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