2022-09-23 16:06:04 +02:00
|
|
|
import fibheap
|
2022-09-25 16:41:41 +02:00
|
|
|
import math
|
2022-09-23 16:06:04 +02:00
|
|
|
|
|
|
|
|
2022-09-25 16:41:41 +02:00
|
|
|
def heuristic_cost(node1, node2):
|
2021-10-31 13:01:58 +01:00
|
|
|
return 0
|
|
|
|
|
|
|
|
|
2022-09-25 16:41:41 +02:00
|
|
|
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 a_star_algorithm(s, t, N, A, w, h):
|
|
|
|
if s == t:
|
|
|
|
return [s], 0
|
|
|
|
|
|
|
|
H = fibheap.makefheap()
|
|
|
|
d = {}
|
|
|
|
heap_node_connection = {}
|
|
|
|
pred = {}
|
|
|
|
f = {}
|
|
|
|
|
|
|
|
for i in N:
|
|
|
|
d[i] = float('inf')
|
|
|
|
d[s] = 0
|
|
|
|
f[s] = d[s] + h(s, t)
|
|
|
|
pred[s] = None
|
|
|
|
heap_node_connection[s] = fibheap.fheappush(H, 0, s)
|
|
|
|
success = False
|
|
|
|
while H.num_nodes > 0:
|
|
|
|
i = fibheap.fheappop(H)[1]
|
|
|
|
if i == t:
|
|
|
|
success = True
|
|
|
|
break
|
|
|
|
for j in A[i]:
|
|
|
|
path_sum = d[i] + w[(i, j)]
|
|
|
|
if d[j] > path_sum:
|
|
|
|
pred[j] = i
|
|
|
|
f[j] = path_sum + h(j, t)
|
|
|
|
if d[j] == float('inf'):
|
|
|
|
d[j] = path_sum
|
|
|
|
heap_node_connection[j] = fibheap.fheappush(H, f[j], j)
|
2022-01-15 14:29:52 +01:00
|
|
|
else:
|
2022-09-25 16:41:41 +02:00
|
|
|
d[j] = path_sum
|
|
|
|
H.decrease_key(heap_node_connection[j], f[j])
|
|
|
|
if success:
|
|
|
|
path = [t]
|
|
|
|
current_node = t
|
|
|
|
while pred[current_node] is not None:
|
|
|
|
path.append(pred[current_node])
|
|
|
|
current_node = pred[current_node]
|
|
|
|
path.reverse()
|
|
|
|
return path, d[t]
|
|
|
|
return None, None
|
2021-10-31 13:01:58 +01:00
|
|
|
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
2022-09-25 16:41:41 +02:00
|
|
|
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(a_star_algorithm(1, 10, N, A, 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(a_star_algorithm((3, 3), (4, 8), N, A, w, heuristic_cost_manhattan))
|