a_star.py

from readMap import *
from heapq import *


class Node(object):

    def __init__(self, x, y, walkable=None):
        self.x = x
        self.y = y
        self.walkable = walkable if walkable else True
        self.opened = False
        self.closed = False
        self.g = None
        self.h = None
        self.parent = None
        self.startNode = "Hei"
        self.goalNode = None

    def __lt__(self, other):
        return self.f() < other.f()

    def f(self):
        return self.g + self.h

    def __str__(self):
        return str(self.x) + ":" + str(self.y)



class Grid(object):
    def __init__(self, width, height, matrix):
        self.width = width
        self.height = height
        self.startNode = None
        self.goalNode = None
        self.nodes = self._buildNodes(matrix)

    def _buildNodes(self, matrix):
        nodes = [[Node(i, j) for j in range(self.width)] for i in range(self.height)]

        for i, row in enumerate(nodes):
            for j, node in enumerate(row):

                # Walls are not walkable
                node.walkable = False if matrix[i][j] == "#" else True

                # Set start and goal node
                if matrix[i][j] == "A":
                    self.startNode = node
                    self.startNode.g = 0
                    print("Start: " + str(self.startNode))
                if matrix[i][j] == "B":
                    self.goalNode = node

        self.startNode.h = self.h(self.startNode, self.goalNode)
        return nodes

    def getNodeAt(self, x, y):
        return self.nodes[x][y]

    def isWalkableAt(self, x, y):
        return self.isInside(x, y) and self.nodes[x][y].walkable

    def isInside(self, x, y):
        return 0 <= x < self.height and 0 <= y < self.width

    def getNeightbors(self, node):
        x = node.x
        y = node.y
        neightbors = []

        if self.isWalkableAt(x, y-1):
            neightbors.append(self.nodes[x][y-1])

        if self.isWalkableAt(x, y+1):
            neightbors.append(self.nodes[x][y+1])

        if self.isWalkableAt(x-1, y):
            neightbors.append(self.nodes[x-1][y])

        if self.isWalkableAt(x+1, y):
            neightbors.append(self.nodes[x+1][y])

        return neightbors

    def h(self, node, goalNode):
        dx = abs(node.x - goalNode.x)
        dy = abs(node.y - goalNode.y)
        return dx + dy


def findPath(grid):
    openList = []

    startNode = grid.startNode
    goalNode = grid.goalNode

    heappush(openList, startNode)
    startNode.opened = True

    while(openList):

        node = heappop(openList)
        node.closed = True

        if (node == grid.goalNode):
            return getBacktrace(node)

        neightbors = grid.getNeightbors(node)

        for neighbor in neightbors:
            print(neighbor)
            if neighbor.closed == True:
                continue

            x = neighbor.x
            y = neighbor.y

            ng = node.g + 1

            if (not neighbor.opened) or (neighbor.g and ng < neighbor.g):
                neighbor.g = ng
                neighbor.h = grid.h(neighbor, goalNode)
                neighbor.parent = node

                if not neighbor.opened:
                    neighbor.opened = True
                    heappush(openList, neighbor)


def getBacktrace(node):
    nodes = []
    while node.parent:
        nodes.append(node)
        node = node.parent
    
    return nodes

map_matrix = readMap('boards/board-1-2.txt')
grid = Grid(20, 7, map_matrix)

bestPath = findPath(grid)

for node in bestPath[1:]:
    map_matrix[node.x][node.y] = u"\u25A1"
    # print(node)

for row in map_matrix:
    print("".join(row))



def print_map():
    for row in grid.nodes:
        for node in row:
            print(str(node.x) + ":" + str(node.y) + ("x" if not node.walkable else " "), end=" ")
        print()