Spaces:
Running
on
Zero
Running
on
Zero
File size: 4,510 Bytes
e330ebf |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 |
#######################################################################
# Name: env.py
#
# - Adapted from https://gist.github.com/betandr/541a1f6466b6855471de5ca30b74cb31
# - Simple graph class to perform distance calculations (E.g. A-Star, Djikstra)
#######################################################################
class Edge:
def __init__(self, to_node, length):
self.to_node = to_node
self.length = length
class Graph:
def __init__(self):
self.nodes = set()
self.edges = dict()
def add_node(self, node):
self.nodes.add(node)
def add_edge(self, from_node, to_node, length):
edge = Edge(to_node, length)
if from_node in self.edges:
from_node_edges = self.edges[from_node]
else:
self.edges[from_node] = dict()
from_node_edges = self.edges[from_node]
from_node_edges[to_node] = edge
def clear_edge(self, from_node):
if from_node in self.edges:
self.edges[from_node] = dict()
def min_dist(q, dist):
"""
Returns the node with the smallest distance in q.
Implemented to keep the main algorithm clean.
"""
min_node = None
for node in q:
if min_node == None:
min_node = node
elif dist[node] < dist[min_node]:
min_node = node
return min_node
INFINITY = float('Infinity')
def dijkstra(graph, source):
q = set()
dist = {}
prev = {}
for v in graph.nodes:
dist[v] = INFINITY # unknown distance from source to v
prev[v] = INFINITY # previous node in optimal path from source
q.add(v) # all nodes initially in q (unvisited nodes)
# distance from source to source
dist[source] = 0
while q:
# node with the least distance selected first
u = min_dist(q, dist)
q.remove(u)
try:
if u in graph.edges:
for _, v in graph.edges[u].items():
alt = dist[u] + v.length
if alt < dist[v.to_node]:
# a shorter path to v has been found
dist[v.to_node] = alt
prev[v.to_node] = u
except:
pass
return dist, prev
def to_array(prev, from_node):
"""Creates an ordered list of labels as a route."""
previous_node = prev[from_node]
route = [from_node]
while previous_node != INFINITY:
route.append(previous_node)
temp = previous_node
previous_node = prev[temp]
route.reverse()
return route
def h(index, destination, node_coords):
current = node_coords[index]
end = node_coords[destination]
h = abs(end[0] - current[0]) + abs(end[1] - current[1])
return h
def a_star(start, destination, node_coords, graph):
if start == destination:
return [], 0
if str(destination) in graph.edges[str(start)].keys():
cost = graph.edges[str(start)][str(destination)].length
return [start, destination], cost
open_list = {start}
closed_list = set([])
g = {start: 0}
parents = {start: start}
while len(open_list) > 0:
n = None
h_n = 1e5
for v in open_list:
h_v = h(v, destination, node_coords)
if n is not None:
h_n = h(n, destination, node_coords)
if n is None or g[v] + h_v < g[n] + h_n:
n = v
if n is None:
print('Path does not exist!')
return None, 1e5
if n == destination:
reconst_path = []
while parents[n] != n:
reconst_path.append(n)
n = parents[n]
reconst_path.append(start)
reconst_path.reverse()
return reconst_path, g[destination]
for edge in graph.edges[str(n)].values():
m = int(edge.to_node)
cost = edge.length
if m not in open_list and m not in closed_list:
open_list.add(m)
parents[m] = n
g[m] = g[n] + cost
else:
if g[m] > g[n] + cost:
g[m] = g[n] + cost
parents[m] = n
if m in closed_list:
closed_list.remove(m)
open_list.add(m)
open_list.remove(n)
closed_list.add(n)
print('Path does not exist!')
return None, 1e5
|