private List <GeoPlacemark> FindShortestPath(GeoPoint source, GeoPoint destination)
{
List <GeoPlacemark> route = new List <GeoPlacemark>();
foreach (GeoGraphPoint point in pointsByID.Values)
{
point.IsVisited = false;
point.PreviousPoint = null;
point.DistanceFromSource = double.PositiveInfinity;
}
GeoGraphPoint sourcePoint = pointsByID[source.ID];
sourcePoint.DistanceFromSource = 0;
sourcePoint.PreviousPoint = null;
unvisitedNodes.Add(sourcePoint);
GeoGraphPoint currentPoint, neighbour;
double dist;
while (unvisitedNodes.Count != 0)
{
currentPoint = unvisitedNodes.ElementAt(0);
for (int i = 0; i < allPoints.Count; i++)
{
neighbour = pointsByID.ElementAt(i).Value;
if (!neighbour.IsVisited)
{
if ((dist = distanceMatrix[pointsIndex[currentPoint.ID], i]) > 0)
{
if (currentPoint.DistanceFromSource + dist < neighbour.DistanceFromSource)
{
neighbour.DistanceFromSource = currentPoint.DistanceFromSource + dist;
neighbour.PreviousPoint = currentPoint;
int count = unvisitedNodes.Count, j;
for (j = 0; j < count; j++)
{
if (unvisitedNodes.ElementAt(j).DistanceFromSource >= neighbour.DistanceFromSource)
{
unvisitedNodes.Insert(j, neighbour);
break;
}
}
if (j == count)
{
unvisitedNodes.Insert(j, neighbour);
}
}
}
}
}
currentPoint.IsVisited = true;
unvisitedNodes.Remove(currentPoint);
}
currentPoint = pointsByID[destination.ID];
while (currentPoint != null)
{
route.Insert(0, new GeoPlacemark(currentPoint));
currentPoint = currentPoint.PreviousPoint;
}
return(route);
}
public GeoPlacemark(GeoGraphPoint point)
{
this.ID = point.ID;
this.Name = point.Name;
this.Position = point.Position;
}