1 /**
2 * @file path.cpp
3 *
4 * Implementation of the path finding algorithms.
5 */
6 #include "all.h"
7
8 DEVILUTION_BEGIN_NAMESPACE
9 /** Notes visisted by the path finding algorithm. */
10 PATHNODE path_nodes[MAXPATHNODES];
11 /** size of the pnode_tblptr stack */
12 int gdwCurPathStep;
13 /** the number of in-use nodes in path_nodes */
14 int gdwCurNodes;
15 /**
16 * for reconstructing the path after the A* search is done. The longest
17 * possible path is actually 24 steps, even though we can fit 25
18 */
19 Sint8 pnode_vals[MAX_PATH_LENGTH];
20 /** A linked list of all visited nodes */
21 PATHNODE *pnode_ptr;
22 /** A stack for recursively searching nodes */
23 PATHNODE *pnode_tblptr[MAXPATHNODES];
24 /** A linked list of the A* frontier, sorted by distance */
25 PATHNODE *path_2_nodes;
26
27 /** For iterating over the 8 possible movement directions */
28 const char pathxdir[8] = { -1, -1, 1, 1, -1, 0, 1, 0 };
29 const char pathydir[8] = { -1, 1, -1, 1, 0, -1, 0, 1 };
30
31 /* data */
32
33 /**
34 * each step direction is assigned a number like this:
35 * dx
36 * -1 0 1
37 * +-----
38 * -1|5 1 6
39 * dy 0|2 0 3
40 * 1|8 4 7
41 */
42 Sint8 path_directions[9] = { 5, 1, 6, 2, 0, 3, 8, 4, 7 };
43
44 /**
45 * find the shortest path from (sx,sy) to (dx,dy), using PosOk(PosOkArg,x,y) to
46 * check that each step is a valid position. Store the step directions (see
47 * path_directions) in path, which must have room for 24 steps
48 */
FindPath(BOOL (* PosOk)(int,int,int),int PosOkArg,int sx,int sy,int dx,int dy,Sint8 path[MAX_PATH_LENGTH])49 int FindPath(BOOL (*PosOk)(int, int, int), int PosOkArg, int sx, int sy, int dx, int dy, Sint8 path[MAX_PATH_LENGTH])
50 {
51 PATHNODE *path_start, *next_node, *current;
52 int path_length, i;
53
54 // clear all nodes, create root nodes for the visited/frontier linked lists
55 gdwCurNodes = 0;
56 path_2_nodes = path_new_step();
57 pnode_ptr = path_new_step();
58 gdwCurPathStep = 0;
59 path_start = path_new_step();
60 path_start->g = 0;
61 path_start->h = path_get_h_cost(sx, sy, dx, dy);
62 path_start->x = sx;
63 path_start->f = path_start->h + path_start->g;
64 path_start->y = sy;
65 path_2_nodes->NextNode = path_start;
66 // A* search until we find (dx,dy) or fail
67 while ((next_node = GetNextPath())) {
68 // reached the end, success!
69 if (next_node->x == dx && next_node->y == dy) {
70 current = next_node;
71 path_length = 0;
72 while (current->Parent) {
73 if (path_length >= MAX_PATH_LENGTH)
74 break;
75 pnode_vals[path_length++] = path_directions[3 * (current->y - current->Parent->y) - current->Parent->x + 4 + current->x];
76 current = current->Parent;
77 }
78 if (path_length != MAX_PATH_LENGTH) {
79 for (i = 0; i < path_length; i++)
80 path[i] = pnode_vals[path_length - i - 1];
81 return i;
82 }
83 return 0;
84 }
85 // ran out of nodes, abort!
86 if (!path_get_path(PosOk, PosOkArg, next_node, dx, dy))
87 return 0;
88 }
89 // frontier is empty, no path!
90 return 0;
91 }
92
93 /**
94 * @brief heuristic, estimated cost from (sx,sy) to (dx,dy)
95 */
path_get_h_cost(int sx,int sy,int dx,int dy)96 int path_get_h_cost(int sx, int sy, int dx, int dy)
97 {
98 int delta_x = abs(sx - dx);
99 int delta_y = abs(sy - dy);
100
101 int min = delta_x < delta_y ? delta_x : delta_y;
102 int max = delta_x > delta_y ? delta_x : delta_y;
103
104 // see path_check_equal for why this is times 2
105 return 2 * (min + max);
106 }
107
108 /**
109 * @brief return 2 if pPath is horizontally/vertically aligned with (dx,dy), else 3
110 *
111 * This approximates that diagonal movement on a square grid should have a cost
112 * of sqrt(2). That's approximately 1.5, so they multiply all step costs by 2,
113 * except diagonal steps which are times 3
114 */
path_check_equal(PATHNODE * pPath,int dx,int dy)115 int path_check_equal(PATHNODE *pPath, int dx, int dy)
116 {
117 if (pPath->x == dx || pPath->y == dy)
118 return 2;
119
120 return 3;
121 }
122
123 /**
124 * @brief get the next node on the A* frontier to explore (estimated to be closest to the goal), mark it as visited, and return it
125 */
GetNextPath()126 PATHNODE *GetNextPath()
127 {
128 PATHNODE *result;
129
130 result = path_2_nodes->NextNode;
131 if (result == NULL) {
132 return result;
133 }
134
135 path_2_nodes->NextNode = result->NextNode;
136 result->NextNode = pnode_ptr->NextNode;
137 pnode_ptr->NextNode = result;
138 return result;
139 }
140
141 /**
142 * @brief check if stepping from pPath to (dx,dy) cuts a corner.
143 *
144 * If you step from A to B, both Xs need to be clear:
145 *
146 * AX
147 * XB
148 *
149 * @return true if step is allowed
150 */
path_solid_pieces(PATHNODE * pPath,int dx,int dy)151 BOOL path_solid_pieces(PATHNODE *pPath, int dx, int dy)
152 {
153 BOOL rv = TRUE;
154 switch (path_directions[3 * (dy - pPath->y) + 3 - pPath->x + 1 + dx]) {
155 case 5:
156 rv = !nSolidTable[dPiece[dx][dy + 1]] && !nSolidTable[dPiece[dx + 1][dy]];
157 break;
158 case 6:
159 rv = !nSolidTable[dPiece[dx][dy + 1]] && !nSolidTable[dPiece[dx - 1][dy]];
160 break;
161 case 7:
162 rv = !nSolidTable[dPiece[dx][dy - 1]] && !nSolidTable[dPiece[dx - 1][dy]];
163 break;
164 case 8:
165 rv = !nSolidTable[dPiece[dx + 1][dy]] && !nSolidTable[dPiece[dx][dy - 1]];
166 break;
167 }
168 return rv;
169 }
170
171 /**
172 * @brief perform a single step of A* bread-first search by trying to step in every possible direction from pPath with goal (x,y). Check each step with PosOk
173 *
174 * @return FALSE if we ran out of preallocated nodes to use, else TRUE
175 */
path_get_path(BOOL (* PosOk)(int,int,int),int PosOkArg,PATHNODE * pPath,int x,int y)176 BOOL path_get_path(BOOL (*PosOk)(int, int, int), int PosOkArg, PATHNODE *pPath, int x, int y)
177 {
178 int dx, dy;
179 int i;
180 BOOL ok;
181
182 for (i = 0; i < 8; i++) {
183 dx = pPath->x + pathxdir[i];
184 dy = pPath->y + pathydir[i];
185 ok = PosOk(PosOkArg, dx, dy);
186 if ((ok && path_solid_pieces(pPath, dx, dy)) || (!ok && dx == x && dy == y)) {
187 if (!path_parent_path(pPath, dx, dy, x, y))
188 return FALSE;
189 }
190 }
191
192 return TRUE;
193 }
194
195 /**
196 * @brief add a step from pPath to (dx,dy), return 1 if successful, and update the frontier/visited nodes accordingly
197 *
198 * @return TRUE if step successfully added, FALSE if we ran out of nodes to use
199 */
path_parent_path(PATHNODE * pPath,int dx,int dy,int sx,int sy)200 BOOL path_parent_path(PATHNODE *pPath, int dx, int dy, int sx, int sy)
201 {
202 int next_g;
203 PATHNODE *dxdy;
204 int i;
205
206 next_g = pPath->g + path_check_equal(pPath, dx, dy);
207
208 // 3 cases to consider
209 // case 1: (dx,dy) is already on the frontier
210 dxdy = path_get_node1(dx, dy);
211 if (dxdy != NULL) {
212 for (i = 0; i < 8; i++) {
213 if (pPath->Child[i] == NULL)
214 break;
215 }
216 pPath->Child[i] = dxdy;
217 if (next_g < dxdy->g) {
218 if (path_solid_pieces(pPath, dx, dy)) {
219 // we'll explore it later, just update
220 dxdy->Parent = pPath;
221 dxdy->g = next_g;
222 dxdy->f = next_g + dxdy->h;
223 }
224 }
225 } else {
226 // case 2: (dx,dy) was already visited
227 dxdy = path_get_node2(dx, dy);
228 if (dxdy != NULL) {
229 for (i = 0; i < 8; i++) {
230 if (pPath->Child[i] == NULL)
231 break;
232 }
233 pPath->Child[i] = dxdy;
234 if (next_g < dxdy->g && path_solid_pieces(pPath, dx, dy)) {
235 // update the node
236 dxdy->Parent = pPath;
237 dxdy->g = next_g;
238 dxdy->f = next_g + dxdy->h;
239 // already explored, so re-update others starting from that node
240 path_set_coords(dxdy);
241 }
242 } else {
243 // case 3: (dx,dy) is totally new
244 dxdy = path_new_step();
245 if (dxdy == NULL)
246 return FALSE;
247 dxdy->Parent = pPath;
248 dxdy->g = next_g;
249 dxdy->h = path_get_h_cost(dx, dy, sx, sy);
250 dxdy->f = next_g + dxdy->h;
251 dxdy->x = dx;
252 dxdy->y = dy;
253 // add it to the frontier
254 path_next_node(dxdy);
255
256 for (i = 0; i < 8; i++) {
257 if (pPath->Child[i] == NULL)
258 break;
259 }
260 pPath->Child[i] = dxdy;
261 }
262 }
263 return TRUE;
264 }
265
266 /**
267 * @brief return a node for (dx,dy) on the frontier, or NULL if not found
268 */
path_get_node1(int dx,int dy)269 PATHNODE *path_get_node1(int dx, int dy)
270 {
271 PATHNODE *result = path_2_nodes->NextNode;
272 while (result != NULL) {
273 if (result->x == dx && result->y == dy)
274 return result;
275 result = result->NextNode;
276 }
277 return NULL;
278 }
279
280 /**
281 * @brief return a node for (dx,dy) if it was visited, or NULL if not found
282 */
path_get_node2(int dx,int dy)283 PATHNODE *path_get_node2(int dx, int dy)
284 {
285 PATHNODE *result = pnode_ptr->NextNode;
286 while (result != NULL) {
287 if (result->x == dx && result->y == dy)
288 return result;
289 result = result->NextNode;
290 }
291 return NULL;
292 }
293
294 /**
295 * @brief insert pPath into the frontier (keeping the frontier sorted by total distance)
296 */
path_next_node(PATHNODE * pPath)297 void path_next_node(PATHNODE *pPath)
298 {
299 PATHNODE *next, *current;
300 int f;
301
302 next = path_2_nodes;
303 if (!path_2_nodes->NextNode) {
304 path_2_nodes->NextNode = pPath;
305 } else {
306 current = path_2_nodes;
307 next = path_2_nodes->NextNode;
308 f = pPath->f;
309 while (next && next->f < f) {
310 current = next;
311 next = next->NextNode;
312 }
313 pPath->NextNode = next;
314 current->NextNode = pPath;
315 }
316 }
317
318 /**
319 * @brief update all path costs using depth-first search starting at pPath
320 */
path_set_coords(PATHNODE * pPath)321 void path_set_coords(PATHNODE *pPath)
322 {
323 PATHNODE *PathOld;
324 PATHNODE *PathAct;
325 int i;
326
327 path_push_active_step(pPath);
328 while (gdwCurPathStep) {
329 PathOld = path_pop_active_step();
330 for (i = 0; i < 8; i++) {
331 PathAct = PathOld->Child[i];
332 if (PathAct == NULL)
333 break;
334
335 if (PathOld->g + path_check_equal(PathOld, PathAct->x, PathAct->y) < PathAct->g) {
336 if (path_solid_pieces(PathOld, PathAct->x, PathAct->y)) {
337 PathAct->Parent = PathOld;
338 PathAct->g = PathOld->g + path_check_equal(PathOld, PathAct->x, PathAct->y);
339 PathAct->f = PathAct->g + PathAct->h;
340 path_push_active_step(PathAct);
341 }
342 }
343 }
344 }
345 }
346
347 /**
348 * @brief push pPath onto the pnode_tblptr stack
349 */
path_push_active_step(PATHNODE * pPath)350 void path_push_active_step(PATHNODE *pPath)
351 {
352 int stack_index = gdwCurPathStep;
353 gdwCurPathStep++;
354 pnode_tblptr[stack_index] = pPath;
355 }
356
357 /**
358 * @brief pop and return a node from the pnode_tblptr stack
359 */
path_pop_active_step()360 PATHNODE *path_pop_active_step()
361 {
362 gdwCurPathStep--;
363 return pnode_tblptr[gdwCurPathStep];
364 }
365
366 /**
367 * @brief zero one of the preallocated nodes and return a pointer to it, or NULL if none are available
368 */
path_new_step()369 PATHNODE *path_new_step()
370 {
371 PATHNODE *new_node;
372
373 if (gdwCurNodes == MAXPATHNODES)
374 return NULL;
375
376 new_node = &path_nodes[gdwCurNodes];
377 gdwCurNodes++;
378 memset(new_node, 0, sizeof(PATHNODE));
379 return new_node;
380 }
381
382 DEVILUTION_END_NAMESPACE
383