def greedy_search(the_map, screen):
queue = [(map.euclidean_distance(the_map.start.pos,
the_map.goal.pos), the_map.start)]
the_map.start.came_from = None
visited = set()
while len(queue) > 0:
(distance, current_node) = heapq.heappop(queue)
visited.add(current_node)
if current_node is the_map.goal:
return reconstruct_path(current_node)
for edge in current_node.edges:
if edge.to_node not in [n for (d, n) in queue
] and edge.to_node not in visited:
edge.to_node.came_from = current_node
distance = map.euclidean_distance(edge.to_node.pos,
the_map.goal.pos)
heapq.heappush(queue, (distance, edge.to_node))
screen.fill((255, 255, 255))
the_map.draw(screen)
for distance, node in queue:
node.draw(screen, (255, 0, 0), 6)
current_node.draw(screen, (0, 255, 0), 12)
pygame.display.flip()
wait_key()
def a_star_search(the_map, screen, visualise=False):
for node in the_map.nodes.values():
try:
del node.g
del node.came_from
except AttributeError:
pass
queue = [(0 + map.euclidean_distance(the_map.start.pos, the_map.goal.pos), the_map.start)]
the_map.start.g = 0
the_map.start.came_from = None
visited = set()
while len(queue) > 0:
(distance, current_node) = heapq.heappop(queue)
visited.add(current_node)
if current_node is the_map.goal:
return reconstruct_path(current_node)
for edge in current_node.edges:
if edge.to_node not in visited:
distance_so_far = current_node.g + edge.length
try:
current_g = edge.to_node.g
except AttributeError:
current_g = 1000000
if distance_so_far < current_g:
edge.to_node.came_from = current_node
distance_to_goal = map.euclidean_distance(edge.to_node.pos, the_map.goal.pos)
edge.to_node.g = distance_so_far
heapq.heappush(queue, (distance_so_far + distance_to_goal, edge.to_node))
if visualise:
screen.fill((255, 255, 255))
the_map.draw(screen)
for distance, node in queue:
node.draw(screen, (255, 0, 0), 6)
current_node.draw(screen, (0, 255, 0), 12)
pygame.display.flip()
pygame.event.get()
def a_star(the_map, screen):
closed_set = set()
open_set = [(0, the_map.start)]
the_map.start.g = 0
the_map.start.came_from = None
while len(open_set) > 0:
(_, node) = heapq.heappop(open_set)
if node is the_map.goal:
path = []
while node is not None:
path.insert(0, node)
node = node.came_from
return path
if node in closed_set:
continue
closed_set.add(node)
for edge in node.edges:
neighbour = edge.to_node
if neighbour not in closed_set:
g_tentative = node.g + edge.length
try:
g_current = neighbour.g
except AttributeError:
g_current = 1000000
if g_tentative < g_current:
neighbour.g = g_tentative
h = map.euclidean_distance(neighbour.pos, the_map.goal.pos)
heapq.heappush(open_set, (neighbour.g + h, neighbour))
neighbour.came_from = node
screen.fill((255, 255, 255))
the_map.draw(screen)
for n in the_map.nodes.itervalues():
try:
came_from = n.came_from
except AttributeError:
continue
if came_from is not None:
pygame.draw.line(screen, (0, 200, 0), n.pos, came_from.pos, 4)
node.draw(screen, (0, 255, 0), 8)
for (_, n) in open_set:
n.draw(screen, (0, 255, 255), 4)
for n in closed_set:
n.draw(screen, (255, 0, 0), 4)
pygame.display.flip()
wait_key()
def a_star_search(the_map, screen):
open_nodes = [the_map.start]
the_map.start.priority = map.euclidean_distance(the_map.start.pos,
the_map.goal.pos)
the_map.start.cost_so_far = 0
visited_nodes = set()
while len(open_nodes) > 0:
open_nodes.sort(key=lambda n: n.priority)
current_node = open_nodes.pop(0)
visited_nodes.add(current_node)
if current_node is the_map.goal:
return reconstruct_path(current_node)
for edge in current_node.edges:
if edge.to_node not in visited_nodes:
new_f = current_node.cost_so_far + edge.length + map.euclidean_distance(
edge.to_node.pos, the_map.goal.pos)
if edge.to_node not in open_nodes or new_f < edge.to_node.priority:
edge.to_node.came_from = current_node
edge.to_node.cost_so_far = current_node.cost_so_far + edge.length
edge.to_node.priority = new_f
if edge.to_node not in open_nodes:
open_nodes.append(edge.to_node)
screen.fill((255, 255, 255))
the_map.draw(screen)
for node in open_nodes:
node.draw(screen, (255, 0, 0), 6)
current_node.draw(screen, (0, 255, 0), 12)
pygame.display.flip()
time.sleep(0.1)
pygame.event.get()
def search(the_map, screen):
# Initialise the open and visited nodes
open_nodes = [the_map.start]
visited_nodes = set()
while len(open_nodes) > 0:
# Pop an open node and mark it as visited
open_nodes.sort(key=lambda n: n.cost_so_far + map.euclidean_distance(
n.pos, the_map.goal.pos))
current_node = open_nodes.pop(0)
visited_nodes.add(current_node)
# If we have reached the goal, return the path
if current_node is the_map.goal:
return reconstruct_path(current_node)
# Loop through the neighbours, adding unvisited neighbours to the open list
for edge in current_node.edges:
if edge.to_node not in visited_nodes:
new_cost_so_far = current_node.cost_so_far + edge.length
if edge.to_node not in open_nodes or new_cost_so_far < edge.to_node.cost_so_far:
edge.to_node.came_from = current_node
edge.to_node.cost_so_far = new_cost_so_far
if edge.to_node not in open_nodes:
open_nodes.append(edge.to_node)
# Redraw the map
screen.fill((255, 255, 255))
the_map.draw(screen)
# Draw the open nodes in red
for node in open_nodes:
node.draw(screen, (255, 0, 0), 6)
# Draw the current node in green
current_node.draw(screen, (0, 255, 0), 12)
# Flip the display and pause for 0.1 seconds
pygame.display.flip()
time.sleep(0.1)
pygame.event.get()
def get_path_length(path):
length = 0
for i in range(1, len(path)):
length += map.euclidean_distance(path[i-1].pos, path[i].pos)
return length