Merge branch 'dijkstra' into 'master'

Dijkstra

See merge request tdde25-2020/tdde25-2020-projekt-sg3-maps-05!2

1 files changed, 32 insertions, 2 deletions

diff --git a/algorithms.py b/algorithms.py
index cc8d35f..6e01287 100644
--- a/algorithms.py
+++ b/algorithms.py
@@ -1,3 +1,4 @@
+import heapq
 import math
 
 
@@ -23,7 +24,7 @@ def get_closest_node_id(nodes, source_node):
 
     for node_id, node in nodes.items():
         length = length_haversine(source_node, node)
-        if min_node == None or length < min_value:
+        if min_node is None or length < min_value:
             min_node = node_id
             min_value = length
 
@@ -32,4 +33,33 @@ def get_closest_node_id(nodes, source_node):
 
 def find_shortest_path(nodes, source_id, target_id):
     """ Return the shortest path using Dijkstra's algortihm. """
-    return []
+    # queue contains multiple (walk_dist, (node_0, node_1, ... node_n))-tuples
+    # where (node_0, node_1, ... node_n) is a walk to node_n
+    # and walk_dist is the total length of the walk in meters
+    queue = [(0, (source_id,))]
+    visited = set()
+
+    while queue:
+        # consider an unchecked walk
+        walk_dist, walk = heapq.heappop(queue)
+        walk_end = walk[-1]
+        if walk_end == target_id:
+            # you have reached your destination
+            return walk
+        if walk_end in visited:
+            # there exists a shorter walk to walk_end
+            continue
+        # otherwise this is the shortest walk to walk_end
+        visited.add(walk_end)
+        # consider all our neighbours
+        for neighbour in nodes[walk_end].neighbours:
+            if neighbour in visited:
+                # there exists a shorter walk to neighbour
+                continue
+            # otherwise this MIGHT be the shortest walk to neighbour
+            # so put it in the queue
+            new_dist = walk_dist + length_haversine(nodes[walk_end], neighbour)
+            new_walk = walk + (neighbour.id,)
+            heapq.heappush(queue, (new_dist, new_walk))
+    # no path found
+    return None