AStar Pathfinding

A* Pathfinding

function aStar(start, goal, graph):
    openSet = priorityQueue()
    openSet.add(start, priority=0)

    cameFrom = {}
    gScore = map with default infinity
    gScore[start] = 0

    fScore = map with default infinity
    fScore[start] = heuristic(start, goal)

    while openSet is not empty:
        current = openSet.popLowestPriority()

        if current == goal:
            return reconstructPath(cameFrom, current)

        for neighbor in graph.neighbors(current):
            tentativeG = gScore[current] + cost(current, neighbor)
            if tentativeG < gScore[neighbor]:
                cameFrom[neighbor] = current
                gScore[neighbor] = tentativeG
                fScore[neighbor] = tentativeG + heuristic(neighbor, goal)
                openSet.add(neighbor, priority=fScore[neighbor])

    return failure   // no path exists

function heuristic(a, b):
    // Manhattan distance for grid movement (4-directional)
    return abs(a.x - b.x) + abs(a.y - b.y)
    // Euclidean distance for free movement / 8-directional
    // return sqrt((a.x-b.x)^2 + (a.y-b.y)^2)

A* is a graph search that finds the shortest path by always expanding the node that looks most promising. Each node tracks g(n) (actual cost from start to n) and h(n) (an estimated/heuristic cost from n to goal). It expands nodes in order of f(n) = g(n) + h(n) — actual cost so far plus the estimated remaining cost. As long as the heuristic never overestimates the true remaining distance ("admissible"), A* is guaranteed to find the optimal path. Manhattan distance (|Δx| + |Δy|) is admissible for 4-directional grid movement because it's exactly the minimum number of steps required; Euclidean distance (straight-line) is admissible for free movement. A* is essentially Dijkstra's algorithm with the heuristic added to guide the search toward the goal instead of expanding equally in all directions — this is why it's almost always faster in practice.