A* Pathfinding

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

typedef struct Vec3 {
    float x, y, z;
} Vec3;

typedef struct GraphVertex {
    float gScore;
    float fScore;
    struct GraphVertex * ancestor;
    Vec3 position;
    struct GraphVertex ** neighbours;
    int neighbourCount;
} GraphVertex;

typedef struct Path {
    GraphVertex ** vertices;
    int count;
} Path;

#define Array_Append(elements,elementCounter,newElement) \
    do { \
        ++elementCounter; \
        elements = realloc( elements, elementCounter * sizeof( *elements )); \
        elements[elementCounter - 1] = newElement; \
    } while( 0 ); \


#define Array_GetIndex(elements,elementCounter,element,outIndex) \
    do { \
        int _k; \
        outIndex = -1; \
        for( _k = 0; _k < elementCounter; ++_k ) { \
            if( elements[_k] == element ) { \
                outIndex = _k; \
            } \
        } \
    } while( 0 ); \

#define Array_Remove(elements,element,elementCounter) \
    do { \
        int ____aIndex; \
        Array_GetIndex(elements,elementCounter,element, ____aIndex ); \
        if( ____aIndex >= 0 && ____aIndex < elementCounter ) { \
            int k; \
            --elementCounter; \
            for( k = ____aIndex; k < elementCounter; ++k ) { \
                elements[ k ] = elements[ k + 1 ]; \
            } \
            elements = realloc( elements, elementCounter * sizeof( *elements )); \
        } \
    } while( 0 ); \

GraphVertex * GraphVertexCreate(const Vec3 * position) {
    GraphVertex * g = calloc(1, sizeof(GraphVertex));
    g->gScore = FLT_MAX;
    g->fScore = FLT_MAX;
    g->position = *position;
    return g;
}

void GraphVertexLink(GraphVertex * a, GraphVertex * b) {
    int i, f;

    f = 0;
    for(i = 0; i < a->neighbourCount; ++i) {
        if(a->neighbours[i] == b) {
            f = 1;
        }
    }
    if(!f) {
        Array_Append(a->neighbours, a->neighbourCount, b);
    }



    f = 0;
    for(i = 0; i < b->neighbourCount; ++i) {
        if(b->neighbours[i] == a) {
            f = 1;
        }
    }
    if(!f) {
        Array_Append(b->neighbours, b->neighbourCount, a);
    }
}

float Heuristic(GraphVertex * a, GraphVertex * b) {
    float dx = a->position.x - b->position.x;
    float dy = a->position.y - b->position.y;
    float dz = a->position.z - b->position.z;
    return dx * dx + dy * dy + dz * dz;
}

float Distance(GraphVertex * a, GraphVertex * b) {
    float dx = a->position.x - b->position.x;
    float dy = a->position.y - b->position.y;
    float dz = a->position.z - b->position.z;
    return sqrt(dx * dx + dy * dy + dz * dz);
}

Path * AStar(GraphVertex * start, GraphVertex * goal) {
    Path * path = calloc(1, sizeof(Path));
    GraphVertex ** closedSet = NULL;
    int closedSetSize = 0;
    GraphVertex ** openSet = NULL;
    int openSetSize = 0;


    Array_Append(openSet, openSetSize, start);

    start->gScore = 0;
    start->fScore = Heuristic(start, goal);

    while(openSetSize > 0) {
        int i, j;
        GraphVertex * current = openSet[0];
        for(i = 1; i < openSetSize; ++i) {
            if(current->fScore < openSet[i]->fScore) {
                current = openSet[i];
            }
        }

        if(current == goal) {
            GraphVertex * v = goal;
            while(v != start) {
                Array_Append(path->vertices, path->count, v);
                v = v->ancestor;
            }

            Array_Append(path->vertices, path->count, start);

            break;
        }

        Array_Remove(openSet, current, openSetSize);
        Array_Append(closedSet, closedSetSize, current);

        for(i = 0; i < current->neighbourCount; ++i) {
            int neighbourInOpenSet = 0;
            int neighbourInClosedSet = 0;
            GraphVertex * neighbour = current->neighbours[i];


            for(j = 0; j < closedSetSize; ++j) {
                if(neighbour == closedSet[j]) {
                    neighbourInClosedSet = 1;
                    break;
                }
            }

            if(neighbourInClosedSet) {
                continue;
            }

            float tentative_gScore = current->gScore + Distance(current, neighbour);

            for(j = 0; j < openSetSize; ++j) {
                if(neighbour == openSet[j]) {
                    neighbourInOpenSet = 1;
                    break;
                }
            }

            if(!neighbourInOpenSet) {
                Array_Append(openSet, openSetSize, neighbour);
            } else if(tentative_gScore >= neighbour->gScore) {
                continue;
            }

            neighbour->ancestor = current;
            neighbour->gScore = tentative_gScore;
            neighbour->fScore = neighbour->gScore + Heuristic(neighbour, goal);
        }
    }

    free(openSet);
    free(closedSet);

    return path;
}


int main(int argc, char ** argv) {
    int i;
    Path * path;
    GraphVertex ** vertices = NULL;
    int vertexCount = 0;
    Vec3 positions[] = {
        {0, 0, 0}, /* 0 */
        {1, 1, 0}, /* 1 */
        {2, 1, 0}, /* 2 */
        {3, 2, 0}, /* 3 */
        {3, 3, 0}, /* 4 */
        {2, 2, 0}, /* 5 */
        {2, 4, 0}, /* 6 */
        {1, 3, 0}, /* 7 */
    };

    for(i = 0; i < sizeof(positions) / sizeof(positions[0]); ++i) {
        Array_Append(vertices, vertexCount, GraphVertexCreate(&positions[i]));
    }

    GraphVertexLink(vertices[0], vertices[1]);
    GraphVertexLink(vertices[1], vertices[2]);
    GraphVertexLink(vertices[2], vertices[3]);
    GraphVertexLink(vertices[3], vertices[4]);
    GraphVertexLink(vertices[4], vertices[5]);
    GraphVertexLink(vertices[5], vertices[6]);
    GraphVertexLink(vertices[5], vertices[7]);
    GraphVertexLink(vertices[7], vertices[1]);


    path = AStar(vertices[6], vertices[0]);

    if(path && path->vertices) {
        for(i = 0; i < path->count; ++i) {
            printf("(%f; %f; %f)\n", path->vertices[i]->position.x, path->vertices[i]->position.y, path->vertices[i]->position.z);
        }
    }

    system("pause");

    return 0;
}