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)
