102 lines
3.0 KiB
Python
102 lines
3.0 KiB
Python
import fibheap
|
|
|
|
|
|
def heuristic_cost(start, goal, graph):
|
|
return 0
|
|
|
|
|
|
def a_star_algorithm(graph, start, goal, h=heuristic_cost, is_directed=False):
|
|
def return_path_and_weight(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 not is_directed:
|
|
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])]
|
|
else:
|
|
weight += graph[(shortest_path[k], shortest_path[k + 1])]
|
|
return shortest_path, weight
|
|
|
|
point_set = dict()
|
|
for arc in g.keys():
|
|
point_set[arc[0]] = []
|
|
point_set[arc[1]] = []
|
|
|
|
for arc in graph.keys():
|
|
point_set[arc[0]].append(arc[1])
|
|
if not is_directed:
|
|
point_set[arc[1]].append(arc[0])
|
|
|
|
pred = dict()
|
|
node_heap = fibheap.makefheap()
|
|
fibheap.fheappush(node_heap, 0, start)
|
|
|
|
g_score = {k: float('inf') for k in point_set.keys()}
|
|
g_score[start] = 0
|
|
|
|
f_score = {k: float('inf') for k in point_set.keys()}
|
|
f_score[start] = h(start, goal, graph)
|
|
|
|
while node_heap.num_nodes > 0:
|
|
# current = list(open_set)[0]
|
|
# for k in open_set:
|
|
# if f_score[k] < f_score[current]:
|
|
# current = k
|
|
current = fibheap.fheappop(node_heap)
|
|
|
|
if current[1] == goal:
|
|
return return_path_and_weight(pred, goal, start)
|
|
|
|
for neighbor in point_set[current[1]]:
|
|
tentative_g_score = g_score[current[1]]
|
|
if not is_directed:
|
|
if current[1] > neighbor:
|
|
tentative_g_score += graph[(current[1], neighbor)]
|
|
else:
|
|
tentative_g_score += graph[(neighbor, current[1])]
|
|
else:
|
|
tentative_g_score += graph[(current[1], neighbor)]
|
|
if tentative_g_score < g_score[neighbor]:
|
|
g_score[neighbor] = tentative_g_score
|
|
f_score[neighbor] = g_score[neighbor] + h(neighbor, goal, graph)
|
|
fibheap.fheappush(node_heap, f_score[neighbor], neighbor)
|
|
pred[neighbor] = current[1]
|
|
|
|
|
|
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,
|
|
(9, 1): 8
|
|
}
|
|
|
|
g2 = {
|
|
(4, 1): 6,
|
|
(1, 3): 4,
|
|
(1, 2): 2,
|
|
(2, 4): 5,
|
|
(3, 4): 1,
|
|
(5, 3): 1
|
|
}
|
|
|
|
print(a_star_algorithm(g, 7, 10, heuristic_cost))
|
|
print(a_star_algorithm(g2, 1, 4, heuristic_cost, is_directed=True))
|
|
print(a_star_algorithm(g2, 1, 5, heuristic_cost, is_directed=True))
|