49 lines
1.6 KiB
Python
49 lines
1.6 KiB
Python
|
import heapq
|
||
|
|
|
||
|
|
|
||
|
|
def heurystyka(a, b):
|
||
|
|
return abs((b[0] - a[0])) + abs((b[1] - a[1]))
|
||
|
|
|
||
|
|
|
||
|
|
def astar(start, cel):
|
||
|
|
import game
|
||
|
|
|
||
|
|
obiekty = game.utworzObiekty()
|
||
|
|
sasiedzi = [(0, 1), (0, -1), (1, 0), (-1, 0)]
|
||
|
|
close_set = set()
|
||
|
|
came_from = {}
|
||
|
|
gscore = {start: 0}
|
||
|
|
fscore = {start: heurystyka(start, cel)}
|
||
|
|
oheap = []
|
||
|
|
heapq.heappush(oheap, (fscore[start], start))
|
||
|
|
while oheap:
|
||
|
|
current = heapq.heappop(oheap)[1]
|
||
|
|
if current == cel:
|
||
|
|
data = []
|
||
|
|
while current in came_from:
|
||
|
|
data.append(current)
|
||
|
|
current = came_from[current]
|
||
|
|
return data[::-1]
|
||
|
|
close_set.add(current)
|
||
|
|
for i, j in sasiedzi:
|
||
|
|
sasiad = current[0] + i, current[1] + j
|
||
|
|
tentative_g_score = gscore[current] + heurystyka(current, sasiad)
|
||
|
|
if 14 < sasiad[0] or sasiad[0] < 0:
|
||
|
|
continue
|
||
|
|
elif 14 < sasiad[1] or sasiad[1] < 0:
|
||
|
|
continue
|
||
|
|
elif 6 <= sasiad[0] <= 7 and 10 <= sasiad[1] <= 11:
|
||
|
|
continue
|
||
|
|
elif 4 < sasiad[0] and 4 < sasiad[1]:
|
||
|
|
continue
|
||
|
|
elif 4 > sasiad[0] and 4 > sasiad[1]:
|
||
|
|
continue
|
||
|
|
if sasiad in close_set and tentative_g_score >= gscore.get(sasiad, 0):
|
||
|
|
continue
|
||
|
|
if tentative_g_score < gscore.get(sasiad, 0) or sasiad not in [i[1] for i in oheap]:
|
||
|
|
came_from[sasiad] = current
|
||
|
|
gscore[sasiad] = tentative_g_score
|
||
|
|
fscore[sasiad] = tentative_g_score + heurystyka(sasiad, cel)
|
||
|
|
heapq.heappush(oheap, (fscore[sasiad], sasiad))
|
||
|
|
return False
|