def a_star(graph, start, goal, heuristic=euclidean_dist_heuristic):
"""
Args:
graph (ExplorableGraph): Undirected graph to search.
start (str): Key for the start node.
goal (str): Key for the end node.
heuristic: Function to determine distance heuristic.
Default: euclidean_dist_heuristic.
Returns:
The best path as a list from the start and goal nodes (including both).
"""
if start == goal:
return []
queue = PriorityQueue()
queue.append((0, [0, start]))
isVisited = sets.Set()
while queue.size() > 0:
dist, nodes = queue.pop()
if nodes[-1] not in isVisited:
isVisited.add(nodes[-1])
if nodes[-1] == goal:
return nodes[1:]
for node in graph[nodes[-1]]:
newPath = copy.deepcopy(nodes)
newPath.append(node)
newPath[0] += graph[nodes[-1]][node]['weight']
newDist = newPath[0]+heuristic(graph, node, goal)
queue.append((newDist, newPath))
raise Error('No path from {} to {}').format(start, goal)
def test_append_and_pop(self):
"""Test the append and pop functions"""
queue = PriorityQueue()
temp_list = []
for _ in xrange(10):
a = random.randint(0, 10000)
queue.append((a, 'a'))
temp_list.append(a)
temp_list = sorted(temp_list)
for item in temp_list:
popped = queue.pop()
self.assertEqual(item, popped[0])
def run_algorithm(self):
"""
run the algorithm
:return: True is there is a path from strat to end, False otherwise
"""
open_list = PriorityQueue(f=self.f)
open_list.append(self.start_point)
closed_list = set()
while not len(open_list) == 0:
next_n = open_list.pop()
closed_list.add(next_n)
if self.end_point == next_n:
self.value = next_n.total_value()
self.path = next_n.arrived_from
return True
else:
self.path_count += 1
for suc in next_n.successors:
suc.arrived_from = next_n.arrived_from + [self.get_direction(next_n.x, next_n.y, suc.x, suc.y)]
suc.fathers = next_n.fathers + [next_n]
if suc not in closed_list and suc not in open_list:
open_list.append(copy.deepcopy(suc))
elif suc in open_list and self.f(suc) < open_list[suc]:
del open_list[suc]
open_list.append(copy.deepcopy(suc))
self.path = self.NO_PATH
return False
def uniform_cost_search(problem):
root = Node(problem.initial)
frontier = PriorityQueue()
frontier.append(root)
explored = set()
while True:
if len(frontier) == 0:
return "fail"
node = frontier.pop()
if problem.goal_test(node.state):
return node
explored.add(node)
for action in problem.actions(node.state):
child = node.child_node(problem, action)
if child.state not in explored:
print(child.state + " > ")
frontier.append(child)
elif child in frontier:
item = frontier.__getitem__(child)
if child.path_cost < item.path_cost:
pass
def bidirectional_a_star(graph, start, goal, heuristic=euclidean_dist_heuristic):
"""
Args:
graph (ExplorableGraph): Undirected graph to search.
start (str): Key for the start node.
goal (str): Key for the end node.
heuristic: Function to determine distance heuristic.
Default: euclidean_dist_heuristic.
Returns:
The best path as a list from the start and goal nodes (including both).
"""
if start == goal:
return []
queue_s = PriorityQueue()
queue_s.append((0, 0, start, ''))
is_visited_s = {}
queue_g = PriorityQueue()
queue_g.append((0, 0, goal, ''))
is_visited_g = {}
small = [float('inf'),'']
p = lambda g, s, t, n : (heuristic(g, n, t)-heuristic(g, n, s)+heuristic(g, t, s))/2
while queue_s.size()+queue_g.size() > 0:
est_s, dist_s, node_s, pa_s = queue_s.pop()
est_g, dist_g, node_g, pa_g = queue_g.pop()
# not really sure about the ending criteria for a*
if est_s+est_g >= small[0]+2*p(graph, start, goal, start):
node = small[1]
ret = [node]
ptr = node
while is_visited_s[ptr][1] != '':
ret.append(is_visited_s[ptr][1])
ptr = is_visited_s[ptr][1]
ret.reverse()
ptr = node
while is_visited_g[ptr][1] != '':
ret.append(is_visited_g[ptr][1])
ptr = is_visited_g[ptr][1]
return ret
"""
forward/backward search
"""
for queue, is_visited, is_visited_other, node, dist, pa, t, s in \
[[queue_s, is_visited_s, is_visited_g, node_s, dist_s, pa_s, goal, start], \
[queue_g, is_visited_g, is_visited_s, node_g, dist_g, pa_g, start, goal]]:
if is_visited.get(node) is not None:
continue
is_visited[node] = [dist, pa]
if is_visited_other.get(node) is not None:
dist_cache, _ = is_visited_other[node]
if dist_cache+dist < small[0]:
small = [dist_cache+dist, node]
else:
for dest in graph[node]:
new_dist = dist+graph[node][dest]['weight']
new_heuristic = p(graph,s,t,dest)
queue.append((new_dist+new_heuristic, new_dist, dest, node))
raise Error('No path from {} to {}').format(start, goal)
