def a_star_search(graph, start, goal):
searchFrontier = PriorityQueue()
searchFrontier.put(start, heuristic[start])
currentCost = {}
currentCost[start] = 0
path = ""
while not searchFrontier.empty():
current = searchFrontier.get()
# add node in path
path += current + " "
if current == goal:
break
for next in graph.neighbors(current):
newCost = currentCost[current] + weights[(current, next)]
if next not in currentCost or newCost < currentCost[next]:
currentCost[next] = newCost
priority = newCost + heuristic[next] # set priority
searchFrontier.put(next,
priority) # add node in priority queue
return path
def main():
selection = int(sys.argv[1])
# Reads in the board from the random_board.py output
board = list(map(int, sys.stdin.read().split()))
board = [[board[0], board[1], board[2]], [board[3], board[4], board[5]],
[board[6], board[7], board[8]]]
state = State(board)
# Setting the x and y position for the blank tile
for i in range(0, 3):
for j in range(0, 3):
if board[i][j] == 0:
state.setx(i)
state.sety(j)
# Initialize the relevent data structures
curr_node = Node(state, h(state, selection))
closed = Set()
frontier = PriorityQueue()
frontier.push(curr_node)
V = 0
# Loops through and creates the search tree
while objective(curr_node.getstate()) == False:
closed.add(curr_node.getstate())
# Creates new states based on the valid_moves returned by the successor
for valid_state in successor(curr_node.getstate()):
V += 1
# Checking that the state we are eveluating has not already been expanded. Only adds to the queue if false
if closed.isMember(valid_state) == False:
frontier.push(
Node(valid_state, h(valid_state, selection), curr_node))
curr_node = frontier.pop()
N = closed.length() + frontier.length()
d = curr_node.getd()
print("V=%d" % (V))
print("N=%d" % (N))
print("d=%d" % (d))
print("b=%f" % (N**(1 / d)))
print()
# Create an empty list in order to retrieve the path from the goal node's parents
stack = []
while curr_node.getid() != 0:
stack.append(curr_node)
curr_node = curr_node.getparent()
print(curr_node.getstate())
# Prints out the solution path
for node in reversed(stack):
if node != None:
print(node.getstate())
def bestfs(graph, start, goal):
searchFrontier = PriorityQueue()
searchFrontier.put(start, heuristic[start])
predecessor = {}
predecessor[start] = None
path = ""
while not searchFrontier.empty():
current = searchFrontier.get()
path += current + " "
if current == goal:
break
for next in graph.neighbors(current):
if next not in predecessor:
priority = heuristic[next]
searchFrontier.put(next, priority)
predecessor[next] = current
return path
def bidirectional_dijkstra_un_p(graph, node_from, node_to):
if not isinstance(graph, classes.Graph.Graph):
raise IOError("Wrong graph type")
if not isinstance(node_from, classes.Node.Node):
raise IOError("Wrong node_from type")
if not isinstance(node_to, classes.Node.Node):
raise IOError("Wrong node_to type")
# time_start = time.time()
dists_fw = {a: -1 for a in graph.nodes}
dists_fw[node_from.id] = 0
dists_bw = {a: -1 for a in graph.nodes}
dists_bw[node_to.id] = 0
edge_to_fw = dict()
edge_to_bw = dict()
covering_fw = dict()
covering_bw = dict()
queue_fw = PQ.PriorityQueueByDict()
queue_fw.update(node_from)
queue_bw = PQ.PriorityQueueByDict()
queue_bw.update(node_to)
center = None
t_fw_done, t_bw_done = False, False
# current_node_f, current_node_b = None, None
iter_f, iter_b = 0, 0
covering_fw_lock = thr.Lock()
covering_bw_lock = thr.Lock()
t_fw_done_lock = thr.Lock()
t_bw_done_lock = thr.Lock()
center_lock = thr.Lock()
def search_f():
nonlocal queue_fw, t_bw_done, center, dists_fw, edge_to_fw, covering_fw, t_fw_done, covering_bw, iter_f
while not queue_fw.empty():
t_bw_done_lock.acquire()
if t_bw_done:
t_bw_done_lock.release()
break
t_bw_done_lock.release()
# print('iter f {}'.format(iter_f))
# iter_f += 1
# Forward step
current_node_f = queue_fw.get()[0]
covering_fw_lock.acquire()
covering_fw[current_node_f] = True
covering_fw_lock.release()
covering_bw_lock.acquire()
if covering_bw.get(current_node_f, False):
covering_bw_lock.release()
center_lock.acquire()
center = current_node_f
center_lock.release()
t_fw_done = True
break
covering_bw_lock.release()
# next_edges = [x for x in current_node.incidentEdges if x.n_from == current_node]
for edge in current_node_f.incidentEdges:
if current_node_f == edge.n_from:
if not covering_fw.get(edge.n_to.id, False) and dists_fw[edge.n_to.id] == -1 or \
dists_fw[edge.n_to.id] > dists_fw[current_node_f.id] + edge.get_weight():
dists_fw[edge.n_to.id] = dists_fw[
current_node_f.id] + edge.get_weight()
edge_to_fw.update({edge.n_to: edge})
queue_fw.update(edge.n_to, dists_fw[edge.n_to.id])
else:
if not covering_fw.get(edge.n_from.id, False) and dists_fw[edge.n_from.id] == -1 or \
dists_fw[edge.n_from.id] > dists_fw[current_node_f.id] + edge.get_weight():
dists_fw[edge.n_from.id] = dists_fw[
current_node_f.id] + edge.get_weight()
edge_to_fw.update({edge.n_from: edge})
queue_fw.update(edge.n_from, dists_fw[edge.n_from.id])
pass
def search_b():
nonlocal queue_bw, t_bw_done, center, dists_bw, edge_to_bw, covering_fw, t_fw_done, covering_bw, iter_b
while not queue_bw.empty():
t_fw_done_lock.acquire()
if t_fw_done:
t_fw_done_lock.release()
break
t_fw_done_lock.release()
# print('iter b {}'.format(iter_b))
# iter_b += 1
# Backward step
current_node_b = queue_bw.get()[0]
covering_bw_lock.acquire()
covering_bw[current_node_b] = True
covering_bw_lock.release()
covering_fw_lock.acquire()
if covering_fw.get(current_node_b, False):
covering_fw_lock.release()
center_lock.acquire()
center = current_node_b
center_lock.release()
t_bw_done = True
break
covering_fw_lock.release()
# next_edges = [x for x in current_node.incidentEdges if x.n_to == current_node]
for edge in current_node_b.incidentEdges:
if current_node_b == edge.n_to:
if not covering_fw.get(edge.n_from.id, False) and dists_bw[edge.n_from.id] == -1 or \
dists_bw[edge.n_from.id] > dists_bw[current_node_b.id] + edge.get_weight():
dists_bw[edge.n_from.id] = dists_bw[
current_node_b.id] + edge.get_weight()
edge_to_bw.update({edge.n_from: edge})
queue_bw.update(edge.n_from, dists_bw[edge.n_from.id])
else:
if not covering_fw.get(edge.n_to.id, False) and dists_bw[edge.n_to.id] == -1 or \
dists_bw[edge.n_to.id] > dists_bw[current_node_b.id] + edge.get_weight():
dists_bw[edge.n_to.id] = dists_bw[
current_node_b.id] + edge.get_weight()
edge_to_bw.update({edge.n_to: edge})
queue_bw.update(edge.n_to, dists_bw[edge.n_to.id])
pass
# def calculate_async():
# t_forward = thr.Thread(target=search_f)
# t_backward = thr.Thread(target=search_b)
#
# t_forward.run()
# t_backward.run()
#
# calc_thread = thr.Thread(target=calculate_async)
# calc_thread.join()
t_forward = thr.Thread(target=search_f)
t_backward = thr.Thread(target=search_b)
t_forward.start()
t_backward.start()
time_start = time.time()
t_forward.join(timeout=0)
t_backward.join(timeout=0)
while not t_fw_done and not t_bw_done:
pass
if center is None:
return -1, [], -1
path_fw = list()
current_node = center
while current_node != node_from:
if current_node == edge_to_fw[current_node].n_to:
current_node = edge_to_fw[current_node].n_from
path_fw.append(current_node)
else:
current_node = edge_to_fw[current_node].n_to
path_fw.append(current_node)
current_node = center
path_bw = list()
while current_node != node_to:
if current_node == edge_to_bw[current_node].n_from:
current_node = edge_to_bw[current_node].n_to
path_bw.append(current_node)
else:
current_node = edge_to_bw[current_node].n_from
path_bw.append(current_node)
path = path_fw[::-1]
path.append(center)
path += path_bw
time_end = time.time()
return dists_fw[center.id] + dists_bw[center.id], [path
], time_end - time_start
def bidirectional_alt(graph, node_from, node_to):
if not isinstance(graph, classes.Graph.Graph):
raise IOError("Wrong graph type")
if not isinstance(node_from, classes.Node.Node):
raise IOError("Wrong node_from type")
if not isinstance(node_to, classes.Node.Node):
raise IOError("Wrong node_to type")
# def heuristic(nf, t, s):
# result = math.sqrt(math.pow((nf.x - t.x), 2) + math.pow((nf.y - t.y), 2)) - \
# math.sqrt(math.pow((nf.x - s.x), 2) + math.pow((nf.y - s.y), 2))
# return result / 2
def heuristic(nf, t, s):
result1 = -1
for i in range(16):
temp1 = abs(nf.dist_to_mark[i] - t.dist_to_mark[i])
# temp2 = abs(nf.dist_to_mark[i] - s.dist_to_mark[i])
if temp1 > result1:
result1 = temp1
result2 = -1
for i in range(16):
temp2 = abs(nf.dist_to_mark[i] - s.dist_to_mark[i])
# temp2 = abs(nf.dist_to_mark[i] - s.dist_to_mark[i])
if temp2 > result2:
result2 = temp2
return (result1 - result2) / 2
time_start = time.time()
dists_fw = {a: -1 for a in graph.nodes}
dists_fw[node_from.id] = 0
dists_bw = {a: -1 for a in graph.nodes}
dists_bw[node_to.id] = 0
edge_to_fw = dict()
edge_to_bw = dict()
covering_fw = dict()
covering_bw = dict()
queue_fw = PQ.PriorityQueueByDict()
queue_fw.update(node_from)
queue_bw = PQ.PriorityQueueByDict()
queue_bw.update(node_to)
center = None
centers_amount = 0
while not queue_fw.empty() and not queue_bw.empty():
# Forward step
current_node = queue_fw.get()[0]
covering_fw.update({current_node: True})
if covering_bw.get(current_node, False):
if center is None or dists_fw[current_node.id] + dists_bw[
current_node.id] < dists_fw[center.id] + dists_bw[
center.id]:
center = current_node
centers_amount += 1
if centers_amount > 20:
break
# next_edges = [x for x in current_node.incidentEdges if x.n_from == current_node]
for edge in current_node.incidentEdges:
if current_node == edge.n_from:
if not covering_fw.get(edge.n_to, False) and dists_fw[
edge.n_to.id] == -1 or dists_fw[
edge.n_to.id] > dists_fw[
current_node.id] + edge.get_weight():
dists_fw[edge.n_to.id] = dists_fw[
current_node.id] + edge.get_weight()
edge_to_fw.update({edge.n_to: edge})
queue_fw.update(
edge.n_to, dists_fw[edge.n_to.id] +
heuristic(edge.n_to, node_to, node_from))
else:
if not covering_fw.get(edge.n_from, False) and dists_fw[
edge.n_from.id] == -1 or dists_fw[
edge.n_from.id] > dists_fw[
current_node.id] + edge.get_weight():
dists_fw[edge.n_from.id] = dists_fw[
current_node.id] + edge.get_weight()
edge_to_fw.update({edge.n_from: edge})
queue_fw.update(
edge.n_from, dists_fw[edge.n_from.id] +
heuristic(edge.n_from, node_to, node_from))
# Backward step
current_node = queue_bw.get()[0]
covering_bw.update({current_node: True})
if covering_fw.get(current_node, False):
if center is None or dists_fw[current_node.id] + dists_bw[
current_node.id] < dists_fw[center.id] + dists_bw[
center.id]:
center = current_node
centers_amount += 1
if centers_amount > 20:
break
# next_edges = [x for x in current_node.incidentEdges if x.n_to == current_node]
for edge in current_node.incidentEdges:
if current_node == edge.n_to:
if not covering_bw.get(edge.n_from, False) and dists_bw[
edge.n_from.id] == -1 or dists_bw[
edge.n_from.id] > dists_bw[
current_node.id] + edge.get_weight():
dists_bw[edge.n_from.id] = dists_bw[
current_node.id] + edge.get_weight()
edge_to_bw.update({edge.n_from: edge})
queue_bw.update(
edge.n_from, dists_bw[edge.n_from.id] +
heuristic(edge.n_from, node_from, node_to))
else:
if not covering_bw.get(edge.n_to, False) and dists_bw[
edge.n_to.id] == -1 or dists_bw[
edge.n_to.id] > dists_bw[
current_node.id] + edge.get_weight():
dists_bw[edge.n_to.id] = dists_bw[
current_node.id] + edge.get_weight()
edge_to_bw.update({edge.n_to: edge})
queue_bw.update(
edge.n_to, dists_bw[edge.n_to.id] +
heuristic(edge.n_to, node_from, node_to))
if center is None:
return -1, [], -1
path_fw = list()
current_node = center
while current_node != node_from:
if current_node == edge_to_fw[current_node].n_to:
current_node = edge_to_fw[current_node].n_from
path_fw.append(current_node)
else:
current_node = edge_to_fw[current_node].n_to
path_fw.append(current_node)
current_node = center
path_bw = list()
while current_node != node_to:
if current_node == edge_to_bw[current_node].n_from:
current_node = edge_to_bw[current_node].n_to
path_bw.append(current_node)
else:
current_node = edge_to_bw[current_node].n_from
path_bw.append(current_node)
path = path_fw[::-1]
path.append(center)
path += path_bw
time_end = time.time()
return dists_fw[center.id] + dists_bw[center.id], [path
], time_end - time_start
def alt(graph, node_from, node_to, k=16):
if not isinstance(graph, classes.Graph.Graph):
raise IOError("Wrong graph type")
if not isinstance(node_from, classes.Node.Node):
raise IOError("Wrong node_from type")
if not isinstance(node_to, classes.Node.Node):
raise IOError("Wrong node_to type")
def heuristic(nf, nt):
result = -1
for i in range(16):
temp = abs(nt.dist_to_mark[i] - nf.dist_to_mark[i])
if temp > result:
result = temp
return result
time_start = time.time()
# Инициализируем расстояния
dists = {a: -1 for a in graph.nodes}
dists[node_from.id] = 0
# Init edges that leads to a node
edges_to = dict()
edges_to.update({node_from.id: []})
# Init queue for BFS
queue = PQ.PriorityQueueByDict()
queue.update(node_from)
is_node_done = dict()
# Ищем расстояния до точек
while not queue.empty():
# Поиск минимального (по расстоянию) элемента
current_node = queue.get()[0]
if current_node == node_to:
break
is_node_done[current_node] = True
# Считаем пути до следующих вершин
# next_edges = [x for x in current_node.incidentEdges if x.n_from == current_node]
# next_edges = [x for x in current_node.incidentEdges]
for x in current_node.incidentEdges:
# Refresh distances
# if we haven't been to node
if current_node == x.n_from:
if not is_node_done.get(x.n_to, False) and dists[
x.n_to.id] == -1 or dists[x.n_to.id] > dists[
current_node.id] + x.get_weight():
# queue.append(x.n_to)
dists[x.n_to.id] = dists[current_node.id] + x.get_weight()
edges_to.update({x.n_to.id: [x]})
queue.update(x.n_to,
dists[x.n_to.id] + heuristic(x.n_to, node_to))
else:
if not is_node_done.get(x.n_from, False) and dists[
x.n_from.id] == -1 or dists[x.n_from.id] > dists[
current_node.id] + x.get_weight():
# queue.append(x.n_to)
dists[
x.n_from.id] = dists[current_node.id] + x.get_weight()
edges_to.update({x.n_from.id: [x]})
queue.update(
x.n_from,
dists[x.n_from.id] + heuristic(x.n_from, node_to))
# Итоговое расстояние и переменная для путей (might be more than one)
length = dists[node_to.id]
print('Dijkstra\'s main done in {} sec'.format(time.time() - time_start))
# Если добраться невозможно
if length == -1:
return -1, [], -1
# Searching for all paths
paths = list()
paths.append([])
path_n = 0
stacks = list()
stack = [node_to]
stacks.append(stack)
count = 0
while stacks:
current_node = stacks[0].pop()
paths[path_n].append(current_node)
amount_of_ways_from = len(edges_to[current_node.id])
if amount_of_ways_from > 0:
if current_node == edges_to[current_node.id][0].n_to:
stacks[0].append(edges_to[current_node.id][0].n_from)
else:
stacks[0].append(edges_to[current_node.id][0].n_to)
else:
stacks.pop(0)
path_n += 1
count += 1
time_end = time.time()
final_time = time_end - time_start
print('Full Dijkstra done in {} sec'.format(final_time))
for i in range(count):
paths[i] = paths[i][::-1]
return dists[node_from.id], paths, final_time
def bidirectional_dijkstra_un(graph, node_from, node_to):
if not isinstance(graph, classes.Graph.Graph):
raise IOError("Wrong graph type")
if not isinstance(node_from, classes.Node.Node):
raise IOError("Wrong node_from type")
if not isinstance(node_to, classes.Node.Node):
raise IOError("Wrong node_to type")
time_start = time.time()
dists_fw = {a: -1 for a in graph.nodes}
dists_fw[node_from.id] = 0
dists_bw = {a: -1 for a in graph.nodes}
dists_bw[node_to.id] = 0
edge_to_fw = dict()
edge_to_bw = dict()
covering_fw = dict()
covering_bw = dict()
queue_fw = PQ.PriorityQueueByDict()
queue_fw.update(node_from)
queue_bw = PQ.PriorityQueueByDict()
queue_bw.update(node_to)
center = None
centers_amount = 0
while not queue_fw.empty() and not queue_bw.empty():
# Forward step
current_node = queue_fw.get()[0]
covering_fw[current_node] = True
if covering_bw.get(current_node, False):
if center is None or dists_fw[current_node.id] + dists_bw[
current_node.id] < dists_fw[center.id] + dists_bw[
center.id]:
center = current_node
centers_amount += 1
if centers_amount > 20:
break
# next_edges = [x for x in current_node.incidentEdges if x.n_from == current_node]
for edge in current_node.incidentEdges:
if current_node == edge.n_from:
if not covering_fw.get(edge.n_to.id, False) and dists_fw[
edge.n_to.id] == -1 or dists_fw[
edge.n_to.id] > dists_fw[
current_node.id] + edge.get_weight():
dists_fw[edge.n_to.id] = dists_fw[
current_node.id] + edge.get_weight()
edge_to_fw.update({edge.n_to: edge})
queue_fw.update(edge.n_to, dists_fw[edge.n_to.id])
else:
if not covering_fw.get(edge.n_from.id, False) and dists_fw[
edge.n_from.id] == -1 or dists_fw[
edge.n_from.id] > dists_fw[
current_node.id] + edge.get_weight():
dists_fw[edge.n_from.id] = dists_fw[
current_node.id] + edge.get_weight()
edge_to_fw.update({edge.n_from: edge})
queue_fw.update(edge.n_from, dists_fw[edge.n_from.id])
# Backward step
current_node = queue_bw.get()[0]
covering_bw[current_node] = True
if covering_fw.get(current_node, False):
if center is None or dists_fw[current_node.id] + dists_bw[
current_node.id] < dists_fw[center.id] + dists_bw[
center.id]:
center = current_node
centers_amount += 1
if centers_amount > 20:
break
# next_edges = [x for x in current_node.incidentEdges if x.n_to == current_node]
for edge in current_node.incidentEdges:
if current_node == edge.n_to:
if not covering_fw.get(edge.n_from.id, False) and dists_bw[
edge.n_from.id] == -1 or dists_bw[
edge.n_from.id] > dists_bw[
current_node.id] + edge.get_weight():
dists_bw[edge.n_from.id] = dists_bw[
current_node.id] + edge.get_weight()
edge_to_bw.update({edge.n_from: edge})
queue_bw.update(edge.n_from, dists_bw[edge.n_from.id])
else:
if not covering_fw.get(edge.n_to.id, False) and dists_bw[
edge.n_to.id] == -1 or dists_bw[
edge.n_to.id] > dists_bw[
current_node.id] + edge.get_weight():
dists_bw[edge.n_to.id] = dists_bw[
current_node.id] + edge.get_weight()
edge_to_bw.update({edge.n_to: edge})
queue_bw.update(edge.n_to, dists_bw[edge.n_to.id])
pass
if center is None:
return -1, [], -1
path_fw = list()
current_node = center
while current_node != node_from:
if current_node == edge_to_fw[current_node].n_to:
current_node = edge_to_fw[current_node].n_from
path_fw.append(current_node)
else:
current_node = edge_to_fw[current_node].n_to
path_fw.append(current_node)
current_node = center
path_bw = list()
while current_node != node_to:
if current_node == edge_to_bw[current_node].n_from:
current_node = edge_to_bw[current_node].n_to
path_bw.append(current_node)
else:
current_node = edge_to_bw[current_node].n_from
path_bw.append(current_node)
path = path_fw[::-1]
path.append(center)
path += path_bw
time_end = time.time()
return dists_fw[center.id] + dists_bw[center.id], [path
], time_end - time_start
def dijkstra_early_stop_way_un(graph, node_from, node_to):
if not isinstance(graph, classes.Graph.Graph):
raise IOError("Wrong graph type")
if not isinstance(node_from, classes.Node.Node):
raise IOError("Wrong node_from type")
if not isinstance(node_to, classes.Node.Node):
raise IOError("Wrong node_to type")
time_start = time.time()
# Инициализируем расстояния
dists = {a: -1 for a in graph.nodes}
is_node_done = {a: False for a in graph.nodes}
dists[node_from.id] = 0
# Init edges that leads to a node
edges_to = dict()
edges_to.update({node_from.id: []})
# Init queue for BFS
queue = PQ.PriorityQueueByDict()
queue.update(node_from)
# Ищем расстояния до точек
while not queue.empty():
# Поиск минимального (по расстоянию) элемента
# minimum = -1
# current_node = None
# for i in queue:
# if minimum == -1 or dists[i.id] <= minimum:
# current_node = i
# minimum = dists[i.id]
current_node = queue.get()[0]
if current_node == node_to:
break
is_node_done[current_node] = True
# Считаем пути до следующих вершин
# next_edges = [x for x in current_node.incidentEdges]
# next_edges = [x for x in current_node.incidentEdges]
# queue.remove(current_node)
for x in current_node.incidentEdges:
# Refresh distances
# if we haven't been to node
if x.n_from == current_node:
if not is_node_done[x.n_to.id] and dists[
x.n_to.id] == -1 or dists[x.n_to.id] > dists[
current_node.id] + x.get_weight():
dists[x.n_to.id] = dists[current_node.id] + x.get_weight()
queue.update(x.n_to, dists[x.n_to.id])
edges_to.update({x.n_to.id: [x]})
else:
if not is_node_done[x.n_from.id] and dists[
x.n_from.id] == -1 or dists[x.n_from.id] > dists[
current_node.id] + x.get_weight():
dists[
x.n_from.id] = dists[current_node.id] + x.get_weight()
queue.update(x.n_from, dists[x.n_from.id])
edges_to.update({x.n_from.id: [x]})
# Итоговое расстояние и переменная для путей (might be more than one)
length = dists[node_to.id]
print('Dijkstra\'s main done in {} sec'.format(time.time() - time_start))
final_time = -1
# Если добраться невозможно
if length == -1:
return -1, [], final_time
# Searching for all paths
paths = list()
paths.append([])
path_n = 0
stacks = list()
stack = [node_to]
stacks.append(stack)
count = 0
while stacks:
current_node = stacks[0].pop()
paths[path_n].append(current_node)
amount_of_ways_from = len(edges_to[current_node.id])
if amount_of_ways_from > 0:
# for i in range(amount_of_ways_from - 1):
# copy = stacks[0].copy()
# copy.append(edges_to[current_node.id][i + 1].n_from)
# stacks.append(copy)
# copy = paths[path_n].copy()
# # copy.append(edges_to[current_node][i + 1].n_from)
# paths.append(copy)
if current_node == edges_to[current_node.id][0].n_to:
stacks[0].append(edges_to[current_node.id][0].n_from)
else:
stacks[0].append(edges_to[current_node.id][0].n_to)
else:
stacks.pop(0)
path_n += 1
count += 1
time_end = time.time()
final_time = time_end - time_start
print('Full Dijkstra done in {} sec'.format(final_time))
for i in range(count):
paths[i] = paths[i][::-1]
return dists[node_from.id], paths, final_time
def alt_un_sc(graph, node_from, node_to, k = 16):
if not isinstance(graph, classes.Graph.Graph):
raise IOError("Wrong graph type")
if not isinstance(node_from, classes.Node.Node):
raise IOError("Wrong node_from type")
if not isinstance(node_to, classes.Node.Node):
raise IOError("Wrong node_to type")
def heuristic(nf, nt):
result = -1
for i in range(16):
temp = abs(nt.dist_to_mark[i] - nf.dist_to_mark[i])
if temp > result:
result = temp
return result
time_start = time.time()
# Инициализируем расстояния
dists = {a: -1 for a in graph.nodes}
is_node_done = {a: False for a in graph.nodes}
dists[node_from.id] = 0
# Init edges that leads to a node
edges_to = dict()
edges_to.update({node_from.id: []})
node_from_s1 = node_from
node_from_s2 = node_from
# Init queue for BFS
queue = PQ.PriorityQueueByDict()
# Get out of a node_from shortcut if it needs
if node_from.hidden_in:
try:
node_from_s1, weight_s1 = node_from.hidden_in.unpack_until(node_from, reverse=False)
node_from_s1 = node_from_s1[0]
node_from_s2, weight_s2 = node_from.hidden_in.unpack_until(node_from, reverse=True)
node_from_s2 = node_from_s2[0]
queue.update(node_from_s1, weight_s1)
queue.update(node_from_s2, weight_s2)
except IndexError:
node_from_s1 = node_from.hidden_in.n_from
node_from_s2 = node_from.hidden_in.n_to
queue.update(node_from_s1)
queue.update(node_from_s2)
print('Warning! Not the whole path unpacked cause of Index Error')
else:
queue.update(node_from)
last_sc_node = None
# May be None
last_sc = None
# Ищем расстояния до точек
while queue:
# Поиск минимального (по расстоянию) элемента
current_node = queue.get()[0]
if current_node == node_to:
last_sc_node = current_node
break
is_node_done[current_node] = True
found = False
for x in current_node.incidentShortcuts:
# if x is a shortcut and it contains the node_to then search only by edges
if isinstance(x, classes.Edge.Shortcut):
if x.is_containing_node(node_to):
last_sc_node = current_node
last_sc = x
found = True
break
# Refresh distances
# if we haven't been to node
if current_node == x.n_from:
if not is_node_done.get(x.n_to, False) and dists[x.n_to.id] == -1 or dists[x.n_to.id] > dists[current_node.id] + x.get_weight():
# queue.append(x.n_to)
dists[x.n_to.id] = dists[current_node.id] + x.get_weight()
edges_to.update({x.n_to.id: [x]})
queue.update(x.n_to, dists[x.n_to.id] + heuristic(x.n_to, node_to))
else:
if not is_node_done.get(x.n_from, False) and dists[x.n_from.id] == -1 or dists[x.n_from.id] > dists[current_node.id] + x.get_weight():
# queue.append(x.n_to)
dists[x.n_from.id] = dists[current_node.id] + x.get_weight()
edges_to.update({x.n_from.id: [x]})
queue.update(x.n_from, dists[x.n_from.id] + heuristic(x.n_from, node_to))
if found:
break
# Итоговое расстояние и переменная для путей (might be more than one)
if last_sc_node is None:
last_sc_node = node_to
length = dists[last_sc_node.id]
print('Dijkstra\'s main done in {} sec'.format(time.time() - time_start))
# Если добраться невозможно
if length == -1:
return -1, [], -1
path = list()
path.append(last_sc_node)
current_node = last_sc_node
side_node = None
while current_node != node_from and current_node != node_from_s1 and current_node != node_from_s2:
sc = edges_to[current_node.id][0]
# Set next 'current'
if current_node == sc.n_from:
current_node = sc.n_to
else:
current_node = sc.n_from
# Build the path
if isinstance(sc, classes.Edge.Shortcut):
if current_node == sc.n_to:
path += sc.unpack()[:-1:1]
else:
path += sc.unpack()[-1::-1]
else:
if current_node == sc.n_from:
path.append(sc.n_from)
else:
path.append(sc.n_to)
if current_node == node_from_s1:
side_node = node_from_s1
elif current_node == node_from_s2:
side_node = node_from_s2
# DONT FORGET TO UNPACK LAST
if last_sc is not None:
if last_sc_node == last_sc.n_from:
try:
path_l, weight_l = last_sc.unpack_until(node_to)
path = path_l + path
length += weight_l
except IndexError:
print('Warning! Not the whole path unpacked cause of Index Error')
else:
try:
path_l, weight_l = last_sc.unpack_until(node_to, reverse=True)
path = path_l[::-1] + path
length += weight_l
except IndexError:
print('Warning! Not the whole path unpacked cause of Index Error')
# DONT FORGET TO UNPACK FIRST
if node_from != node_from_s1 and node_from != node_from_s2:
nodes_s1, w1 = node_from.hidden_in.unpack_until(node_from)
nodes_s2, w2 = node_from.hidden_in.unpack_until(node_from, reverse=True)
if side_node == node_from_s1:
length += w1
path += nodes_s1[::-1]
elif node_from == node_from_s2:
path += nodes_s2[::-1]
length += w2
time_end = time.time()
final_time = time_end - time_start
print('Full Dijkstra done in {} sec'.format(final_time))
path = path[::-1]
return dists[node_from.id], [path], final_time
def bidirectional_astar(graph, node_from, node_to):
if not isinstance(graph, classes.Graph.Graph):
raise IOError("Wrong graph type")
if not isinstance(node_from, classes.Node.Node):
raise IOError("Wrong node_from type")
if not isinstance(node_to, classes.Node.Node):
raise IOError("Wrong node_to type")
def heuristic(nf, nt):
result = math.sqrt(
math.pow((nf.x - nt.x), 2) +
math.pow((nf.y - nt.y), 2)
)
return result
time_start = time.time()
dists_fw = {a: -1 for a in graph.nodes}
dists_fw[node_from.id] = 0
dists_bw = {a: -1 for a in graph.nodes}
dists_bw[node_to.id] = 0
edge_to_fw = dict()
edge_to_bw = dict()
covering_fw = dict()
covering_bw = dict()
queue_fw = PQ.PriorityQueueByDict()
queue_fw.update(node_from)
queue_bw = PQ.PriorityQueueByDict()
queue_bw.update(node_to)
center = None
while not queue_fw.empty() and not queue_bw.empty():
# Forward step
current_node = queue_fw.get()[0]
covering_fw.update({current_node: True})
if covering_bw.get(current_node, False):
center = current_node
break
next_edges = [x for x in current_node.incidentEdges if x.n_from == current_node]
for edge in next_edges:
if dists_fw[edge.n_to.id] == -1 or dists_fw[edge.n_to.id] > dists_fw[current_node.id] + edge.get_weight():
dists_fw[edge.n_to.id] = dists_fw[current_node.id] + edge.get_weight()
edge_to_fw.update({edge.n_to: edge})
queue_fw.update(edge.n_to, dists_fw[edge.n_to.id] + heuristic(edge.n_to, node_to))
# Backward step
current_node = queue_bw.get()[0]
covering_bw.update({current_node: True})
if covering_fw.get(current_node, False):
center = current_node
break
next_edges = [x for x in current_node.incidentEdges if x.n_to == current_node]
for edge in next_edges:
if dists_bw[edge.n_from.id] == -1 or dists_bw[edge.n_from.id] > dists_bw[current_node.id] + edge.get_weight():
dists_bw[edge.n_from.id] = dists_bw[current_node.id] + edge.get_weight()
edge_to_bw.update({edge.n_from: edge})
queue_bw.update(edge.n_from, dists_bw[edge.n_from.id] + heuristic(edge.n_from, node_from))
pass
if center is None:
return -1, [], -1
path_fw = list()
current_node = center
while current_node != node_from:
current_node = edge_to_fw[current_node].n_from
path_fw.append(current_node)
current_node = center
path_bw = list()
while current_node != node_to:
current_node = edge_to_bw[current_node].n_to
path_bw.append(current_node)
path = path_fw[::-1]
path.append(center)
path += path_bw
time_end = time.time()
return dists_fw[center.id] + dists_bw[center.id], [path], time_end - time_start