Introduction

In this blog post, we'll dive into creating a pathfinding system using the A* (A-star) algorithm in C++. The A* algorithm is widely used in games and robotics for finding the shortest path between two points while navigating around obstacles. This guide will cover the fundamentals of the algorithm, how to implement it in C++, and how to visualize the results.

Overview of A* Algorithm

The A* algorithm combines the advantages of Dijkstra's algorithm and Greedy Best-First Search. It uses a heuristic to estimate the cost of the path to the goal, which helps in efficiently finding the shortest path.

Key Concepts:

G-cost: The cost from the start node to the current node.
H-cost: The heuristic cost from the current node to the goal node.
F-cost: The total cost (G-cost + H-cost).

Setting Up the Project

Create a new C++ project: Use your preferred IDE or command line tools to set up a new C++ project.
Add necessary files: Create the following files:
- main.cpp: The entry point of your application.
- pathfinding.cpp and pathfinding.h: For the A* algorithm implementation.
- node.cpp and node.h: For node representation and operations.

Also read: Creating a Game Engine with C++ and OpenGL

Code Implementation

Node Structure

Define the Node class to represent each node in the grid.

node.h

#ifndef NODE_H
#define NODE_H
 
class Node {
public:
    int x, y;
    float gCost, hCost, fCost;
    Node* parent;
    bool walkable;
 
    Node(int x, int y, bool walkable);
    void calculateFCost();
};
 
#endif // NODE_H

node.cpp

#include "node.h"
 
Node::Node(int x, int y, bool walkable) : x(x), y(y), walkable(walkable), gCost(0), hCost(0), fCost(0), parent(nullptr) {}
 
void Node::calculateFCost() {
    fCost = gCost + hCost;
}

A* Algorithm

Implement the Pathfinding class to handle the A* algorithm.

pathfinding.h

#ifndef PATHFINDING_H
#define PATHFINDING_H
 
#include "node.h"
#include <vector>
#include <queue>
#include <algorithm>
 
class Pathfinding {
public:
    Pathfinding(int width, int height);
    std::vector<Node*> findPath(Node* startNode, Node* endNode);
 
private:
    std::vector<std::vector<Node>> grid;
    std::vector<Node*> getNeighbors(Node* node);
    float calculateHeuristic(Node* a, Node* b);
    void reconstructPath(Node* endNode, std::vector<Node*>& path);
};
 
#endif // PATHFINDING_H

pathfinding.cpp

#include "pathfinding.h"
 
Pathfinding::Pathfinding(int width, int height) {
    grid.resize(height, std::vector<Node>(width, Node(0, 0, true)));
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            grid[y][x] = Node(x, y, true);
        }
    }
}
 
std::vector<Node*> Pathfinding::findPath(Node* startNode, Node* endNode) {
    std::vector<Node*> openSet;
    std::vector<Node*> closedSet;
 
    openSet.push_back(startNode);
 
    while (!openSet.empty()) {
        Node* currentNode = *std::min_element(openSet.begin(), openSet.end(), [](Node* a, Node* b) {
            return a->fCost < b->fCost;
        });
 
        if (currentNode == endNode) {
            std::vector<Node*> path;
            reconstructPath(endNode, path);
            return path;
        }
 
        openSet.erase(std::remove(openSet.begin(), openSet.end(), currentNode), openSet.end());
        closedSet.push_back(currentNode);
 
        for (Node* neighbor : getNeighbors(currentNode)) {
            if (std::find(closedSet.begin(), closedSet.end(), neighbor) != closedSet.end() || !neighbor->walkable) {
                continue;
            }
 
            float tentativeGCost = currentNode->gCost + 1; // Assuming uniform cost
            if (std::find(openSet.begin(), openSet.end(), neighbor) == openSet.end()) {
                openSet.push_back(neighbor);
            } else if (tentativeGCost >= neighbor->gCost) {
                continue;
            }
 
            neighbor->gCost = tentativeGCost;
            neighbor->hCost = calculateHeuristic(neighbor, endNode);
            neighbor->calculateFCost();
            neighbor->parent = currentNode;
        }
    }
 
    return {}; // No path found
}
 
std::vector<Node*> Pathfinding::getNeighbors(Node* node) {
    std::vector<Node*> neighbors;
    int directions[4][2] = { {0, -1}, {0, 1}, {-1, 0}, {1, 0} };
 
    for (auto& dir : directions) {
        int newX = node->x + dir[0];
        int newY = node->y + dir[1];
 
        if (newX >= 0 && newX < grid[0].size() && newY >= 0 && newY < grid.size()) {
            neighbors.push_back(&grid[newY][newX]);
        }
    }
 
    return neighbors;
}
 
float Pathfinding::calculateHeuristic(Node* a, Node* b) {
    return std::abs(a->x - b->x) + std::abs(a->y - b->y);
}
 
void Pathfinding::reconstructPath(Node* endNode, std::vector<Node*>& path) {
    Node* currentNode = endNode;
    while (currentNode) {
        path.push_back(currentNode);
        currentNode = currentNode->parent;
    }
    std::reverse(path.begin(), path.end());
}

Visualization

To visualize the pathfinding process, you can use SFML. Here’s a basic setup:

main.cpp

#include <SFML/Graphics.hpp>
#include "pathfinding.h"
 
const int WIDTH = 20;
const int HEIGHT = 20;
const int TILE_SIZE = 30;
 
int main() {
    sf::RenderWindow window(sf::VideoMode(WIDTH * TILE_SIZE, HEIGHT * TILE_SIZE), "A* Pathfinding");
 
    Pathfinding pathfinding(WIDTH, HEIGHT);
 
    // Set start and end nodes
    Node* startNode = &pathfinding.grid[0][0];
    Node* endNode = &pathfinding.grid[HEIGHT - 1][WIDTH - 1];
 
    // Create obstacles
    for (int i = 5; i < 15; ++i) {
        pathfinding.grid[i][10].walkable = false;
    }
 
    std::vector<Node*> path = pathfinding.findPath(startNode, endNode);
 
    while (window.isOpen()) {
        sf::Event event;
        while (window.pollEvent(event)) {
            if (event.type == sf::Event::Closed) {
                window.close();
            }
        }
 
        window.clear();
 
        // Draw nodes
        for (int y = 0; y < HEIGHT; ++y) {
            for (int x = 0; x < WIDTH; ++x) {
                sf::RectangleShape tile(sf::Vector2f(TILE_SIZE, TILE_SIZE));
                tile.setPosition(x * TILE_SIZE, y * TILE_SIZE);
                if (&pathfinding.grid[y][x] == startNode) {
                    tile.setFillColor(sf::Color::Green);
                } else if (&pathfinding.grid[y][x] == endNode) {
                    tile.setFillColor(sf::Color::Red);
                } else if (pathfinding.grid[y][x].walkable) {
                    tile.setFillColor(sf::Color::White);
                } else {
                    tile.setFillColor(sf::Color::Black);
                }
                window.draw(tile);
            }
        }
 
        // Draw path
        for (Node* node : path) {
            sf::RectangleShape tile(sf::Vector2f(TILE_SIZE, TILE_SIZE));
            tile.setPosition(node->x * TILE_SIZE, node->y * TILE_SIZE);
            tile.setFillColor(sf::Color::Blue);
            window.draw(tile);
        }
 
        window.display();
    }
 
    return 0;
}

Conclusion

In this blog post, we have explored how to implement an AI pathfinding system using the A* algorithm in C++. We covered setting up the project, defining the Node structure, implementing the A* algorithm, and visualizing the pathfinding process with SFML. This pathfinding system can be integrated into various applications, including games and robotic navigation systems.

Feel free to experiment with different heuristics, optimizations, and visualizations to suit your specific needs!

Building an AI Pathfinding System with C++ and A* Algorithm

Table of contents