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))