80 lines
2.3 KiB
Python
80 lines
2.3 KiB
Python
def heuristic_cost(start, goal, graph):
|
|
return 0
|
|
|
|
|
|
def a_star_algorithm(graph, point_set, start, goal, h=heuristic_cost):
|
|
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 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
|
|
|
|
for edge in graph.keys():
|
|
point_set[edge[0]].append(edge[1])
|
|
point_set[edge[1]].append(edge[0])
|
|
|
|
open_set = set()
|
|
open_set.add(start)
|
|
|
|
came_from = {}
|
|
|
|
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 len(open_set) > 0:
|
|
current = list(open_set)[0]
|
|
for k in open_set:
|
|
if f_score[k] < f_score[current]:
|
|
current = k
|
|
if current == goal:
|
|
return return_path_and_weight(came_from, current, start)
|
|
open_set.remove(current)
|
|
for neighbor in point_set[current]:
|
|
tentative_g_score = g_score[current]
|
|
if current > neighbor:
|
|
tentative_g_score += graph[(current, neighbor)]
|
|
else:
|
|
tentative_g_score += graph[(neighbor, current)]
|
|
if tentative_g_score < g_score[neighbor]:
|
|
came_from[neighbor] = current
|
|
g_score[neighbor] = tentative_g_score
|
|
f_score[neighbor] = g_score[neighbor] + h(neighbor, goal, graph)
|
|
if neighbor not in open_set:
|
|
open_set.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(a_star_algorithm(g, v, 7, 10, heuristic_cost))
|