diff --git a/Source/path.cpp b/Source/path.cpp index c81798a70..662a4f31c 100644 --- a/Source/path.cpp +++ b/Source/path.cpp @@ -9,138 +9,119 @@ #include "objects.h" namespace devilution { +namespace { -#define MAXPATHNODES 300 +/** A linked list of the A* frontier, sorted by distance */ +PATHNODE *path_2_nodes; +/** + * @brief return a node for a position on the frontier, or NULL if not found + */ +PATHNODE *path_get_node1(Point targetPosition) +{ + PATHNODE *result = path_2_nodes->NextNode; + while (result != nullptr) { + if (result->position == targetPosition) + return result; + result = result->NextNode; + } + return nullptr; +} -/** Notes visisted by the path finding algorithm. */ -PATHNODE path_nodes[MAXPATHNODES]; -/** size of the pnode_tblptr stack */ -int gdwCurPathStep; -/** the number of in-use nodes in path_nodes */ -int gdwCurNodes; /** - * for reconstructing the path after the A* search is done. The longest - * possible path is actually 24 steps, even though we can fit 25 + * @brief insert pPath into the frontier (keeping the frontier sorted by total distance) */ -int8_t pnode_vals[MAX_PATH_LENGTH]; -/** A linked list of all visited nodes */ -PATHNODE *pnode_ptr; -/** A stack for recursively searching nodes */ -PATHNODE *pnode_tblptr[MAXPATHNODES]; -/** A linked list of the A* frontier, sorted by distance */ -PATHNODE *path_2_nodes; +void path_next_node(PATHNODE *pPath) +{ + if (path_2_nodes->NextNode == nullptr) { + path_2_nodes->NextNode = pPath; + return; + } -/** For iterating over the 8 possible movement directions */ -const Displacement PathDirs[8] = { - // clang-format off - { -1, -1 }, - { -1, 1 }, - { 1, -1 }, - { 1, 1 }, - { -1, 0 }, - { 0, -1 }, - { 1, 0 }, - { 0, 1 }, - // clang-format on -}; - -/* data */ + PATHNODE *current = path_2_nodes; + PATHNODE *next = path_2_nodes->NextNode; + int f = pPath->f; + while (next != nullptr && next->f < f) { + current = next; + next = next->NextNode; + } + pPath->NextNode = next; + current->NextNode = pPath; +} +/** A linked list of all visited nodes */ +PATHNODE *pnode_ptr; /** - * each step direction is assigned a number like this: - * dx - * -1 0 1 - * +----- - * -1|5 1 6 - * dy 0|2 0 3 - * 1|8 4 7 + * @brief return a node for this position if it was visited, or NULL if not found */ -int8_t path_directions[9] = { 5, 1, 6, 2, 0, 3, 8, 4, 7 }; - -bool IsTileNotSolid(Point position) +PATHNODE *path_get_node2(Point targetPosition) { - return !nSolidTable[dPiece[position.x][position.y]]; + PATHNODE *result = pnode_ptr->NextNode; + while (result != nullptr) { + if (result->position == targetPosition) + return result; + result = result->NextNode; + } + return nullptr; } -bool IsTileSolid(Point position) +/** + * @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 + */ +PATHNODE *GetNextPath() { - if (position.x < 0 || position.y < 0 || position.x >= MAXDUNX || position.y >= MAXDUNY) { - return false; + PATHNODE *result = path_2_nodes->NextNode; + if (result == nullptr) { + return result; } - return nSolidTable[dPiece[position.x][position.y]]; + path_2_nodes->NextNode = result->NextNode; + result->NextNode = pnode_ptr->NextNode; + pnode_ptr->NextNode = result; + return result; } +constexpr size_t MAXPATHNODES = 300; + +/** Notes visisted by the path finding algorithm. */ +PATHNODE path_nodes[MAXPATHNODES]; +/** the number of in-use nodes in path_nodes */ +int gdwCurNodes; /** - * @brief Checks the position is solid or blocked by an object + * @brief zero one of the preallocated nodes and return a pointer to it, or NULL if none are available */ -bool IsTileWalkable(Point position, bool ignoreDoors) +PATHNODE *path_new_step() { - if (dObject[position.x][position.y] != 0) { - int oi = abs(dObject[position.x][position.y]) - 1; - if (ignoreDoors && Objects[oi].IsDoor()) - return true; - if (Objects[oi]._oSolidFlag) - return false; - } + if (gdwCurNodes >= MAXPATHNODES) + return nullptr; - return !IsTileSolid(position); + PATHNODE *newNode = &path_nodes[gdwCurNodes]; + gdwCurNodes++; + memset(newNode, 0, sizeof(PATHNODE)); + return newNode; } +/** A stack for recursively searching nodes */ +PATHNODE *pnode_tblptr[MAXPATHNODES]; +/** size of the pnode_tblptr stack */ +int gdwCurPathStep; /** - * find the shortest path from (sx,sy) to (dx,dy), using PosOk(PosOkArg,x,y) to - * check that each step is a valid position. Store the step directions (see - * path_directions) in path, which must have room for 24 steps + * @brief push pPath onto the pnode_tblptr stack */ -int FindPath(const std::function &posOk, Point start, Point destination, int8_t path[MAX_PATH_LENGTH]) +void path_push_active_step(PATHNODE *pPath) { - // clear all nodes, create root nodes for the visited/frontier linked lists - gdwCurNodes = 0; - path_2_nodes = path_new_step(); - pnode_ptr = path_new_step(); - gdwCurPathStep = 0; - PATHNODE *pathStart = path_new_step(); - pathStart->g = 0; - pathStart->h = path_get_h_cost(start, destination); - pathStart->f = pathStart->h + pathStart->g; - pathStart->position = start; - path_2_nodes->NextNode = pathStart; - // A* search until we find (dx,dy) or fail - PATHNODE *nextNode; - while ((nextNode = GetNextPath()) != nullptr) { - // reached the end, success! - if (nextNode->position == destination) { - PATHNODE *current = nextNode; - int pathLength = 0; - while (current->Parent != nullptr) { - if (pathLength >= MAX_PATH_LENGTH) - break; - pnode_vals[pathLength++] = path_directions[3 * (current->position.y - current->Parent->position.y) - current->Parent->position.x + 4 + current->position.x]; - current = current->Parent; - } - if (pathLength != MAX_PATH_LENGTH) { - int i; - for (i = 0; i < pathLength; i++) - path[i] = pnode_vals[pathLength - i - 1]; - return i; - } - return 0; - } - // ran out of nodes, abort! - if (!path_get_path(posOk, nextNode, destination)) - return 0; - } - // frontier is empty, no path! - return 0; + assert(gdwCurPathStep < MAXPATHNODES); + pnode_tblptr[gdwCurPathStep] = pPath; + gdwCurPathStep++; } /** - * @brief heuristic, estimated cost from (sx,sy) to (dx,dy) + * @brief pop and return a node from the pnode_tblptr stack */ -int path_get_h_cost(Point source, Point destination) +PATHNODE *path_pop_active_step() { - // see path_check_equal for why this is times 2 - return 2 * source.ManhattanDistance(destination); + gdwCurPathStep--; + assert(gdwCurPathStep >= 0); + return pnode_tblptr[gdwCurPathStep]; } /** @@ -159,72 +140,44 @@ int path_check_equal(Point position, Point destination) } /** - * @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 + * @brief update all path costs using depth-first search starting at pPath */ -PATHNODE *GetNextPath() +void path_set_coords(PATHNODE *pPath) { - PATHNODE *result; + path_push_active_step(pPath); + // while there are path nodes to check + while (gdwCurPathStep > 0) { + PATHNODE *pathOld = path_pop_active_step(); + for (auto *pathAct : pathOld->Child) { + if (pathAct == nullptr) + break; - result = path_2_nodes->NextNode; - if (result == nullptr) { - return result; + if (pathOld->g + path_check_equal(pathOld->position, pathAct->position) < pathAct->g) { + if (path_solid_pieces(pathOld->position, pathAct->position)) { + pathAct->Parent = pathOld; + pathAct->g = pathOld->g + path_check_equal(pathOld->position, pathAct->position); + pathAct->f = pathAct->g + pathAct->h; + path_push_active_step(pathAct); + } + } + } } - - path_2_nodes->NextNode = result->NextNode; - result->NextNode = pnode_ptr->NextNode; - pnode_ptr->NextNode = result; - return result; } /** - * @brief check if stepping from a given position to a neighbouring tile cuts a corner. - * - * If you step from A to B, both Xs need to be clear: - * - * AX - * XB - * - * @return true if step is allowed + * each step direction is assigned a number like this: + * dx + * -1 0 1 + * +----- + * -1|5 1 6 + * dy 0|2 0 3 + * 1|8 4 7 */ -bool path_solid_pieces(Point position, Point destination) -{ - // These checks are written as if working backwards from the destination to the source, given - // both tiles are expected to be adjacent this doesn't matter beyond being a bit confusing - bool rv = true; - switch (path_directions[3 * (destination.y - position.y) + 3 - position.x + 1 + destination.x]) { - case 5: // Stepping north - rv = IsTileNotSolid(destination + DIR_SW) && IsTileNotSolid(destination + DIR_SE); - break; - case 6: // Stepping east - rv = IsTileNotSolid(destination + DIR_SW) && IsTileNotSolid(destination + DIR_NW); - break; - case 7: // Stepping south - rv = IsTileNotSolid(destination + DIR_NE) && IsTileNotSolid(destination + DIR_NW); - break; - case 8: // Stepping west - rv = IsTileNotSolid(destination + DIR_SE) && IsTileNotSolid(destination + DIR_NE); - break; - } - return rv; -} +constexpr int8_t path_directions[9] = { 5, 1, 6, 2, 0, 3, 8, 4, 7 }; -/** - * @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 - * - * @return false if we ran out of preallocated nodes to use, else true - */ -bool path_get_path(const std::function &posOk, PATHNODE *pPath, Point destination) +int8_t GetPathDirection(Point sourcePosition, Point destinationPosition) { - for (auto dir : PathDirs) { - Point tile = pPath->position + dir; - bool ok = posOk(tile); - if ((ok && path_solid_pieces(pPath->position, tile)) || (!ok && tile == destination)) { - if (!path_parent_path(pPath, tile, destination)) - return false; - } - } - - return true; + return path_directions[3 * (destinationPosition.y - sourcePosition.y) + 4 + destinationPosition.x - sourcePosition.x]; } /** @@ -297,111 +250,127 @@ bool path_parent_path(PATHNODE *pPath, Point candidatePosition, Point destinatio } /** - * @brief return a node for a position on the frontier, or NULL if not found + * @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 + * + * @return false if we ran out of preallocated nodes to use, else true */ -PATHNODE *path_get_node1(Point targetPosition) +bool path_get_path(const std::function &posOk, PATHNODE *pPath, Point destination) { - PATHNODE *result = path_2_nodes->NextNode; - while (result != nullptr) { - if (result->position == targetPosition) - return result; - result = result->NextNode; + for (auto dir : PathDirs) { + Point tile = pPath->position + dir; + bool ok = posOk(tile); + if ((ok && path_solid_pieces(pPath->position, tile)) || (!ok && tile == destination)) { + if (!path_parent_path(pPath, tile, destination)) + return false; + } } - return nullptr; + + return true; } -/** - * @brief return a node for a given position if it was visited, or NULL if not found - */ -PATHNODE *path_get_node2(Point targetPosition) +} + +bool IsTileNotSolid(Point position) { - PATHNODE *result = pnode_ptr->NextNode; - while (result != nullptr) { - if (result->position == targetPosition) - return result; - result = result->NextNode; - } - return nullptr; + return !nSolidTable[dPiece[position.x][position.y]]; } -/** - * @brief insert pPath into the frontier (keeping the frontier sorted by total distance) - */ -void path_next_node(PATHNODE *pPath) +bool IsTileSolid(Point position) { - if (path_2_nodes->NextNode == nullptr) { - path_2_nodes->NextNode = pPath; - return; + if (position.x < 0 || position.y < 0 || position.x >= MAXDUNX || position.y >= MAXDUNY) { + return false; } - PATHNODE *current = path_2_nodes; - PATHNODE *next = path_2_nodes->NextNode; - int f = pPath->f; - while (next != nullptr && next->f < f) { - current = next; - next = next->NextNode; + return nSolidTable[dPiece[position.x][position.y]]; +} + +bool IsTileWalkable(Point position, bool ignoreDoors) +{ + if (dObject[position.x][position.y] != 0) { + int oi = abs(dObject[position.x][position.y]) - 1; + if (ignoreDoors && Objects[oi].IsDoor()) + return true; + if (Objects[oi]._oSolidFlag) + return false; } - pPath->NextNode = next; - current->NextNode = pPath; + + return !IsTileSolid(position); } -/** - * @brief update all path costs using depth-first search starting at pPath - */ -void path_set_coords(PATHNODE *pPath) +int FindPath(const std::function &posOk, Point start, Point destination, int8_t path[MAX_PATH_LENGTH]) { - path_push_active_step(pPath); - // while there are path nodes to check - while (gdwCurPathStep > 0) { - PATHNODE *pathOld = path_pop_active_step(); - for (auto *pathAct : pathOld->Child) { - if (pathAct == nullptr) - break; + /** + * for reconstructing the path after the A* search is done. The longest + * possible path is actually 24 steps, even though we can fit 25 + */ + static int8_t pnode_vals[MAX_PATH_LENGTH]; - if (pathOld->g + path_check_equal(pathOld->position, pathAct->position) < pathAct->g) { - if (path_solid_pieces(pathOld->position, pathAct->position)) { - pathAct->Parent = pathOld; - pathAct->g = pathOld->g + path_check_equal(pathOld->position, pathAct->position); - pathAct->f = pathAct->g + pathAct->h; - path_push_active_step(pathAct); - } + // clear all nodes, create root nodes for the visited/frontier linked lists + gdwCurNodes = 0; + path_2_nodes = path_new_step(); + pnode_ptr = path_new_step(); + gdwCurPathStep = 0; + PATHNODE *pathStart = path_new_step(); + pathStart->g = 0; + pathStart->h = path_get_h_cost(start, destination); + pathStart->f = pathStart->h + pathStart->g; + pathStart->position = start; + path_2_nodes->NextNode = pathStart; + // A* search until we find (dx,dy) or fail + PATHNODE *nextNode; + while ((nextNode = GetNextPath()) != nullptr) { + // reached the end, success! + if (nextNode->position == destination) { + PATHNODE *current = nextNode; + int pathLength = 0; + while (current->Parent != nullptr) { + if (pathLength >= MAX_PATH_LENGTH) + break; + pnode_vals[pathLength++] = GetPathDirection(current->Parent->position, current->position); + current = current->Parent; + } + if (pathLength != MAX_PATH_LENGTH) { + int i; + for (i = 0; i < pathLength; i++) + path[i] = pnode_vals[pathLength - i - 1]; + return i; } + return 0; } + // ran out of nodes, abort! + if (!path_get_path(posOk, nextNode, destination)) + return 0; } + // frontier is empty, no path! + return 0; } -/** - * @brief push pPath onto the pnode_tblptr stack - */ -void path_push_active_step(PATHNODE *pPath) +int path_get_h_cost(Point sourcePosition, Point destinationPosition) { - assert(gdwCurPathStep < MAXPATHNODES); - pnode_tblptr[gdwCurPathStep] = pPath; - gdwCurPathStep++; + // see path_check_equal for why this is times 2 + return 2 * sourcePosition.ManhattanDistance(destinationPosition); } -/** - * @brief pop and return a node from the pnode_tblptr stack - */ -PATHNODE *path_pop_active_step() +bool path_solid_pieces(Point sourcePosition, Point destinationPosition) { - gdwCurPathStep--; - assert(gdwCurPathStep >= 0); - return pnode_tblptr[gdwCurPathStep]; -} - -/** - * @brief zero one of the preallocated nodes and return a pointer to it, or NULL if none are available - */ -PATHNODE *path_new_step() -{ - if (gdwCurNodes >= MAXPATHNODES) - return nullptr; - - PATHNODE *newNode = &path_nodes[gdwCurNodes]; - gdwCurNodes++; - memset(newNode, 0, sizeof(PATHNODE)); - return newNode; + // These checks are written as if working backwards from the destination to the source, given + // both tiles are expected to be adjacent this doesn't matter beyond being a bit confusing + bool rv = true; + switch (GetPathDirection(sourcePosition, destinationPosition)) { + case 5: // Stepping north + rv = IsTileNotSolid(destinationPosition + DIR_SW) && IsTileNotSolid(destinationPosition + DIR_SE); + break; + case 6: // Stepping east + rv = IsTileNotSolid(destinationPosition + DIR_SW) && IsTileNotSolid(destinationPosition + DIR_NW); + break; + case 7: // Stepping south + rv = IsTileNotSolid(destinationPosition + DIR_NE) && IsTileNotSolid(destinationPosition + DIR_NW); + break; + case 8: // Stepping west + rv = IsTileNotSolid(destinationPosition + DIR_SE) && IsTileNotSolid(destinationPosition + DIR_NE); + break; + } + return rv; } } // namespace devilution diff --git a/Source/path.h b/Source/path.h index 10af1736a..e26ebfba1 100644 --- a/Source/path.h +++ b/Source/path.h @@ -28,23 +28,49 @@ struct PATHNODE { bool IsTileNotSolid(Point position); bool IsTileSolid(Point position); + +/** + * @brief Checks the position is solid or blocked by an object + */ bool IsTileWalkable(Point position, bool ignoreDoors = false); + +/** + * @brief Find the shortest path from startPosition to destinationPosition, using PosOk(Point) to check that each step is a valid position. + * Store the step directions (corresponds to an index in PathDirs) in path, which must have room for 24 steps + */ int FindPath(const std::function &posOk, Point start, Point destination, int8_t path[MAX_PATH_LENGTH]); -int path_get_h_cost(Point source, Point destination); -PATHNODE *GetNextPath(); -bool path_solid_pieces(Point position, Point destination); -bool path_get_path(const std::function &posOk, PATHNODE *pPath, Point destination); -bool path_parent_path(PATHNODE *pPath, Point candidatePosition, Point destinationPosition); -PATHNODE *path_get_node1(Point); -PATHNODE *path_get_node2(Point); -void path_next_node(PATHNODE *pPath); -void path_set_coords(PATHNODE *pPath); -void path_push_active_step(PATHNODE *pPath); -PATHNODE *path_pop_active_step(); -PATHNODE *path_new_step(); - -/* rdata */ - -extern const Displacement PathDirs[8]; + +/** + * @brief check if stepping from a given position to a neighbouring tile cuts a corner. + * + * If you step from A to B, both Xs need to be clear: + * + * AX + * XB + * + * @return true if step is allowed + */ +bool path_solid_pieces(Point sourcePosition, Point destinationPosition); + +/** + * @brief heuristic, estimated cost from sourcePosition to destinationPosition. + * + * This is an internal function that is only exposed to allow for testing the way path weights are determined. + */ +int path_get_h_cost(Point sourcePosition, Point destinationPosition); + +/** For iterating over the 8 possible movement directions */ +const Displacement PathDirs[8] = { + // clang-format off + { -1, -1 }, //DIR_N + { -1, 1 }, //DIR_W + { 1, -1 }, //DIR_E + { 1, 1 }, //DIR_S + { -1, 0 }, //DIR_NW + { 0, -1 }, //DIR_NE + { 1, 0 }, //DIR_SE + { 0, 1 }, //DIR_SW + // clang-format on +}; } // namespace devilution