151 lines
5.5 KiB
Python
151 lines
5.5 KiB
Python
def heuristic_cost(start, goal, graph):
|
|
return 0
|
|
|
|
|
|
def bidirectional_algorithm(graph, point_set, start, goal, h=heuristic_cost):
|
|
def return_path_and_weight_front(c_f, c, s):
|
|
current_node = c_f[c]
|
|
shortest_path = [c, current_node]
|
|
while current_node != s:
|
|
current_node = c_f[current_node]
|
|
shortest_path.append(current_node)
|
|
weight = 0
|
|
for k in range(len(shortest_path) - 1):
|
|
if shortest_path[k] > shortest_path[k+1]:
|
|
weight += graph[(shortest_path[k], shortest_path[k+1])]
|
|
else:
|
|
weight += graph[(shortest_path[k + 1], shortest_path[k])]
|
|
return shortest_path, weight
|
|
|
|
def return_path_and_weight_back(c_f, c, s):
|
|
current_node = c_f[c]
|
|
shortest_path = [c, current_node]
|
|
while current_node != s:
|
|
current_node = c_f[current_node]
|
|
shortest_path.append(current_node)
|
|
weight = 0
|
|
shortest_path.reverse()
|
|
for k in range(len(shortest_path) - 1):
|
|
if shortest_path[k] > shortest_path[k+1]:
|
|
weight += graph[(shortest_path[k], shortest_path[k+1])]
|
|
else:
|
|
weight += graph[(shortest_path[k + 1], shortest_path[k])]
|
|
return shortest_path, weight
|
|
|
|
def return_path_and_weight_front_meet_back(c_f_f, c_f_b, c_f, s, g):
|
|
shortest_path_front, weight_front = return_path_and_weight_back(c_f_f, c_f, s)
|
|
shortest_path_back, weight_back = return_path_and_weight_back(c_f_b, c_f, g)
|
|
shortest_path_back.reverse()
|
|
return shortest_path_front + shortest_path_back[1:], weight_front + weight_back
|
|
|
|
def return_path_and_weight_back_meet_front(c_f_f, c_f_b, c_b, s, g):
|
|
shortest_path_front, weight_front = return_path_and_weight_back(c_f_f, c_b, s)
|
|
shortest_path_back, weight_back = return_path_and_weight_back(c_f_b, c_b, g)
|
|
shortest_path_back.reverse()
|
|
return shortest_path_front + shortest_path_back[1:], weight_front + weight_back
|
|
|
|
for edge in graph.keys():
|
|
point_set[edge[0]].append(edge[1])
|
|
point_set[edge[1]].append(edge[0])
|
|
open_set_front = set()
|
|
open_set_front.add(start)
|
|
|
|
open_set_back = set()
|
|
open_set_back.add(goal)
|
|
|
|
came_from_front = {}
|
|
came_from_back = {}
|
|
|
|
g_score_front = {k: float('inf') for k in point_set.keys()}
|
|
g_score_front[start] = 0
|
|
|
|
g_score_back = {k: float('inf') for k in point_set.keys()}
|
|
g_score_back[goal] = 0
|
|
|
|
f_score_front = {k: float('inf') for k in point_set.keys()}
|
|
f_score_front[start] = h(start, goal, graph)
|
|
|
|
f_score_back = {k: float('inf') for k in point_set.keys()}
|
|
f_score_back[goal] = h(goal, start, graph)
|
|
|
|
while len(open_set_front) > 0 and len(open_set_back) > 0:
|
|
current_front = list(open_set_front)[0]
|
|
current_back = list(open_set_back)[0]
|
|
|
|
for k in open_set_front:
|
|
if f_score_front[k] < f_score_front[current_front]:
|
|
current_front = k
|
|
|
|
for k in open_set_back:
|
|
if f_score_back[k] < f_score_back[current_back]:
|
|
current_back = k
|
|
|
|
if current_front == goal:
|
|
return return_path_and_weight_front(came_from_front, current_front, start)
|
|
|
|
if current_back == start:
|
|
return return_path_and_weight_back(came_from_back, current_back, goal)
|
|
|
|
if current_front in came_from_back.keys():
|
|
return return_path_and_weight_front_meet_back(came_from_front, came_from_back, current_front, start, goal)
|
|
|
|
if current_back in came_from_front.keys():
|
|
return return_path_and_weight_back_meet_front(came_from_front, came_from_back, current_back, start, goal)
|
|
|
|
open_set_front.remove(current_front)
|
|
open_set_back.remove(current_back)
|
|
|
|
for neighbor in point_set[current_front]:
|
|
tentative_g_score = g_score_front[current_front]
|
|
|
|
if current_front > neighbor:
|
|
tentative_g_score += graph[(current_front, neighbor)]
|
|
else:
|
|
tentative_g_score += graph[(neighbor, current_front)]
|
|
|
|
if tentative_g_score < g_score_front[neighbor]:
|
|
came_from_front[neighbor] = current_front
|
|
g_score_front[neighbor] = tentative_g_score
|
|
f_score_front[neighbor] = g_score_front[neighbor] + h(neighbor, goal, graph)
|
|
if neighbor not in open_set_front:
|
|
open_set_front.add(neighbor)
|
|
|
|
for neighbor in point_set[current_back]:
|
|
tentative_g_score = g_score_back[current_back]
|
|
|
|
if current_back > neighbor:
|
|
tentative_g_score += graph[(current_back, neighbor)]
|
|
else:
|
|
tentative_g_score += graph[(neighbor, current_back)]
|
|
|
|
if tentative_g_score < g_score_back[neighbor]:
|
|
came_from_back[neighbor] = current_back
|
|
g_score_back[neighbor] = tentative_g_score
|
|
f_score_back[neighbor] = g_score_back[neighbor] + h(neighbor, goal, graph)
|
|
if neighbor not in open_set_back:
|
|
open_set_back.add(neighbor)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
g = {
|
|
(2, 1): 3,
|
|
(3, 2): 2,
|
|
(5, 3): 1,
|
|
(9, 5): 5,
|
|
(10, 9): 4,
|
|
(9, 4): 3,
|
|
(4, 1): 4,
|
|
(7, 1): 6,
|
|
(3, 1): 4,
|
|
(6, 2): 3,
|
|
(8, 6): 8,
|
|
(8, 3): 2
|
|
}
|
|
|
|
v = dict()
|
|
for i in g.keys():
|
|
v[i[0]] = []
|
|
v[i[1]] = []
|
|
|
|
print(bidirectional_algorithm(g, v, 7, 8, heuristic_cost))
|