Add A* search algorithm option for path solving

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

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)

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."""