public static Path FindPath(Road source, Road dest)
{
List <PathOpen> openPaths = new List <PathOpen>();
int shortestDistFound = Map.mapHeight * Map.mapWidth;
Path shortestPathFound = new Path();
PathOpen initialPath = new PathOpen(source, dest);
openPaths.Add(initialPath);
while (openPaths.Count > 0)
{
PathOpen workingPath = openPaths[0];
openPaths.RemoveAt(0);
// Checks if the working path is on the same street as the destination.
if (AreSameStreet(workingPath.waypoints.Last(), dest))
{
// If it's on the same street finalize the working path
Path finalizedPath = FinalizePathOpen(workingPath);
// Check if the newly finialized path is shorter than the shortest found.
if (finalizedPath.distActual < shortestDistFound)
{
// Then Update the shortest path.
shortestPathFound = finalizedPath;
shortestDistFound = finalizedPath.distActual;
openPaths = CullOpenPaths(openPaths, shortestDistFound);
}
}
else
{
List <PathOpen> branchedPath = BranchPath(workingPath, shortestDistFound);
openPaths.AddRange(branchedPath);
openPaths = CullOpenPaths(openPaths, shortestDistFound);
openPaths.OrderBy(x => x.GetDistWorkingTotal()).ToList();
}
}
return(shortestPathFound);
}
public static int EstimateDistance(Road r1, Road r2)
{
return((int)Math.Abs(r2.Loc.X - r1.Loc.X) + (int)Math.Abs(r2.Loc.Y - r1.Loc.Y));
}
