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