## Saturday, September 22, 2012

### A* implementation for grids in C

Hello there, for the very first post I wanted to do something big (and by that I mean something useful and that would took some time to code properly). So I decided to share my implementation of the A* path finding algorithm in C.

I will split this post in a few sections, as I want to explain the problem that A* solves, the algorithm limitations, how you interface it with your own code and customize it to your own needs. If you are in a hurry, the code download link is on section 6.

1) Introduction - The Path Finding Problem:

Look at this picture, it is a screenshot from the game Lufia II - Rise of the Sinistrals (for the SNES).

Let's imagine that we want to take the main char (the guy with red hair) from its starting position to the point A. It would be a simple task, all we need to do is moving him along the Y axis a few cells up.

On the other hand, if we want to move the character from its starting position to the point B, then we would have a totally different problem. We can't just move in a straight line since there are several bushes in the way (in this game those are not considered walkable cells, unless you cut them). To find a path, we can use several solutions, the fastest one known by the date of this post is the A* (normally referred as A star).

The algorithm is a modification of the Djikstra's algorithm that uses an heuristic to reduce the search space. Basically it will look at the starting point and mark it as the current point, it will then check all the points around it and determine a score for each one. The score uses both a precise distance function (that is used to determine the distance between the current point and a neighbor) and an heuristic function (that is used to estimate the distance between the neighbor and the goal). After a neighbor point is analyzed, it is added to a list, known as the open list, and each neighbor point is marked as having the current point as parent (it will be used later on).

After checking all the points around the current point, the algorithm will remove it from the open list and add it to another list (known as the closed list). It will pick a new point to be set as current, which shall be the one with the lowest score in the open list.

The algorithm will then use pretty much the same logic as described in the beginning, with two differences: the first one is that it will ignore points that are in the closed list. The second is that it will check if the neighbor's distance from the current point is not smaller than the distance between the neighbor and the point marked as its parent. If it is the neighbor's parent is changed and its score is updated.

The algorithm ends when the goal is reached or when the open list is empty (in this case there is no path available to the goal). If it found a path it can be recreated by looking at the goal parent point, then the parent of that point and so on, until the starting point is reached.

If you want to read more about the subjects related to the A* and its implementation, here are some links (the fourth one has an illustrated explanation, bigger, but way easier to understand than mine):
A* Algorithm on Wikipedia.
Dijkstra Algorithm on Wikipedia.
Dynamic programming on Wikipedia.
A nice tutorial on how A* works.
Great info on path finding (and AI in general).

2) Algorithm Limitations:

Although A* is known to be the fastest way to solve a path finding problem, this implementation I am publishing is not.

The reason for such limitation is due to the data structure I used to save the list of cells that need to be checked. During the process of searching for the best path, the algorithm does two main operations: inserting the possible cells (at this point, they already have an score) in a list (know as open list) and finding the cell with the lower score in the open list.

In my list implementation the complexity of such operations are O(1) for the inserting and O(n) for finding the lowest score cell. On the other hand, if the implementation was using a heap the complexities would be O(log2n) and O(1) respectively.

The reason I didn't use a heap is that I already had a list working in my game and this implementation is already fast enough for my needs.

Another limitation is that INFINITY is defined as 1000000000, this can be easily changed, but it can give users problems if they use their own score functions (if you are using the default one, this means you will start having problems by the 71428571th cell, which is very unlikely to happen).

The first thing to do is to determine if you need to change anything in the base code. If you just compile and use it, it will obey the following rules:
- The agent may move in diagonals, but it can't if there is something in its way. For instance, if there is a wall to its right, it can't move to the upper right or lower right cells.
- The cost to move in a diagonal is bigger than the cost to move in a straight line (14 vs 10 per cell, respectively);
- The algorithm will iterate 1000 times before it stops and assume there is no path.

If your movement rules are different, check in the next section to learn how to change the implementation's behavior.

Now, the first thing to do is writing a function with the following prototype:
int name_of_your_function (void* yourGrid, int x, int y);

This function will be used by my implementation to find if the cell in index y and x is walkable or not. The first argument (the void*) is a pointer to your grid, you should cast it your own implementation type and return 0 if the cell is not walkable, and 1 if it is). A very basic example would be:
int walkable_interface(void* pt, int x, int y){
int ** matrix = (int**)pt;
return matrix[y][x];
}

In this example the grid is simply a two dimension int pointer that maps directly (1 or 0) if the region is walkable or not. Since a real grid implementation is likely to be much more complex, it will be passed simply as a void*.

The next step is creating a pathFindingStruct pointer, this can be done by using the function:
pathFindingStruct* createPathFindingStruct(void* , unsigned , unsigned , int (*)(void*, int, int));

Where:
- The first argument is a pointer to your grid.
- The second argument is the number of cells in the x axis of your grid.
- The third argument is the number of cells in the y axis of your grid.
- The forth argument is the function described just above.

To search for a path between two points, all you have to do is call this function:
list* processPathFinding(pathFindingStruct* , unsigned , unsigned , unsigned , unsigned );

Where:
- The first is the pathFindingStruct* you just created.
- The second argument is the index of the x cell where the path should start.
- The third argument is the index of the y cell where the path should start.
- The forth argument is the index of the x cell you want to get to.
- The fifth argument is the index of the y cell you want to get to.

This function will return a list of cellReference pointers. The cellReference struct has two unsigned int elements x and y. They are the cell indexes of the path found from the starting point to the end point.

To recover the results from a list you should use the listIterator type. The list iterator functions that are available are:
listIterator* createIteratorForList(list* l);
void startListIterator(list* l, listIterator* it);
void* getFirstListElement(listIterator*);
void* getNextListElement(listIterator*);
void* getCurrentListElement(listIterator*);
void resetListIterator(listIterator*);

If you create a list iterator with the first function you must free it by using the free function. The easiest way to use it is by declaring a listIterator variable and then using the second function, so you don't need to free it. Freeing a list iterator will not free the list nor the list's elements, when you no longer need a list you should use this function to free it:
freeList(&listReturned, free);

Where the second argument is the function that will be used to free the list elements. An example of the use of most of the functions described is:

pathFindingStruct* pathFinder = createPathFindingStruct(mainGrid, len, lines, walkable_interface);
if (
pathFinder == NULL){
printf("Error.");
}
list* pathTo = processPathFinding(pathFinder, begin.x, begin.y, end.x, end.y);
listIterator it;
startListIterator(pathTo, &it);
cellReference* pathCell;
for (pathCell = getFirstListElement(&it); pathCell != NULL; pathCell = getNextListElement(&it)){
//Do something...
}
freeList(&pathTo, free);
freePathFindingStruct(pathFinder);

You may reuse the pathFindingStruct for as many searches as you like. When you don't need it anymore don't forget to free it by using:
void freePathFindingStruct(pathFindingStruct*)

4) How to Customize the Algorithm:

The implementation offers some limited customization, if you look at the pathfinding.h file you will find the following defines:
#define CONSTANT_STRAIGHT_COST 10
#define CONSTANT_DIAGONAL_COST 14
#define PATHFINDING_INFINITY 1000000000
#define PATHFINDING_MAY_MOVE_DIAGONALLY 1
#define PATHFINDING_MAX_ITER 1000

The first two lines define the cost of moving one cell. The third one defines the cost considered infinity, this is used to find the lowest cost, if the costs passes this value, this implementation will not work properly. The fourth one defines if the agent may move in diagonals or not, you must comment this line if you want the algorithm not to consider diagonals (it will also change the default heuristic to the Manhattan distance). Finally, the last one is the default maximum number of iterations.

You can also change the maximum iteration number, heuristic function and neighbor distance function by using the following functions:
void setPathFindingMaxIter(pathFindingStruct*, unsigned );
void setPathFindingHeuristic(pathFindingStruct*, int (*)(void*, int, int, int, int));
void setPathFindingNeighborDistanceFunction(pathFindingStruct*, int (*)(void*, int, int, int, int));

The distance functions must have the following form:
int yourFunctionName(void* , int , int , int , int );

Where:
- The first argument shall be a void* to the grid that was passed to the createPathFindingStruct.
- The second and third arguments shall be the current point x and y indexes, respectively.
- The forth and fifth arguments shall be the either the neighbor point or the ending point, depending on which function you are setting, x and y indexes, respectively.

5) Examples of Use:

In the download you can find a full example of use, just get it and you will have six files:
- main.c
- list.c
- list.h
- pathfinding.c
- pathfinding.h
- input_example.txt

On windows I used code::blocks with MINGW to compile, but it should be compilable on any IDE. On linux I used vim and gcc, in this case all you need to do is use this command:
gcc -O2 main.c list.c pathfinding.c -o pathfinding_example

When you have the executable, you need to run it with a text file as argument.  The input file must have up to 100 lines and columns and all the lines must have the same length. The text may have the following letters:
- ".": an walkable cell.
- "0": an non-walkable cell.
- "s": the starting point.
- "e": the goal.

The file named "input_example.txt" can be used as a base, you can also use it to check the program working. It will print the text of the input file with the path marked by the "x" letter. Here are some examples of an input file (the left row) and the generated output (the right row):
 .s........... ......0...... ......0...... ......0...... 00000000000.0 ............. ............. .e........... ............. ............. ............. .xxxxxxx.... ......0.x... ......0..x.. ......0...xx 00000000000x ...........x ..........x. .xxxxxxxxx.. ............ ............ ............ ..............0...s....0...0000000.0e....0....00..0........0.00......... ....xxx.....x.0x..xxx..0.x.0000000x0xxxx.0.x..00x.0x.....x.0x00....xxx.. ..........0......0............0....0........0.....s....0....0......00000.000000000.000......0.................00000.0000......00000....0........0000.0.....000000........0........................0...e. ....xxx...0......0xxx.......x.0.x..0.....xxx0.xx..xxx..0.x..0.....x00000x000000000x000....x.0....x..xxxxxxxx..00000x0000...x.x00000....0....x...0000x0x....000000...x....0...x..xxxxxxxxxxxxx.....0...x.