136 lines
6.4 KiB
Python
136 lines
6.4 KiB
Python
import fibheap
|
|
import math
|
|
|
|
|
|
def heuristic_cost(node1, node2):
|
|
return 0
|
|
|
|
|
|
def heuristic_cost_manhattan(node1, node2):
|
|
part1 = (node1[0]-node2[0])**2
|
|
part1 = part1
|
|
part2 = (node1[1]-node2[1])**2
|
|
return math.sqrt(part1 + part2)
|
|
|
|
|
|
def bidirectional_algorithm(s, t, N, A, A_b, w, h):
|
|
if s == t:
|
|
return [s], 0
|
|
|
|
H_f = fibheap.makefheap()
|
|
H_b = fibheap.makefheap()
|
|
|
|
d_f = {}
|
|
d_b = {}
|
|
heap_node_connection_f = {}
|
|
heap_node_connection_b = {}
|
|
pred_f = {}
|
|
pred_b = {}
|
|
f_f = {}
|
|
f_b = {}
|
|
perm_f = {}
|
|
perm_b = {}
|
|
|
|
for i in N:
|
|
d_f[i] = float('inf')
|
|
d_b[i] = float('inf')
|
|
perm_f[i] = False
|
|
perm_b[i] = False
|
|
|
|
d_f[s] = 0
|
|
d_b[t] = 0
|
|
|
|
f_f[s] = d_f[s] + h(s, t)
|
|
f_b[t] = d_b[t] + h(t, s)
|
|
|
|
pred_f[s] = None
|
|
pred_b[t] = None
|
|
heap_node_connection_f[s] = fibheap.fheappush(H_f, 0, s)
|
|
heap_node_connection_b[s] = fibheap.fheappush(H_b, 0, t)
|
|
|
|
ending_node = s
|
|
success = False
|
|
while H_f.num_nodes > 0 and H_b.num_nodes > 0:
|
|
i_f = fibheap.fheappop(H_f)[1]
|
|
i_b = fibheap.fheappop(H_b)[1]
|
|
if f_f[i_f] <= f_b[i_b]:
|
|
perm_f[i_f] = True
|
|
heap_node_connection_b[i_b] = fibheap.fheappush(H_b, f_b[i_b], i_b)
|
|
if perm_b[i_f] or i_f == t:
|
|
ending_node = i_f
|
|
success = True
|
|
break
|
|
for j in A[i_f]:
|
|
path_sum = d_f[i_f] + w[(i_f, j)]
|
|
if d_f[j] > path_sum:
|
|
pred_f[j] = i_f
|
|
f_f[j] = path_sum + h(j, t)
|
|
if d_f[j] == float('inf'):
|
|
d_f[j] = path_sum
|
|
heap_node_connection_f[j] = fibheap.fheappush(H_f, f_f[j], j)
|
|
else:
|
|
d_f[j] = path_sum
|
|
H_f.decrease_key(heap_node_connection_f[j], f_f[j])
|
|
else:
|
|
perm_f[i_f] = True
|
|
heap_node_connection_f[i_f] = fibheap.fheappush(H_f, f_f[i_f], i_f)
|
|
if perm_f[i_b] or i_b == s:
|
|
ending_node = i_b
|
|
success = True
|
|
break
|
|
for j in A_b[i_b]:
|
|
path_sum = d_b[i_b] + w[(j, i_b)]
|
|
if d_b[j] > path_sum:
|
|
pred_b[j] = i_b
|
|
f_b[j] = path_sum + h(j, s)
|
|
if d_b[j] == float('inf'):
|
|
d_b[j] = path_sum
|
|
heap_node_connection_b[j] = fibheap.fheappush(H_b, f_b[j], j)
|
|
else:
|
|
d_b[j] = path_sum
|
|
H_b.decrease_key(heap_node_connection_b[j], f_b[j])
|
|
if success:
|
|
if ending_node == t:
|
|
path = [t]
|
|
current_node = ending_node
|
|
while pred_f[current_node] is not None:
|
|
path.append(pred_f[current_node])
|
|
current_node = pred_f[current_node]
|
|
path.reverse()
|
|
return path, d_f[t]
|
|
elif ending_node == s:
|
|
path = [s]
|
|
current_node = s
|
|
while pred_b[current_node] is not None:
|
|
path.append(pred_b[current_node])
|
|
current_node = pred_b[current_node]
|
|
return path, d_b[s]
|
|
else:
|
|
path1 = [ending_node]
|
|
current_node = ending_node
|
|
while pred_f[current_node] is not None:
|
|
path1.append(pred_f[current_node])
|
|
current_node = pred_f[current_node]
|
|
path1.reverse()
|
|
path2 = []
|
|
current_node = ending_node
|
|
while pred_b[current_node] is not None:
|
|
path2.append(pred_b[current_node])
|
|
current_node = pred_b[current_node]
|
|
return path1 + path2, d_f[ending_node] + d_b[ending_node]
|
|
return None, None
|
|
|
|
|
|
if __name__ == "__main__":
|
|
N = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10}
|
|
A = {1: [2, 8, 6], 2: [1, 3, 5], 8: [1, 4], 3: [2], 5: [2, 7, 9, 10], 4: [6, 7, 8], 6: [4, 1], 7: [4, 5], 9: [5], 10: [5]}
|
|
A_b = {2: [1, 3, 5], 1: [2, 8, 6], 8: [1, 4], 3: [2], 5: [2, 7, 9, 10], 6: [4, 1], 4: [6, 7, 8], 7: [4, 5], 9: [5], 10: [5]}
|
|
w = {(1, 2): 84, (2, 1): 84, (1, 8): 52, (8, 1): 52, (2, 3): 4, (3, 2): 4, (2, 5): 39, (5, 2): 39, (4, 6): 74, (6, 4): 74, (4, 7): 81, (7, 4): 81, (5, 7): 45, (7, 5): 45, (5, 9): 66, (9, 5): 66, (5, 10): 19, (10, 5): 19, (4, 8): 87, (8, 4): 87, (1, 6): 4, (6, 1): 4}
|
|
print(bidirectional_algorithm(1, 10, N, A, A_b, w, heuristic_cost))
|
|
|
|
N = {(3, 4), (5, 7), (7, 7), (6, 5), (4, 5), (3, 3), (4, 8), (3, 6), (8, 5), (6, 4), (6, 7), (3, 5), (5, 5), (5, 8), (8, 7), (2, 6), (6, 6), (7, 5), (7, 8)}
|
|
A = {(2, 6): [(3, 6)], (3, 3): [(3, 4)], (3, 4): [(3, 3), (3, 5)], (3, 5): [(4, 5), (3, 4), (3, 6)], (3, 6): [(2, 6), (3, 5)], (4, 5): [(3, 5), (5, 5)], (4, 8): [(5, 8)], (5, 5): [(4, 5), (6, 5)], (5, 7): [(6, 7), (5, 8)], (5, 8): [(4, 8), (5, 7)], (6, 4): [(6, 5)], (6, 5): [(5, 5), (7, 5), (6, 4), (6, 6)], (6, 6): [(6, 5), (6, 7)], (6, 7): [(5, 7), (7, 7), (6, 6)], (7, 5): [(6, 5), (8, 5)], (7, 7): [(6, 7), (8, 7), (7, 8)], (7, 8): [(7, 7)], (8, 5): [(7, 5)], (8, 7): [(7, 7)]}
|
|
A_b = {(3, 6): [(2, 6), (3, 5)], (3, 4): [(3, 3), (3, 5)], (3, 3): [(3, 4)], (3, 5): [(3, 4), (3, 6), (4, 5)], (4, 5): [(3, 5), (5, 5)], (2, 6): [(3, 6)], (5, 5): [(4, 5), (6, 5)], (5, 8): [(4, 8), (5, 7)], (6, 5): [(5, 5), (6, 4), (6, 6), (7, 5)], (6, 7): [(5, 7), (6, 6), (7, 7)], (4, 8): [(5, 8)], (5, 7): [(5, 8), (6, 7)], (7, 5): [(6, 5), (8, 5)], (6, 4): [(6, 5)], (6, 6): [(6, 5), (6, 7)], (7, 7): [(6, 7), (7, 8), (8, 7)], (8, 5): [(7, 5)], (8, 7): [(7, 7)], (7, 8): [(7, 7)]}
|
|
w = {((2, 6), (3, 6)): 1, ((3, 3), (3, 4)): 1, ((3, 4), (3, 3)): 1, ((3, 4), (3, 5)): 1, ((3, 5), (4, 5)): 1, ((3, 5), (3, 4)): 1, ((3, 5), (3, 6)): 1, ((3, 6), (2, 6)): 1, ((3, 6), (3, 5)): 1, ((4, 5), (3, 5)): 1, ((4, 5), (5, 5)): 1, ((4, 8), (5, 8)): 1, ((5, 5), (4, 5)): 1, ((5, 5), (6, 5)): 1, ((5, 7), (6, 7)): 1, ((5, 7), (5, 8)): 1, ((5, 8), (4, 8)): 1, ((5, 8), (5, 7)): 1, ((6, 4), (6, 5)): 1, ((6, 5), (5, 5)): 1, ((6, 5), (7, 5)): 1, ((6, 5), (6, 4)): 1, ((6, 5), (6, 6)): 1, ((6, 6), (6, 5)): 1, ((6, 6), (6, 7)): 1, ((6, 7), (5, 7)): 1, ((6, 7), (7, 7)): 1, ((6, 7), (6, 6)): 1, ((7, 5), (6, 5)): 1, ((7, 5), (8, 5)): 1, ((7, 7), (6, 7)): 1, ((7, 7), (8, 7)): 1, ((7, 7), (7, 8)): 1, ((7, 8), (7, 7)): 1, ((8, 5), (7, 5)): 1, ((8, 7), (7, 7)): 1}
|
|
print(bidirectional_algorithm((3, 3), (4, 8), N, A, A_b, w, heuristic_cost_manhattan))
|