A* Path Finding algorithm visualized with PyGame

a_star_pathfinding.py

# Example GIF: https://user-images.githubusercontent.com/20902250/174349934-7a241462-93da-4376-8fac-71d83048f5bf.gif
# NOTE: This is a re-upload of a program I made several years ago.

import math
import time
import typing as t
from queue import PriorityQueue

import pygame

WIDTH = 1000
WIN = pygame.display.set_mode((WIDTH, WIDTH))
pygame.display.set_caption("A* Path Finding Algorithm")

TOTAL_ROWS = 20
HEURISTICS = "euclid"


class Colors:
    """This class is only used to store colors."""

    RED = (255, 0, 0)
    GREEN = (0, 255, 0)
    BLUE = (0, 0, 255)
    YELLOW = (255, 255, 0)
    WHITE = (255, 255, 255)
    BLACK = (0, 0, 0)
    PURPLE = (128, 0, 128)
    ORANGE = (255, 165, 0)
    GREY = (128, 128, 128)
    TURQUOISE = (64, 224, 208)


class Node:
    def __init__(self, row: int, col: int, width: int) -> None:
        self.row = row
        self.col = col
        self.x = row * width
        self.y = col * width
        self.color = Colors.WHITE
        self.neighbors = []
        self.width = width

    def get_pos(self) -> t.Tuple[int, int]:
        return self.row, self.col

    def is_closed(self):
        return self.color == Colors.RED

    def is_open(self):
        return self.color == Colors.GREEN

    def is_barrier(self):
        return self.color == Colors.BLACK

    def is_start(self):
        return self.color == Colors.ORANGE

    def is_end(self):
        return self.color == Colors.TURQUOISE

    def is_path(self):
        return self.color == Colors.PURPLE

    def reset(self):
        self.color = Colors.WHITE

    def make_closed(self):
        self.color = Colors.RED

    def make_open(self):
        self.color = Colors.GREEN

    def make_barrier(self):
        self.color = Colors.BLACK

    def make_start(self):
        self.color = Colors.ORANGE

    def make_end(self):
        self.color = Colors.TURQUOISE

    def make_path(self):
        self.color = Colors.PURPLE

    def draw(self, win: pygame.Surface):
        pygame.draw.rect(win, self.color, (self.x, self.y, self.width, self.width))

    def update_neighbors(self, grid):
        self.neighbors = []
        # Check DOWN
        if self.row < TOTAL_ROWS - 1 and not grid[self.row + 1][self.col].is_barrier():
            self.neighbors.append(grid[self.row + 1][self.col])
        # Check UP
        if self.row > 0 and not grid[self.row - 1][self.col].is_barrier():
            self.neighbors.append(grid[self.row - 1][self.col])
        # Check LEFT
        if self.col < TOTAL_ROWS - 1 and not grid[self.row][self.col + 1].is_barrier():
            self.neighbors.append(grid[self.row][self.col + 1])
        # Check RIGHT
        if self.col > 0 and not grid[self.row][self.col - 1].is_barrier():
            self.neighbors.append(grid[self.row][self.col - 1])

    def __lt__(self, other):
        return False


def h(p1: t.Tuple[int, int], p2: t.Tuple[int, int]) -> int:
    """Heuristics function: Calculate expected distance"""
    x1, y1 = p1
    x2, y2 = p2

    # Taxicab (Manhattan) distance
    if HEURISTICS == "taxicab":
        return abs(x1 - x2) + abs(y1 - y2)

    # Euclidean distance
    elif HEURISTICS == "euclid":
        return math.sqrt(abs(x1 - x2) ** 2 + abs(y1 - y2) ** 2)

    # Fast euclidean distance
    elif HEURISTICS == "fast":
        return abs(x1 - x2) ** 2 + abs(y1 - y2) ** 2


def draw_path(node: Node, came_from: list, draw: t.Callable) -> None:
    """Draw the path."""
    path_length = 0
    while node in came_from:
        path_length += 1
        node = came_from[node]
        node.make_path()
        draw()
    print(f"Path has: {path_length}")


def algorithm(draw: t.Callable, grid: t.List[list], start: Node, end: Node) -> bool:
    """Full A* Path Finding algorithm."""
    start_time = time.time()
    count = 0
    open_set = PriorityQueue()
    open_set.put((0, count, start))
    came_from = {}
    g_score = {node: float("inf") for row in grid for node in row}
    g_score[start] = 0
    f_score = {node: float("inf") for row in grid for node in row}
    f_score[start] = h(start.get_pos(), end.get_pos())

    # Keep track of what is in PriorityQueue (since it doesn't provide a check)
    open_set_hash = {start}

    while not open_set.empty():
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                pygame.quit()

        current = open_set.get()[2]
        open_set_hash.remove(current)

        if current == end:
            draw_path(end, came_from, draw)
            taken = time.time() - start_time
            print(f"Took: {taken} s")
            start.make_start()
            end.make_end()
            return True

        for neighbor in current.neighbors:
            temp_g_score = g_score[current] + 1

            # Found a better way to access this neighbor
            if temp_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = temp_g_score
                f_score[neighbor] = temp_g_score + h(neighbor.get_pos(), end.get_pos())
                if neighbor not in open_set_hash:
                    count += 1
                    open_set.put((f_score[neighbor], count, neighbor))
                    open_set_hash.add(neighbor)
                    neighbor.make_open()

        draw()
        if current != start:
            current.make_closed()

    return False


def make_grid(rows: int, width: int) -> list:
    """Generate a new grid filled with Nodes."""
    grid = []
    gap = width // rows
    for i in range(rows):
        grid.append([])
        for j in range(rows):
            node = Node(i, j, gap)
            grid[i].append(node)

    return grid


def draw_grid(win: pygame.Surface, rows: int, width: int) -> None:
    """Draw lines on grid to visualize each node."""
    gap = width // rows
    for i in range(rows):
        pygame.draw.line(win, Colors.GREY, (0, i * gap), (width, i * gap))
        for j in range(rows):
            pygame.draw.line(win, Colors.GREY, (j * gap, 0), (j * gap, width))


def draw(win, grid, rows, width) -> None:
    """Main (re)draw function."""
    win.fill(Colors.WHITE)

    for row in grid:
        for node in row:
            node.draw(win)

    draw_grid(win, rows, width)
    pygame.display.update()


def get_clicked_position(
    mouse_pos: t.Tuple[int, int], rows: int, width: int
) -> t.Tuple[int, int]:
    """Get position as row, col from mouse click (y, x)."""
    gap = width // rows
    y, x = mouse_pos

    row = y // gap
    col = x // gap

    return row, col


def main(win: pygame.Surface, width: int, rows: int):
    global HEURISTICS
    grid = make_grid(rows, width)

    start = None
    end = None

    run = True

    while run:
        draw(win, grid, rows, width)
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                run = False

            if pygame.mouse.get_pressed()[0]:  # LEFT
                pos = pygame.mouse.get_pos()
                row, col = get_clicked_position(pos, rows, width)
                node = grid[row][col]

                if not start and node != end:
                    start = node
                    start.make_start()
                elif not end and node != start:
                    end = node
                    end.make_end()
                elif node != end and node != start:
                    node.make_barrier()

            elif pygame.mouse.get_pressed()[2]:  # RIGHT
                pos = pygame.mouse.get_pos()
                row, col = get_clicked_position(pos, rows, width)
                node = grid[row][col]
                node.reset()

                if node == start:
                    start = None
                elif node == end:
                    end = None

            if event.type == pygame.KEYDOWN:
                # Start
                if event.key == pygame.K_SPACE and start and end:
                    for row in grid:
                        for node in row:
                            node.update_neighbors(grid)

                    algorithm(lambda: draw(win, grid, rows, width), grid, start, end)

                # Clean grid (reset everything)
                if event.key == pygame.K_c:
                    start = None
                    end = None
                    grid = make_grid(rows, width)

                # Reset used grid (keep barriers)
                if event.key == pygame.K_r:
                    for row in grid:
                        for node in row:
                            if node.is_open():
                                node.reset()
                            if node.is_closed():
                                node.reset()
                            if node.is_path():
                                node.reset()

                # Switch heuristics
                if event.key == pygame.K_h:
                    if HEURISTICS == "euclid":
                        print("Using fast heuristics")
                        HEURISTICS = "fast"
                    elif HEURISTICS == "fast":
                        print("Using taxicab heuristics")
                        HEURISTICS = "taxicab"
                    elif HEURISTICS == "taxicab":
                        print("Using euclidean heuristics")
                        HEURISTICS = "euclid"
    pygame.quit()


main(WIN, WIDTH, TOTAL_ROWS)