praca_magisterska/algorithms/a_star.py

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