A* Search Algorithm Explained — Heuristics, Optimality and Real-World Pitfalls

import java.util.*;

/**
 * A* Search on a weighted directed graph.
 * Nodes are integers. Edge weights are positive doubles.
 * Heuristic is supplied as a lambda so callers can plug in
 * Manhattan, Euclidean, or any admissible function.
 */
public class AStarSearch {

    // ── Data structures ───────────────────────────────────────────

    /** An edge leaving a node, carrying a travel cost. */
    static class Edge {
        final int targetNode;
        final double cost;
        Edge(int targetNode, double cost) {
            this.targetNode = targetNode;
            this.cost = cost;
        }
    }

    /**
     * One entry in the open set priority queue.
     * We store fScore here so the PQ can order by it.
     * We intentionally separate this from the node's gScore
     * stored in the gScore map — a stale entry in the PQ
     * is handled by checking gScore at expansion time.
     */
    static class PQEntry implements Comparable<PQEntry> {
        final int node;
        final double fScore;   // g(node) + h(node) at time of insertion
        PQEntry(int node, double fScore) {
            this.node = node;
            this.fScore = fScore;
        }
        @Override
        public int compareTo(PQEntry other) {
            return Double.compare(this.fScore, other.fScore);
        }
    }

    // ── Core algorithm ────────────────────────────────────────────

    /**
     * @param adjacencyList  graph[node] = list of outgoing edges
     * @param startNode      index of the start node
     * @param goalNode       index of the goal node
     * @param heuristic      admissible h(n, goal) estimate
     * @return list of node indices forming the shortest path,
     *         or empty list if no path exists
     */
    public static List<Integer> findPath(
            List<List<Edge>> adjacencyList,
            int startNode,
            int goalNode,
            java.util.function.ToDoubleFunction<Integer> heuristic) {

        int totalNodes = adjacencyList.size();

        // gScore[n] = cheapest known cost from startNode to n.
        // Initialise all to +infinity; we only know start = 0.
        double[] gScore = new double[totalNodes];
        Arrays.fill(gScore, Double.MAX_VALUE);
        gScore[startNode] = 0.0;

        // cameFrom[n] = the node we arrived from on the best known path.
        // Used to reconstruct the path once we reach the goal.
        int[] cameFrom = new int[totalNodes];
        Arrays.fill(cameFrom, -1);

        // Open set: nodes discovered but not yet expanded.
        // A PriorityQueue in Java is a min-heap, which is exactly what we need.
        PriorityQueue<PQEntry> openSet = new PriorityQueue<>();
        openSet.add(new PQEntry(startNode, heuristic.applyAsDouble(startNode)));

        // Closed set: nodes whose optimal g-value is finalised.
        boolean[] closed = new boolean[totalNodes];

        while (!openSet.isEmpty()) {
            PQEntry current = openSet.poll();  // node with lowest f-score

            // Stale-entry check: if we already found a cheaper path
            // to this node and processed it, skip this outdated entry.
            // This avoids the overhead of a decrease-key operation.
            if (closed[current.node]) {
                continue;
            }

            // Goal check: expand the goal only when it is popped,
            // not when it is first discovered. This is the correct
            // placement for optimality with admissible heuristics.
            if (current.node == goalNode) {
                return reconstructPath(cameFrom, goalNode);
            }

            closed[current.node] = true;

            // Relax all outgoing edges from current node
            for (Edge edge : adjacencyList.get(current.node)) {
                if (closed[edge.targetNode]) {
                    // With a CONSISTENT heuristic this skip is always safe.
                    // With only admissible (not consistent) heuristic,
                    // you'd need to allow re-opening — see callout below.
                    continue;
                }

                double tentativeG = gScore[current.node] + edge.cost;

                if (tentativeG < gScore[edge.targetNode]) {
                    // Found a cheaper path to this neighbour — update records
                    gScore[edge.targetNode] = tentativeG;
                    cameFrom[edge.targetNode] = current.node;

                    double fScore = tentativeG + heuristic.applyAsDouble(edge.targetNode);
                    // We push a new entry rather than updating in place.
                    // The stale-entry check above handles the old entry.
                    openSet.add(new PQEntry(edge.targetNode, fScore));
                }
            }
        }

        return Collections.emptyList(); // no path found
    }

    /** Walks cameFrom[] backwards from goal to start, then reverses. */
    private static List<Integer> reconstructPath(int[] cameFrom, int goalNode) {
        List<Integer> path = new ArrayList<>();
        for (int node = goalNode; node != -1; node = cameFrom[node]) {
            path.add(node);
        }
        Collections.reverse(path);
        return path;
    }

    // ── Demo: 6-node weighted graph ───────────────────────────────

    public static void main(String[] args) {
        //  Graph layout (directed, weighted):
        //
        //   0 --2.0--> 1 --1.0--> 3
        //   |          |          |
        //  4.0        3.0        2.0
        //   |          |          |
        //   2 --1.0--> 4 --1.0--> 5 (GOAL)
        //   \__________3.0________^
        //
        //  Straight-line heuristic distances to node 5 (admissible):
        //  h = { 0->4.0, 1->3.0, 2->2.0, 3->2.0, 4->1.0, 5->0.0 }

        int nodeCount = 6;
        List<List<Edge>> graph = new ArrayList<>();
        for (int i = 0; i < nodeCount; i++) graph.add(new ArrayList<>());

        graph.get(0).add(new Edge(1, 2.0));
        graph.get(0).add(new Edge(2, 4.0));
        graph.get(1).add(new Edge(3, 1.0));
        graph.get(1).add(new Edge(4, 3.0));
        graph.get(2).add(new Edge(4, 1.0));
        graph.get(2).add(new Edge(5, 3.0));
        graph.get(3).add(new Edge(5, 2.0));
        graph.get(4).add(new Edge(5, 1.0));

        // Heuristic: straight-line estimate to goal node 5
        double[] heuristicValues = { 4.0, 3.0, 2.0, 2.0, 1.0, 0.0 };

        List<Integer> path = findPath(
                graph,
                0,   // start
                5,   // goal
                node -> heuristicValues[node]
        );

        System.out.println("Shortest path: " + path);

        // Compute and print the total cost
        double totalCost = 0;
        for (int i = 0; i < path.size() - 1; i++) {
            int from = path.get(i);
            int to   = path.get(i + 1);
            for (Edge e : graph.get(from)) {
                if (e.targetNode == to) {
                    totalCost += e.cost;
                    break;
                }
            }
        }
        System.out.printf("Total path cost: %.1f%n", totalCost);
    }
}

/**
 * Demonstrates three common heuristics for grid-based A* pathfinding.
 * All three are admissible. Their tightness varies with movement type.
 */
public class GridHeuristics {

    // A simple 2D coordinate used as a grid cell
    record GridCell(int row, int col) {}

    // ── Heuristic 1: Manhattan Distance ───────────────────────────
    // Use when: 4-directional movement only (up/down/left/right).
    // Inadmissible for 8-dir or diagonal movement — it underestimates
    // wait, Manhattan >= true cost with diagonals? No — with diagonals
    // allowed, a diagonal costs ~1.414 but covers 2 Manhattan units,
    // so Manhattan OVERESTIMATES. Switch to Octile instead.
    public static double manhattan(GridCell current, GridCell goal) {
        return Math.abs(current.row() - goal.row())
             + Math.abs(current.col() - goal.col());
    }

    // ── Heuristic 2: Chebyshev Distance ───────────────────────────
    // Use when: 8-directional movement, all moves cost exactly 1
    // (i.e., diagonal cost == straight cost). Tight for that case.
    public static double chebyshev(GridCell current, GridCell goal) {
        int rowDiff = Math.abs(current.row() - goal.row());
        int colDiff = Math.abs(current.col() - goal.col());
        return Math.max(rowDiff, colDiff);
    }

    // ── Heuristic 3: Octile Distance ──────────────────────────────
    // Use when: 8-directional movement, diagonal cost = sqrt(2) ≈ 1.414.
    // This is the tightest admissible heuristic for that movement model.
    // Formula: straight_cost * |rowDiff - colDiff| + diagonal_cost * min(rowDiff, colDiff)
    public static double octile(GridCell current, GridCell goal) {
        double straightCost  = 1.0;
        double diagonalCost  = Math.sqrt(2); // ≈ 1.41421

        int rowDiff = Math.abs(current.row() - goal.row());
        int colDiff = Math.abs(current.col() - goal.col());

        int diagonalSteps = Math.min(rowDiff, colDiff);
        int straightSteps = Math.abs(rowDiff - colDiff);

        return diagonalCost * diagonalSteps + straightCost * straightSteps;
    }

    // ── Comparison demo ───────────────────────────────────────────
    public static void main(String[] args) {
        GridCell start = new GridCell(0, 0);
        GridCell goal  = new GridCell(4, 7);

        System.out.printf("From %s to %s:%n", start, goal);
        System.out.printf("  Manhattan : %.3f  (best for 4-dir movement)%n",
                manhattan(start, goal));
        System.out.printf("  Chebyshev : %.3f  (best for 8-dir, uniform cost)%n",
                chebyshev(start, goal));
        System.out.printf("  Octile    : %.3f  (best for 8-dir, sqrt(2) diagonal)%n",
                octile(start, goal));

        // Verify admissibility: none should exceed true cost.
        // True cost with 4-dir = 11, with 8-dir Chebyshev = 7,
        // with 8-dir Octile ~= 7 + 3*(sqrt(2)-1) ≈ 8.243
        System.out.println();
        System.out.println("Note: Manhattan overestimates for 8-dir movement");
        System.out.println("  -> INADMISSIBLE for diagonals (would break optimality)");
    }
}

import java.util.*;

/**
 * A* on a uniform-cost grid with tie-breaking by h-score.
 * Demonstrates the performance difference tie-breaking makes
 * on a 10x10 grid where all edge costs are 1.
 */
public class AStarWithTieBreaking {

    record Cell(int row, int col) {}

    // Directions: 4-connected grid
    static final int[][] DIRECTIONS = {{-1,0},{1,0},{0,-1},{0,1}};

    /**
     * Priority queue entry with tie-breaking.
     * Primary sort: ascending fScore.
     * Tie-break: ascending hScore (prefer nodes closer to goal).
     */
    record PQEntry(Cell cell, double fScore, double hScore)
            implements Comparable<PQEntry> {
        @Override
        public int compareTo(PQEntry other) {
            int cmp = Double.compare(this.fScore, other.fScore);
            if (cmp != 0) return cmp;
            // Tie-break: lower h means closer to goal — expand that first
            return Double.compare(this.hScore, other.hScore);
        }
    }

    static double manhattan(Cell a, Cell b) {
        return Math.abs(a.row() - b.row()) + Math.abs(a.col() - b.col());
    }

    /**
     * Returns the number of nodes expanded to find the path.
     * We use this to measure how much tie-breaking helps.
     */
    public static int searchAndCountExpansions(
            boolean[][] blocked,
            Cell start,
            Cell goal,
            boolean useTieBreaking) {

        int rows = blocked.length;
        int cols = blocked[0].length;

        double[][] gScore = new double[rows][cols];
        for (double[] row : gScore) Arrays.fill(row, Double.MAX_VALUE);
        gScore[start.row()][start.col()] = 0.0;

        boolean[][] closed = new boolean[rows][cols];
        int expansions = 0;

        PriorityQueue<PQEntry> openSet = new PriorityQueue<>();
        double startH = manhattan(start, goal);
        openSet.add(new PQEntry(start, startH, startH));

        while (!openSet.isEmpty()) {
            PQEntry current = openSet.poll();
            Cell cell = current.cell();

            if (closed[cell.row()][cell.col()]) continue;
            closed[cell.row()][cell.col()] = true;
            expansions++;

            if (cell.equals(goal)) return expansions;

            for (int[] dir : DIRECTIONS) {
                int newRow = cell.row() + dir[0];
                int newCol = cell.col() + dir[1];

                if (newRow < 0 || newRow >= rows ||
                    newCol < 0 || newCol >= cols ||
                    blocked[newRow][newCol]) continue;

                Cell neighbor = new Cell(newRow, newCol);
                if (closed[newRow][newCol]) continue;

                double tentativeG = gScore[cell.row()][cell.col()] + 1.0;

                if (tentativeG < gScore[newRow][newCol]) {
                    gScore[newRow][newCol] = tentativeG;
                    double h = manhattan(neighbor, goal);
                    // With tie-breaking: pass real h so compareTo can use it.
                    // Without tie-breaking: always pass 0 as h-tiebreaker
                    //   so the PQ treats all ties as equal (arbitrary order).
                    double tieH = useTieBreaking ? h : 0.0;
                    openSet.add(new PQEntry(neighbor,
                                            tentativeG + h,
                                            tieH));
                }
            }
        }
        return expansions; // goal unreachable
    }

    public static void main(String[] args) {
        // 10x10 open grid — no blocked cells
        // Start: top-left (0,0), Goal: bottom-right (9,9)
        boolean[][] grid = new boolean[10][10]; // false = passable
        Cell start = new Cell(0, 0);
        Cell goal  = new Cell(9, 9);

        int withoutTie = searchAndCountExpansions(grid, start, goal, false);
        int withTie    = searchAndCountExpansions(grid, start, goal, true);

        System.out.println("=== Tie-Breaking Impact on 10x10 Open Grid ===");
        System.out.printf("Nodes expanded WITHOUT tie-breaking: %d%n", withoutTie);
        System.out.printf("Nodes expanded WITH    tie-breaking: %d%n", withTie);
        System.out.printf("Improvement: %.0f%% fewer expansions%n",
                100.0 * (withoutTie - withTie) / withoutTie);
    }
}