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++) {
                    //  If not off the map (do this first to avoid array
                    //  out-of-bounds errors)
                    if (a != -1 && b != -1 &&
                            a != MAP_WIDTH && b != MAP_HEIGHT)
                    {
                        // If not already on the closed list (items on the
                        // closed list have already been considered and can now
                        // be ignored).
                        if (whichList[a][b] != onClosedList) {
                            // If not a wall/obstacle square.
                            if (tiledMap.getWalk(a, b)) {
                                //  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;
                                    }
                                }

                                if (corner == WALKABLE) {
                                    // 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
                                } // If not cutting a corner
                            } // If not a wall/obstacle square.
                        } // If not already on the closed list
                    } // If not off the map
                } // 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;
}