SZI-Smieciarka/astar.py

import heapq


def heurystyka(obiekty, a, b):
    heur = abs((b[0] - a[0])) + abs((b[1] - a[1]))
    if obiekty["plansza"][b[0], b[1]].jestDomem is True:
        heur += 2
    if obiekty["plansza"][b[0], b[1]].jestWysypiskiem is True:
        heur += 1
    if obiekty["plansza"][b[0], b[1]].jestKontenerem is True:
        heur += 2
    return heur


def astar(obiekty, start, cel):

    sasiedzi = [(0, 1), (0, -1), (1, 0), (-1, 0)]
    close_set = set()
    came_from = {}
    hscore = {start: heurystyka(obiekty, start, cel)}
    gscore = {start: 0}
    fscore = {start: heurystyka(obiekty, 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

            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
            tentative_h_score = heurystyka(obiekty, sasiad, cel) + heurystyka(obiekty, current, sasiad)

            if sasiad in [i[1] for i in oheap] and tentative_h_score < hscore.get(current, 0):
                continue
            elif sasiad not in close_set and sasiad not in [i[1] for i in oheap]:
                came_from[sasiad] = current
                hscore[sasiad] = tentative_h_score
                fscore[sasiad] = tentative_h_score + gscore[current]
                gscore[sasiad] = gscore[current] + heurystyka(obiekty, current, sasiad)
                heapq.heappush(oheap, (fscore[sasiad], sasiad))
    return False