summaryrefslogtreecommitdiff
path: root/src/astar.cpp
blob: d121ca1fb6b2286543d322cfaf8448f054575917 (plain) (blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
#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;
}