def __update_hole_matrix(self): """ Updates the hole tracking matrix to reflect the current state of the maze :return: Nothing """ # reevaluate is the queue that contains all locations to be evaluated reevaluate = PriorityQueue.PriorityQueue() reevaluate.priority = False # number of the current hole self.hole_index = 0 # add all empty squares to the reevaluate queue for row in range(len(self.initMaze)): for col in range(len(self.initMaze)): # only check empty squares self.hole_matrix[row][col] = -1 if not self.has_been_colored[row][col]: reevaluate.put([row, col]) # evaluate all squares in the queue, when an error is detected add the violating square back on to the queue while not reevaluate.empty: row, col = reevaluate.get() adj_queue = PriorityQueue.PriorityQueue() for dx, dy in self.compare: i, j = row + dx, col + dy # check for out of bounds if not (i < 0 or j < 0 or i >= len(self.initMaze) or j >= len(self.initMaze)): # check for empty square if not self.hole_matrix[i][j] == -1: adj_queue.put([self.hole_matrix[i][j], [i, j]], self.hole_matrix[i][j]) # update this square with lowest value adj square, if higher values exist put them back on queue if not adj_queue.empty: # set this hole to the lowest available value min_val = adj_queue.get()[0] # set this square to the min self.hole_matrix[row][col] = min_val # update higher adj values while not adj_queue.empty: next_val = adj_queue.get() # don't reevaluate squares in the same hole if not next_val[0] == min_val: reevaluate.put(next_val[1]) # no adj squares else: self.hole_matrix[row][col] = self.hole_index self.hole_index += 1
def a_star(matrix, start, end, heuristic): if is_matrix(matrix): #init n = len(matrix) visited = [False] * n done_marks = [False] * n queue = pq.PriorityQueue() marks = [inf] * n path = [[]] * n nodes = [] #init node visited[start] = True marks[start] = 0 queue.push(start, 0) path[start].append(start) while queue.len() > 0: node = queue.get() nodes.append(node) if node == end: return [nodes, path[node]] for child in get_children(matrix, node): if not done_marks[child] and ( marks[node] + matrix[node][child]) < marks[child]: marks[child] = marks[node] + matrix[node][child] queue.push(child, -(marks[child] + heuristic(child, end, matrix))) path[child] = [] path[child].extend(path[node]) path[child].append(child) if not visited[child]: visited[child] = True done_marks[node] = True
def astar_search(searchStartState, goalState): frontier = PriorityQueue() frontier.put(searchStartState, 0) came_from: Dict[TrackState, Optional[TrackState]] = {} transition: Dict[(TrackState, TrackState), Optional[TrackPiece]] = {} cost_so_far: Dict[TrackState, int] = {} came_from[searchStartState] = None cost_so_far[searchStartState] = 0 while not frontier.empty(): currentState: TrackState = frontier.get() if currentState == goalState: print("SOLUTION FOUND") break for track in generatePossibleTracks(currentState): if (currentState != searchStartState): currentPath = stationSpiralLiftHillSequence + \ reconstruct_path(came_from, startHillState, currentState, transition) if not validateTrack(startState, currentState, currentPath, track): continue new_cost = cost_so_far[currentState] + getTrackPieceCost(track) nextState = simulateTrackSequence(currentState, [track]) if nextState not in cost_so_far or new_cost < cost_so_far[ nextState]: cost_so_far[nextState] = new_cost priority = new_cost + \ manhattanStateHeuristic(currentState, goalState) frontier.put(nextState, priority) came_from[nextState] = currentState transition[(currentState, nextState)] = track return came_from, cost_so_far, transition
def calculate_path(self, start_node, destination_node): #A* Search Algorithm frontier = PriorityQueue() frontier.put(start_node, 0) came_from = dict() came_from[start_node] = None cost_so_far = dict() cost_so_far[start_node.ID] = 0 while not frontier.empty(): current_node = frontier.get() if current_node.ID == destination_node.ID: #handle this equality break for next_node_id, next_node_values in current_node.edges.iteritems( ): new_cost = cost_so_far[current_node.ID] + int( next_node_values[0]) #total cost if next_node_id not in cost_so_far or new_cost < cost_so_far[ next_node_id]: cost_so_far[self.nodes[next_node_id].ID] = new_cost priority = new_cost + self.heuristic( destination_node, self.nodes[next_node_id]) frontier.put(self.nodes[next_node_id], priority) came_from[next_node_id] = current_node.ID return came_from, cost_so_far
def astar(istate, fstate): fringe = PriorityQueue.PriorityQueue() explored = [] fringe.insert(Node(istate)) while True: if fringe.isEmpty(): return None elem = fringe.delete() if elem.name == fstate: print("done?") return elem explored.append(elem) for stan in elem.name.succ(): stan.priority = stan.cost + stan.f(fstate) x = Node(stan, elem) infringe = any(st.name == x.name for st in fringe.queue) inexplored = any(st.name == x.name for st in explored) if not infringe and not inexplored: fringe.insert(x) else: if infringe: i = next( fringe.queue.index(z) for z in fringe.queue if z.name == x.name) if x.name.priority < fringe.queue[i].name.priority: fringe.queue[i] = x
def search(board): """ The A star search algorithm :return: dictionary of previous positions so we can reconstruct path later """ start = board.start frontier = pq.PriorityQueue() frontier.put(start, 0) prev_pos = {} #dictionary to keep track of where we came from cost_so_far = {} prev_pos[start] = None cost_so_far[start] = 0 while not frontier.empty(): current = frontier.get() if current == board.goal: break for next in board.neighbors(current): new_cost = cost_so_far[current] + board.cost(next) #calculate the cost to the neighbor coming from current if next not in cost_so_far or new_cost < cost_so_far[next]: #if we've found a cheaper path to the neighbor cost_so_far[next] = new_cost #update cost priority = new_cost + heuristic(board.goal, next) #set priority f(n) = g(n) + h(n) frontier.put(next, priority) #add to frontier prev_pos[next] = current #set where we came from return prev_pos
def BFS(self): que = PriorityQueue.PriorityQueue() que.enqueue((self.startPoint[0], self.startPoint[1], -self.dist(self.startPoint[0], self.startPoint[1]))) while not que.isEmpty(): here = que.dequeue() x, y, _ = here if self.mapList[1][y][x] == 9: return True else: self.mapList[1][y][x] = -1 if self.isValidPos(x, y - 1): que.enqueue((x, y - 1, -self.dist(x, y - 1))) # 위 if self.isValidPos(x, y + 1): que.enqueue((x, y + 1, -self.dist(x, y + 1))) # 아래 if self.isValidPos(x - 1, y): que.enqueue((x - 1, y, -self.dist(x - 1, y))) # 왼쪽 if self.isValidPos(x + 1, y): que.enqueue((x + 1, y, -self.dist(x + 1, y))) # 오른쪽 return False
def findPath(self, start_coord, goal_coord): self.start = Node(start_coord) self.goal = Node(goal_coord) self.start.G = 0 self.start.F = self.start.G + self.getHeuristic(self.start, self.goal) self.reachables = PriorityQueue() self.reachables.put(self.start) self.explored = list() self.found = False while not self.reachables.empty(): current = self.reachables.get() if current == goal: return self.buildPath(current) self.explored.append(current) self.app.drawExplored(self.explored) pygame.display.flip() for reachable_coord in self.getReachables(current.coord): reachable = Node(reachable_coord) if reachable in self.explored: continue if reachable in self.reachables: # old reachable pass else: # new reachable reachable.G = current.G + self.moveCost(current, reachable) reachable.F = reachable.G + self.getHeuristic( reachable, goal) #print 'new reachable',reachable.coord reachable.camefrom = current self.reachables.put(reachable) raise Exception('not found path')
def astart_closest(self, startState, board, endState): pq_open = PriorityQueue((lambda x, y: y[0] - x[0])) pq_open.enqueue((self.__heuristic(startState, endState), startState, None)) closed = set() while not pq_open.isEmpty(): current_node = pq_open.dequeue() if current_node[1] == endState: path = [] while current_node is not None: path.append(current_node[2]) current_node = current_node[2] path.reverse() return path closed.add(current_node[1]) for child in self.__expand(current_node[1], board): if child not in closed: pq_open.enqueue( (self.__heuristic(startState, endState), child, current_node)) return []
def best_first_graph_search(problem, f): """Search the nodes with the lowest f scores first. You specify the function f(node) that you want to minimize; for example, if f is a heuristic estimate to the goal, then we have greedy best first search; if f is node.depth then we have breadth-first search. There is a subtlety: the line "f = memoize(f, 'f')" means that the f values will be cached on the nodes as they are computed. So after doing a best first search you can examine the f values of the path returned.""" f = memoize(f, 'f') node = Node(problem.initial) if problem.goal_test(node.state): return node frontier = PriorityQueue(min, f) frontier.append(node) explored = set() while frontier: node = frontier.pop() if problem.goal_test(node.state): return node explored.add(node.state) for child in node.expand(problem): if child.state not in explored and child not in frontier: frontier.append(child) elif child in frontier: incumbent = frontier[child] if f(child) < f(incumbent): del frontier[incumbent] frontier.append(child) return None
def dijkstra(self, a): ''' Método para calcular el camino más corto para cada vértice dado un nodo inicial Args: a: nodo inicial :a: Vertice :return: None ''' if (a in self.listaVertices): #inicializar vertices self.initialize_single_source(a) l = [] #agregar todos los vertices a la lista l for i in self.listaVertices.values(): l.append(i) heapDikstra = PriorityQueue(l) while (len(heapDikstra.heap) != 0): #extraer el vertice con peso mínimo current = heapDikstra.heap_extract_min() #recorrer las vecinos de current for i in current.getConexiones().keys(): if i.getVisited() != True: #ver si cambiamos el peso del vecino o no self.relax(current, i, heapDikstra) current.setVisited(True) self.ResultadoDijkstra()
def aStarSearch(self, gridworld, start, goal): # Frontier of the A* algorithm. Not to be confused with the frontier that we use in exploration # Initialize the frontier and put the start cell on it frontier = PriorityQueue.PriorityQueue() frontier.put(start, 0) # Declare and initialize other variables to store the computed path and cost path = {} cost = {} path[start] = None cost[start] = 0 # Compute the next location to be added to the path while not frontier.isEmpty(): current = frontier.get() if current == goal: break for next in gridworld.get4Neighbors(current): newCost = cost[current] + 1 if next not in cost or newCost < cost[next]: cost[next] = newCost priority = newCost + self.heuristic(goal, next) frontier.put(next, priority) path[next] = current return path, cost
def __init__(self, airplaneRequests, lanes=1): """ airplaneRequests: string formated as "name, submission time, requested time, take off time" repeated for as many requests as there are, with newlines between. list of strings formated as "name, submssion time, requested time, take off time" lanes -> integer greater then 0, defaults to 1 """ #if airplaneRequests is a string if isinstance(airplaneRequests, str): self.__airplaneRequests = [] for line in airplaneRequests.split('\n'): if len(line) > 1: self.__airplaneRequests.append(lineToRequst(line)) #if airplaneRequests is a list elif isinstance(airplaneRequests, list): self.__airplaneRequests = [ lineToRequst(line) for line in airplaneRequests ] #expected to be in correct format self.__currentIndexInAirplaneRequests = 0 self.__queue = PriorityQueue.PriorityQueue( PriorityQueue.CreateComparetor([('requested time', False), ('submission time', False), ('take off time', False)], False)) self.__currentTime = -1 self.__runways = [None] * lanes # [{"end time", "request"}]
def solve(self): frontier = PriorityQueue() frontier.push(0, (0, self._problem.getInitial())) seen = set() parent = dict() size = self._problem._size # Do not remove the timeRemaining check from the while loop while len(frontier) > 0 and self.timeRemaining(): self._numExpansions += 1 priority, (depth, currentState) = frontier.pop() seen.add(currentState) for action in self._problem.actions(currentState): resultingState = self._problem.result(currentState, action) if self._problem.isGoal(resultingState): # Goal reached parent[resultingState] = (currentState, action) path = "" current = resultingState while current != self._problem.getInitial(): (current, action) = parent[current] path = action + path return path if resultingState not in seen: #f=depth #h=self.heuristic(resultingState) frontier.push(depth + self.heuristic(resultingState, size), (depth + 1, resultingState)) seen.add(resultingState) parent[resultingState] = (currentState, action) return []
def search(self, start, goal): frontier = PriorityQueue.PriorityQueue() frontier.put(start, 0) came_from = {} cost_so_far = {} came_from[start] = None cost_so_far[start] = 0 while not frontier.empty(): current = frontier.get() if current == goal: break for next in self.erreichbareNachbarn(current): richtung = (next[0] - current[0], next[1] - current[1]) richtung = self.spielfeld.offset_to_cube(richtung) new_cost = cost_so_far[current] if not self.wetter.zugGratis(richtung): new_cost += 1 if next not in cost_so_far or new_cost < cost_so_far[next]: cost_so_far[next] = new_cost priority = new_cost + self.heuristic(goal, next) frontier.put(next, priority) came_from[next] = current (path, erfolg) = self.extractPath(came_from, start, goal) return (path, cost_so_far, erfolg)
def __init__(self, k): self.__mean = 0 self.__standard_deviation = 0 self.__k = k self.__cpu = os.cpu_count() self.__lock = threading.Lock() self.__best_queue = PriorityQueue(maxsize=k)
def astar(maze): # TODO: Write your code here frontier = PriorityQueue() frontier.put(maze.getStart(), 0) # return path, num_states_explored return [], 0
def queue_dict(dict): term_queue = PriorityQueue() for t in dict: this_q_item = Q_item(t, dict[t]) term_queue.add(this_q_item) return term_queue
def prim(G,start): pq = PriorityQueue() inf = float("infinity") for i in G.get_vertices(): v = G.get_vertex(i) v.set_distance(inf) v.set_previous(None) s = G.get_vertex(start) s.set_distance(0) for v in G: pq.add(v.get_distance(), v.get_vertex_ID()) MST = [] while not pq.empty(): t = pq.extract_min() currentVert = G.get_vertex(t[1]) MST.append((currentVert.get_previous(), currentVert.get_vertex_ID())) for nextVert in currentVert.get_connections(): newCost = currentVert.get_weight(nextVert) + currentVert.get_distance() if nextVert in pq and newCost<nextVert.get_distance(): nextVert.set_previous(currentVert) nextVert.set_distance(newCost) pq.replace_key(nextVert,newCost) print MST
def test_for_add_and_get_size(self): p = PriorityQueue() p.add(999) p.add(222) p.add(33) p.add(33) self.assertTrue(p.size(), 5)
def dijkstra(G, s): #create a priority queue pq = PriorityQueue() #set distance for all other vertices to "infinity" inf = float("infinity") for i in G.get_vertices(): v = G.get_vertex(i) v.set_distance(inf) #set distance of source to zero source = G.get_vertex(s) source.set_distance(0) #insert all vertices into the priority queue (distance is priority) for v in G: pq.add(v.get_distance(), v.id) #loop while priority queue is not empty while not(pq.empty()): #remove vertex with smallest distance from priority queue t = pq.extract_min() v = G.get_vertex(t[1]) #for each vertex w adjacent to v for w in v.get_connections(): if w.get_distance() > (v.get_distance() + v.get_weight(w)): w.set_previous(v) w.set_distance(v.get_distance() + v.get_weight(w)) #loop over all nodes and print their distances for v in G: print "Node", v.get_vertex_ID(), "with distance", v.get_distance()
def split_equal_list(priorityq, letter_dict): # check if more than 1 node in priority queue if len(priorityq.return_all()) > 1: priorityq1 = PriorityQueue() priorityq2 = PriorityQueue() # add code '0' for priority queue 1 temp1 = priorityq.pop() temp1[2] += "0" priorityq1.add(temp1) count0 = temp1[1] # add code '1' for priority queue 2 temp1 = priorityq.pop() temp1[2] += "1" priorityq2.add(temp1) count1 = temp1[1] # place all nodes from priorityq into correct place while not priorityq.empty(): temp1 = priorityq.pop() # compare where to place next node if count0 + temp1[1] - count1 < count1 + temp1[1] - count0: temp1[2] += "0" priorityq1.add(temp1) count0 += temp1[1] else: temp1[2] += "1" priorityq2.add(temp1) count1 += temp1[1] # call itself until base case is reached return [ split_equal_list(priorityq1, letter_dict), split_equal_list(priorityq2, letter_dict) ] # base case else: # save code into dictionary letter_dict[priorityq.return_all()[0] [0]] = priorityq.return_all()[0][2] return priorityq.return_all()
def test_we_can_add_five_elements(self): p = PriorityQueue() p.add(10) p.add(20) p.add(30) p.add(40) p.add(50) self.assertTrue(p.size(), 5) self.assertTrue(p.peek(), 10)
def create_queue(activites, resources): queue = PriorityQueue() # resources_standardized = standardize_list(activites, resources) for activity_index in range(len(activites)): for resource_index in range(len(resources)): queue.put((-(np.log(activites[activity_index]) + np.log(resources[resource_index])), [activity_index, resource_index])) return queue
def sift_down_test(self): p = PriorityQueue() p.add(999) p.add(222) p.add(33) p.add(33) self.assertTrue(p.sift_down, 3) p.add(45) self.assertTrue(p.sift_down, 2) p.add(22)
def sf_compression(file_name): # open file to be read file = open(file_name, "r") text = file.read() file.close() #check for empty text file if text == "": print("Error: Empty Text") else: letter_dict = {text[0]: 1} # place each letter into dictionary and count each letter for x in range(1, len(text)): if text[x] in letter_dict: letter_dict[text[x]] = letter_dict[text[x]] + 1 else: letter_dict[text[x]] = 1 priorityq = PriorityQueue() # insert everything into priority queue for x in letter_dict: priorityq.add([x, letter_dict[x], ""]) # generate shannon-fano code split_equal_list(priorityq, letter_dict) print(letter_dict) # create compressed version in text format file = open(file_name[0:-4] + "_SFcompressed.txt", "w") result = "" for x in text: result = result + letter_dict[x] file.write(letter_dict[x]) file.close() # create compressed version in bin format file = open(file_name[0:-4] + "_SFcompressed.bin", "wb") file.write(_to_Bytes(result)) file.close() # create compression data(bitcount dictionary) in text format file = open(file_name[0:-4] + "_SFcode.txt", "w") file.write(str(len(result))) file.write(str(letter_dict)) file.close() print(result)
def a_star(self, start, goal): """ Referenced the following website: https://www.redblobgames.com/pathfinding/a-star/introduction.html This is where the A* algorithim calculates a dictionary of tuples containing the path from start to end using the given heuristics h(n) and g(n). :param start: tuple of start pose :param goal: tuple of goal pose :return: dict of tuples """ # Make sure the given start end tuples are integer start = (int(start[0]), int(start[1])) goal = (int(goal[0]), int(goal[1])) # Start Priority Queue and dictionaries frontier = PriorityQueue() frontier.put(start, 0) came_from = {} cost_so_far = {} came_from[start] = None cost_so_far[start] = 0 frontierGC = GridCells() localMap = enlarge_obstacles(astar.mapData) astar.pubOpenGrid.publish(map_to_cells(localMap)) while not frontier.empty(): current = frontier.get() # Finish the procese if the goal is reached if current == goal: break # Iterate through the neighbors of the node for next in get_neighbors(current, localMap): # Publish to display in RViz grid cells the wavefront frontierGC = add_tuple_to_gridcell(frontierGC, next, localMap) self.pubProgGrid.publish(frontierGC) # Calculate the net cost new_cost = cost_so_far[current] + self.move_cost(current, next) # Decide if the node has already been seen or has a lower cost to add to the Queue if next not in cost_so_far or new_cost < cost_so_far[next]: cost_so_far[next] = new_cost priority = new_cost + self.euclidean_heuristic(goal, next) frontier.put(next, priority) came_from[next] = current # Return a dictionary of tuples with the calculated path return came_from
def a_star_impl(grid, goal, heuristic_ptr): heuristic_ptr = config.heuristic_fn start = Node(h=heuristic_ptr(grid), empty_case_index=grid.index(0), grid=grid) open_set = PriorityQueue() closed_set = PriorityQueue() open_set.put(start) time_complexity = 0 size_complexity = 1 while open_set: process = open_set.get() if process.h is 0 or process.grid is goal: print("Ordered Sequence:") print_for_visu(process) print("Number of moves: {}\n" "Time Complexity: {}\n" "Size Complexity: {}".format(process.g, time_complexity, size_complexity)) return closed_set.put(process) process.set_parent() for node in process.parents: in_close = node.grid in [x.grid for (p, x) in closed_set.elements] in_open = node.grid in [x.grid for (p, x) in open_set.elements] if in_close: continue new_g = process.g + 1 if not in_open: node.g = new_g open_set.put(node) size_complexity += 1 else: if (node.g > new_g): node.g = new_g node.f = config.calc_fScore(node.h, node.g) time_complexity += 1 raise ValueError('No Path Found')
class a_star: frontiere = PriorityQueue() came_from = {} cost_so_far = {} came_from[start] = None cost_so_far[start] = 0 def __init__(self, start, goal, wall, nbLignes, nbColonnes): # self.board = board # terrain self.start = start # point de depart self.goal = goal # point d'arrivé self.wall = wall # obstacles self.nbLignes = nbLignes # nbLignes du terrain self.nbColonnes = nbColonnes # nbColonnes du terrain def reset(): a_star.frontiere.clear() a_star.came_from.clear() a_star.cost_so_far.clear() def heuristique(a,b): (x1,y1) = a (x2,y2) = b return abs(x1 - x2) + abs (y1 - y2) def a_star_search(self): a_star.frontiere.put(a_star.start, 0) while not frontiere.empty(): current = frontiere.get() if current == goal: break x,y = current nord = (x,y+1) sud = (x,y-1) est = (x+1,y) ouest = (x-1,y) neighbors = [nord, sud, est, ouest] for next in neighbors: if next[0] < nbLignes and next[1] < nbColonnes and next[0] >= 0 and next[1] >= 0 and next not in wall: # si nous sommes toujours dans le terrain et hors obstacles new_cost = cost_so_far[current] + 1 # chanque deplacement à un cout de 1 if next not in cost_so_far or new_cost < cost_so_far[next]: # si nous ne sommes pas deja allé sur la case, et qu'il n'y a pas de chemins de couts inferieur cost_so_far[next] = new_cost priority = new_cost + heuristique(goal, next) # on voit si on se rapproche frontiere.put(next, priority) # on étend la frontiere came_from[next] = current return came_from, cost_so_far # le chemin et le cout
def test_for_priority_removal(self): p = PriorityQueue() p.add(10) p.add(20) p.add(1) p.add(30) p.add(5) self.assertTrue(p.peek(), 1) p.remove() self.assertTrue(p.peek(), 5) p.remove() self.assertTrue(p.peek(), 10)