Pathfinding (A* Algorithm)

Intelligent Navigation with Pathfinding

Understanding Pathfinding

Interactive A* Pathfinding Demo

A* Algorithm Implementation

import heapq
import math
from typing import List, Tuple, Optional, Dict, Set
from dataclasses import dataclass, field

@dataclass
class Node:
    """Node for pathfinding"""
    x: int
    y: int
    g: float = 0  # Cost from start
    h: float = 0  # Heuristic cost to goal
    f: float = 0  # Total cost (g + h)
    parent: Optional['Node'] = None
    
    def __lt__(self, other: 'Node') -> bool:
        return self.f < other.f
    
    def __eq__(self, other: 'Node') -> bool:
        return self.x == other.x and self.y == other.y
    
    def __hash__(self) -> int:
        return hash((self.x, self.y))

class AStar:
    """A* pathfinding algorithm"""
    def __init__(self, grid: List[List[int]], diagonal: bool = True) -> None:
        self.grid: List[List[int]] = grid
        self.height: int = len(grid)
        self.width: int = len(grid[0]) if grid else 0
        self.diagonal: bool = diagonal
        
        # Cost for different terrain types
        self.terrain_cost: Dict[int, float] = {
            0: 1,      # Empty
            1: float('inf'),  # Wall
            2: 3,      # Forest (higher cost)
            3: 10,     # Water (very high cost)
            4: 2       # Sand (medium cost)
        }
    
    def heuristic(self, node: Node, goal: Node, method: str = 'manhattan') -> float:
        """Calculate heuristic distance"""
        dx = abs(node.x - goal.x)
        dy = abs(node.y - goal.y)
        
        if method == 'manhattan':
            return dx + dy
        elif method == 'euclidean':
            return math.sqrt(dx**2 + dy**2)
        elif method == 'diagonal':
            return max(dx, dy) + (math.sqrt(2) - 1) * min(dx, dy)
        else:
            return 0
    
    def get_neighbors(self, node: Node) -> List[Node]:
        """Get valid neighboring nodes"""
        neighbors = []
        
        # Cardinal directions
        directions = [(0, -1), (1, 0), (0, 1), (-1, 0)]
        
        # Add diagonal directions if enabled
        if self.diagonal:
            directions.extend([(-1, -1), (1, -1), (1, 1), (-1, 1)])
        
        for dx, dy in directions:
            x, y = node.x + dx, node.y + dy
            
            # Check bounds
            if 0 <= x < self.width and 0 <= y < self.height:
                # Check if not a wall
                if self.get_cost(x, y) != float('inf'):
                    # For diagonal movement, check if path is clear
                    if abs(dx) + abs(dy) == 2:  # Diagonal
                        if (self.get_cost(node.x + dx, node.y) == float('inf') and
                            self.get_cost(node.x, node.y + dy) == float('inf')):
                            continue  # Both adjacent cells are walls
                    
                    neighbors.append(Node(x, y))
        
        return neighbors
    
    def get_cost(self, x: int, y: int) -> float:
        """Get movement cost for a cell"""
        if 0 <= x < self.width and 0 <= y < self.height:
            terrain = self.grid[y][x]
            return self.terrain_cost.get(terrain, 1)
        return float('inf')
    
    def find_path(self, start: Tuple[int, int], goal: Tuple[int, int]) -> Optional[List[Tuple[int, int]]]:
        """Find path from start to goal using A*"""
        start_node = Node(start[0], start[1])
        goal_node = Node(goal[0], goal[1])
        
        # Priority queue for open set
        open_set = [start_node]
        heapq.heapify(open_set)
        
        # Track visited nodes
        closed_set: Set[Node] = set()
        
        # Track best g scores
        g_scores: Dict[Tuple[int, int], float] = {(start_node.x, start_node.y): 0}
        
        # Statistics
        nodes_explored = 0
        
        while open_set:
            current = heapq.heappop(open_set)
            nodes_explored += 1
            
            # Goal reached
            if current == goal_node:
                path = self.reconstruct_path(current)
                print(f"Path found! Length: {len(path)}, Nodes explored: {nodes_explored}")
                return path
            
            closed_set.add(current)
            
            # Explore neighbors
            for neighbor in self.get_neighbors(current):
                if neighbor in closed_set:
                    continue
                
                # Calculate tentative g score
                move_cost = 1.414 if abs(neighbor.x - current.x) + abs(neighbor.y - current.y) == 2 else 1
                terrain_cost = self.get_cost(neighbor.x, neighbor.y)
                tentative_g = g_scores[(current.x, current.y)] + move_cost * terrain_cost
                
                # Check if this path is better
                if (neighbor.x, neighbor.y) not in g_scores or tentative_g < g_scores[(neighbor.x, neighbor.y)]:
                    # Update node
                    neighbor.g = tentative_g
                    neighbor.h = self.heuristic(neighbor, goal_node)
                    neighbor.f = neighbor.g + neighbor.h
                    neighbor.parent = current
                    
                    g_scores[(neighbor.x, neighbor.y)] = tentative_g
                    
                    # Add to open set if not already there
                    if neighbor not in open_set:
                        heapq.heappush(open_set, neighbor)
        
        print(f"No path found. Nodes explored: {nodes_explored}")
        return None
    
    def reconstruct_path(self, node: Node) -> List[Tuple[int, int]]:
        """Reconstruct path from goal to start"""
        path = []
        current = node
        
        while current:
            path.append((current.x, current.y))
            current = current.parent
        
        return list(reversed(path))
    
    def smooth_path(self, path: List[Tuple[int, int]]) -> List[Tuple[int, int]]:
        """Smooth path by removing unnecessary waypoints"""
        if len(path) < 3:
            return path
        
        smoothed = [path[0]]
        current = 0
        
        while current < len(path) - 1:
            farthest = current + 1
            
            # Find farthest visible point
            for i in range(current + 2, len(path)):
                if self.line_of_sight(path[current], path[i]):
                    farthest = i
                else:
                    break
            
            smoothed.append(path[farthest])
            current = farthest
        
        return smoothed
    
    def line_of_sight(self, p1: Tuple[int, int], p2: Tuple[int, int]) -> bool:
        """Check if there's a clear line of sight between two points"""
        x0, y0 = p1
        x1, y1 = p2
        
        dx = abs(x1 - x0)
        dy = abs(y1 - y0)
        sx = 1 if x0 < x1 else -1
        sy = 1 if y0 < y1 else -1
        err = dx - dy
        
        while True:
            # Check if current position is walkable
            if self.get_cost(x0, y0) == float('inf'):
                return False
            
            if x0 == x1 and y0 == y1:
                break
            
            e2 = 2 * err
            if e2 > -dy:
                err -= dy
                x0 += sx
            if e2 < dx:
                err += dx
                y0 += sy
        
        return True

Best Practices

Key Takeaways

import pygame, heapq, math, sys

pygame.init()
W, H, CELL = 768, 480, 32
COLS, ROWS = W // CELL, H // CELL  # 24 x 15
screen = pygame.display.set_mode((W, H + 80))
pygame.display.set_caption("A* Pathfinding: Terrain + Heuristic + Multiplier")
clock = pygame.time.Clock()
font = pygame.font.SysFont("monospace", 13)

EMPTY, WALL, FOREST, WATER, SAND, HAZARD = 0, 1, 2, 3, 4, 5
TERRAIN_COST = {EMPTY: 1, WALL: float('inf'), FOREST: 3, WATER: 10, SAND: 2, HAZARD: 5}
TERRAIN_COLOR = {EMPTY: (60, 70, 90), WALL: (20, 20, 20), FOREST: (40, 110, 50),
                 WATER: (60, 90, 200), SAND: (220, 180, 120), HAZARD: (200, 80, 80)}

grid = [[EMPTY] * COLS for _ in range(ROWS)]
start, goal = (2, 2), (21, 12)
heuristic_name, multiplier, paint = "manhattan", 1.0, WALL
closed_list, open_list, path = set(), set(), []
nodes_explored, search_ms, path_cost = 0, 0, 0.0

def manhattan(a, b): return abs(a[0] - b[0]) + abs(a[1] - b[1])
def octile(a, b):
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1])
    return max(dx, dy) + (math.sqrt(2) - 1) * min(dx, dy)
def euclidean(a, b):
    return math.sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2)
def zero(a, b): return 0
HEURISTICS = {"manhattan": manhattan, "octile": octile, "euclidean": euclidean, "zero": zero}

def neighbors(p):
    x, y = p
    for dx, dy in [(-1,0),(1,0),(0,-1),(0,1),(-1,-1),(-1,1),(1,-1),(1,1)]:
        nx, ny = x + dx, y + dy
        if 0 <= nx < COLS and 0 <= ny < ROWS and TERRAIN_COST[grid[ny][nx]] != float('inf'):
            move_cost = math.sqrt(2) if dx and dy else 1
            yield (nx, ny), move_cost * TERRAIN_COST[grid[ny][nx]]

def astar():
    global closed_list, open_list, path, nodes_explored, search_ms, path_cost
    t0 = pygame.time.get_ticks()
    h = HEURISTICS[heuristic_name]
    open_heap, came_from, g_score = [], {}, {start: 0}
    counter = 0
    heapq.heappush(open_heap, (h(start, goal) * multiplier, counter, start))
    open_set, closed = {start}, set()
    nodes_explored, path, path_cost = 0, [], 0.0
    while open_heap:
        _, _, current = heapq.heappop(open_heap)
        if current in closed: continue
        open_set.discard(current)
        closed.add(current)
        nodes_explored += 1
        if current == goal:
            path = [current]
            while current in came_from:
                current = came_from[current]
                path.append(current)
            path.reverse()
            path_cost = g_score[goal]
            break
        for nb, step in neighbors(current):
            if nb in closed: continue
            tentative_g = g_score[current] + step
            if nb not in g_score or tentative_g < g_score[nb]:
                g_score[nb] = tentative_g
                came_from[nb] = current
                f = tentative_g + h(nb, goal) * multiplier
                counter += 1
                heapq.heappush(open_heap, (f, counter, nb))
                open_set.add(nb)
    closed_list, open_list = closed, open_set
    search_ms = pygame.time.get_ticks() - t0

def reset():
    global grid, start, goal, closed_list, open_list, path, nodes_explored, path_cost
    grid = [[EMPTY] * COLS for _ in range(ROWS)]
    start, goal = (2, 2), (21, 12)
    closed_list, open_list, path = set(), set(), []
    nodes_explored, path_cost = 0, 0.0

PAINT_KEYS = {pygame.K_1: WALL, pygame.K_2: FOREST, pygame.K_3: WATER,
              pygame.K_4: SAND, pygame.K_5: HAZARD, pygame.K_e: EMPTY}
HEUR_KEYS = {pygame.K_h: "manhattan", pygame.K_o: "octile",
             pygame.K_u: "euclidean", pygame.K_z: "zero"}
MULT_LIST = [0.5, 1.0, 1.5, 2.0]

while True:
    for e in pygame.event.get():
        if e.type == pygame.QUIT: pygame.quit(); sys.exit()
        if e.type == pygame.KEYDOWN:
            if e.key in PAINT_KEYS: paint = PAINT_KEYS[e.key]
            elif e.key in HEUR_KEYS: heuristic_name = HEUR_KEYS[e.key]
            elif e.key in (pygame.K_PLUS, pygame.K_EQUALS):
                multiplier = MULT_LIST[(MULT_LIST.index(multiplier) + 1) % len(MULT_LIST)]
            elif e.key == pygame.K_MINUS:
                multiplier = MULT_LIST[(MULT_LIST.index(multiplier) - 1) % len(MULT_LIST)]
            elif e.key == pygame.K_SPACE: astar()
            elif e.key == pygame.K_r: reset()
    mb = pygame.mouse.get_pressed()
    if any(mb):
        mx, my = pygame.mouse.get_pos()
        if my < H:
            cx, cy = mx // CELL, my // CELL
            if mb[0]:
                if pygame.key.get_mods() & pygame.KMOD_SHIFT: start = (cx, cy)
                else: grid[cy][cx] = paint
            elif mb[2]: goal = (cx, cy)

    screen.fill((30, 30, 30))
    for y in range(ROWS):
        for x in range(COLS):
            pygame.draw.rect(screen, TERRAIN_COLOR[grid[y][x]],
                             (x*CELL+1, y*CELL+1, CELL-2, CELL-2))
            if (x, y) in closed_list and grid[y][x] == EMPTY:
                pygame.draw.rect(screen, (255, 182, 193),
                                 (x*CELL+10, y*CELL+10, CELL-20, CELL-20))
            elif (x, y) in open_list and grid[y][x] == EMPTY:
                pygame.draw.rect(screen, (144, 238, 144),
                                 (x*CELL+10, y*CELL+10, CELL-20, CELL-20))
    if path:
        pts = [(x*CELL + CELL//2, y*CELL + CELL//2) for x, y in path]
        pygame.draw.lines(screen, (255, 215, 0), False, pts, 4)
    sx, sy = start; pygame.draw.circle(screen, (60,200,60), (sx*CELL+CELL//2, sy*CELL+CELL//2), 10)
    gx, gy = goal; pygame.draw.circle(screen, (220,60,60), (gx*CELL+CELL//2, gy*CELL+CELL//2), 10)

    adm = "ADMISSIBLE" if multiplier <= 1.0 else "INFLATED"
    paint_name = {WALL:"WALL",FOREST:"FOREST",WATER:"WATER",SAND:"SAND",HAZARD:"HAZARD",EMPTY:"EMPTY"}[paint]
    hud = [
        f"heuristic: {heuristic_name}  multiplier: {multiplier}x ({adm})  paint: {paint_name}",
        f"open: {len(open_list)}  closed: {len(closed_list)}  explored: {nodes_explored}  cost: {path_cost:.2f}  len: {len(path)}  ms: {search_ms}",
        "L-drag paint  R goal  Shift+L start  1-5 terrain  E erase  H/O/U/Z heuristic  +/- mult  SPACE run  R reset",
    ]
    for i, line in enumerate(hud):
        screen.blit(font.render(line, True, (220,220,220)), (8, H + 8 + i * 20))
    pygame.display.flip()
    clock.tick(60)

What's Next?

Now that you've mastered pathfinding, next we'll explore state machines for creating intelligent NPC behaviors!