static double Dijkstra(int v1, double v1Init, int v2, double v2Init, MapPosition dest, out int[] route)
{
double[] dist = new double[nodeList.Count];
int[] prev = new int[nodeList.Count];
PriorityQueue <DijkElement> q = new PriorityQueue <DijkElement>();
for (int i = 0; i < nodeList.Count; ++i)
{
dist[i] = DIST_INF;
prev[i] = -1;
}
dist[v1] = v1Init;
dist[v2] = v2Init;
q.Enqueue(new DijkElement(v1, v1Init));
q.Enqueue(new DijkElement(v2, v2Init));
while (q.Count > 0)
{
DijkElement now = q.Dequeue();
if (dist[now.Index] < now.Cost)
{
continue;
}
foreach (var next in nodeConnection[now.Index])
{
int nextIndex = next.Node.NodeNumber;
double nextDist = next.Dist + now.Cost;
if (dist[nextIndex] > nextDist)
{
dist[nextIndex] = nextDist;
prev[nextIndex] = now.Index;
q.Enqueue(new DijkElement(nextIndex, nextDist));
}
}
}
if (dist[dest.Node1.NodeNumber] == DIST_INF && dist[dest.Node2.NodeNumber] == DIST_INF)
{
route = null;
return(-1);
}
Stack <int> routeTrace = new Stack <int>();
double result;
double n1Dist = dest.Dist1;
double n2Dist = dest.Dist2;
int currIndex;
if (n1Dist + dist[dest.Node1.NodeNumber] >= n2Dist + dist[dest.Node2.NodeNumber])
{
currIndex = dest.Node2.NodeNumber;
result = n2Dist + dist[dest.Node2.NodeNumber];
}
else
{
currIndex = dest.Node1.NodeNumber;
result = n1Dist + dist[dest.Node1.NodeNumber];
}
while (currIndex != v1 && currIndex != v2)
{
routeTrace.Push(currIndex);
currIndex = prev[currIndex];
}
routeTrace.Push(currIndex);
route = new int[routeTrace.Count];
for (int i = 0; i < route.Length; ++i)
{
route[i] = routeTrace.Pop();
}
return(result);
}
public int CompareTo(DijkElement dij)
{
return(Cost.CompareTo(dij.Cost));
}
