path: root/src/astar.cpp

#include "astar.h"

#define MAP_WIDTH  200
#define MAP_HEIGHT 200

#define WALKABLE     0
#define NOT_WALKABLE 1

#define NOT_STARTED 0
#define FOUND 1
#define NOT_FOUND 2

// path-related constants
const int numberPeople = 1;
int onClosedList = 10;
const int notfinished = 0;

// Declare needed arrays
//char tiledMap.getPathWalk [MAP_WIDTH][MAP_HEIGHT];

/** 1 dimensional array holding ID# of open list items */
int openList[MAP_WIDTH * MAP_HEIGHT + 2];

/**
 * 2 dimensional array used to record whether a cell is on the open list or
 * on the closed list
 */
int whichList[MAP_WIDTH + 1][MAP_HEIGHT + 1];

/** 1d array stores the x location of an item on the open list */
int openX[MAP_WIDTH * MAP_HEIGHT + 2];

/** 1d array stores the y location of an item on the open list */
int openY[MAP_WIDTH * MAP_HEIGHT + 2];

/** 2d array to store parent of each cell (x) */
int parentX[MAP_WIDTH + 1][MAP_HEIGHT + 1];

/** 2d array to store parent of each cell (y) */
int parentY[MAP_WIDTH + 1][MAP_HEIGHT + 1];

/** 1d array to store F cost of a cell on the open list */
int F_cost[MAP_WIDTH * MAP_HEIGHT + 2];

/** 2d array to store G_cost cost for each cell */
int G_cost[MAP_WIDTH + 1][MAP_HEIGHT + 1];

/** 1d array to store H cost of a cell on the open list */
int H_cost[MAP_WIDTH * MAP_HEIGHT + 2];

int pathLength;     /**< length of the FOUND path for critter */
int pathLocation;   /**< current position along the chosen path for critter */
int* path_bank = NULL;

// Path reading variables
int pathStatus;
int xPath;
int yPath;

/** Initialize pathfinder */
void pathfinder_init() {
    path_bank = (int*)malloc(4);
}

/** Exit pathfinder */
void pathfinder_exit() {
    free(path_bank);
}

/** Find path */
PATH_NODE *find_path(int pathfinderID, int s_x, int s_y, int e_x, int e_y)
{
    int onOpenList = 0, parentXval = 0, parentYval = 0;
    int a = 0, b = 0, m = 0, u = 0, v = 0, temp = 0, corner = 0;
    int numberOfOpenListItems = 0, addedGCost = 0, tempG = 0, path = 0, x = 0;
    int y = 0, tempx, pathX, pathY, cellPosition, newOpenListItemID = 0;

    // If starting location and target are in the same location...
    if (s_x == e_x && s_y == e_y && pathLocation > 0) return NULL;
    else if (s_x == e_x && s_y == e_y && pathLocation == 0) return NULL;

    // If dest tile is not walkable, return that it's a NOT_FOUND path.
    if (!tiledMap.getWalk(e_x, e_y)) {
        xPath = s_x;
        yPath = s_y;
        return NULL;
    }

    // Reset some variables that need to be cleared
    for (x = 0; x < MAP_WIDTH; x++) {
        for (y = 0; y < MAP_HEIGHT; y++) {
            whichList[x][y] = 0;
        }
    }

    // Changing the values of onOpenList and onClosed list is faster than
    // redimming whichList() array
    onClosedList = 2;
    onOpenList = 1;
    pathLength = NOT_STARTED;
    pathLocation = NOT_STARTED;

    // Reset starting square's G_cost value to 0
    G_cost[s_x][s_y] = 0;

    // Add the starting location to the open list of tiles to be checked.
    numberOfOpenListItems = 1;

    // Assign it as the top (and currently only) item in the open list, which
    // is maintained as a binary heap (explained below)
    openList[1] = 1;
    openX[1] = s_x ; openY[1] = s_y;

    // Do the following until a path is FOUND or deemed NOT_FOUND.
    do {
        // If the open list is not empty, take the first cell off of the list.
        // This is the lowest F cost cell on the open list.
        if (numberOfOpenListItems != 0)
        {
            // Pop the first item off the open list.

            // Record cell coordinates of the item
            parentXval = openX[openList[1]];
            parentYval = openY[openList[1]];

            // Add the item to the closed list
            whichList[parentXval][parentYval] = onClosedList;

            // Open List = Binary Heap: Delete this item from the open list,
            // which
            // Reduce number of open list items by 1
            numberOfOpenListItems = numberOfOpenListItems - 1;

            // Delete the top item in binary heap and reorder the heap, with
            // the lowest F cost item rising to the top.
            // Move the last item in the heap up to slot #1
            openList[1] = openList[numberOfOpenListItems + 1];
            v = 1;

            // Repeat the following until the new item in slot #1 sinks to its
            // proper spot in the heap.
            do {
                u = v;
                // If both children exist
                if (2 * u + 1 <= numberOfOpenListItems) {
                    // Check if the F cost of the parent is greater than each
                    // child.
                    // Select the lowest of the two children.
                    if (F_cost[openList[u]] >= F_cost[openList[2 * u]])
                        v = 2 * u;
                    if (F_cost[openList[v]] >= F_cost[openList[2 * u + 1]])
                        v = 2 * u + 1;
                } else {
                    // If only child #1 exists
                    if (2 * u <= numberOfOpenListItems) {
                        // Check if the F cost of the parent is greater than
                        // child #1
                        if (F_cost[openList[u]] >= F_cost[openList[2 * u]])
                            v = 2 * u;
                    }
                }

                if (u != v) {
                    // If parent's F is > one of its children, swap them
                    temp = openList[u];
                    openList[u] = openList[v];
                    openList[v] = temp;
                }
                else {
                    // Otherwise, exit loop
                    break;
                }
            } while (u != v); // Reorder the binary heap


            // Check the adjacent squares. (Its "children" -- these path
            // children are similar, conceptually, to the binary heap children
            // mentioned above, but don't confuse them. They are different.
            // Path children are portrayed in Demo 1 with grey pointers
            // pointing toward their parents.) Add these adjacent child squares
            // to the open list for later consideration if appropriate (see
            // various if statements below).

            for (b = parentYval - 1; b <= parentYval + 1; b++) {
                for (a = parentXval - 1; a <= parentXval + 1; a++) {
                    // Skip if off the map (do this first to avoid array
                    // out-of-bounds errors)
                    if (!(a != -1 && b != -1 &&
                                a != MAP_WIDTH && b != MAP_HEIGHT))
                    {
                        continue;
                    }

                    // Skip if already on the closed list (items on the
                    // closed list have already been considered and can now
                    // be ignored). Skip also if the tile is obstructed.
                    if (whichList[a][b] == onClosedList ||
                            !tiledMap.getWalk(a, b))
                    {
                        continue;
                    }

                    //  Don't cut across corners
                    corner = WALKABLE;

                    if (a == parentXval - 1) {
                        if (b == parentYval - 1) {
                            if (!tiledMap.getWalk(parentXval - 1, parentYval) || !tiledMap.getWalk(parentXval, parentYval - 1)) // cera slash
                                corner = NOT_WALKABLE;
                        } else if (b == parentYval + 1) {
                            if (!tiledMap.getWalk(parentXval, parentYval + 1) || !tiledMap.getWalk(parentXval - 1, parentYval))
                                corner = NOT_WALKABLE;
                        }
                    }
                    else if (a == parentXval + 1) {
                        if (b == parentYval - 1) {
                            if (!tiledMap.getWalk(parentXval, parentYval - 1) || !tiledMap.getWalk(parentXval + 1, parentYval))
                                corner = NOT_WALKABLE;
                        } else if (b == parentYval + 1) {
                            if (!tiledMap.getWalk(parentXval + 1, parentYval) || !tiledMap.getWalk(parentXval, parentYval + 1))
                                corner = NOT_WALKABLE;
                        }
                    }

                    // We can't skip corners, skip to the next square instead.
                    if (corner != WALKABLE) {
                        continue;
                    }

                    // If not already on the open list, add it to the open
                    // list.
                    if (whichList[a][b] != onOpenList) {
                        // Create a new open list item in the binary heap.
                        // Each new item has a unique ID #
                        newOpenListItemID += 1;
                        m = numberOfOpenListItems + 1;

                        // Place the new open list item (actually, its ID#)
                        // at the bottom of the heap
                        openList[m] = newOpenListItemID;
                        // Record the x and y coordinates of the new item
                        openX[newOpenListItemID] = a;
                        openY[newOpenListItemID] = b;

                        // Figure out its G_cost cost
                        if (abs(a - parentXval) == 1 &&
                                abs(b - parentYval) == 1)
                        {
                            // Cost of going to diagonal squares.
                            addedGCost = 14;
                        }
                        else {
                            // Cost of going to non-diagonal squares.
                            addedGCost = 10;
                        }

                        G_cost[a][b] = G_cost[parentXval][parentYval] +
                            addedGCost;

                        // Figure out its H and F costs and parent.
                        H_cost[openList[m]] =
                            10 * (abs(a - e_x) + abs(b - e_y));
                        F_cost[openList[m]] =
                            G_cost[a][b] + H_cost[openList[m]];
                        parentX[a][b] = parentXval;
                        parentY[a][b] = parentYval;

                        // Move the new open list item to the proper place in
                        // the binary heap.  Starting at the bottom,
                        // successively compare to parent items, swapping as
                        // needed until the item finds its place in the heap or
                        // bubbles all the way to the top (if it has the lowest
                        // F cost).

                        // While item hasn't bubbled to the top (m = 1)
                        while (m != 1)
                        {
                            // Check if child's F cost is < parent's F
                            // cost. If so, swap them.

                            if (F_cost[openList[m]] <= F_cost[openList[m / 2]])
                            {
                                temp = openList[m / 2];
                                openList[m / 2] = openList[m];
                                openList[m] = temp;
                                m = m / 2;
                            }
                            else {
                                break;
                            }
                        }

                        // Add one to the number of items in the heap.
                        numberOfOpenListItems += 1;
                        // Change whichList to show that the new item is on
                        // the open list.
                        whichList[a][b] = onOpenList;
                    } else {
                        // If whichList(a,b) = onOpenList

                        // If adjacent cell is already on the open list, check
                        // to see if this path to that cell from the starting
                        // location is a better one.  If so, change the parent
                        // of the cell and its G_cost and F costs.  Figure out
                        // the G_cost cost of this possible new path

                        if (abs(a - parentXval) == 1 &&
                                abs(b - parentYval) == 1)
                        {
                            // Cost of going to diagonal tiles
                            addedGCost = 14;
                        }
                        else {
                            // Cost of going to non-diagonal tiles
                            addedGCost = 10;
                        }

                        tempG = G_cost[parentXval][parentYval]
                            + addedGCost;

                        // If this path is shorter (G_cost cost is lower)
                        // then change the parent cell, G_cost cost and F
                        // cost.
                        if (tempG < G_cost[a][b])
                        {
                            // If G_cost cost is less,
                            // change the square's parent
                            parentX[a][b] = parentXval;
                            parentY[a][b] = parentYval;
                            // Change the G_cost cost
                            G_cost[a][b] = tempG;

                            // Because changing the G_cost cost also changes
                            // the F cost, if the item is on the open list we
                            // need to change the item's recorded F cost and
                            // its position on the open list to make sure that
                            // we maintain a properly ordered open list.

                            // Look for the item in the heap
                            for (int x = 1; x <= numberOfOpenListItems; x++)
                            {
                                if (openX[openList[x]] == a &&
                                        openY[openList[x]] == b)
                                {
                                    // Item FOUND, change the F cost
                                    F_cost[openList[x]] = G_cost[a][b] +
                                        H_cost[openList[x]];

                                    // See if changing the F score bubbles the
                                    // item up from it's current location in
                                    // the heap
                                    m = x;

                                    // While item hasn't bubbled to the top
                                    // (m = 1)
                                    while (m != 1)
                                    {
                                        // Check if child is < parent. If so,
                                        // swap them.
                                        if (F_cost[openList[m]] <
                                                F_cost[openList[m / 2]]) {
                                            temp = openList[m / 2];
                                            openList[m / 2] = openList[m];
                                            openList[m] = temp;
                                            m = m / 2;
                                        }
                                        else {
                                            break;
                                        }
                                    }
                                    // Exit for x = loop
                                    break;
                                } // If openX(openList(x)) = a
                            } // F x = 1 To nrOfOpenListItems
                        } // If tempG < G_cost(a, b)
                    } // else If whichList(a, b) = onOpenList
                } // for (a = parentXval - 1; a <= parentXval + 1; a++)
            } // for (b = parentYval - 1; b <= parentYval + 1; b++)
        } else { // if (numberOfOpenListItems != 0)
            // If open list is empty then there is no path.
            path = NOT_FOUND;
            break;
        }

        // If target is added to open list then path has been FOUND.
        if (whichList[e_x][e_y] == onOpenList) {
            path = FOUND;
            break;
        }

    }
    // Do until path is FOUND or deemed NOT_FOUND
    while (path != FOUND && path != NOT_FOUND);

    // Save the path if it exists.
    if (path == FOUND)
    {
        // Working backwards from the target to the starting location by
        // checking each cell's parent, figure out the length of the path.
        pathX = e_x; pathY = e_y;
        do {
            // Look up the parent of the current cell.
            tempx = parentX[pathX][pathY];
            pathY = parentY[pathX][pathY];
            pathX = tempx;

            // Figure out the path length
            pathLength = pathLength + 1;
        }
        while (pathX != s_x || pathY != s_y);

        // Resize the data bank to the right size in bytes
        path_bank = (int*)realloc(path_bank, pathLength * 8);

        // Now copy the path information over to the databank. Since we are
        // working backwards from the target to the start location, we copy the
        // information to the data bank in reverse order. The result is a
        // properly ordered set of path data, from the first step to the last.

        pathX = e_x ; pathY = e_y;
        // Start at the end
        cellPosition = pathLength * 2;
        do {
            // Work backwards 2 integers
            cellPosition = cellPosition - 2;
            path_bank[cellPosition] = pathX;
            path_bank[cellPosition + 1] = pathY;
            // Look up the parent of the current cell.
            tempx = parentX[pathX][pathY];
            pathY = parentY[pathX][pathY];
            pathX = tempx;
            // If we have reached the starting square, exit the loop.
        }
        while (pathX != s_x || pathY != s_y);

        PATH_NODE *ret = NULL, *temp = NULL;
        pathLocation = 1;
        ret = new PATH_NODE(s_x, s_y);
        temp = ret;
        while (pathLocation < pathLength) {
            temp->next = new PATH_NODE(
                    path_bank[pathLocation * 2 - 2],
                    path_bank[pathLocation * 2 - 1]);
            if (temp->next == NULL) throw "Unable to create path node";
            temp = temp->next;
            pathLocation++;
        }
        if (temp != NULL) {
            temp->next = new PATH_NODE(e_x, e_y);
        }
        else {
            throw "Null reference";
        }

        return ret;
    }

    // Path not found
    return NULL;
}