praca_magisterska/algorithms/a_star.py

99 lines
2.9 KiB
Python

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])
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 not is_directed:
if current > neighbor:
tentative_g_score += graph[(current, neighbor)]
else:
tentative_g_score += graph[(neighbor, current)]
else:
tentative_g_score += graph[(current, neighbor)]
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,
(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))