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 dijkstra(graph, origin, destination, add_to_visgraph):
D = {}
P = {}
Q = priority_dict()
Q[origin] = 0
for v in Q:
# if v not in graph.convex_edges: continue
D[v] = Q[v]
if v == destination: break
edges = graph[v]
if add_to_visgraph != None and len(add_to_visgraph[v]) > 0:
graph_edges = graph[v]
new_edges = set([])
for edge in graph_edges:
if edge.p1 in graph.convex_edges and edge.p2 in graph.convex_edges:
new_edges.add(edge)
edges = add_to_visgraph[v] | new_edges
for e in edges:
w = e.get_adjacent(v)
elength = D[v] + edge_distance(v, w)
if w in D:
if elength < D[w]:
raise ValueError
elif w not in Q or elength < Q[w]:
Q[w] = elength
P[w] = v
return (D, P)
def dijkstra(graph, origin, destination=None):
D = {}
P = {}
Q = priority_dict()
Q[origin] = 0
for v in Q:
D[v] = Q[v]
if v == destination: break
for e in graph[v]:
w = e.get_adjacent(v)
elength = D[v] + edge_distance(v, w)
if w in D:
if elength < D[w]:
raise ValueError
elif w not in Q or elength < Q[w]:
Q[w] = elength
P[w] = v
return (D, P)
def a_star_single(graph, origin, destination, add_to_visgraph, occupied, heuristic=lambda x, y: 0):
frontier = heapdict()
g_score = {}
P = {}
frontier[origin] = 0
g_score[origin] = 0
timestep = 0
while frontier:
v, _ = frontier.popitem()
if timestep >= len(occupied):
occupied.append(set())
if v in occupied[timestep]:
continue
# record expanded nodes
occupied[timestep].add(v)
timestep += 1
# if found result, return
if v == destination:
return P
# else expand
edges = graph[v]
if add_to_visgraph != None and len(add_to_visgraph[v]) > 0:
edges = add_to_visgraph[v] | graph[v]
for e in edges:
w = e.get_adjacent(v)
new_score = g_score[v] + edge_distance(v, w)
if w not in g_score or new_score < g_score[w]:
g_score[w] = new_score
frontier[w] = (new_score + heuristic(w, destination), w)
P[w] = v
return None
def dijkstra(graph, origin, destination, add_to_visgraph):
D = {}
P = {}
Q = priority_dict()
Q[origin] = 0
for v in Q:
D[v] = Q[v]
if v == destination: break
edges = graph[v]
if add_to_visgraph != None and len(add_to_visgraph[v]) > 0:
edges = add_to_visgraph[v] | graph[v]
for e in edges:
w = e.get_adjacent(v)
elength = D[v] + edge_distance(v, w)
if w in D:
if elength < D[w]:
raise ValueError
elif w not in Q or elength < Q[w]:
Q[w] = elength
P[w] = v
return (D, P)
def dijkstra(graph, origin, destination, add_to_visgraph):
D = {}
P = {}
Q = priority_dict()
Q[origin] = 0
for v in Q:
D[v] = Q[v]
if v == destination: break
edges = graph[v]
if add_to_visgraph != None and len(add_to_visgraph[v]) > 0:
edges = add_to_visgraph[v] | graph[v]
for e in edges:
w = e.get_adjacent(v)
elength = D[v] + edge_distance(v, w)
if w in D:
if elength < D[w]:
raise ValueError
elif w not in Q or elength < Q[w]:
Q[w] = elength
P[w] = v
return (D, P)
def test_closest_point_length(self):
pid = self.g.point_in_polygon(self.point_d)
cp = self.g.closest_point(self.point_d, pid, length=0.5)
ip = intersect_point(self.point_d, cp, Edge(self.point_a, self.point_b))
assert edge_distance(ip, cp) == 0.5
def a_star_multi(graph, agents, add_to_visgraph, heuristic=lambda x, y: 0):
frontiers = {}
g_scores = {}
Ps = {}
locations = {}
accum_dists = {}
for (origin, destination) in agents:
frontiers[origin] = heapdict()
g_scores[origin] = {}
Ps[origin] = {}
Ps[origin][origin] = origin
locations[origin] = origin
frontiers[origin][origin] = 0
g_scores[origin][origin] = 0
accum_dists[origin] = 0
# Each timestep of A*
agents_copy = list(agents)
while len(agents_copy) > 0:
occupied = set()
# comment this out for sort agents via f score
agents_copy.sort(reverse=True, key=lambda x: accum_dists[x[0]] + edge_distance(x[1], locations[x[0]]))
# or comment this out for shuffle randomly
#random.shuffle(agents_copy)
for (origin, destination) in agents_copy:
frontier = frontiers[origin]
v = None
while frontier:
v, _ = frontier.popitem()
if v not in occupied:
break
# if no paths, remove agent from agents
if v is None:
Ps[origin] = None
agents_copy.remove((origin, destination))
continue
occupied.add(v)
locations[origin] = v
P = Ps[origin]
accum_dists[origin] += edge_distance(v, P[v])
# if found result, remove agent from agents
if v == destination:
agents_copy.remove((origin, destination))
# else expand
edges = graph[v]
if add_to_visgraph != None and len(add_to_visgraph[v]) > 0:
edges = add_to_visgraph[v] | graph[v]
g_score = g_scores[origin]
for e in edges:
w = e.get_adjacent(v)
new_score = g_score[v] + edge_distance(v, w)
if w not in g_score or new_score < g_score[w]:
g_score[w] = new_score
h = heuristic(w, destination)
# comment this block out for look_ahead heuristics:
'''
nbs = neighbors(w, add_to_visgraph[w] | graph[w])
if destination not in nbs:
turn = False
for x in nbs:
if destination in neighbors(x, add_to_visgraph[x] | graph[x]):
turn = True
break
if turn:
h *= 1.4
else:
h *= 2
'''
frontier[w] = (new_score + h, w)
P[w] = v
return Ps
def test_closest_point_length(self):
pid = self.g.point_in_polygon(self.point_d)
cp = self.g.closest_point(self.point_d, pid, length=0.5)
ip = intersect_point(self.point_d, cp, Edge(self.point_a,
self.point_b))
assert edge_distance(ip, cp) == 0.5
