diff --git a/algorithms/a_star.py b/algorithms/a_star.py index e69de29..696cfe4 100644 --- a/algorithms/a_star.py +++ b/algorithms/a_star.py @@ -0,0 +1,59 @@ +def heuristic_cost(start, goal, graph): + return 0 + + +def a_star_algorithm(graph, point_set, start, goal, h=heuristic_cost): + 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 came_from, current + 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))