Algorithm to find a path

Question

I have implemented A* in C, and was wondering about what I could do to improve it.

My main concerns are readability, usability and last but not least performance. My NodeList is an attempt at making a clone of ArrayList in Java.

This is based on what I read on this website:

1. Add the starting square (or node) to the open list.

2. Repeat the following:

a) Look for the lowest F cost square on the open list. We refer to this as the current square.

b) Switch it to the closed list.

c) For each of the 8 squares adjacent to this current square …

If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.

If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square.

If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.

d) Stop when you:

Add the target square to the closed list, in which case the path has been found (see note below), or Fail to find the target square, and the open list is empty. In this case, there is no path.
3) Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path.

Input example:

7
- - - # - - -
! # - # * - -
# - # # - - -
- # - # - # -
# - - # # - -
- # - - # - -
- - # # - - -

Output example:

- - - # - - -
+ # - # + - -
# + # # + - -
- # + # + # -
# - + # # + -
- # - + # + -
- - # # + - -

#AStar.c

#ifndef ASTAR_C
#define ASTAR_C

#include "AStar.h"

#include "NodeList.h"

#include <math.h>

#define DIRS 8

Coordinates directions[DIRS] =
{
    {  0,  1 }, // E
    {  1,  0 }, // S
    {  0, -1 }, // W
    { -1,  0 }, // N
    { -1, -1 }, // NW
    { -1,  1 }, // NE
    {  1,  1 }, // SE
    {  1, -1 }  // SW
};

// Finds the coordinates of the start/finish coordinates
void astar_find_checkpoints(MAP map, int size, char source, char destination, Coordinates *begin, Coordinates *end)
{
    for(int i = 0; i < size; i++)
    {
        for(int j = 0; j < size; j++)
        {
            if (map[i][j] == source)
            {
                begin->x = i;
                begin->y = j;
            }
            else if (map[i][j] == destination)
            {
                end->x = i;
                end->y = j;
            }
        }
    }
}

// Manhattan distance
int astar_calculate_h(Coordinates start, Coordinates end)
{
    return (abs(start.x - end.x) + abs(start.y - end.y))  * 10;
}

Node * astar_find_cheapest(NodeList list)
{
    Node * current = list.nodes[0];

    for(int i = 1; i < list.size; i++)
    {
        if (list.nodes[i]->g + list.nodes[i]->h < current->g + current->h)
        {
            current = list.nodes[i];
        }
    }

    return current;
}

Node * astar_find_in_list(NodeList list, Coordinates coordinates)
{
    Node * current = malloc(sizeof(Node));
    for(int i = 0; i < list.size; i++)
    {
        if (coordinates_equals(list.nodes[i]->coordinates, coordinates))
        {
            return list.nodes[i];
        }
    }

    free(current);

    return NULL;
}

bool astar_check_invalid(MAP map, int size, char wall, Coordinates coordinates)
{
    return coordinates.x < 0 || coordinates.x >= size ||
           coordinates.y < 0 || coordinates.y >= size ||
           map[coordinates.x][coordinates.y] == wall;
}

Node * astar_find_path(MAP map, int size, char wall, NodeList * open_list, NodeList * closed_list, Coordinates begin, Coordinates end)
{
// Initialization
    Node * current = NULL;

    Node * start = malloc(sizeof(Node));
    start->coordinates = begin;
    start->g = 0;
    start->h = astar_calculate_h(begin, end);
    start->parent = NULL;
    nodelist_add(open_list, start);

    while(!nodelist_is_empty(*open_list))
    {
        current = astar_find_cheapest(*open_list);
    if (coordinates_equals(current->coordinates, end))
    {
        break;
    }
    
    nodelist_add(closed_list, current);
    nodelist_remove(open_list, current);

    for(int i = 0; i < DIRS; i++)
    {
        Coordinates new_coordinates = { current->coordinates.x + directions[i].x , current->coordinates.y + directions[i].y };

        // Check if node is invalid
        if (astar_check_invalid(map, size, wall, new_coordinates)  || 
            astar_find_in_list(*closed_list, new_coordinates)   != NULL)
            {
                continue;
            }
        // Check if we have a better path going straight to the current node
        int adj_g = current->g + ((i < 4) ? 10 : 14);
        
        Node * successor = astar_find_in_list(*open_list, new_coordinates);
        // If the current node isn't in the open list, add it
        if (successor == NULL)
        {
            successor = malloc(sizeof(Node));
            successor->coordinates = new_coordinates;
            successor->g = adj_g;
            successor->h = astar_calculate_h(new_coordinates, end);
            successor->parent = current;
            nodelist_add(open_list, successor);
        }
        // If this path is shorter use it
        else if (adj_g < successor->g)
        {
            successor->g = adj_g;
            successor->parent = current;
        }
    }
}

return current;
}

void astar_print_path(Node * end)
{
    while(end != NULL)
    {
        printf("x:%d y:%d\n", end->coordinates.x, end->coordinates.y);
        end = end->parent;
    }
}

void astar_print_map(Node * end, char ** map, int size)
{
   while(end != NULL)
   {
        map[end->coordinates.x][end->coordinates.y] = '+';
        end = end->parent;
    }

    for(int i = 0; i < size; i++)
    {
        for(int j = 0; j < size; j++)
        {
            printf("%c ", map[i][j]);
        }
        printf("\n");
    }
}

#endif

The NodeList is defined as:

#define INITIAL_CAPACITY 1

#define ERROR -1

struct NodeList
{
    Node ** nodes;
    int capacity;
    int size;
};

typedef struct NodeList NodeList;

The Coordinates are defined as:

struct Coordinates
{
    int x, y;
};

typedef struct Coordinates Coordinates;

bool coordinates_equals(Coordinates a, Coordinates b)
{
    if (a.x == b.x && a.y == b.y)
        return true;
    return false;
}

The Node is defined as:

struct Node
{
    Coordinates coordinates;
    int g, h;
    struct Node ** parent;
};

typedef struct Node Node;

#NodeList.c

#ifndef NODELIST_C
#define NODELIST_C

#include <stdio.h>
#include <string.h>

#include "NodeList.h"

#include "Coordinates.h"

void nodelist_init(NodeList * list)
{
    list->nodes = malloc(sizeof(Node *) * INITIAL_CAPACITY);
    for(int i = 0; i < INITIAL_CAPACITY; i++)
    { 
        list->nodes[i] = malloc(sizeof(Node));
    }
    list->capacity = INITIAL_CAPACITY;
    list->size = 0;
}

void nodelist_add(NodeList * list, Node * node)
{
    if (list->size == list->capacity)
    {

        Node ** temp = realloc(list->nodes, 2 * list->capacity * sizeof(Node *));
        for(int i = list->capacity; i < 2 * list->capacity; i++)
        { 
            temp[i] = malloc(sizeof(Node));
        }

        list->capacity *= 2;
        
        list->nodes = temp;
    }

    list->nodes[list->size] = node;
    list->size += 1;
}

void nodelist_remove(NodeList * list, Node * node)
{
    for(int i = 0 ; i < list->size; i++)
    {
        if (coordinates_equals(list->nodes[i]->coordinates, node->coordinates))
        {
            for(int j = i; j < list->size - 1; j++)
            {
                memcpy(&list->nodes[j], &list->nodes[j] + 1, sizeof(Node *));
            }

            list->size -=1;

            return;
        }
    }
}

int nodelist_index(NodeList list, Node node)
{
    for(int i = 0; i < list.size; i++)
    {
        if (coordinates_equals(list.nodes[i]->coordinates, node.coordinates))
        {
            return i;
        }
    }

    return ERROR;
}

bool nodelist_is_empty(NodeList list)
{
    return list.size == 0;
}

#endif

Answer 1

Currently, you are doing far too many allocations, and for way too much space.
Remember that dynamic allocation is quite costly, even more so in C than in Java, at least for smaller structures.

What you should do is allocating the maze as one chunk, of elements which can easily store the length of any possible path, size_t looks good for that.

Then store a 0 for open space, SIZE_MAX for a barrier, and the id for start- and end-point separately.
Mark the end-point as cost d = 1 and put it in the priority-queue as d + h().

Now, get the most promising field (marked as cost d) from the queue, mark all neighbours not marked yet as cost d = d_parent + 1 and put them in the priority-queue as d + h().

Repeat the last step until you marked the start-point.

Follow the decrementing costs from the start-point to the end-point to reconstruct the path.

---

Advantages:

• Only two big allocations, one for the maze and one for the priority-queue, instead of hundreds of separate allocations for more space together.
• Determining whether a field was already visited is \$O(1)\$ (marked at a dedicated index in an array) instead of \$O(\it{closed})\$ (an element in a list).
• Finding the most promising field is \$O(\log(\it{open}))\$ (top of a priority queue) instead of \$O(\it{open})\$ (an element of a list, again).

The algorithms complexity decreases from \$O(n ^ 2)\$ to \$O(n \log n)\$.

---

Now let's talk about your coding-style:

1. I commend you for using a named symbol instead of just scattering a literal 8 around for DIRS, there are multiple problems with that:

   • The name isn't all that good. DIRECTION_COUNT or something alike would be better.
   • Its first use inhibits auto-sizing of the array directions.
   • Its second and last use (that's a paltry number of uses anyway) would be better served by deriving it from the array. Use sizeof directions / sizeof *directions.

   In conclusion proper named constant are better than assorted magic numbers, but naming is hard, and its even better to avoid them completely.

2. directions should be const.

3. astar_find_checkpoints() cannot signal failure, meaning more or less than one end- and start-point found.

4. astar_calculate_h() must not over-estimate the distance or the algorithm won't find an optimal solution. That means the factor 10 should be removed.

5. I don't understan why astar_find_in_list() allocates memory just to ignore and optionally leak it.

6. Writing p != NULL in a boolean contet is no clearer than just plain p, just more verbose.

7. You can combine definition of a struct (with name) and a typename for it:

   typedef struct X {...} X;
   

8. I'm not quite sure whether you have more interesting adventures with reuse of allocation, memory leaks and the like, but it's not well abstracted and I really don't feel like combing it in detail.
   I suggest trying Valgrind or another nenoryleak-detector.

Answer 2

In terms of performance, by far your biggest gain will be in using the correct data-structures.

open_list is supposed to be a priority queue and closed_list is supposed to be a set, but they are currently both linked lists. Enqueuing/Dequeuing should be O(log n) and checking/adding to the closed_set should be O(1), but they are currently all O(n)