A minimal, inefficient, beginner's intro to the A* pathfinding algorithm

mini-astar.c

/**********************

mini-astar
a very minimal A* pathfinding implementation

Map Key
0 = normal tile
1 = impassable tile
5 = start
9 = goal
8 = path

0, 5, 1, 0
0, 8, 1, 0
0, 8, 8, 8
0, 0, 1, 9

***********************/

#include <stdio.h>
#include <math.h>

//map dimensions
#define DIM 4

/*
 * representation of a 4x4 map
 */
static int map [DIM * DIM] = {
    0, 0, 0, 0,
    1, 1, 1, 0,
    1, 1, 1, 0,
    0, 0, 0, 0
};

static int open_set[DIM * DIM];    //nodes to be evaluated
static int closed_set[DIM * DIM];  //nodes already evaluated
static int parents[DIM * DIM];     //parents of each node
static int g_score[DIM * DIM];     //cost from start along best known path
static int f_score[DIM * DIM];     //estimated cost from start to goal through 'i'

//start & goal tiles
static int start = 0;
static int goal = 12;

int heuristic(int tile)
{
    //get the x and y coordinates of the tile
    int tile_y = tile / DIM;
    int tile_x = tile - (tile_y * DIM);

    //get the x and y coordinates of the goal
    int goal_y = goal / DIM;
    int goal_x = goal - (goal_y * DIM);

    //calculate the Manhattan distance between tile and goal
    int dx = abs(tile_x - goal_x);
    int dy = abs(tile_y - goal_y);

    return dx + dy;
}

int open_set_is_empty()
{
    int i;
    int test = 1;
    for (i = 0; i < DIM * DIM; i++) {

        if (open_set[i])
            test = 0;
    }

    return test;
}

int lowest_in_open_set()
{
    //hold the best (lowest) f_score
    int lowest_f = DIM * DIM;
    int current_lowest = 0;
 
    //loop through each tile
    int i;
    for (i = 0; i < DIM * DIM; i++) {
 
        //if this tile is in the open set
        if (open_set[i]) {

            //and if this tile's f_score is lower than the current lowest
            if (f_score[i] < lowest_f) {
 
                //set this tile to be the current lowest
                lowest_f = f_score[i];
                current_lowest = i;
            }
        }
    }
 
    return current_lowest;
}

void reconstruct_path()
{
    int t = goal;

    while(t != start) {
        if (parents[t] != start)
            map[parents[t]] = 8;

        t = parents[t];
    }
}

void print_map()
{
    int x, y;
    for (y = 0; y < DIM; y++) {
        for (x = 0; x < DIM; x++) {
            printf("%d ", map[y * DIM + x]);
        }

        printf("\n");
    }

    printf("\n");
}

int main()
{
    //add the symbols for the start end goal tiles to the map
    map[start] = 5;
    map[goal] = 9;

    //put the start tile in the open set
    open_set[start] = 1;

    //the g_score of the start tile is always 0
    g_score[start] = 0;

    //figure out the start tile's estimated distance from the goal (heuristic)
    f_score[start] = heuristic(start);

    int goal_found = 0;
    int neighbors[4]; //neighbor nodes

    printf("BASE MAP:\n");
    print_map();

    while (goal_found != 1) {
        int current = lowest_in_open_set();
        
        if (current == goal) {
            reconstruct_path();
            goal_found = 1;
        } else if (open_set_is_empty()) {
            printf("Error...no path available.\n");
            goal_found = 1;
            continue;
        } else {

            //remove current from open set
            open_set[current] = 0;

            //add current to closed set
            closed_set[current] = 1;

            //get neighbor nodes
            //up (just subtract DIM)
            neighbors[0] = current - DIM;

            //right (if we're at the right edge, there is no right neighbor so return -1,
            //otherwise just add 1 to current to get the tile)
            neighbors[1] = ((current + 1) % DIM) == 0 ? -1 : current + 1;

            //down (just add DIM)
            neighbors[2] = current + DIM; //down

            //left (if we're at the left edge, there is no left neighbor so return -1,
            //otherwise just subtract 1 to current to get the tile)
            neighbors[3] = (current % DIM) == 0 ? -1 : current - 1; //left

            int j, n, temp_g_score;
            for (j = 0; j < 4; j++) {
                n = neighbors[j];

                //if the neighbor is inside the bounds of the map AND not in the closed set
                if (n > -1 && n < DIM * DIM && closed_set[n] != 1) {

                    //if the terrain is impassable, add it to the closed set
                    if (map[n] == 1) {
                        closed_set[n] = 1;
                    } else {

                        //make a temporary g_score for this neighbor tile
                        temp_g_score = g_score[current] + 1;

                        //if this neighbor tile is not in the open set OR the
                        //temp_g_score is less than this tile's g_score
                        if (open_set[n] == 0 || temp_g_score < g_score[n]) {

                            //set this neighbor's parent to current
                            parents[n] = current;

                            //set this neighbor's g_score to temp_g_score
                            g_score[n] = temp_g_score;

                            //set this neighbor's f_score to its g_score plus the
                            //estimate of its distance from the goal (heuristic)
                            f_score[n] = g_score[n] + heuristic(n);

                            //add this neighbor to the open set
                            open_set[n] = 1;
                        }
                    }
                }
            }
        }
    }

    printf("MAP WITH PATH:\n");
    print_map();

    return 0;
}