praca_magisterska/algorithms/a_star.py

72 lines
4.0 KiB
Python
Raw Permalink Normal View History

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 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)
else:
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
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(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))