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