praca_magisterska/algorithms/a_star.py

import fibheap


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

    pred = dict()
    node_heap = fibheap.makefheap()
    fibheap.fheappush(node_heap, 0, start)

    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 node_heap.num_nodes > 0:
        # current = list(open_set)[0]
        # for k in open_set:
        #     if f_score[k] < f_score[current]:
        #         current = k
        current = fibheap.fheappop(node_heap)

        if current[1] == goal:
            return return_path_and_weight(pred, goal, start)

        for neighbor in point_set[current[1]]:
            tentative_g_score = g_score[current[1]]
            if not is_directed:
                if current[1] > neighbor:
                    tentative_g_score += graph[(current[1], neighbor)]
                else:
                    tentative_g_score += graph[(neighbor, current[1])]
            else:
                tentative_g_score += graph[(current[1], neighbor)]
            if tentative_g_score < g_score[neighbor]:
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = g_score[neighbor] + h(neighbor, goal, graph)
                fibheap.fheappush(node_heap, f_score[neighbor], neighbor)
                pred[neighbor] = current[1]


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