diff --git a/Source/engine/path.cpp b/Source/engine/path.cpp index 9891c6d2366..1457d3c5303 100644 --- a/Source/engine/path.cpp +++ b/Source/engine/path.cpp @@ -5,6 +5,7 @@ */ #include "engine/path.h" +#include #include #include @@ -12,212 +13,141 @@ #include "crawl.hpp" #include "levels/gendung.h" -#include "lighting.h" #include "objects.h" +#include "utils/algorithm/container.hpp" +#include "utils/static_vector.hpp" namespace devilution { + +// The frame times for axis-aligned and diagonal steps are actually the same. +// +// However, we set the diagonal step cost a bit higher to avoid +// excessive diagonal movement. For example, the frame times for these +// two paths are the same: ↑↑ and ↗↖. However, ↑↑ looks more natural. +const int PathAxisAlignedStepCost = 100; +const int PathDiagonalStepCost = 101; + namespace { constexpr size_t MaxPathNodes = 300; -struct PathNode { - static constexpr uint16_t InvalidIndex = std::numeric_limits::max(); - static constexpr size_t MaxChildren = 8; - - int16_t x = 0; - int16_t y = 0; - uint16_t parentIndex = InvalidIndex; - uint16_t childIndices[MaxChildren] = { InvalidIndex, InvalidIndex, InvalidIndex, InvalidIndex, InvalidIndex, InvalidIndex, InvalidIndex, InvalidIndex }; - uint16_t nextNodeIndex = InvalidIndex; - uint8_t f = 0; - uint8_t h = 0; - uint8_t g = 0; +using NodeIndexType = uint16_t; +using CoordType = uint8_t; +using CostType = uint16_t; +using PointT = PointOf; - [[nodiscard]] Point position() const - { - return Point { x, y }; - } +struct FrontierNode { + PointT position; - void addChild(uint16_t childIndex) - { - size_t index = 0; - for (; index < MaxChildren; ++index) { - if (childIndices[index] == InvalidIndex) - break; - } - assert(index < MaxChildren); - childIndices[index] = childIndex; - } + // Current best guess of the cost of the path to destination + // if it goes through this node. + CostType f; }; -PathNode PathNodes[MaxPathNodes]; +struct ExploredNode { + // Preceding node (needed to reconstruct the path at the end). + PointT prev; -/** A linked list of the A* frontier, sorted by distance */ -PathNode *Path2Nodes; + // The current lowest cost from start to this node (0 for the start node). + CostType g; +}; -/** - * @brief return a node for a position on the frontier, or `PathNode::InvalidIndex` if not found - */ -uint16_t GetNode1(Point targetPosition) -{ - uint16_t result = Path2Nodes->nextNodeIndex; - while (result != PathNode::InvalidIndex) { - if (PathNodes[result].position() == targetPosition) - return result; - result = PathNodes[result].nextNodeIndex; - } - return PathNode::InvalidIndex; -} +// A simple map with 4 buckets and static storage. +class ExploredNodes { + static const size_t NumBuckets = 4; + static const size_t BucketCapacity = MaxPathNodes / NumBuckets; + using Entry = std::pair; + using Bucket = StaticVector; -/** - * @brief insert `front` node into the frontier (keeping the frontier sorted by total distance) - */ -void NextNode(uint16_t front) -{ - if (Path2Nodes->nextNodeIndex == PathNode::InvalidIndex) { - Path2Nodes->nextNodeIndex = front; - return; - } +public: + using value_type = Entry; + using iterator = value_type *; + using const_iterator = const value_type *; - PathNode *current = Path2Nodes; - uint16_t nextIndex = Path2Nodes->nextNodeIndex; - const uint8_t maxF = PathNodes[front].f; - while (nextIndex != PathNode::InvalidIndex && PathNodes[nextIndex].f < maxF) { - current = &PathNodes[nextIndex]; - nextIndex = current->nextNodeIndex; + [[nodiscard]] const_iterator find(const PointT &point) const + { + const Bucket &b = bucket(point); + const auto it = c_find_if(b, [&point](const Entry &e) { return e.first == point; }); + if (it == b.end()) return nullptr; + return it; + } + [[nodiscard]] iterator find(const PointT &point) + { + Bucket &b = bucket(point); + auto it = c_find_if(b, [&point](const Entry &e) { return e.first == point; }); + if (it == b.end()) return nullptr; + return it; } - PathNodes[front].nextNodeIndex = nextIndex; - current->nextNodeIndex = front; -} -/** A linked list of all visited nodes */ -PathNode *VisitedNodes; + // NOLINTNEXTLINE(readability-convert-member-functions-to-static) + [[nodiscard]] const_iterator end() const { return nullptr; } + // NOLINTNEXTLINE(readability-convert-member-functions-to-static) + [[nodiscard]] iterator end() { return nullptr; } -/** - * @brief return a node for this position if it was visited, or NULL if not found - */ -uint16_t GetNode2(Point targetPosition) -{ - uint16_t result = VisitedNodes->nextNodeIndex; - while (result != PathNode::InvalidIndex) { - if (PathNodes[result].position() == targetPosition) - return result; - result = PathNodes[result].nextNodeIndex; + void emplace(const PointT &point, const ExploredNode &exploredNode) + { + bucket(point).emplace_back(point, exploredNode); } - return result; -} -/** - * @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 - */ -uint16_t GetNextPath() -{ - uint16_t result = Path2Nodes->nextNodeIndex; - if (result == PathNode::InvalidIndex) { - return result; + [[nodiscard]] bool canInsert(const PointT &point) const + { + return bucket(point).size() < BucketCapacity; } - Path2Nodes->nextNodeIndex = PathNodes[result].nextNodeIndex; - PathNodes[result].nextNodeIndex = VisitedNodes->nextNodeIndex; - VisitedNodes->nextNodeIndex = result; - return result; -} - -/** the number of in-use nodes in path_nodes */ -uint32_t gdwCurNodes; -/** - * @brief zero one of the preallocated nodes and return a pointer to it, or NULL if none are available - */ -uint16_t NewStep() -{ - if (gdwCurNodes >= MaxPathNodes) - return PathNode::InvalidIndex; +private: + [[nodiscard]] const Bucket &bucket(const PointT &point) const { return buckets_[bucketIndex(point)]; } + [[nodiscard]] Bucket &bucket(const PointT &point) { return buckets_[bucketIndex(point)]; } + [[nodiscard]] static size_t bucketIndex(const PointT &point) + { + return ((point.x & 0x1) << 1) | (point.y & 0x1); + } - PathNodes[gdwCurNodes] = {}; - return gdwCurNodes++; -} + std::array buckets_; +}; -/** A stack for recursively searching nodes */ -uint16_t pnode_tblptr[MaxPathNodes]; -/** size of the pnode_tblptr stack */ -uint32_t gdwCurPathStep; -/** - * @brief push pPath onto the pnode_tblptr stack - */ -void PushActiveStep(uint16_t pPath) +bool IsDiagonalStep(const Point &a, const Point &b) { - assert(gdwCurPathStep < MaxPathNodes); - pnode_tblptr[gdwCurPathStep] = pPath; - gdwCurPathStep++; -} - -/** - * @brief pop and return a node from the pnode_tblptr stack - */ -uint16_t PopActiveStep() -{ - gdwCurPathStep--; - return pnode_tblptr[gdwCurPathStep]; + return a.x != b.x && a.y != b.y; } /** * @brief Returns the distance between 2 adjacent nodes. - * - * The distance is 2 for nodes in the same row or column, - * and 3 for diagonally adjacent nodes. - * - * This approximates that diagonal movement on a square grid should have a cost - * of sqrt(2). That's approximately 1.5, so they multiply all step costs by 2, - * except diagonal steps which are times 3 */ -int GetDistance(Point startPosition, Point destinationPosition) +CostType GetDistance(PointT startPosition, PointT destinationPosition) { - if (startPosition.x == destinationPosition.x || startPosition.y == destinationPosition.y) - return 2; - - return 3; + return IsDiagonalStep(startPosition, destinationPosition) + ? PathDiagonalStepCost + : PathAxisAlignedStepCost; } /** * @brief heuristic, estimated cost from startPosition to destinationPosition. */ -int GetHeuristicCost(Point startPosition, Point destinationPosition) +CostType GetHeuristicCost(PointT startPosition, PointT destinationPosition) { - // see GetDistance for why this is times 2 - return 2 * startPosition.ManhattanDistance(destinationPosition); -} - -/** - * @brief update all path costs using depth-first search starting at pPath - */ -void SetCoords(uint16_t pPath) -{ - PushActiveStep(pPath); - // while there are path nodes to check - while (gdwCurPathStep > 0) { - uint16_t pathOldIndex = PopActiveStep(); - const PathNode &pathOld = PathNodes[pathOldIndex]; - for (uint16_t childIndex : pathOld.childIndices) { - if (childIndex == PathNode::InvalidIndex) - break; - PathNode &pathAct = PathNodes[childIndex]; - - if (pathOld.g + GetDistance(pathOld.position(), pathAct.position()) < pathAct.g) { - if (CanStep(pathOld.position(), pathAct.position())) { - pathAct.parentIndex = pathOldIndex; - pathAct.g = pathOld.g + GetDistance(pathOld.position(), pathAct.position()); - pathAct.f = pathAct.g + pathAct.h; - PushActiveStep(childIndex); - } - } - } - } + // This function needs to be admissible, i.e. it should never over-estimate + // the distance. + // + // This calculation assumes we can take diagonal steps until we reach + // the same row or column and then take the remaining axis-aligned steps. + const int dx = std::abs(static_cast(startPosition.x) - static_cast(destinationPosition.x)); + const int dy = std::abs(static_cast(startPosition.y) - static_cast(destinationPosition.y)); + const int diagSteps = std::min(dx, dy); + + // After we've taken `diagSteps`, the remaining steps in one coordinate + // will be zero, and in the other coordinate it will be reduced by `diagSteps`. + // We then still need to take the remaining steps: + // max(dx, dy) - diagSteps = max(dx, dy) - min(dx, dy) = abs(dx - dy) + const int axisAlignedSteps = std::abs(dx - dy); + return diagSteps * PathDiagonalStepCost + axisAlignedSteps * PathAxisAlignedStepCost; } /** * Returns a number representing the direction from a starting tile to a neighbouring tile. * - * Used in the pathfinding code, each step direction is assigned a number like this: + * Used to represent the steps in the resulting path. + * + * Each step direction is assigned a number like this: * dx * -1 0 1 * +----- @@ -231,85 +161,25 @@ int8_t GetPathDirection(Point startPosition, Point destinationPosition) return PathDirections[3 * (destinationPosition.y - startPosition.y) + 4 + destinationPosition.x - startPosition.x]; } -/** - * @brief add a step from pPath to destination, return 1 if successful, and update the frontier/visited nodes accordingly - * - * @param pathIndex index of the current path node - * @param candidatePosition expected to be a neighbour of the current path node position - * @param destinationPosition where we hope to end up - * @return true if step successfully added, false if we ran out of nodes to use - */ -bool ExploreFrontier(uint16_t pathIndex, Point candidatePosition, Point destinationPosition) -{ - PathNode &path = PathNodes[pathIndex]; - int nextG = path.g + GetDistance(path.position(), candidatePosition); - - // 3 cases to consider - // case 1: (dx,dy) is already on the frontier - uint16_t dxdyIndex = GetNode1(candidatePosition); - if (dxdyIndex != PathNode::InvalidIndex) { - path.addChild(dxdyIndex); - PathNode &dxdy = PathNodes[dxdyIndex]; - if (nextG < dxdy.g) { - if (CanStep(path.position(), candidatePosition)) { - // we'll explore it later, just update - dxdy.parentIndex = pathIndex; - dxdy.g = nextG; - dxdy.f = nextG + dxdy.h; - } - } - } else { - // case 2: (dx,dy) was already visited - dxdyIndex = GetNode2(candidatePosition); - if (dxdyIndex != PathNode::InvalidIndex) { - path.addChild(dxdyIndex); - PathNode &dxdy = PathNodes[dxdyIndex]; - if (nextG < dxdy.g && CanStep(path.position(), candidatePosition)) { - // update the node - dxdy.parentIndex = pathIndex; - dxdy.g = nextG; - dxdy.f = nextG + dxdy.h; - // already explored, so re-update others starting from that node - SetCoords(dxdyIndex); - } - } else { - // case 3: (dx,dy) is totally new - dxdyIndex = NewStep(); - if (dxdyIndex == PathNode::InvalidIndex) - return false; - PathNode &dxdy = PathNodes[dxdyIndex]; - dxdy.parentIndex = pathIndex; - dxdy.g = nextG; - dxdy.h = GetHeuristicCost(candidatePosition, destinationPosition); - dxdy.f = nextG + dxdy.h; - dxdy.x = static_cast(candidatePosition.x); - dxdy.y = static_cast(candidatePosition.y); - // add it to the frontier - NextNode(dxdyIndex); - path.addChild(dxdyIndex); - } - } - return true; -} - -/** - * @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 GetPath(tl::function_ref posOk, uint16_t pathIndex, Point destination) +int ReconstructPath(const ExploredNodes &explored, PointT dest, int8_t path[MaxPathLength]) { - for (Displacement dir : PathDirs) { - const PathNode &path = PathNodes[pathIndex]; - const Point tile = path.position() + dir; - const bool ok = posOk(tile); - if ((ok && CanStep(path.position(), tile)) || (!ok && tile == destination)) { - if (!ExploreFrontier(pathIndex, tile, destination)) - return false; + int len = 0; + PointT cur = dest; + while (true) { + const auto it = explored.find(cur); + if (it == explored.end()) app_fatal("Failed to reconstruct path"); + if (it->second.g == 0) break; // reached start + path[len++] = GetPathDirection(it->second.prev, cur); + cur = it->second.prev; + if (len == MaxPathLength) { + // Path too long. + len = 0; + break; } } - - return true; + std::reverse(path, path + len); + std::fill(path + len, path + MaxPathLength, -1); + return len; } } // namespace @@ -371,52 +241,90 @@ bool IsTileOccupied(Point position) int FindPath(tl::function_ref posOk, Point startPosition, Point destinationPosition, int8_t path[MaxPathLength]) { - /** - * 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 pnodeVals[MaxPathLength]; - - // clear all nodes, create root nodes for the visited/frontier linked lists - gdwCurNodes = 0; - Path2Nodes = &PathNodes[NewStep()]; - VisitedNodes = &PathNodes[NewStep()]; - gdwCurPathStep = 0; - const uint16_t pathStartIndex = NewStep(); - PathNode &pathStart = PathNodes[pathStartIndex]; - pathStart.x = static_cast(startPosition.x); - pathStart.y = static_cast(startPosition.y); - pathStart.f = pathStart.h + pathStart.g; - pathStart.h = GetHeuristicCost(startPosition, destinationPosition); - pathStart.g = 0; - Path2Nodes->nextNodeIndex = pathStartIndex; - // A* search until we find (dx,dy) or fail - uint16_t nextNodeIndex; - while ((nextNodeIndex = GetNextPath()) != PathNode::InvalidIndex) { - // reached the end, success! - if (PathNodes[nextNodeIndex].position() == destinationPosition) { - const PathNode *current = &PathNodes[nextNodeIndex]; - size_t pathLength = 0; - while (current->parentIndex != PathNode::InvalidIndex) { - if (pathLength >= MaxPathLength) - break; - pnodeVals[pathLength++] = GetPathDirection(PathNodes[current->parentIndex].position(), current->position()); - current = &PathNodes[current->parentIndex]; + const PointT start { startPosition }; + const PointT dest { destinationPosition }; + + StaticVector frontier; + ExploredNodes explored; + { + frontier.emplace_back(FrontierNode { .position = start, .f = GetHeuristicCost(start, dest) }); + explored.emplace(start, ExploredNode { .prev = {}, .g = 0 }); + } + + const auto frontierComparator = [&explored](const FrontierNode &a, const FrontierNode &b) { + // We use heap functions from which form a max-heap. + // We reverse the comparison sign here to get a min-heap. + if (a.f != b.f) return a.f > b.f; + + // Prefer diagonal steps first. + const bool isDiagonalA = IsDiagonalStep(explored.find(a.position)->second.prev, a.position); + const bool isDiagonalB = IsDiagonalStep(explored.find(a.position)->second.prev, b.position); + if (isDiagonalA != isDiagonalB) return isDiagonalB; + + // Finally, disambiguate by coordinate: + if (a.position.x != b.position.x) return a.position.x > b.position.x; + return a.position.y > b.position.y; + }; + + while (!frontier.empty()) { + const FrontierNode cur = frontier.front(); // argmin(node.f) for node in openSet + + if (cur.position == destinationPosition) { + return ReconstructPath(explored, cur.position, path); + } + + std::pop_heap(frontier.begin(), frontier.end(), frontierComparator); + frontier.pop_back(); + const CostType curG = explored.find(cur.position)->second.g; + + // Discard invalid nodes. + + // If this node is already at the maximum number of steps, we can skip processing it. + // We don't keep track of the maximum number of steps, so we approximate it. + if (curG >= PathDiagonalStepCost * MaxPathLength) continue; + + // When we discover a better path to a node, we push the node to the heap + // with the new `f` value even if the node is already in the heap. + if (curG + GetHeuristicCost(cur.position, dest) > cur.f) continue; + + for (const DisplacementOf d : PathDirs) { + // We're using `uint8_t` for coordinates. Avoid underflow: + if ((cur.position.x == 0 && d.deltaX < 0) || (cur.position.y == 0 && d.deltaY < 0)) continue; + const PointT neighborPos = cur.position + d; + const bool ok = posOk(neighborPos); + if (ok) { + if (!CanStep(cur.position, neighborPos)) continue; + } else { + // We only check CanStep for destination if `posOk` returns true for it. + // This seems like a bug + if (neighborPos != dest) continue; } - if (pathLength != MaxPathLength) { - size_t i; - for (i = 0; i < pathLength; i++) - path[i] = pnodeVals[pathLength - i - 1]; - return static_cast(i); + const CostType g = curG + GetDistance(cur.position, neighborPos); + if (curG >= PathDiagonalStepCost * MaxPathLength) continue; + bool improved = false; + if (auto it = explored.find(neighborPos); it == explored.end()) { + if (explored.canInsert(neighborPos)) { + explored.emplace(neighborPos, ExploredNode { .prev = cur.position, .g = g }); + improved = true; + } + } else if (it->second.g > g) { + it->second.prev = cur.position; + it->second.g = g; + improved = true; + } + if (improved) { + const CostType f = g + GetHeuristicCost(neighborPos, dest); + if (frontier.size() < MaxPathNodes) { + // We always push the node to the heap, even if the same position already exists in it. + // When popping from the heap, we discard invalid nodes by checking that `g + h <= f`. + frontier.emplace_back(FrontierNode { .position = neighborPos, .f = f }); + std::push_heap(frontier.begin(), frontier.end(), frontierComparator); + } } - return 0; } - // ran out of nodes, abort! - if (!GetPath(posOk, nextNodeIndex, destinationPosition)) - return 0; } - // frontier is empty, no path! - return 0; + + return 0; // no path } bool CanStep(Point startPosition, Point destinationPosition) diff --git a/Source/engine/path.h b/Source/engine/path.h index 931c545c473..24bf5fe8ff3 100644 --- a/Source/engine/path.h +++ b/Source/engine/path.h @@ -19,6 +19,12 @@ namespace devilution { constexpr size_t MaxPathLength = 25; +// Cost for an axis-aligned step (up/down/left/right). Visible for testing. +extern const int PathAxisAlignedStepCost; + +// Cost for a diagonal step. Visible for testing. +extern const int PathDiagonalStepCost; + bool IsTileNotSolid(Point position); bool IsTileSolid(Point position); diff --git a/Source/utils/static_vector.hpp b/Source/utils/static_vector.hpp index 5e894dabbab..b57fcf8afe9 100644 --- a/Source/utils/static_vector.hpp +++ b/Source/utils/static_vector.hpp @@ -22,7 +22,12 @@ class StaticVector { using value_type = T; using reference = T &; using const_reference = const T &; + using pointer = T *; + using const_pointer = const T *; using size_type = size_t; + using iterator = T *; + using const_iterator = const T *; + using difference_type = std::ptrdiff_t; StaticVector() = default; diff --git a/test/path_test.cpp b/test/path_test.cpp index a05019345b7..d5d1101af69 100644 --- a/test/path_test.cpp +++ b/test/path_test.cpp @@ -1,15 +1,24 @@ -#include - #include "engine/path.h" +#include +#include + +#include +#include + // The following headers are included to access globals used in functions that have not been isolated yet. #include "levels/gendung.h" #include "objects.h" +#include "utils/algorithm/container.hpp" namespace devilution { extern int TestPathGetHeuristicCost(Point startPosition, Point destinationPosition); +namespace { + +using ::testing::ElementsAreArray; + TEST(PathTest, Heuristics) { constexpr Point source { 25, 32 }; @@ -17,29 +26,29 @@ TEST(PathTest, Heuristics) EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 0) << "Wrong cost for travelling to the same tile"; destination = source + Direction::NorthEast; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 2) << "Wrong cost for travelling to horizontal/vertical adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathAxisAlignedStepCost) << "Wrong cost for travelling to horizontal/vertical adjacent tile"; destination = source + Direction::SouthEast; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 2) << "Wrong cost for travelling to horizontal/vertical adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathAxisAlignedStepCost) << "Wrong cost for travelling to horizontal/vertical adjacent tile"; destination = source + Direction::SouthWest; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 2) << "Wrong cost for travelling to horizontal/vertical adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathAxisAlignedStepCost) << "Wrong cost for travelling to horizontal/vertical adjacent tile"; destination = source + Direction::NorthWest; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 2) << "Wrong cost for travelling to horizontal/vertical adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathAxisAlignedStepCost) << "Wrong cost for travelling to horizontal/vertical adjacent tile"; destination = source + Direction::North; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 4) << "Wrong cost for travelling to diagonally adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathDiagonalStepCost) << "Wrong cost for travelling to diagonally adjacent tile"; destination = source + Direction::East; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 4) << "Wrong cost for travelling to diagonally adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathDiagonalStepCost) << "Wrong cost for travelling to diagonally adjacent tile"; destination = source + Direction::South; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 4) << "Wrong cost for travelling to diagonally adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathDiagonalStepCost) << "Wrong cost for travelling to diagonally adjacent tile"; destination = source + Direction::West; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 4) << "Wrong cost for travelling to diagonally adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathDiagonalStepCost) << "Wrong cost for travelling to diagonally adjacent tile"; destination = source + Direction::SouthWest + Direction::SouthEast; // Effectively the same as Direction::South - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 4) << "Wrong cost for travelling to diagonally adjacent tile"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathDiagonalStepCost) << "Wrong cost for travelling to diagonally adjacent tile"; destination = source + Direction::NorthEast + Direction::North; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 6) << "Wrong cost for travelling to a { 2, 1 } offset"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), PathAxisAlignedStepCost + PathDiagonalStepCost) << "Wrong cost for travelling to a { 2, 1 } offset"; destination = source + Direction::SouthEast + Direction::SouthEast; - EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 4) << "Wrong cost for travelling to a { 2, 0 } offset"; + EXPECT_EQ(TestPathGetHeuristicCost(source, destination), 2 * PathAxisAlignedStepCost) << "Wrong cost for travelling to a { 2, 0 } offset"; } TEST(PathTest, Solid) @@ -101,40 +110,97 @@ TEST(PathTest, CanStepTest) dPiece[1][1] = 0; } -void CheckPath(Point startPosition, Point destinationPosition, std::vector expectedSteps) +// These symbols are in terms of coordinates (not in terms of on-screen direction). +// -1, -1 is top-left. +enum class Dir { + None, + Up, + Left, + Right, + Down, + UpLeft, + UpRight, + DownRight, + DownLeft +}; +std::array DirSymbols = { "∅", "↑", "←", "→", "↓", "↖", "↗", "↘", "↙" }; + +std::ostream &operator<<(std::ostream &os, Dir dir) { - static int8_t pathSteps[MaxPathLength]; - auto pathLength = FindPath([](Point) { return true; }, startPosition, destinationPosition, pathSteps); + return os << DirSymbols[static_cast(dir)]; +} - EXPECT_EQ(pathLength, expectedSteps.size()) << "Wrong path length for a path from " << startPosition << " to " << destinationPosition; - // Die early if the wrong path length is returned as we don't want to read oob in expectedSteps - ASSERT_LE(pathLength, expectedSteps.size()) << "Path is longer than expected."; +std::vector ToSyms(std::span strings) +{ + std::vector result; + result.reserve(strings.size()); + for (const std::string &str : strings) + result.emplace_back(static_cast(std::distance(DirSymbols.begin(), c_find(DirSymbols, str)))); + return result; +} - for (int i = 0; i < pathLength; i++) { - EXPECT_EQ(pathSteps[i], expectedSteps[i]) << "Path step " << i << " differs from expectation for a path from " - << startPosition << " to " << destinationPosition; // this shouldn't be a requirement but... +std::vector ToSyms(std::span indices) +{ + std::vector result; + result.reserve(indices.size()); + for (const int8_t idx : indices) + result.emplace_back(static_cast(idx)); + return result; +} - // Path directions are all jacked up compared to the Direction enum. Most consumers have their own mapping definition - // startPosition += Direction { path[i] - 1 }; - } - // Given that we can't really make any assumptions about how the path is actually used. - // EXPECT_EQ(startPosition, destinationPosition) << "Path doesn't lead to destination"; +void CheckPath(Point startPosition, Point destinationPosition, std::vector expectedSteps) +{ + int8_t pathSteps[MaxPathLength]; + auto pathLength = FindPath([](Point) { return true; }, startPosition, destinationPosition, pathSteps); + EXPECT_THAT(ToSyms(std::span(pathSteps, pathLength)), ElementsAreArray(ToSyms(expectedSteps))) + << "Path steps differs from expectation for a path from " + << startPosition << " to " << destinationPosition; } -TEST(PathTest, FindPath) +TEST(PathTest, FindPathToSelf) { CheckPath({ 8, 8 }, { 8, 8 }, {}); +} - // Traveling in cardinal directions is the only way to get a first step in a cardinal direction - CheckPath({ 8, 8 }, { 8, 6 }, { 1, 1 }); - CheckPath({ 8, 8 }, { 6, 8 }, { 2, 2 }); - CheckPath({ 8, 8 }, { 10, 8 }, { 3, 3 }); - CheckPath({ 8, 8 }, { 8, 10 }, { 4, 4 }); +TEST(PathTest, FindPathTwoStepsUp) +{ + CheckPath({ 8, 8 }, { 8, 6 }, { "↑", "↑" }); +} - // Otherwise pathing biases along diagonals and the diagonal steps will always be first - CheckPath({ 8, 8 }, { 5, 6 }, { 5, 5, 2 }); - CheckPath({ 8, 8 }, { 4, 4 }, { 5, 5, 5, 5 }); - CheckPath({ 8, 8 }, { 12, 20 }, { 7, 7, 7, 7, 4, 4, 4, 4, 4, 4, 4, 4 }); +TEST(PathTest, FindPathTwoStepsLeft) +{ + CheckPath({ 8, 8 }, { 6, 8 }, { "←", "←" }); +} + +TEST(PathTest, FindPathTwoStepsRight) +{ + CheckPath({ 8, 8 }, { 10, 8 }, { "→", "→" }); +} + +TEST(PathTest, FindPathTwoStepsDown) +{ + CheckPath({ 8, 8 }, { 8, 10 }, { "↓", "↓" }); +} + +TEST(PathTest, FindPathDiagonalsFirst3Left2Up) +{ + // Pathing biases along diagonals and the diagonal steps will always be first + CheckPath({ 8, 8 }, { 5, 6 }, { "↖", "↖", "←" }); +} + +TEST(PathTest, FindPathDiagonalsFirst4Left4Up) +{ + CheckPath({ 8, 8 }, { 4, 4 }, { "↖", "↖", "↖", "↖" }); +} + +TEST(PathTest, FindPathDiagonalsFirst2Right4Down) +{ + CheckPath({ 8, 8 }, { 10, 12 }, { "↘", "↘", "↓", "↓" }); +} + +TEST(PathTest, FindPathDiagonalsFirst4Right12Down) +{ + CheckPath({ 8, 8 }, { 12, 20 }, { "↘", "↘", "↘", "↘", "↓", "↓", "↓", "↓", "↓", "↓", "↓", "↓" }); } TEST(PathTest, LongPaths) @@ -144,7 +210,7 @@ TEST(PathTest, LongPaths) // Longest possible path is currently 24 steps meaning tiles 24 units away are reachable Point startingPosition { 56, 56 }; - CheckPath(startingPosition, startingPosition + Displacement { 24, 24 }, std::vector(24, 7)); + CheckPath(startingPosition, startingPosition + Displacement { 24, 24 }, std::vector(24, "↘")); // But trying to navigate 25 units fails CheckPath(startingPosition, startingPosition + Displacement { 25, 25 }, {}); @@ -285,4 +351,6 @@ TEST(PathTest, FindClosest) EXPECT_EQ(*nearPosition, (Point { 50, 50 } + Displacement { 0, 21 })) << "First candidate position with a minimum radius should be at {0, +y}"; } } + +} // namespace } // namespace devilution