diff --git a/src/action/action_move.cpp b/src/action/action_move.cpp index cc2e65a2f5..ac8429b672 100644 --- a/src/action/action_move.cpp +++ b/src/action/action_move.cpp @@ -173,7 +173,7 @@ int DoActionMove(CUnit &unit) } UnmarkUnitFieldFlags(unit); - d = NextPathElement(unit, &posd.x, &posd.y); + d = NextPathElement(unit, &posd); MarkUnitFieldFlags(unit); switch (d) { case PF_UNREACHABLE: // Can't reach, stop diff --git a/src/include/actions.h b/src/include/actions.h index 7ad3d804c8..9dc0d65ac7 100644 --- a/src/include/actions.h +++ b/src/include/actions.h @@ -35,6 +35,9 @@ #include "unitptr.h" #include "vec2i.h" +#include +#include + /*---------------------------------------------------------------------------- -- Declarations ----------------------------------------------------------------------------*/ diff --git a/src/include/pathfinder.h b/src/include/pathfinder.h index f87368320e..e020a950ef 100644 --- a/src/include/pathfinder.h +++ b/src/include/pathfinder.h @@ -32,6 +32,7 @@ //@{ +void DrawLastAStar(const class CViewport &vp); #if defined(DEBUG_ASTAR) #define AstarDebugPrint(format, ...) DebugPrint(format, ##__VA_ARGS__) #else @@ -225,7 +226,7 @@ extern void InitPathfinder(); extern void FreePathfinder(); /// Returns the next element of the path -extern int NextPathElement(CUnit &unit, short int *xdp, short int *ydp); +extern int NextPathElement(CUnit &unit, Vec2i *dir); /// Return path length to unit 'dst'. extern int UnitReachable(const CUnit &src, const CUnit &dst, int range, bool from_outside_container); /// Return path length to unit 'dst' or error code. diff --git a/src/include/tile.h b/src/include/tile.h index cd9ea80de8..8f213c9d54 100644 --- a/src/include/tile.h +++ b/src/include/tile.h @@ -180,7 +180,7 @@ class CMapFieldPlayerInfo class CMapField { public: - CMapField(); + CMapField() = default; void Save(CFile &file) const; void parse(lua_State *l); @@ -231,30 +231,29 @@ class CMapField unsigned int getFlag() const { return Flags; } void setGraphicTile(unsigned int tile) { this->tile = tile; } #ifdef DEBUG - int64_t lastAStarCost; /// debugging pathfinder + int64_t lastAStarCost = 0; /// debugging pathfinder #endif uint8_t getElevation() const { return this->ElevationLevel; } - void setElevation(const uint8_t newLevel) { this->ElevationLevel = newLevel; } + void setElevation(const uint8_t newLevel) { this->ElevationLevel = newLevel; } private: #ifdef DEBUG - tile_index tilesetTile; /// tileset tile number + tile_index tilesetTile = 0; /// tileset tile number #endif - graphic_index tile; /// graphic tile number + graphic_index tile = 0; /// graphic tile number public: - tile_flags Flags; /// field flags + tile_flags Flags = 0; /// field flags private: - unsigned char cost; /// unit cost to move in this tile + unsigned char cost = 0; /// unit cost to move in this tile public: - unsigned int Value; /// HP for walls/Wood Regeneration, value of stored resource for forest or harvestable terrain + unsigned int Value = 0; /// HP for walls/Wood Regeneration, value of stored resource for forest or harvestable terrain std::vector UnitCache; /// A unit on the map field. CMapFieldPlayerInfo playerInfo; /// stuff related to player private: - uint8_t ElevationLevel {0}; /// highground elevation level - + uint8_t ElevationLevel {0}; /// highground elevation level }; extern PixelSize PixelTileSize; /// Size of a tile in pixels diff --git a/src/map/map_draw.cpp b/src/map/map_draw.cpp index 34dbfa1809..e1506505c2 100644 --- a/src/map/map_draw.cpp +++ b/src/map/map_draw.cpp @@ -346,6 +346,7 @@ void CViewport::DrawMapBackgroundInViewport(const fieldHighlightChecker highligh } } } + DrawLastAStar(*this); #endif /// Highlight layer if needed (editor stuff) if (highlightChecker && highlightChecker(mf)) { diff --git a/src/map/mapfield.cpp b/src/map/mapfield.cpp index 03acf4a9da..1c06735f4d 100644 --- a/src/map/mapfield.cpp +++ b/src/map/mapfield.cpp @@ -46,17 +46,6 @@ #include "unit.h" #include "unit_manager.h" -CMapField::CMapField() : -#ifdef DEBUG - tilesetTile(0), -#endif - tile(0), - Flags(0), - cost(0), - Value(0), - UnitCache() -{} - bool CMapField::IsTerrainResourceOnMap(int resource) const { switch (resource) { diff --git a/src/pathfinder/astar.cpp b/src/pathfinder/astar.cpp index f0cc44a011..9773fa4355 100644 --- a/src/pathfinder/astar.cpp +++ b/src/pathfinder/astar.cpp @@ -47,15 +47,18 @@ -- Includes ----------------------------------------------------------------------------*/ -#include "stratagus.h" - #include "map.h" +#include "pathfinder.h" #include "settings.h" +#include "stratagus.h" #include "tileset.h" #include "unit.h" #include "unit_find.h" -#include "pathfinder.h" +#if defined(DEBUG_ASTAR) +# include "font.h" +# include "viewport.h" +#endif #include @@ -68,15 +71,20 @@ struct Node { void SetCostFromStart(uint64_t cost); int32_t GetCostToGoal() const; void SetCostToGoal(uint64_t cost); - bool IsInGoal(); + bool IsInGoal() const; void SetInGoal(); - uint8_t GetDirection(); + uint8_t GetDirection() const; void SetDirection(uint8_t dir); private: - int32_t CostFromStart; /// Real costs to reach this point - uint16_t CostToGoal; /// Estimated cost to goal - int8_t InGoal; /// is this point in the goal - int8_t Direction; /// Direction for trace back + // align the matrix, the open set, and the cost to move cache + // on 64-byte boundary, in case the memset/memmove operations + // of the libc we're using has a 128/256/512bit SIMD vector + // instruction branch, since we might be clearing 8M of + // memory for a 2048x2048 map + alignas(64) int32_t CostFromStart = 0; /// Real costs to reach this point + uint16_t CostToGoal = 0; /// Estimated cost to goal + int8_t InGoal = 0; /// is this point in the goal + int8_t Direction = 0; /// Direction for trace back }; struct Open { @@ -112,18 +120,16 @@ int Heading2O[9];//heading to offset const int XY2Heading[3][3] = { {7, 6, 5}, {0, 0, 4}, {1, 2, 3}}; /// cost matrix -static Node *AStarMatrix; +static std::vector AStarMatrix; /// a list of close nodes, helps to speed up the matrix cleaning -static int OpenSetMaxSize; -static int AStarMatrixSize; #define MAX_CLOSE_SET_RATIO 4 #define MAX_OPEN_SET_RATIO 8 // 10,16 to small /// see pathfinder.h int AStarFixedUnitCrossingCost;// = MaxMapWidth * MaxMapHeight; int AStarMovingUnitCrossingCost = 5; -int AStarMaxSearchIterations = 1024; +int AStarMaxSearchIterations = 1024 * 5; bool AStarKnowUnseenTerrain = false; int AStarUnknownTerrainCost = 2; /// Used to temporary make enemy units unpassable (needs for correct path lenght calculating for automatic targeting alorithm) @@ -131,7 +137,6 @@ static bool AStarFixedEnemyUnitsUnpassable = false; static int AStarMapWidth; static int AStarMapHeight; -static int AStarMapMax; static int AStarGoalX; static int AStarGoalY; @@ -142,12 +147,11 @@ static int AStarGoalY; */ /// The set of Open nodes -static Open *OpenSet; +static std::vector OpenSet; /// The size of the open node set static int OpenSetSize; -static int32_t *CostMoveToCache; -static int CostMoveToCacheSize; +static std::vector CostMoveToCache; static constexpr int CacheNotSet = -1; /*---------------------------------------------------------------------------- @@ -280,11 +284,10 @@ void Node::SetCostFromStart(uint64_t cost) { } int32_t Node::GetCostToGoal() const { - return this->CostToGoal << 4; + return this->CostToGoal; } void Node::SetCostToGoal(uint64_t cost) { - cost >>= 4; if (cost > UINT16_MAX) { this->CostToGoal = UINT16_MAX; } else { @@ -292,7 +295,7 @@ void Node::SetCostToGoal(uint64_t cost) { } } -bool Node::IsInGoal() { +bool Node::IsInGoal() const { return this->InGoal; } @@ -300,7 +303,7 @@ void Node::SetInGoal() { this->InGoal = 1; } -uint8_t Node::GetDirection() { +uint8_t Node::GetDirection() const { return this->Direction; } @@ -334,32 +337,19 @@ uint32_t Open::GetOffset() const { void InitAStar(int mapWidth, int mapHeight) { // Should only be called once - Assert(!AStarMatrix); + Assert(AStarMatrix.empty()); AStarMapWidth = mapWidth; AStarMapHeight = mapHeight; - AStarMapMax = AStarMapWidth * AStarMapHeight; - // align the matrix, the open set, and the cost to move cache - // on 64-byte boundary, in case the memset/memmove operations - // of the libc we're using has a 128/256/512bit SIMD vector - // instruction branch, since we might be clearing 8M of - // memory for a 2048x2048 map - AStarMatrixSize = sizeof(Node) * AStarMapMax; - AStarMatrix = (Node *)aligned_malloc(64, AStarMatrixSize); - memset(AStarMatrix, 0, AStarMatrixSize); + AStarMatrix.resize(AStarMapWidth * AStarMapHeight); #ifdef DEBUG - for (int i = 0; i < AStarMapMax; i++) { - AStarMatrix[i].SetDirection(-1); + for (auto& node : AStarMatrix) { + node.SetDirection(-1); } #endif - - OpenSetMaxSize = AStarMapMax / MAX_OPEN_SET_RATIO; - OpenSet = (Open *)aligned_malloc(64, OpenSetMaxSize * sizeof(Open)); - - CostMoveToCacheSize = sizeof(int32_t) * AStarMapMax; - CostMoveToCache = (int32_t*)aligned_malloc(64, CostMoveToCacheSize); - memset(CostMoveToCache, CacheNotSet, CostMoveToCacheSize); + OpenSet.resize(AStarMapWidth * AStarMapHeight / MAX_OPEN_SET_RATIO); + CostMoveToCache.resize(AStarMapWidth * AStarMapHeight, CacheNotSet); for (int i = 0; i < 9; ++i) { Heading2O[i] = Heading2Y[i] * AStarMapWidth; @@ -373,13 +363,10 @@ void InitAStar(int mapWidth, int mapHeight) */ void FreeAStar() { - aligned_free(AStarMatrix); - AStarMatrix = nullptr; - aligned_free(OpenSet); - OpenSet = nullptr; + AStarMatrix.clear(); + OpenSet.clear(); OpenSetSize = 0; - aligned_free(CostMoveToCache); - CostMoveToCache = nullptr; + CostMoveToCache.clear(); ProfilePrint(); } @@ -389,10 +376,10 @@ void FreeAStar() */ static void AStarPrepare() { - memset(AStarMatrix, 0, AStarMatrixSize); + ranges::fill(AStarMatrix, Node{}); #ifdef DEBUG - for (int i = 0; i < AStarMapMax; i++) { - AStarMatrix[i].SetDirection(-1); + for (auto& node : AStarMatrix) { + node.SetDirection(-1); } #endif } @@ -411,7 +398,7 @@ static void AStarCleanUp() static void CostMoveToCacheCleanUp() { - memset(CostMoveToCache, CacheNotSet, CostMoveToCacheSize); + ranges::fill(CostMoveToCache, CacheNotSet); } /** @@ -436,7 +423,7 @@ static void AStarRemoveMinimum(int pos) ** ** @return 0 or PF_FAILED */ -static inline int AStarAddNode(const Vec2i &pos, int o, int64_t costs) +static inline int AStarAddNode(const Vec2i &pos, int64_t costs) { ProfileBegin("AStarAddNode"); @@ -447,14 +434,14 @@ static inline int AStarAddNode(const Vec2i &pos, int o, int64_t costs) int32_t midDist; const Open *open; - if (OpenSetSize + 1 >= OpenSetMaxSize) { + if (OpenSetSize + 1 >= OpenSet.size()) { ErrorPrint("A* internal error: raise Open Set Max Size (current value %d)\n", - OpenSetMaxSize); + (int)OpenSet.size()); ProfileEnd("AStarAddNode"); return PF_FAILED; } - const int costToGoal = AStarMatrix[o].GetCostToGoal(); + const int costToGoal = costs; const int dist = std::abs(pos.x - AStarGoalX) + std::abs(pos.y - AStarGoalY); // find where we should insert this node. @@ -515,7 +502,7 @@ static void AStarReplaceNode(int pos) memmove(&OpenSet[pos], &OpenSet[pos+1], sizeof(Open) * (OpenSetSize-pos)); // Re-add the node with the new cost - AStarAddNode(node.pos, node.GetOffset(), node.GetCosts()); + AStarAddNode(node.pos, node.GetCosts()); ProfileEnd("AStarReplaceNode"); } @@ -622,7 +609,7 @@ static int CostMoveToCallBack_Default(unsigned int index, const CUnit &unit) } while (--i); index += AStarMapWidth; } while (--h); - return cost; + return cost / (unit.Type->TileWidth * unit.Type->TileHeight); } @@ -1049,7 +1036,7 @@ int AStarFindPath(const Vec2i &startPos, const Vec2i &goalPosIn, int gw, int gh, // place start point in open, it that failed, try another pathfinder int costToGoal = AStarCosts(startPos, goalPos); AStarMatrix[eo].SetCostToGoal(costToGoal); - if (AStarAddNode(startPos, eo, 1 + costToGoal) == PF_FAILED) { + if (AStarAddNode(startPos, 1 + costToGoal) == PF_FAILED) { ret = PF_FAILED; ProfileEnd("AStarFindPath"); return ret; @@ -1125,7 +1112,7 @@ int AStarFindPath(const Vec2i &startPos, const Vec2i &goalPosIn, int gw, int gh, //eo = GetIndex(ex, ey); eo = o + Heading2X[i] + Heading2O[i]; - if (eo < 0 || eo >= CostMoveToCacheSize) { + if (eo < 0 || eo >= CostMoveToCache.size()) { // unaccessible tile continue; } @@ -1149,7 +1136,7 @@ int AStarFindPath(const Vec2i &startPos, const Vec2i &goalPosIn, int gw, int gh, AStarMatrix[eo].SetDirection(i); costToGoal = AStarCosts(endPos, goalPos); AStarMatrix[eo].SetCostToGoal(costToGoal); - if (AStarAddNode(endPos, eo, new_cost + costToGoal) == PF_FAILED) { + if (AStarAddNode(endPos, new_cost + costToGoal) == PF_FAILED) { ret = PF_FAILED; ProfileEnd("AStarFindPath"); return ret; @@ -1165,7 +1152,7 @@ int AStarFindPath(const Vec2i &startPos, const Vec2i &goalPosIn, int gw, int gh, if (j == -1) { costToGoal = AStarCosts(endPos, goalPos); AStarMatrix[eo].SetCostToGoal(costToGoal); - if (AStarAddNode(endPos, eo, new_cost + costToGoal) == PF_FAILED) { + if (AStarAddNode(endPos, new_cost + costToGoal) == PF_FAILED) { ret = PF_FAILED; ProfileEnd("AStarFindPath"); return ret; @@ -1204,31 +1191,31 @@ void AStarDumpStats() int32_t maxCostToGoal = 0; int32_t minCostToGoal = INT_MAX; - for (int i = 0; i < AStarMapMax; i++) { - Node *m = &AStarMatrix[i]; - maxCostFromHome = std::max(maxCostFromHome, m->GetCostFromStart()); - maxCostToGoal = std::max(maxCostToGoal, m->GetCostToGoal()); - minCostFromHome = m->GetCostFromStart() ? std::min(minCostFromHome, m->GetCostFromStart()) : minCostFromHome; - minCostToGoal = m->GetCostToGoal() ? std::min(minCostToGoal, m->GetCostToGoal()) : minCostToGoal; + for (const Node &m : AStarMatrix) { + + maxCostFromHome = std::max(maxCostFromHome, m.GetCostFromStart()); + maxCostToGoal = std::max(maxCostToGoal, m.GetCostToGoal()); + minCostFromHome = m.GetCostFromStart() ? std::min(minCostFromHome, m.GetCostFromStart()) : minCostFromHome; + minCostToGoal = m.GetCostToGoal() ? std::min(minCostToGoal, m.GetCostToGoal()) : minCostToGoal; } if (minCostToGoal) minCostToGoal--; maxCostToGoal++; maxCostFromHome++; if (minCostFromHome) minCostFromHome--; - for (int i = 0; i < AStarMapMax; i++) { - Node *m = &AStarMatrix[i]; + int i = 0; + for (const Node &m : AStarMatrix) { int r = 0; int g = 0; - if (m->GetCostFromStart() && maxCostFromHome - minCostFromHome) { - g = (1.0 - ((double)(m->GetCostFromStart() - minCostFromHome) / (maxCostFromHome - minCostFromHome))) * 255; + if (m.GetCostFromStart() && maxCostFromHome - minCostFromHome) { + g = (1.0 - ((double)(m.GetCostFromStart() - minCostFromHome) / (maxCostFromHome - minCostFromHome))) * 255; } - if (m->GetCostToGoal() && maxCostToGoal - minCostToGoal) { - r = (1.0 - ((double)(m->GetCostToGoal() - minCostToGoal) / (maxCostToGoal - minCostToGoal))) * 255; + if (m.GetCostToGoal() && maxCostToGoal - minCostToGoal) { + r = (1.0 - ((double)(m.GetCostToGoal() - minCostToGoal) / (maxCostToGoal - minCostToGoal))) * 255; } const char *direction; // N NE E SE S SW W NW - switch (m->GetDirection()) { + switch (m.GetDirection()) { case 0: direction = "v"; break; @@ -1259,16 +1246,41 @@ void AStarDumpStats() default: direction = " "; } - if (m->IsInGoal()) { + if (m.IsInGoal()) { direction = "X"; } fprintf(stdout, "\33[48;2;%d;%d;0m%s\33[49m", r, g, direction); - if (!(i % AStarMapWidth)) { + if (!(i++ % AStarMapWidth)) { fprintf(stdout, "\n"); } } } +void DrawLastAStar(const CViewport& vp) +{ +#if defined(DEBUG_ASTAR) + for (auto y = vp.MapPos.y; y != vp.MapPos.y + vp.MapHeight; ++y) { + for (auto x = vp.MapPos.x; x != vp.MapPos.x + vp.MapWidth; ++x) { + const auto &node = AStarMatrix[GetIndex(x, y)]; + const auto direction = node.GetDirection(); + if (direction == 255) { + continue; + } + const auto nextX = x - Heading2X[direction]; + const auto nextY = y - Heading2Y[direction]; + const auto pixel1 = vp.TilePosToScreen_Center(Vec2i(x, y)); + const auto pixel2 = vp.TilePosToScreen_Center(Vec2i(nextX, nextY)); + + if (0 <= pixel2.x && pixel2.x < Video.Width && 0 <= pixel2.y + && pixel2.y < Video.Height) { + Video.DrawLine(ColorLightGray, pixel1.x, pixel1.y, pixel2.x, pixel2.y); + CLabel(GetSmallFont()).Draw(pixel1.x - 10, pixel1.y - 10, node.GetCostFromStart() + node.GetCostToGoal()); + } + } + } +#endif +} + /*---------------------------------------------------------------------------- -- Configurable costs ----------------------------------------------------------------------------*/ diff --git a/src/pathfinder/pathfinder.cpp b/src/pathfinder/pathfinder.cpp index b5b9f9ad3c..b19dad8c49 100644 --- a/src/pathfinder/pathfinder.cpp +++ b/src/pathfinder/pathfinder.cpp @@ -431,13 +431,12 @@ static int NewPath(PathFinderInput &input, PathFinderOutput &output) ** Returns the next element of a path. ** ** @param unit Unit that wants the path element. -** @param pxd Pointer for the x direction. -** @param pyd Pointer for the y direction. +** @param dir Pointer for the direction. ** ** @return >0 remaining path length, 0 wait for path, -1 ** reached goal, -2 can't reach the goal. */ -int NextPathElement(CUnit &unit, short int *pxd, short int *pyd) +int NextPathElement(CUnit &unit, Vec2i *dir) { PathFinderInput &input = unit.pathFinderData->input; PathFinderOutput &output = unit.pathFinderData->output; @@ -445,8 +444,7 @@ int NextPathElement(CUnit &unit, short int *pxd, short int *pyd) unit.CurrentOrder()->UpdatePathFinderData(input); // Attempt to use path cache // FIXME: If there is a goal, it may have moved, ruining the cache - *pxd = 0; - *pyd = 0; + *dir = {0, 0}; // Goal has moved, need to recalculate path or no cached path if (output.Length <= 0 || input.IsRecalculateNeeded()) { @@ -461,12 +459,11 @@ int NextPathElement(CUnit &unit, short int *pxd, short int *pyd) } } - *pxd = Heading2X[(int)output.Path[(int)output.Length - 1]]; - *pyd = Heading2Y[(int)output.Path[(int)output.Length - 1]]; - const Vec2i dir(*pxd, *pyd); + dir->x = Heading2X[(int)output.Path[(int)output.Length - 1]]; + dir->y = Heading2Y[(int)output.Path[(int)output.Length - 1]]; int result = output.Length; output.Length--; - if (!UnitCanBeAt(unit, unit.tilePos + dir)) { + if (!UnitCanBeAt(unit, unit.tilePos + *dir)) { // If obstructing unit is moving, wait for a bit. if (output.Fast) { output.Fast--; @@ -481,13 +478,12 @@ int NextPathElement(CUnit &unit, short int *pxd, short int *pyd) AstarDebugPrint("WAIT expired\n"); result = NewPath(input, output); if (result > 0) { - *pxd = Heading2X[(int)output.Path[(int)output.Length - 1]]; - *pyd = Heading2Y[(int)output.Path[(int)output.Length - 1]]; - if (!UnitCanBeAt(unit, unit.tilePos + dir)) { + dir->x = Heading2X[(int)output.Path[(int)output.Length - 1]]; + dir->y = Heading2Y[(int)output.Path[(int)output.Length - 1]]; + if (!UnitCanBeAt(unit, unit.tilePos + *dir)) { // There may be unit in the way, Astar may allow you to walk onto it. result = PF_UNREACHABLE; - *pxd = 0; - *pyd = 0; + *dir = {0, 0}; } else { result = output.Length; output.Length--; diff --git a/tests/stratagus/test_pathfinder.cpp b/tests/stratagus/test_pathfinder.cpp new file mode 100644 index 0000000000..94c83e0bc3 --- /dev/null +++ b/tests/stratagus/test_pathfinder.cpp @@ -0,0 +1,150 @@ +// _________ __ __ +// / _____// |_____________ _/ |______ ____ __ __ ______ +// \_____ \\ __\_ __ \__ \\ __\__ \ / ___\| | \/ ___/ +// / \| | | | \// __ \| | / __ \_/ /_/ > | /\___ | +// /_______ /|__| |__| (____ /__| (____ /\___ /|____//____ > +// \/ \/ \//_____/ \/ +// ______________________ ______________________ +// T H E W A R B E G I N S +// Stratagus - A free fantasy real time strategy game engine +// +/**@name test_pathfinder.cpp - The test file for pathfinder.cpp. */ +// +// (c) Copyright 2024 by Joris Dauphin +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; only version 2 of the License. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA +// 02111-1307, USA. +// + +#include + +#include "action/action_move.h" +#include "actions.h" +#include "map.h" +#include "pathfinder.h" +#include "stratagus.h" +#include "unit.h" +#include "unittype.h" + +namespace doctest +{ +template +struct StringMaker> +{ + static String convert(const Vec2T &value) + { + String res = "{"; + res += toString(value.x); + res += ", "; + res += toString(value.y); + res += "}"; + return res; + } +}; +} // namespace doctest + +namespace +{ +Vec2i FollowedPath(const Vec2i &origin, const PathFinderOutput &data) +{ + Vec2i res = origin; + for (int i = 0; i != data.Length; ++i) { + const auto direction = data.Path[data.Length - 1 - i]; + res.x += Heading2X[direction]; + res.y += Heading2Y[direction]; + } + return res; +} +} // namespace + +TEST_CASE("PathFinding on clear map 128x128") +{ + CPlayer player; + player.Index = 0; + CUnitType type; + type.TileWidth = 2; + type.TileHeight = 2; + type.BoolFlag.resize(UnitTypeVar.GetNumberBoolFlag()); // SOLID_INDEX + CUnit unit; + unit.Player = &player; + unit.Type = &type; + unit.tilePos = {42, 1}; + + Map.Info.MapWidth = 128; + Map.Info.MapHeight = 128; + + Map.Create(); + + extern void InitAStar(int mapWidth, int mapHeight); + InitAStar(Map.Info.MapWidth, Map.Info.MapHeight); + + SUBCASE("Already reached") + { + const auto dest = unit.tilePos; + unit.Orders.push_back(COrder::NewActionMove(dest)); + + Vec2i dir; + const auto d = NextPathElement(unit, &dir); + + CHECK(d == PF_REACHED); + CHECK(unit.pathFinderData->output.Length == 0); + CHECK(dir == Vec2i(0, 0)); + CHECK(dest == FollowedPath(unit.tilePos + dir, unit.pathFinderData->output)); + + unit.Orders.clear(); + } + + SUBCASE("short path (10)") + { + const short dist = 10; + REQUIRE(dist < std::size(unit.pathFinderData->output.Path)); + const auto dest = unit.tilePos + Vec2i{0, dist}; + unit.Orders.push_back(COrder::NewActionMove(dest)); + + Vec2i dir; + const auto d = NextPathElement(unit, &dir); + + CHECK(d == dist); + + CHECK(unit.pathFinderData->output.Length == dist - 1); + CHECK(dest == FollowedPath(unit.tilePos + dir, unit.pathFinderData->output)); + + unit.Orders.clear(); + } + + SUBCASE("long path (30)") + { + const short dist = 30; + REQUIRE(30 > std::size(unit.pathFinderData->output.Path)); + const auto dest = unit.tilePos + Vec2i{0, dist}; + unit.Orders.push_back(COrder::NewActionMove(dest)); + + Vec2i dir; + const auto d = NextPathElement(unit, &dir); + + CHECK(0 < d); + CHECK(d <= std::size(unit.pathFinderData->output.Path)); + CHECK(unit.pathFinderData->output.Length + unit.pathFinderData->output.OverflowLength == dist - 1); + + CHECK(unit.pathFinderData->output.Length == d - 1); + CHECK(unit.tilePos + Vec2i(0, d) == FollowedPath(unit.tilePos + dir, unit.pathFinderData->output)); + + unit.Orders.clear(); + } + + Map.Fields.clear(); + + extern void FreeAStar(); // free the a* data structures + FreeAStar(); +}