Grid Pathfinding

Step 1 - Create the project and the maze file

1. Create a new Console App (C++) project the same way as the SFML project in the Environment Setup lab, named Pathfinding, with SFML linked, the platform set to x64 and the C++ Language Standard set to C++17 or later. Visual Studio creates Pathfinding.cpp with a Hello World main(). Leave it alone for now.
2. In Solution Explorer, right-click the project and use Add > New Item. Choose Text File (under Utility) and name it maze.txt.
3. Paste in the maze below. This is your starting maze. Later on you can edit it or draw your own.

Each character is one cell. # is a wall, . (or a space) is floor that costs 1 to walk onto, ~ is mud that costs 5, S is the start and E is the exit. Notice the band of mud down the middle. The shortest route in steps goes straight through it.

Visual Studio runs the program from the project folder, which is where maze.txt now lives, so the program will find it when you press F5.

########################################
#.......#...........~~~~...............#
#.#####.#.#######...~~~~...#######.###.#
#.#.....#.#.........~~~~.........#...#.#
#.#.#####.#.#####...~~~~...#####.###.#.#
#.#.......#.#.......~~~~.......#.....#.#
#.#########.#.###...~~~~...###.#######.#
#...........#...#...~~~~...#...........#
#.#######.#####.#...~~~~...#.#########.#
#S......#.......#...~~~~...#.........E.#
#.#####.#########...~~~~...###########.#
#.....#.............~~~~...............#
#####.#####.####....~~~~....####.#######
#.........#.........####...............#
#.#######.#............................#
#.......#.........................######
#######.#..............................#
#..........##############..............#
#......................................#
########################################

Step 2 - The Grid class

In Solution Explorer use Add > Class... and create a class named Grid. This generates Grid.h and Grid.cpp. Replace everything in Grid.h with the header below.

Every cell is addressed by a single int, index = y * width + x, so per-cell data can live in a flat std::vector. toX() and toY() turn an index back into a column and row for drawing. Walls are stored with a cost of WALL (-1).

#pragma once
#include <string>
#include <vector>

/**
 * GRID - the maze every pathfinder searches
 * =========================================
 *
 * Built from a plain ASCII file (maze.txt) so the maze can be drawn in any
 * text editor:
 *
 *   #        wall   - impassable
 *   . or ' ' floor  - costs 1 to enter
 *   ~        mud    - costs 5 to enter (walkable, just expensive)
 *   S        start  - exactly one
 *   E        exit   - exactly one
 *
 * Mud is what makes Dijkstra and A* worth having: BFS only counts STEPS, so
 * it will happily wade straight through mud, while the cost-aware searches
 * walk further to stay dry when that's cheaper overall.
 *
 * CELL INDICES
 * Every cell is addressed by one int, index = y * width + x. That lets the
 * pathfinders store everything per-cell in a flat std::vector instead of a
 * 2D array or a hash map. toX()/toY() convert back for drawing.
 */
class Grid {
public:
    static constexpr int WALL = -1;
    static constexpr int FLOOR_COST = 1;
    static constexpr int MUD_COST = 5;

    // Returns false (and fills 'error') if the file is missing or has no S / E.
    bool load(const std::string& path, std::string& error);

    int width() const { return w; }
    int height() const { return h; }
    int cellCount() const { return w * h; }

    int toIndex(int x, int y) const { return y * w + x; }
    int toX(int index) const { return index % w; }
    int toY(int index) const { return index / w; }

    bool isWall(int index) const { return costs[index] == WALL; }
    int cost(int index) const { return costs[index]; }

    int start() const { return startCell; }
    int goal() const { return goalCell; }

    // Walkable cells directly up/down/left/right of 'index' (no diagonals).
    std::vector<int> neighbours(int index) const;

private:
    int w = 0;
    int h = 0;
    std::vector<int> costs;   // WALL, FLOOR_COST or MUD_COST per cell
    int startCell = -1;
    int goalCell = -1;
};

Leave Grid.cpp containing just #include "Grid.h" for now. The project still builds. load() and neighbours() are declared but not written yet, which is fine as long as nothing calls them.

Step 3 - Grid::load

Open Grid.cpp and replace its contents with the code below. load() works in four stages:

1. Read every line of the file, trimming a stray \r and any blank lines at the end.
2. Use the longest line as the width, so shorter rows are padded with floor.
3. Turn each character into a cost and record where S and E are.
4. Fail with a clear message if either S or E is missing.

#include "Grid.h"
#include <fstream>
#include <algorithm>

bool Grid::load(const std::string& path, std::string& error) {
    std::ifstream file(path);
    if (!file) {
        error = "Could not open " + path;
        return false;
    }

    std::vector<std::string> lines;
    std::string line;
    while (std::getline(file, line)) {
        if (!line.empty() && line.back() == '\r') line.pop_back();
        lines.push_back(line);
    }
    // Ignore trailing blank lines so an extra newline at the end of the file
    // doesn't add an empty row.
    while (!lines.empty() && lines.back().empty()) lines.pop_back();

    if (lines.empty()) {
        error = path + " is empty";
        return false;
    }

    // Ragged rows are padded with floor out to the widest row.
    h = static_cast<int>(lines.size());
    w = 0;
    for (const std::string& row : lines) w = std::max(w, static_cast<int>(row.size()));

    costs.assign(w * h, FLOOR_COST);
    startCell = -1;
    goalCell = -1;

    for (int y = 0; y < h; ++y) {
        for (int x = 0; x < static_cast<int>(lines[y].size()); ++x) {
            const int index = toIndex(x, y);
            switch (lines[y][x]) {
                case '#': costs[index] = WALL; break;
                case '~': costs[index] = MUD_COST; break;
                case 'S': startCell = index; break;
                case 'E': goalCell = index; break;
                default: break;   // '.', ' ' or anything unknown = floor
            }
        }
    }

    if (startCell < 0) { error = path + " has no start cell 'S'"; return false; }
    if (goalCell < 0)  { error = path + " has no exit cell 'E'"; return false; }
    return true;
}

Step 4 - Grid::neighbours

Add this function to the bottom of Grid.cpp. It returns the walkable cells directly up, right, down and left of a cell. Cells off the edge of the maze and walls are skipped. There are no diagonal moves.

The order of dx/dy (up, right, down, left) decides the order neighbours are found in. It makes no difference to BFS, Dijkstra or A*, but it decides which way DFS turns at every junction.

std::vector<int> Grid::neighbours(int index) const {
    const int x = toX(index);
    const int y = toY(index);
    const int dx[4] = { 0, 1, 0, -1 };   // up, right, down, left
    const int dy[4] = { -1, 0, 1, 0 };

    std::vector<int> result;
    for (int i = 0; i < 4; ++i) {
        const int nx = x + dx[i];
        const int ny = y + dy[i];
        if (nx < 0 || ny < 0 || nx >= w || ny >= h) continue;
        const int n = toIndex(nx, ny);
        if (!isWall(n)) result.push_back(n);
    }
    return result;
}

Step 5 - main: load the maze and open a window

Open Pathfinding.cpp and replace the Hello World code with the code below. main() loads the maze and stops with an error message if that fails. It then opens a window exactly big enough for the maze at CELL_SIZE pixels per cell, plus a strip at the bottom for text.

#include <SFML/Graphics.hpp>
#include <iostream>
#include <string>
#include "Grid.h"

namespace {
    constexpr float CELL_SIZE = 24.0f;
    constexpr float HUD_HEIGHT = 70.0f;
}

int main() {
    Grid grid;
    std::string error;
    if (!grid.load("maze.txt", error)) {
        std::cerr << error << std::endl;
        return 1;
    }

    const unsigned int windowWidth = static_cast<unsigned int>(grid.width() * CELL_SIZE);
    const unsigned int windowHeight = static_cast<unsigned int>(grid.height() * CELL_SIZE + HUD_HEIGHT);
    sf::RenderWindow window(sf::VideoMode(sf::Vector2u(windowWidth, windowHeight)), "Pathfinding");
    window.setFramerateLimit(60);

    while (window.isOpen()) {
        while (const std::optional event = window.pollEvent()) {
            if (event->is<sf::Event::Closed>()) {
                window.close();
            }
        }

        window.clear(sf::Color(20, 20, 25));

        window.display();
    }
    return 0;
}

Run. A dark, empty window opens, 960 by 550 pixels for the default maze (40 by 20 cells). If a console message says Could not open maze.txt instead, check that maze.txt is in the same folder as Pathfinding.cpp.

Step 6 - cellColour and drawing the cells

Three edits to Pathfinding.cpp:

1. Inside the namespace { } block, after HUD_HEIGHT, add the colours and the cellColour() function. It picks one colour for each kind of cell.

    const sf::Color WALL_COLOUR(40, 40, 50);
    const sf::Color FLOOR_COLOUR(225, 225, 215);
    const sf::Color MUD_COLOUR(140, 100, 60);
    const sf::Color START_COLOUR(40, 180, 70);
    const sf::Color GOAL_COLOUR(220, 50, 50);

    sf::Color cellColour(const Grid& grid, int cell) {
        if (grid.isWall(cell)) return WALL_COLOUR;
        if (cell == grid.start()) return START_COLOUR;
        if (cell == grid.goal()) return GOAL_COLOUR;
        return grid.cost(cell) == Grid::MUD_COST ? MUD_COLOUR : FLOOR_COLOUR;
    }

2. After window.setFramerateLimit(60);, create one rectangle that is reused for every cell. It is one pixel smaller than a cell so a thin grid line shows between cells.

    sf::RectangleShape cellShape(sf::Vector2f(CELL_SIZE - 1.0f, CELL_SIZE - 1.0f));

3. After window.clear(...), loop over every cell, move the rectangle there, colour it and draw it.

        for (int cell = 0; cell < grid.cellCount(); ++cell) {
            cellShape.setPosition(sf::Vector2f(grid.toX(cell) * CELL_SIZE, grid.toY(cell) * CELL_SIZE));
            cellShape.setFillColor(cellColour(grid, cell));
            window.draw(cellShape);
        }

Run. The maze from maze.txt appears: dark walls, light floor, a brown band of mud, a green start on the left and a red exit on the right.

Step 7 - The Pathfinder base class

Use Add > Class... to create a class named Pathfinder, then replace everything in Pathfinder.h with the header below.

This is the abstract base class every algorithm derives from. It owns everything the four searches share: a CellState per cell (used for colouring), cameFrom, costSoFar, a count of cells explored, and the finished path. step(), begin() and name() are pure virtual, so each algorithm must supply its own.

#pragma once
#include <vector>
#include "Grid.h"

/**
 * PATHFINDER - the common interface every search algorithm implements
 * ===================================================================
 *
 * BFS, DFS, Dijkstra and A* are all the SAME loop:
 *
 *   put start in the frontier
 *   while the frontier isn't empty:
 *       take a cell out of the frontier
 *       if it's the goal -> done, walk cameFrom back to start
 *       for each neighbour: if it's new (or now cheaper), record how we
 *           got there in cameFrom and put it in the frontier
 *
 * The ONLY real difference between them is which cell the frontier hands
 * back next:
 *
 *   BFS       queue           - oldest first      -> spreads out in rings
 *   DFS       stack           - newest first      -> dives down one corridor
 *   Dijkstra  priority queue  - cheapest so far   -> cost-aware rings
 *   A*        priority queue  - cheapest so far + guess to goal -> aims at E
 *
 * So this base class owns everything that is shared - per-cell state for
 * drawing, cameFrom, costSoFar and rebuilding the final path - and each
 * subclass only supplies its own frontier container plus begin()/step().
 *
 * STEPPING
 * step() expands exactly ONE cell and returns. main() calls it a few times
 * per frame so the search animates instead of finishing instantly.
 */

// What the renderer colours each cell as.
enum class CellState {
    Unseen,     // never reached
    Frontier,   // discovered, waiting in the frontier ("open set")
    Visited     // taken out of the frontier and expanded ("closed set")
};

class Pathfinder {
public:
    virtual ~Pathfinder() = default;

    virtual const char* name() const = 0;

    // Clears all shared state for a fresh search on 'grid', then calls begin().
    void reset(const Grid& grid);

    // Expand one cell. Returns false once the search has finished.
    virtual bool step() = 0;

    CellState state(int cell) const { return states[cell]; }
    const std::vector<int>& path() const { return pathCells; }
    bool finished() const { return isFinished; }
    bool found() const { return wasFound; }
    int expandedCount() const { return expanded; }
    int pathCost() const;

protected:
    // Seed the subclass's frontier with grid->start().
    virtual void begin() = 0;

    // Ends the search. If 'reachedGoal', walks cameFrom back from the goal
    // to build pathCells (start -> goal).
    void finish(bool reachedGoal);

    const Grid* grid = nullptr;
    std::vector<CellState> states;
    std::vector<int> cameFrom;    // cell we arrived from, -1 = none
    std::vector<int> costSoFar;   // cheapest known cost from start, -1 = unknown
    int expanded = 0;

private:
    std::vector<int> pathCells;
    bool isFinished = false;
    bool wasFound = false;
};

Step 8 - Pathfinder::reset

Replace the contents of Pathfinder.cpp with the code below. reset() sizes every per-cell vector to the grid, clears it, and then calls begin(). Because begin() is virtual, this runs whichever subclass's version is being used, so every search starts from a clean slate the same way.

#include "Pathfinder.h"
#include <algorithm>

void Pathfinder::reset(const Grid& g) {
    grid = &g;
    states.assign(g.cellCount(), CellState::Unseen);
    cameFrom.assign(g.cellCount(), -1);
    costSoFar.assign(g.cellCount(), -1);
    expanded = 0;
    pathCells.clear();
    isFinished = false;
    wasFound = false;
    begin();
}

Step 9 - Pathfinder::finish

Add this to the bottom of Pathfinder.cpp. A subclass calls finish() when its search ends. If the exit was reached, it follows the cameFrom links backwards from the exit until it reaches the start (whose cameFrom is -1), then reverses the list so the path runs start to exit.

void Pathfinder::finish(bool reachedGoal) {
    isFinished = true;
    wasFound = reachedGoal;
    if (!reachedGoal) return;

    // Follow the breadcrumbs from the goal back to the start, then flip.
    for (int cell = grid->goal(); cell != -1; cell = cameFrom[cell]) {
        pathCells.push_back(cell);
    }
    std::reverse(pathCells.begin(), pathCells.end());
}

Step 10 - Pathfinder::pathCost

Add this to the bottom of Pathfinder.cpp. It adds up the cost of every cell on the path, skipping the start cell because you are already standing on it. This is the number to compare between algorithms, because walking through mud makes it higher than the step count.

int Pathfinder::pathCost() const {
    // The start cell is where we stand, so it isn't "entered" - skip it.
    int total = 0;
    for (std::size_t i = 1; i < pathCells.size(); ++i) total += grid->cost(pathCells[i]);
    return total;
}

Step 11 - The BFS class

Use Add > Class... to create a class named BFS, then replace everything in BFS.h with the header below. BFS derives from Pathfinder and adds just one thing of its own: a std::queue<int> frontier.

#pragma once
#include <queue>
#include "Pathfinder.h"

/**
 * BREADTH-FIRST SEARCH (BFS)
 * ==========================
 *
 * Frontier: std::queue - first in, first out.
 *
 * Because the oldest cell always comes out first, BFS finishes every cell
 * 1 step from the start before any cell 2 steps away, then 3, and so on.
 * Watch it spread out in rings.
 *
 * GUARANTEE: the path has the fewest STEPS possible. It does NOT look at
 * cell cost at all, so if the shortest route goes through mud, BFS takes
 * it - compare its path cost with Dijkstra's on the same maze.
 *
 * Cells are marked as soon as they're pushed, so each cell enters the
 * queue at most once. COMPLEXITY: O(V + E) - every cell and edge once.
 */
class BFS : public Pathfinder {
public:
    const char* name() const override { return "BFS  (queue - fewest steps, ignores cost)"; }
    bool step() override;

protected:
    void begin() override;

private:
    std::queue<int> frontier;
};

Step 12 - BFS::begin

Replace the contents of BFS.cpp with the code below. begin() empties the queue, then puts the start cell in it and marks it as part of the frontier.

#include "BFS.h"

void BFS::begin() {
    frontier = {};
    const int start = grid->start();
    frontier.push(start);
    states[start] = CellState::Frontier;
}

Step 13 - BFS::step

Add this to the bottom of BFS.cpp. Each call explores exactly one cell:

1. If the queue is empty, every reachable cell has been explored without finding the exit, so finish with no path.
2. Take the oldest cell out of the queue and mark it Visited.
3. If it is the exit, finish with a path.
4. Otherwise queue every neighbour not seen before, recording where it came from.

A cell is marked as soon as it is queued, so it can never be queued twice. The function returns false once the search is over.

bool BFS::step() {
    if (finished()) return false;
    if (frontier.empty()) { finish(false); return false; }

    const int cell = frontier.front();
    frontier.pop();
    states[cell] = CellState::Visited;
    ++expanded;

    if (cell == grid->goal()) { finish(true); return false; }

    for (int next : grid->neighbours(cell)) {
        if (states[next] != CellState::Unseen) continue;   // already queued or done
        states[next] = CellState::Frontier;
        cameFrom[next] = cell;
        frontier.push(next);
    }
    return true;
}

Step 14 - Run BFS in main

Now connect BFS to the window. In Pathfinding.cpp:

1. Add two includes under #include "Grid.h".

#include <memory>
#include "BFS.h"

2. In the namespace, add a steps-per-frame constant after HUD_HEIGHT, and two colours after MUD_COLOUR. Yellow is the frontier and blue is explored.

    constexpr int STEPS_PER_FRAME = 2;

    const sf::Color FRONTIER_COLOUR(250, 210, 70);
    const sf::Color VISITED_COLOUR(150, 190, 235);

3. Replace the whole cellColour() function with this version. It now also takes the Pathfinder and colours floor and mud by their search state. Explored mud gets a darker blue so you can still see where the mud is.

    // Visited cells over mud are tinted darker so the cost stays visible.
    sf::Color cellColour(const Grid& grid, const Pathfinder& finder, int cell) {
        if (grid.isWall(cell)) return WALL_COLOUR;
        if (cell == grid.start()) return START_COLOUR;
        if (cell == grid.goal()) return GOAL_COLOUR;

        const bool mud = grid.cost(cell) == Grid::MUD_COST;
        switch (finder.state(cell)) {
            case CellState::Frontier: return FRONTIER_COLOUR;
            case CellState::Visited:  return mud ? sf::Color(95, 120, 160) : VISITED_COLOUR;
            default:                  return mud ? MUD_COLOUR : FLOOR_COLOUR;
        }
    }

4. After window.setFramerateLimit(60);, create the search. The variable's type is the base class pointer, which is what lets the other algorithms be dropped in later.

    std::unique_ptr<Pathfinder> finder = std::make_unique<BFS>();
    finder->reset(grid);

5. After the event loop's closing brace and before window.clear(...), advance the search.

        for (int i = 0; i < STEPS_PER_FRAME; ++i) finder->step();

6. In the drawing loop, change cellColour(grid, cell) to cellColour(grid, *finder, cell).

Run. Starting at the green cell, a yellow edge spreads outwards with blue explored cells behind it, in rings of equal step distance. Notice it goes straight through the mud at the same speed as the floor. When the yellow reaches the red exit, the animation stops.

Step 15 - Draw the path

1. In the namespace, add a path colour after VISITED_COLOUR.

    const sf::Color PATH_COLOUR(220, 40, 160);

2. After the cellShape line, create a small square centred on its own origin to mark path cells.

    sf::RectangleShape pathShape(sf::Vector2f(CELL_SIZE * 0.5f, CELL_SIZE * 0.5f));
    pathShape.setOrigin(sf::Vector2f(CELL_SIZE * 0.25f, CELL_SIZE * 0.25f));
    pathShape.setFillColor(PATH_COLOUR);

3. After the cell drawing loop, draw a dot in the middle of each path cell. Until the search finishes, path() is empty and nothing is drawn.

        for (int cell : finder->path()) {
            pathShape.setPosition(sf::Vector2f((grid.toX(cell) + 0.5f) * CELL_SIZE, (grid.toY(cell) + 0.5f) * CELL_SIZE));
            window.draw(pathShape);
        }

Run. When BFS reaches the exit, a line of magenta dots shows the path it found. It runs straight across the mud, because BFS counts steps and ignores cost.

Step 16 - On-screen stats

1. After window.setFramerateLimit(60);, load a font and set up a text object in the strip below the maze.

    sf::Font font;
    if (!font.openFromFile("C:/Windows/Fonts/arial.ttf")) {
        std::cerr << "Failed to load font - stats text will not render." << std::endl;
    }
    sf::Text hud(font, "", 16);
    hud.setPosition(sf::Vector2f(8.0f, grid.height() * CELL_SIZE + 6.0f));
    hud.setFillColor(sf::Color::White);

2. Just before window.display();, build the text: the algorithm's name, how many cells it has explored, and, once finished, the path length in steps and its cost.

        std::string status = "searching...";
        if (finder->finished()) {
            status = finder->found()
                ? "path: " + std::to_string(finder->path().size() - 1) + " steps, cost " + std::to_string(finder->pathCost())
                : "NO PATH";
        }
        hud.setString(std::string(finder->name()) +
            "\nexpanded: " + std::to_string(finder->expandedCount()) + "   " + status);
        window.draw(hud);

Run. On the default maze BFS finishes with expanded: 448, path: 42 steps, cost 58. Keep these numbers to compare against the other algorithms.

Step 17 - Pause, step and restart controls

1. After finder->reset(grid);, add a pause flag.

    bool paused = false;

2. In the event loop, extend the if that checks for Closed with a key-press branch, so it reads as below. R restarts the search, Space pauses, and N explores exactly one cell while paused.

            if (event->is<sf::Event::Closed>()) {
                window.close();
            } else if (const auto* key = event->getIf<sf::Event::KeyPressed>()) {
                using K = sf::Keyboard::Key;
                if (key->code == K::R) finder->reset(grid);
                if (key->code == K::Space) paused = !paused;
                if (key->code == K::N && paused) finder->step();
            }

3. Wrap the stepping loop so it only runs when not paused.

        if (!paused) {
            for (int i = 0; i < STEPS_PER_FRAME; ++i) finder->step();
        }

4. Replace the hud.setString(...) call so it also shows when the program is paused and lists the controls.

        hud.setString(std::string(finder->name()) + (paused ? "   [PAUSED]" : "") +
            "\nexpanded: " + std::to_string(finder->expandedCount()) + "   " + status +
            "\n1-4 algorithm   Space pause   N step   R restart");

Run. Press R to restart, then Space straight away to pause. Tap N to watch BFS take one cell from the front of the queue at a time and add its neighbours to the yellow frontier.

Step 18 - The DFS class

Use Add > Class... to create a class named DFS, then replace everything in DFS.h with the header below. It has the same shape as BFS, with a std::stack<int> in place of the queue.

#pragma once
#include <stack>
#include "Pathfinder.h"

/**
 * DEPTH-FIRST SEARCH (DFS)
 * ========================
 *
 * Frontier: std::stack - last in, first out.
 *
 * The cell pushed most recently comes out first, so DFS commits to one
 * corridor and keeps going until it hits a dead end, then backs up to the
 * last junction with an unexplored branch. Watch it snake rather than
 * spread.
 *
 * NO GUARANTEE: DFS finds A path, not the shortest one - it's whichever
 * route it happened to dive down first. Which neighbour gets pushed last
 * (Grid::neighbours order) decides which way it turns at every junction.
 *
 * A cell can sit in the stack more than once (pushed from two different
 * neighbours before it's expanded), so stale entries are skipped when
 * popped. COMPLEXITY: O(V + E).
 */
class DFS : public Pathfinder {
public:
    const char* name() const override { return "DFS  (stack - any path, not shortest)"; }
    bool step() override;

protected:
    void begin() override;

private:
    std::stack<int> frontier;
};

Step 19 - DFS::begin

Replace the contents of DFS.cpp with the code below. It is the same as BFS::begin(), pushing the start cell onto the stack.

#include "DFS.h"

void DFS::begin() {
    frontier = {};
    const int start = grid->start();
    frontier.push(start);
    states[start] = CellState::Frontier;
}

Step 20 - DFS::step

Add this to the bottom of DFS.cpp. The newest cell comes off the stack first, so DFS keeps following one corridor until it hits a dead end.

A cell can be pushed more than once (from two different neighbours) before it is explored, so the while loop at the top throws away any copy that has already been visited. The newest push always pops first, which is why overwriting cameFrom is correct here.

bool DFS::step() {
    if (finished()) return false;

    // Skip cells that were pushed twice and have already been expanded.
    while (!frontier.empty() && states[frontier.top()] == CellState::Visited) frontier.pop();
    if (frontier.empty()) { finish(false); return false; }

    const int cell = frontier.top();
    frontier.pop();
    states[cell] = CellState::Visited;
    ++expanded;

    if (cell == grid->goal()) { finish(true); return false; }

    for (int next : grid->neighbours(cell)) {
        if (states[next] == CellState::Visited) continue;
        // Overwrite cameFrom: the newest push is the one that pops first.
        states[next] = CellState::Frontier;
        cameFrom[next] = cell;
        frontier.push(next);
    }
    return true;
}

Step 21 - Switching algorithms with the number keys

1. Add #include "DFS.h" under #include "BFS.h".
2. In the namespace, after GOAL_COLOUR, add a factory function that turns a key number into a new Pathfinder. It returns the base class pointer, so main() never needs to know which algorithm it is running.

    std::unique_ptr<Pathfinder> makePathfinder(int key) {
        switch (key) {
            case 2:  return std::make_unique<DFS>();
            default: return std::make_unique<BFS>();
        }
    }

3. Replace the line that creates finder with these two lines.

    int currentKey = 1;
    std::unique_ptr<Pathfinder> finder = makePathfinder(currentKey);

4. In the key-press branch, after using K = sf::Keyboard::Key;, read the number keys. Choosing a key replaces the pathfinder with a new one and resets it.

                int chosen = 0;
                if (key->code == K::Num1) chosen = 1;
                if (key->code == K::Num2) chosen = 2;
                if (chosen != 0) {
                    currentKey = chosen;
                    finder = makePathfinder(currentKey);
                    finder->reset(grid);
                }

Run. Press 2. DFS snakes down one corridor at a time instead of spreading out, and finishes with expanded: 394, path: 352 steps, cost 480. It found a path, just nowhere near the shortest one. Press 1 to watch BFS again for comparison.

Step 22 - The Dijkstra class

Use Add > Class... to create a class named Dijkstra, then replace everything in Dijkstra.h with the header below.

The frontier is a std::priority_queue of (costSoFar, cell) pairs. By default std::priority_queue hands back the largest value first. Passing std::greater flips that, so the cheapest cell always comes out first.

#pragma once
#include <queue>
#include <vector>
#include <functional>
#include "Pathfinder.h"

/**
 * DIJKSTRA'S ALGORITHM
 * ====================
 *
 * Frontier: std::priority_queue ordered by costSoFar - cheapest first.
 *
 * Like BFS it spreads out in rings, but the rings are measured in COST,
 * not steps: a mud cell (cost 5) counts as five floor cells, so the search
 * stalls at mud and floods round it if there's a cheaper way.
 *
 * GUARANTEE: the path with the lowest total cost (all costs must be >= 0).
 *
 * RELAXATION: when a neighbour is reached more cheaply than before, its
 * costSoFar and cameFrom are updated and it's pushed AGAIN.
 * std::priority_queue can't change an entry's priority in place, so the
 * old, more expensive entry is left behind and skipped when popped
 * ("lazy deletion").
 *
 * COMPLEXITY: O((V + E) log V) with a binary heap.
 */
class Dijkstra : public Pathfinder {
public:
    const char* name() const override { return "DIJKSTRA  (priority queue - cheapest cost)"; }
    bool step() override;

protected:
    void begin() override;

private:
    // (costSoFar, cell) - std::greater turns the default max-heap into a min-heap.
    using Entry = std::pair<int, int>;
    std::priority_queue<Entry, std::vector<Entry>, std::greater<Entry>> frontier;
};

Step 23 - Dijkstra::begin

Replace the contents of Dijkstra.cpp with the code below. Unlike BFS and DFS, Dijkstra tracks cost, so the start cell is given a costSoFar of 0 and pushed with priority 0.

#include "Dijkstra.h"

void Dijkstra::begin() {
    frontier = {};
    const int start = grid->start();
    costSoFar[start] = 0;
    frontier.push({ 0, start });
    states[start] = CellState::Frontier;
}

Step 24 - Dijkstra::step

Add this to the bottom of Dijkstra.cpp. The cheapest cell so far comes out first. For each neighbour, the cost of getting there through this cell is worked out. If that is the first route found, or cheaper than the best one so far, the neighbour's cost and cameFrom are updated and it is pushed again.

A std::priority_queue cannot change the priority of an entry already in it, so the older, more expensive entry is simply left in the queue. The while loop at the top throws those stale entries away when they reach the front.

bool Dijkstra::step() {
    if (finished()) return false;

    // Lazy deletion: throw away entries for cells already expanded.
    while (!frontier.empty() && states[frontier.top().second] == CellState::Visited) frontier.pop();
    if (frontier.empty()) { finish(false); return false; }

    const int cell = frontier.top().second;
    frontier.pop();
    states[cell] = CellState::Visited;
    ++expanded;

    if (cell == grid->goal()) { finish(true); return false; }

    for (int next : grid->neighbours(cell)) {
        if (states[next] == CellState::Visited) continue;
        const int newCost = costSoFar[cell] + grid->cost(next);
        if (costSoFar[next] != -1 && newCost >= costSoFar[next]) continue;   // no improvement

        costSoFar[next] = newCost;
        cameFrom[next] = cell;
        states[next] = CellState::Frontier;
        frontier.push({ newCost, next });
    }
    return true;
}

Step 25 - Add Dijkstra to key 3

1. Add #include "Dijkstra.h" under #include "DFS.h".
2. In makePathfinder(), add a case after case 2.

            case 3:  return std::make_unique<Dijkstra>();

3. In the key-press branch, add a line after the Num2 check.

                if (key->code == K::Num3) chosen = 3;

Run. Press 3. Dijkstra spreads out like BFS, but the explored area stalls when it reaches the mud and floods around it. It finishes with path: 48 steps, cost 48. That is 6 more steps than BFS, but a cost of 48 instead of 58, because it walked around the mud. It explored 452 cells, roughly as many as BFS.

Step 26 - The AStar class

Use Add > Class... to create a class named AStar, then replace everything in AStar.h with the header below.

The frontier now stores (f, h, cell), where f is the cost so far plus h, a guess at the cost still to go. When two cells have the same f, the one with the smaller h, which is closer to the exit, comes out first.

#pragma once
#include <queue>
#include <tuple>
#include <vector>
#include <functional>
#include "Pathfinder.h"

/**
 * A* SEARCH
 * =========
 *
 * Frontier: std::priority_queue ordered by f = g + h
 *   g = costSoFar   - what it actually cost to get here (Dijkstra's number)
 *   h = heuristic   - a GUESS of the cost still to go from here to the exit
 *
 * Dijkstra explores evenly in every direction because it only knows g.
 * Adding h pulls the search towards the exit: cells that are cheap AND
 * close to E come out first. Watch the visited area lean towards E instead
 * of growing as a blob.
 *
 * HEURISTIC: Manhattan distance |dx| + |dy| (we only move in 4 directions)
 * times the cheapest possible cell cost (1). That can never OVERestimate
 * the real remaining cost - it's "admissible" - so A* still returns the
 * cheapest path, the same cost Dijkstra finds, usually after expanding
 * far fewer cells. Try making h bigger (e.g. * 5) to see it get faster
 * but stop being optimal.
 *
 * Set h = 0 and A* IS Dijkstra.
 */
class AStar : public Pathfinder {
public:
    const char* name() const override { return "A*  (priority queue - cost + distance to exit)"; }
    bool step() override;

protected:
    void begin() override;

private:
    int heuristic(int cell) const;

    // (f, h, cell). On equal f, smaller h wins - prefer cells nearer the exit.
    using Entry = std::tuple<int, int, int>;
    std::priority_queue<Entry, std::vector<Entry>, std::greater<Entry>> frontier;
};

Step 27 - AStar::heuristic

Replace the contents of AStar.cpp with the code below. The heuristic is the Manhattan distance to the exit, the number of up, down, left and right moves needed if there were no walls, multiplied by the cheapest cell cost. It can never be more than the real remaining cost, which is what keeps A*'s path the cheapest one.

#include "AStar.h"
#include <cstdlib>

int AStar::heuristic(int cell) const {
    const int dx = std::abs(grid->toX(cell) - grid->toX(grid->goal()));
    const int dy = std::abs(grid->toY(cell) - grid->toY(grid->goal()));
    return (dx + dy) * Grid::FLOOR_COST;
}

Step 28 - AStar::begin

Add this to the bottom of AStar.cpp. It is Dijkstra::begin() again, except the start cell's priority is its heuristic, since its cost so far is 0.

void AStar::begin() {
    frontier = {};
    const int start = grid->start();
    costSoFar[start] = 0;
    frontier.push({ heuristic(start), heuristic(start), start });
    states[start] = CellState::Frontier;
}

Step 29 - AStar::step

Add this to the bottom of AStar.cpp. Put it side by side with Dijkstra::step(). The only meaningful change is the last two lines of the loop, where a cell is pushed with priority newCost + h instead of newCost.

bool AStar::step() {
    if (finished()) return false;

    // Lazy deletion, exactly as in Dijkstra.
    while (!frontier.empty() && states[std::get<2>(frontier.top())] == CellState::Visited) frontier.pop();
    if (frontier.empty()) { finish(false); return false; }

    const int cell = std::get<2>(frontier.top());
    frontier.pop();
    states[cell] = CellState::Visited;
    ++expanded;

    if (cell == grid->goal()) { finish(true); return false; }

    for (int next : grid->neighbours(cell)) {
        if (states[next] == CellState::Visited) continue;
        const int newCost = costSoFar[cell] + grid->cost(next);
        if (costSoFar[next] != -1 && newCost >= costSoFar[next]) continue;

        costSoFar[next] = newCost;
        cameFrom[next] = cell;
        states[next] = CellState::Frontier;
        // The only line that differs from Dijkstra: priority is g + h, not g.
        const int h = heuristic(next);
        frontier.push({ newCost + h, h, next });
    }
    return true;
}

Step 30 - Add A* to key 4

1. Add #include "AStar.h" under #include "Dijkstra.h".
2. In makePathfinder(), add a case after case 3.

            case 4:  return std::make_unique<AStar>();

3. In the key-press branch, add a line after the Num3 check.

                if (key->code == K::Num4) chosen = 4;

Run. Press 4. Instead of growing evenly in every direction, the explored area leans towards the exit. A* finds the same answer as Dijkstra, 48 steps, cost 48, but explores only 168 cells compared with Dijkstra's 452. Flick between 3 and 4 to see the difference.

Your four results on the default maze should be:

Step 31 - Make your own maze

Close the program, edit maze.txt and run it again. The grid resizes itself to whatever you draw. Some things to try:

1. Wall off the exit completely with #. Every algorithm explores everything it can reach and reports NO PATH.
2. Remove the mud. Replace the ~ cells with . and compare BFS with Dijkstra. When every cell costs the same, they find paths of the same cost.
3. Draw a wide open room with few walls, and compare how many cells Dijkstra and A* explore.
4. Delete the S. The program prints maze.txt has no start cell 'S' to the console and exits.