def run_random_search(maze, put_on_a_show): print('Random search chosen.') queue = [] closed = [] prev = {} queue.append(maze.start) while queue: open_node = random.choice(queue) if open_node == maze.end: maze = utils.reconstruct_path(maze, prev, maze.start, maze.end) maze.report('Hur dur (Random search)') return maze neighbours = maze.get_neighbours(open_node) for neighbour in neighbours: if maze.layout[neighbour[1]][neighbour[0]] != 'X': if neighbour not in closed: queue.append(neighbour) prev[neighbour] = open_node queue.remove(open_node) closed.append(open_node) if open_node != maze.start: maze.layout[open_node[1]][open_node[0]] = 'O' if put_on_a_show: maze.print_maze()
def run_dfs(maze, put_on_a_show): print('Depth first search (DFS) chosen.') queue = [] closed = [] prev = {} queue.append(maze.start) while queue: open_node = queue.pop() if open_node == maze.end: maze = utils.reconstruct_path(maze, prev, maze.start, maze.end) maze.report(name='DFS') return maze neighbours = maze.get_neighbours(open_node) for neighbour in neighbours: if maze.layout[neighbour[1]][neighbour[0]] != 'X': if neighbour not in closed: queue.append(neighbour) closed.append(neighbour) prev[neighbour] = open_node closed.append(open_node) if open_node != maze.start: maze.layout[open_node[1]][open_node[0]] = 'O' if put_on_a_show: maze.print_maze()
def greed_best_first(grid, wall, goal, h, frontier, inner, come_from): curr = next(iter(frontier)) # choose as current the node with higher heuristic value (euclidean distance from goal) for node in frontier: if h[node] < h[curr]: curr = node # check if it is the goal if curr == goal: path = reconstruct_path(curr, come_from) return None, None, path # switch the selected node from frontier to inner set frontier.discard(curr) inner.add(curr) # get all the nodes adjacent to the current one neighborhood = get_neighborhood(curr, grid, wall) for neighbour in neighborhood: # if a node is reachable for the first time, add it to the frontier if (neighbour not in inner) and (neighbour not in frontier): frontier.add(neighbour) come_from[neighbour] = curr return frontier, inner, come_from
def run_dijkstra(maze, put_on_a_show): print('Dijkstra chosen.') queue = [] prev = {} distance = {} queue.append(maze.start) distance[maze.start] = 0 while queue: open_node = queue.pop(0) if open_node == maze.end: maze = utils.reconstruct_path(maze, prev, maze.start, maze.end) maze.report(name='Dijkstra') return maze neighbours = maze.get_neighbours(open_node) for neighbour in neighbours: distance_to_node = distance[open_node] + utils.get_distance( open_node, neighbour) if maze.layout[neighbour[1]][neighbour[0]] != 'X': if (neighbour not in distance.keys()) or (distance_to_node < distance[neighbour]): queue.append(neighbour) prev[neighbour] = open_node distance[neighbour] = distance_to_node if open_node != maze.start: maze.layout[open_node[1]][open_node[0]] = 'O' if put_on_a_show: maze.print_maze()
def astar(draw, grid, start, end): count = 0 frontier = PriorityQueue() frontier.put((0, count, start)) frontier_hash = {start} g_score = {spot: float("inf") for row in grid for spot in row} g_score[start] = 0 f_score = {spot: float("inf") for row in grid for spot in row} f_score[start] = utils.h(start.get_pos(), end.get_pos()) came_from = {} while not frontier.empty(): for event in pygame.event.get(): if utils.is_quit(event): pygame.quit() current = frontier.get()[2] frontier_hash.remove(current) if current == end: utils.reconstruct_path(came_from, end, draw) start.make_start() end.make_end() return True for neighbor in current.neighbors: temp_g_score = g_score[current] + 1 if temp_g_score < g_score[neighbor]: came_from[neighbor] = current g_score[neighbor] = temp_g_score f_score[neighbor] = temp_g_score + utils.h( neighbor.get_pos(), end.get_pos()) if neighbor not in frontier_hash: count += 1 frontier.put((f_score[neighbor], count, neighbor)) frontier_hash.add(neighbor) neighbor.make_visited() draw() if current != start: current.make_expanded() return False
def dijkstra(draw, grid, start, end): count = 0 frontier = PriorityQueue() frontier.put((0, count, start)) frontier_hash = {start} dist = {spot: float("inf") for row in grid for spot in row} dist[start] = 0 explored = set() came_from = {} while not frontier.empty(): for event in pygame.event.get(): if(utils.is_quit(event)): pygame.quit() current = frontier.get()[2] frontier_hash.remove(current) explored.add(current) if current == end: utils.reconstruct_path(came_from, end, draw) start.make_start() end.make_end() return True for neighbor in current.neighbors: temp_dist = dist[current] + 1 if temp_dist < dist[neighbor]: dist[neighbor] = temp_dist came_from[neighbor] = current if neighbor not in explored and neighbor not in frontier_hash: count += 1 frontier.put((dist[neighbor], count, neighbor)) frontier_hash.add(neighbor) neighbor.make_visited() draw() if current != start: current.make_expanded() return False
def Theta_star(grid, wall, start, goal, h, frontier, inner, g_score, f_score, come_from): curr = next(iter(frontier)) # choose as current the node with the higher F score, where F(n) = G(n) + H(n) for node in frontier: if f_score[node] < f_score[curr]: curr = node # check if it is the goal if curr == goal: path = reconstruct_path(curr, come_from) return None, None, None, path # switch the selected node from frontier to inner set frontier.discard(curr) inner.add(curr) # get all the nodes adjacent to the current one neighborhood = get_neighborhood(curr, grid, wall) for neighbour in neighborhood: # if a node is reachable for the first time, initialize its G score to Inf if neighbour not in g_score.keys(): g_score[neighbour] = Inf come_from[neighbour] = curr # skip the inner nodes if neighbour in inner: continue # store the parent of the current node parent_curr = come_from[curr] if curr is not start else start # check if the neighbor (n) is reachable from the parent of the current node (parent_curr) if line_of_sight(parent_curr, neighbour, wall): # compute the new G score for n, considering it reachable from parent_curr new_gScore = g_score[parent_curr] + dist(parent_curr, neighbour) # if the path from parent_curr --> n is shorter then curr --> n, update the F score accordingly if new_gScore < g_score[neighbour]: g_score[neighbour] = new_gScore f_score[neighbour] = new_gScore + h[neighbour] come_from[neighbour] = parent_curr frontier.add(neighbour) # the neighbor (n) is not reachable from parent_curr, then execute the standard F score update (as A*) else: new_gScore = g_score[curr] + dist(curr, neighbour) if new_gScore < g_score[neighbour]: g_score[neighbour] = new_gScore f_score[neighbour] = new_gScore + h[neighbour] come_from[neighbour] = curr frontier.add(neighbour) return frontier, inner, g_score, come_from
def a_star(start, goal, grid, tail, ignore_list): """ A-Star algorithm for pathfinding\n Originally from https://github.com/noahspriggs/battlesnake-python\n @param start -> Starting point\n @param goal -> End point\n @param grid -> Updated grid\n @param tail -> Tail of snake / additional coords to ignore\n @param ignore_list -> Grid locations to avoid """ start = tuple(start) goal = tuple(goal) closed_set = [] open_set = [start] came_from = {} g_score = [[10] * len(grid[0]) for _ in xrange(len(grid))] g_score[start[0]][start[1]] = 0 f_score = [[10] * len(grid[0]) for _ in xrange(len(grid))] f_score[start[0]][start[1]] = distance(start,goal) while(len(open_set) > 0): current = min(open_set, key=lambda p: f_score[p[0]][p[1]]) if (current == goal): return reconstruct_path(came_from, goal) open_set.remove(current) closed_set.append(current) for neighbour in neighbours(current, grid, int(g_score[current[0]][current[1]]), tail, ignore_list): if neighbour in closed_set: continue tentative_g_score = g_score[current[0]][current[1]] + distance(current,neighbour) if neighbour not in open_set: open_set.append(neighbour) elif tentative_g_score == g_score[neighbour[0]][neighbour[1]]: dx1 = current[0] - goal[0] dy1 = current[1] - goal[1] dx2 = start[0] - goal[0] dy2 = start[1] - goal[1] cross = abs(dx1*dy2 - dx2*dy1) tentative_g_score += cross*0.001 elif tentative_g_score > g_score[neighbour[0]][neighbour[1]]: continue came_from[neighbour] = current g_score[neighbour[0]][neighbour[1]] = tentative_g_score f_score[neighbour[0]][neighbour[1]] = tentative_g_score + distance(neighbour,goal) return None
def run_a_star(maze, put_on_a_show): print('A* chosen.') queue = [] closed = [] prev = {} distance = {} heapq.heappush(queue, (0, maze.start)) distance[maze.start] = 0 while queue: open_node = heapq.heappop(queue)[1] if open_node == maze.end: maze = utils.reconstruct_path(maze, prev, maze.start, maze.end) maze.report(name='A*') return maze neighbours = maze.get_neighbours(open_node) for neighbour in neighbours: if neighbour not in closed: distance_to_node = distance[open_node] + utils.get_distance( open_node, neighbour) distance_to_end = utils.get_distance(neighbour, maze.end) if maze.layout[neighbour[1]][neighbour[0]] != 'X': if (neighbour not in queue) or (distance_to_node < distance[neighbour]): prev[neighbour] = open_node distance[neighbour] = distance_to_node if neighbour not in queue: heapq.heappush(queue, (distance_to_node + distance_to_end, neighbour)) closed.append(neighbour) closed.append(open_node) if open_node != maze.start: maze.layout[open_node[1]][open_node[0]] = 'O' if put_on_a_show: maze.print_maze()
def A_star(grid, wall, goal, h, frontier, inner, g_score, f_score, come_from): curr = next(iter(frontier)) # choose as current the node with the higher F score, where F(n) = G(n) + H(n) for node in frontier: if f_score[node] < f_score[curr]: curr = node # check if it is the goal if curr == goal: path = reconstruct_path(curr, come_from) return None, None, None, path # switch the selected node from frontier to inner set frontier.discard(curr) inner.add(curr) # get all the nodes adjacent to the current one neighborhood = get_neighborhood(curr, grid, wall) for neighbour in neighborhood: # if a node is reachable for the first time, initialize its G score to Inf if neighbour not in g_score.keys(): g_score[neighbour] = Inf # skip the inner nodes if neighbour in inner: continue # compute the new G score new_gScore = g_score[curr] + dist(curr, neighbour) # if a shorter path to reach the neighbour has been found --> update its F score if new_gScore < g_score[neighbour]: come_from[neighbour] = curr g_score[neighbour] = new_gScore f_score[neighbour] = new_gScore + h[neighbour] if neighbour not in frontier: frontier.add(neighbour) return frontier, inner, g_score, come_from
def dijkstra(stdscr, matrix, coordinates, animated_flag): """This is a Dijkstra's algorithm implementation for a weighted graph. This algorithm works with a weighted graph, but changes the given matrix. The weighted graph is simply converted from the matrix, and all weights are equal to 1.0. This implementation does not degrade to the BFS algorithm in the same named module, because BFS there doesn't use a graph, where neighbors of a node aren't listed in the order used in the utils.MOVES dictionary, so the node expansion order is not the same here. This is well visible due to the animation of these two algorithms using testovaci_data/test_5.txt input file. :param stdscr: a curses screen to animate the process :param matrix: a matrix with a pattern to work with :param coordinates: start and end points :param animated_flag: a flag - do we want to animate the process or not :return: the changed matrix, the start and end points coordinates, a number of opened nodes and a length of path to be printed either to the terminal or written to the output file """ start_node = coordinates[0] end_node = coordinates[1] matrix[start_node[1]][start_node[0]] = utils.START_NODE matrix[end_node[1]][end_node[0]] = utils.END_NODE graph = utils.matrix_to_weighted_graph(matrix) distances = {node: float('inf') for node in graph.keys()} priority_queue = [(0, start_node)] parents = {} visited_nodes = set() found = False distances[start_node] = 0.0 while len(priority_queue) > 0: utils.refresh_screen(stdscr, matrix, animated_flag) current_distance, current_node = heapq.heappop(priority_queue) if current_node == end_node: found = True for neighbor, cost in graph[current_node].items(): neighbor_distance = current_distance + cost if distances[neighbor] > neighbor_distance: distances[neighbor] = neighbor_distance if neighbor != start_node: matrix[neighbor[1]][neighbor[0]] = utils.OPENED_NODE visited_nodes.add(neighbor) heapq.heappush(priority_queue, (neighbor_distance, neighbor)) parents[neighbor] = current_node if current_node not in [start_node, end_node]: matrix[current_node[1]][current_node[0]] = utils.CLOSED_NODE if found: break path = utils.reconstruct_path(start_node, end_node, parents) path_length = len(path) node_count = len(visited_nodes) for col, row in path[:-1]: matrix[row][col] = utils.PATH_NODE matrix[end_node[1]][end_node[0]] = utils.END_NODE utils.show_path(stdscr, path, animated_flag) utils.show_final_message(stdscr, matrix.shape[0], 0, animated_flag) return matrix, coordinates, node_count, path_length
def random_search(stdscr, matrix, coordinates, animated_flag): """This is a Random Search algorithm implementation for an adjacency matrix. This algorithm doesn't convert a matrix into graph. It uses utils.MOVES dictionary to get all four possible directions of steps and then checks if the next node is valid to move to it. :param stdscr: a curses screen to animate the process :param matrix: a matrix with a pattern to work with :param coordinates: start and end points :param animated_flag: a flag - do we want to animate the process or not :return: the changed matrix, the start and end points coordinates, a number of opened nodes and a length of path to be printed either to the terminal or written to the output file """ start_node = coordinates[0] end_node = coordinates[1] matrix[start_node[1]][start_node[0]] = utils.START_NODE matrix[end_node[1]][end_node[0]] = utils.END_NODE node_counter = 0 found = False p = {} q = [start_node] while q: utils.refresh_screen(stdscr, matrix, animated_flag) current_node = random.choice(q) q.remove(current_node) if current_node == end_node: found = True for move_col, move_row in utils.MOVES.values(): next_col = current_node[0] + move_col next_row = current_node[1] + move_row if next_col < 0 or next_col > matrix.shape[1] - 1 \ or next_row < 0 or next_row > matrix.shape[0] - 1: continue elif matrix[next_row][next_col] in [ utils.FREE_NODE, utils.END_NODE ]: matrix[next_row][next_col] = utils.OPENED_NODE q.append((next_col, next_row)) p[(next_col, next_row)] = current_node node_counter += 1 if current_node not in [start_node, end_node]: matrix[current_node[1]][current_node[0]] = utils.CLOSED_NODE if found: break path = utils.reconstruct_path(start_node, end_node, p) path_length = len(path) for col, row in path[:-1]: matrix[row][col] = utils.PATH_NODE matrix[end_node[1]][end_node[0]] = utils.END_NODE utils.show_path(stdscr, path, animated_flag) utils.show_final_message(stdscr, matrix.shape[0], 0, animated_flag) return matrix, coordinates, node_counter, path_length
def run_a_star(maze, layout, start, end, put_on_a_show, constraints): queue = [] closed = [] prev = {} distance = {} timestep = 0 heapq.heappush(queue, (0, start, timestep)) distance[start] = 0 layout[start[1]][start[0]] = 'S' layout[end[1]][end[0]] = 'E' current_constraint = [] waits = [] while queue: open_node = heapq.heappop(queue)[1] if open_node == end: path = utils.reconstruct_path(layout, prev, start, end, waits) return path if constraints: current_constraint = utils.import_current_constraints( constraints, timestep) neighbours = maze.get_neighbours(open_node) for neighbour in neighbours: if neighbour not in closed: distance_to_node = distance[open_node] + \ utils.get_manhattan_distance(open_node, neighbour) distance_to_end = utils.get_manhattan_distance(neighbour, end) if constraints: occupied = utils.is_occupied(neighbour, current_constraint) else: occupied = False if maze.layout[neighbour[1]][neighbour[0]] != 'X': if not occupied: if (neighbour not in queue) or (distance_to_node < distance[neighbour]): prev[neighbour] = open_node distance[neighbour] = distance_to_node if neighbour not in queue: heapq.heappush( queue, (distance_to_node + distance_to_end, neighbour, timestep)) closed.append(neighbour) elif occupied: if neighbour not in waits: waits.append(neighbour) closed.append(open_node) timestep += 1
def greedy_search(stdscr, matrix, coordinates, animated_flag): """This is a Greedy Search algorithm implementation for a weighted graph. This algorithm works with a weighted graph, but changes the given matrix. The weighted graph is simply converted from the matrix, and all weights are equal to 1.0. Heuristics is built by the utils.build_heuristics(...) function and then applied to get the priority of a node to be extended. :param stdscr: a curses screen to animate the process :param matrix: a matrix with a pattern to work with :param coordinates: start and end points :param animated_flag: a flag - do we want to animate the process or not :return: the changed matrix, the start and end points coordinates, a number of opened nodes and a length of path to be printed either to the terminal or written to the output file """ start_node = coordinates[0] end_node = coordinates[1] matrix[start_node[1]][start_node[0]] = utils.START_NODE matrix[end_node[1]][end_node[0]] = utils.END_NODE graph = utils.matrix_to_weighted_graph(matrix) heuristics = utils.build_heuristics(end_node, graph) priority_queue = [(heuristics[start_node], start_node)] closed_nodes = set() parents = {} found = False node_counter = 0 while len(priority_queue) > 0: utils.refresh_screen(stdscr, matrix, animated_flag) _, current_node = heapq.heappop(priority_queue) if current_node == end_node: found = True for neighbor in graph[current_node].keys(): if (heuristics[neighbor], neighbor) not in priority_queue and neighbor not in closed_nodes: matrix[neighbor[1]][neighbor[0]] = utils.OPENED_NODE heapq.heappush(priority_queue, (heuristics[neighbor], neighbor)) parents[neighbor] = current_node node_counter += 1 closed_nodes.add(current_node) if current_node not in [start_node, end_node]: matrix[current_node[1]][current_node[0]] = utils.CLOSED_NODE if found: break path = utils.reconstruct_path(start_node, end_node, parents) path_length = len(path) for col, row in path[:-1]: matrix[row][col] = utils.PATH_NODE matrix[end_node[1]][end_node[0]] = utils.END_NODE utils.show_path(stdscr, path, animated_flag) utils.show_final_message(stdscr, matrix.shape[0], 0, animated_flag) return matrix, coordinates, node_counter, path_length