diff --git a/examples/2_calculate_shortest_distance.py b/examples/2_calculate_shortest_distance.py index 56ed0d6..549f38a 100755 --- a/examples/2_calculate_shortest_distance.py +++ b/examples/2_calculate_shortest_distance.py @@ -23,6 +23,7 @@ """ import pyvisgraph as vg from haversine import haversine +import time # In this example we will find the shortest path between two points on a # sphere, i.e. on earth. To calculate the total distance of that path, we @@ -37,8 +38,16 @@ graph = vg.VisGraph() graph.load('GSHHS_c_L1.graph') -# Get the shortest path +# Get the shortest path with dijkstra's +startTime = time.time() shortest_path = graph.shortest_path(start_point, end_point) +print("Solved with Dijkstra's algorithm in {} seconds.".format(time.time() - startTime)) + +# Get the shortest path with A* +startTime = time.time() +shortest_path = graph.shortest_path(start_point, end_point, solver = "astar") +print("Solved with A* algorithm in {} seconds.".format(time.time() - startTime)) + # Calculate the total distance of the shortest path in km path_distance = 0 diff --git a/pyvisgraph/shortest_path.py b/pyvisgraph/shortest_path.py index 1c269c7..c3e8f29 100644 --- a/pyvisgraph/shortest_path.py +++ b/pyvisgraph/shortest_path.py @@ -60,9 +60,45 @@ def dijkstra(graph, origin, destination, add_to_visgraph): P[w] = v return (D, P) - -def shortest_path(graph, origin, destination, add_to_visgraph=None): - D, P = dijkstra(graph, origin, destination, add_to_visgraph) +def astar(graph, origin, destination, add_to_visgraph): + """ + A* search algorithm, using Euclidean distance heuristic + Note that this is a modified version of an + A* implementation by Amit Patel. + https://www.redblobgames.com/pathfinding/a-star/implementation.html + """ + frontier = priority_dict() + frontier[origin] = 0 + cameFrom = {} + costSoFar = {} + cameFrom[origin] = None + costSoFar[origin] = 0 + + while len(frontier) > 0: + current = frontier.pop_smallest() + if current == destination: + break + + edges = graph[current] + if add_to_visgraph != None and len(add_to_visgraph[current]) > 0: + edges = add_to_visgraph[current] | graph[current] + for e in edges: + w = e.get_adjacent(current) + new_cost = costSoFar[current] + edge_distance(current, w) + if w not in costSoFar or new_cost < costSoFar[w]: + costSoFar[w] = new_cost + priority = new_cost + edge_distance(w, destination) + frontier[w] = priority + cameFrom[w] = current + + return (frontier, cameFrom) + + +def shortest_path(graph, origin, destination, add_to_visgraph=None, solver="dijkstra"): + if solver == "astar": + D, P = astar(graph, origin, destination, add_to_visgraph) + else: # Default to dijkstra + D, P = dijkstra(graph, origin, destination, add_to_visgraph) path = [] while 1: path.append(destination) diff --git a/pyvisgraph/vis_graph.py b/pyvisgraph/vis_graph.py index 54302c1..5b92728 100644 --- a/pyvisgraph/vis_graph.py +++ b/pyvisgraph/vis_graph.py @@ -105,7 +105,7 @@ def update(self, points, origin=None, destination=None): destination=destination): self.visgraph.add_edge(Edge(p, v)) - def shortest_path(self, origin, destination): + def shortest_path(self, origin, destination, solver = "dijkstra"): """Find and return shortest path between origin and destination. Will return in-order list of Points of the shortest path found. If @@ -117,7 +117,7 @@ def shortest_path(self, origin, destination): origin_exists = origin in self.visgraph dest_exists = destination in self.visgraph if origin_exists and dest_exists: - return shortest_path(self.visgraph, origin, destination) + return shortest_path(self.visgraph, origin, destination, solver) orgn = None if origin_exists else origin dest = None if dest_exists else destination add_to_visg = Graph([]) @@ -127,7 +127,7 @@ def shortest_path(self, origin, destination): if not dest_exists: for v in visible_vertices(destination, self.graph, origin=orgn): add_to_visg.add_edge(Edge(destination, v)) - return shortest_path(self.visgraph, origin, destination, add_to_visg) + return shortest_path(self.visgraph, origin, destination, add_to_visg, solver) def point_in_polygon(self, point): """Return polygon_id if point in a polygon, -1 otherwise."""