Implementation Walkthrough

Let’s turn the algorithms into working code. We’ll use Python, but the concepts translate to any language.

Data Structures

First, let’s understand what we need:

Grid representation:

• 2D list/array: grid[row][col] where 0 = empty, 1 = wall

Distance tracking:

• Dictionary: distances[node] = cost or array for grids

Priority queue:

• Min-heap: Always get the node with smallest cost
• Python: heapq module
• JavaScript: No built-in, use array + sort or a library

Visited set:

• Set: Track which nodes we’ve processed
• Python: set()
• JavaScript: Set()

Parents map:

• Dictionary: parents[node] = previous_node for path reconstruction

Dijkstra Implementation

Here’s a complete Dijkstra implementation:

import heapq

def dijkstra(grid, start, goal):
    """
    Find shortest path from start to goal using Dijkstra's algorithm.
    
    Args:
        grid: 2D list, 0 = empty, 1 = wall
        start: (row, col) tuple
        goal: (row, col) tuple
    
    Returns:
        List of (row, col) tuples representing the path, or None if no path
    """
    rows, cols = len(grid), len(grid[0])
    
    # Initialize distances (infinity for all nodes)
    distances = {}
    for r in range(rows):
        for c in range(cols):
            if grid[r][c] == 0:  # Only empty cells
                distances[(r, c)] = float('inf')
    
    distances[start] = 0
    
    # Priority queue: (distance, row, col)
    queue = [(0, start[0], start[1])]
    heapq.heapify(queue)
    
    # Track visited nodes
    visited = set()
    
    # Track parents for path reconstruction
    parents = {}
    
    # Directions: up, down, left, right
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
    
    while queue:
        # Get node with smallest distance
        current_dist, row, col = heapq.heappop(queue)
        current = (row, col)
        
        # Skip if already processed
        if current in visited:
            continue
        
        # Mark as visited
        visited.add(current)
        
        # Check if we reached the goal
        if current == goal:
            return reconstruct_path(parents, start, goal)
        
        # Explore neighbors
        for dr, dc in directions:
            new_row, new_col = row + dr, col + dc
            
            # Check bounds
            if not (0 <= new_row < rows and 0 <= new_col < cols):
                continue
            
            # Check if it's a wall
            if grid[new_row][new_col] == 1:
                continue
            
            neighbor = (new_row, new_col)
            
            # Skip if already visited
            if neighbor in visited:
                continue
            
            # Calculate new distance
            new_dist = current_dist + 1  # Assuming uniform cost
            
            # Update if we found a shorter path
            if new_dist < distances[neighbor]:
                distances[neighbor] = new_dist
                parents[neighbor] = current
                heapq.heappush(queue, (new_dist, new_row, new_col))
    
    # No path found
    return None

def reconstruct_path(parents, start, goal):
    """Reconstruct path from parents map."""
    path = []
    current = goal
    
    while current is not None:
        path.append(current)
        current = parents.get(current)
    
    path.reverse()
    return path if path[0] == start else None

A* Implementation

A* is almost identical, but uses f(n) = g(n) + h(n):

import heapq

def manhattan_distance(r1, c1, r2, c2):
    """Manhattan distance heuristic for 4-direction movement."""
    return abs(r2 - r1) + abs(c2 - c1)

def astar(grid, start, goal):
    """
    Find shortest path from start to goal using A* algorithm.
    
    Args:
        grid: 2D list, 0 = empty, 1 = wall
        start: (row, col) tuple
        goal: (row, col) tuple
    
    Returns:
        List of (row, col) tuples representing the path, or None if no path
    """
    rows, cols = len(grid), len(grid[0])
    
    # g(n) = known cost from start to n
    g = {}
    for r in range(rows):
        for c in range(cols):
            if grid[r][c] == 0:
                g[(r, c)] = float('inf')
    
    g[start] = 0
    
    # f(n) = g(n) + h(n)
    f = {}
    f[start] = manhattan_distance(start[0], start[1], goal[0], goal[1])
    
    # Priority queue: (f_score, row, col)
    queue = [(f[start], start[0], start[1])]
    heapq.heapify(queue)
    
    visited = set()
    parents = {}
    
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
    
    while queue:
        # Get node with smallest f_score
        current_f, row, col = heapq.heappop(queue)
        current = (row, col)
        
        if current in visited:
            continue
        
        visited.add(current)
        
        if current == goal:
            return reconstruct_path(parents, start, goal)
        
        for dr, dc in directions:
            new_row, new_col = row + dr, col + dc
            
            if not (0 <= new_row < rows and 0 <= new_col < cols):
                continue
            
            if grid[new_row][new_col] == 1:
                continue
            
            neighbor = (new_row, new_col)
            
            if neighbor in visited:
                continue
            
            # Calculate new g and f
            new_g = g[current] + 1
            new_f = new_g + manhattan_distance(new_row, new_col, goal[0], goal[1])
            
            # Update if we found a better path
            if new_f < f.get(neighbor, float('inf')):
                g[neighbor] = new_g
                f[neighbor] = new_f
                parents[neighbor] = current
                heapq.heappush(queue, (new_f, new_row, new_col))
    
    return None

def reconstruct_path(parents, start, goal):
    """Reconstruct path from parents map."""
    path = []
    current = goal
    
    while current is not None:
        path.append(current)
        current = parents.get(current)
    
    path.reverse()
    return path if path[0] == start else None

Key Implementation Details

Priority Queue Handling

Important: When you update a node’s distance, you might add it to the queue again. That’s okay - the old entry will be skipped when we check if current in visited.

Alternative: Use a more sophisticated priority queue that supports decrease-key operations. But the simple approach works fine.

Path Reconstruction

The reconstruct_path function:

1. Starts from the goal
2. Follows parent pointers back to start
3. Reverses the list to get start → goal order

Stopping Early

For single-goal pathfinding, we can stop as soon as we process the goal node. This is an optimization - we don’t need to find paths to all nodes.

Handling Edge Cases

No path exists:

• Algorithm will explore all reachable nodes
• Queue will become empty
• Return None

Start equals goal:

• Return [start] immediately

Start or goal is a wall:

• Check at the beginning and return None

Empty grid:

• Handle gracefully (return None or empty path)

Variable Movement Costs

To support different terrain costs:

# Instead of: new_dist = current_dist + 1
# Use: new_dist = current_dist + get_cost(grid, new_row, new_col)

def get_cost(grid, row, col):
    """Get movement cost for a cell."""
    terrain = grid[row][col]
    costs = {
        0: 1,      # Empty
        2: 5,      # Mud
        3: 0.5,    # Road
    }
    return costs.get(terrain, float('inf'))

Code Checkpoint

JavaScript Version

Here’s A* in JavaScript (no built-in heap, so we’ll use array + sort):

function manhattanDistance(r1, c1, r2, c2) {
    return Math.abs(r2 - r1) + Math.abs(c2 - c1);
}

function astar(grid, start, goal) {
    const rows = grid.length;
    const cols = grid[0].length;
    
    const g = new Map();
    const f = new Map();
    const visited = new Set();
    const parents = new Map();
    const queue = [];
    
    // Initialize
    for (let r = 0; r < rows; r++) {
        for (let c = 0; c < cols; c++) {
            if (grid[r][c] === 0) {
                g.set(`${r},${c}`, Infinity);
            }
        }
    }
    
    g.set(`${start[0]},${start[1]}`, 0);
    f.set(`${start[0]},${start[1]}`, manhattanDistance(start[0], start[1], goal[0], goal[1]));
    queue.push({ f: f.get(`${start[0]},${start[1]}`), row: start[0], col: start[1] });
    
    const directions = [[-1, 0], [1, 0], [0, -1], [0, 1]];
    
    while (queue.length > 0) {
        // Sort and pop (simulating priority queue)
        queue.sort((a, b) => a.f - b.f);
        const { row, col } = queue.shift();
        const current = `${row},${col}`;
        
        if (visited.has(current)) continue;
        visited.add(current);
        
        if (row === goal[0] && col === goal[1]) {
            return reconstructPath(parents, start, goal);
        }
        
        for (const [dr, dc] of directions) {
            const newRow = row + dr;
            const newCol = col + dc;
            
            if (newRow < 0 || newRow >= rows || newCol < 0 || newCol >= cols) continue;
            if (grid[newRow][newCol] === 1) continue;
            
            const neighbor = `${newRow},${newCol}`;
            if (visited.has(neighbor)) continue;
            
            const newG = g.get(current) + 1;
            const newF = newG + manhattanDistance(newRow, newCol, goal[0], goal[1]);
            
            if (newF < (f.get(neighbor) || Infinity)) {
                g.set(neighbor, newG);
                f.set(neighbor, newF);
                parents.set(neighbor, current);
                queue.push({ f: newF, row: newRow, col: newCol });
            }
        }
    }
    
    return null;
}

function reconstructPath(parents, start, goal) {
    const path = [];
    let current = `${goal[0]},${goal[1]}`;
    
    while (current) {
        const [r, c] = current.split(',').map(Number);
        path.push([r, c]);
        current = parents.get(current);
    }
    
    path.reverse();
    return path[0][0] === start[0] && path[0][1] === start[1] ? path : null;
}

Key Takeaways

Before moving forward:

1. Data structures matter - Priority queue, visited set, parents map
2. Priority = distance (Dijkstra) or f(n) (A)* - Only difference
3. Path reconstruction - Follow parents from goal to start, then reverse
4. Handle edge cases - No path, start=goal, walls, etc.
5. Variable costs - Easy to add by changing the cost calculation

What’s Next?

Now that you can implement both algorithms, let’s build a small project. In the next page, you’ll create your own pathfinding toy that you can extend.