praca_magisterska/algorithms/bidirectional.py

def heuristic_cost(start, goal, graph):
    return 0


def bidirectional_algorithm(graph, point_set, start, goal, h=heuristic_cost):
    def return_path_and_weight_front(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
        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

    def return_path_and_weight_back(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

    def return_path_and_weight_front_meet_back(c_f_f, c_f_b, c_f, s, g):
        shortest_path_front, weight_front = return_path_and_weight_back(c_f_f, c_f, s)
        shortest_path_back, weight_back = return_path_and_weight_back(c_f_b, c_f, g)
        shortest_path_back.reverse()
        return shortest_path_front + shortest_path_back[1:], weight_front + weight_back

    def return_path_and_weight_back_meet_front(c_f_f, c_f_b, c_b, s, g):
        shortest_path_front, weight_front = return_path_and_weight_back(c_f_f, c_b, s)
        shortest_path_back, weight_back = return_path_and_weight_back(c_f_b, c_b, g)
        shortest_path_back.reverse()
        return shortest_path_front + shortest_path_back[1:], weight_front + weight_back

    for edge in graph.keys():
        point_set[edge[0]].append(edge[1])
        point_set[edge[1]].append(edge[0])
    open_set_front = set()
    open_set_front.add(start)

    open_set_back = set()
    open_set_back.add(goal)

    came_from_front = {}
    came_from_back = {}

    g_score_front = {k: float('inf') for k in point_set.keys()}
    g_score_front[start] = 0

    g_score_back = {k: float('inf') for k in point_set.keys()}
    g_score_back[goal] = 0

    f_score_front = {k: float('inf') for k in point_set.keys()}
    f_score_front[start] = h(start, goal, graph)

    f_score_back = {k: float('inf') for k in point_set.keys()}
    f_score_back[goal] = h(goal, start, graph)

    while len(open_set_front) > 0 and len(open_set_back) > 0:
        current_front = list(open_set_front)[0]
        current_back = list(open_set_back)[0]

        for k in open_set_front:
            if f_score_front[k] < f_score_front[current_front]:
                current_front = k

        for k in open_set_back:
            if f_score_back[k] < f_score_back[current_back]:
                current_back = k

        if current_front == goal:
            return return_path_and_weight_front(came_from_front, current_front, start)

        if current_back == start:
            return return_path_and_weight_back(came_from_back, current_back, goal)

        if current_front in came_from_back.keys():
            return return_path_and_weight_front_meet_back(came_from_front, came_from_back, current_front, start, goal)

        if current_back in came_from_front.keys():
            return return_path_and_weight_back_meet_front(came_from_front, came_from_back, current_back, start, goal)

        open_set_front.remove(current_front)
        open_set_back.remove(current_back)

        for neighbor in point_set[current_front]:
            tentative_g_score = g_score_front[current_front]

            if current_front > neighbor:
                tentative_g_score += graph[(current_front, neighbor)]
            else:
                tentative_g_score += graph[(neighbor, current_front)]

            if tentative_g_score < g_score_front[neighbor]:
                came_from_front[neighbor] = current_front
                g_score_front[neighbor] = tentative_g_score
                f_score_front[neighbor] = g_score_front[neighbor] + h(neighbor, goal, graph)
                if neighbor not in open_set_front:
                    open_set_front.add(neighbor)

        for neighbor in point_set[current_back]:
            tentative_g_score = g_score_back[current_back]

            if current_back > neighbor:
                tentative_g_score += graph[(current_back, neighbor)]
            else:
                tentative_g_score += graph[(neighbor, current_back)]

            if tentative_g_score < g_score_back[neighbor]:
                came_from_back[neighbor] = current_back
                g_score_back[neighbor] = tentative_g_score
                f_score_back[neighbor] = g_score_back[neighbor] + h(neighbor, goal, graph)
                if neighbor not in open_set_back:
                    open_set_back.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
    }

    v = dict()
    for i in g.keys():
        v[i[0]] = []
        v[i[1]] = []

    print(bidirectional_algorithm(g, v, 7, 10, heuristic_cost))