Index: source/simulation2/components/CCmpPathfinder.cpp =================================================================== --- source/simulation2/components/CCmpPathfinder.cpp +++ source/simulation2/components/CCmpPathfinder.cpp @@ -670,6 +670,35 @@ } } +bool CCmpPathfinder::MakeGoalReachable(entity_pos_t x0, entity_pos_t z0, PathGoal &goal, pass_class_t passClass) +{ + u16 i0, j0; + Pathfinding::NearestNavcell(x0, z0, i0, j0, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + if (!IS_PASSABLE(m_Grid->get(i0, j0), passClass)) + FindNearestPassableNavcell(i0, j0, passClass); + + return m_LongPathfinder.MakeGoalReachable(i0, j0, goal, passClass); +} + +u32 CCmpPathfinder::FindNearestPassableNavcell(entity_pos_t x, entity_pos_t z, u16& outI, u16& outJ, pass_class_t passClass) +{ + Pathfinding::NearestNavcell(x, z, outI, outJ, m_MapSize*Pathfinding::NAVCELLS_PER_TILE, m_MapSize*Pathfinding::NAVCELLS_PER_TILE); + u16 i0 = outI; + u16 j0 = outJ; + FindNearestPassableNavcell(outI, outJ, passClass); + return abs(i0 - outI) + abs(j0 - outJ); +} + +void CCmpPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) +{ + m_LongPathfinder.FindNearestPassableNavcell(i, j, passClass); +} + +bool CCmpPathfinder::NavcellIsReachable(u16 i0, u16 j0, u16 i1, u16 j1, pass_class_t passClass) +{ + return m_LongPathfinder.NavcellIsReachable(i0, j0, i1, j1, passClass); +} + ////////////////////////////////////////////////////////// // Async path requests: Index: source/simulation2/components/CCmpPathfinder_Common.h =================================================================== --- source/simulation2/components/CCmpPathfinder_Common.h +++ source/simulation2/components/CCmpPathfinder_Common.h @@ -245,6 +245,13 @@ virtual Grid ComputeShoreGrid(bool expandOnWater = false); + virtual bool MakeGoalReachable(entity_pos_t x0, entity_pos_t z0, PathGoal &goal, pass_class_t passClass); + + virtual u32 FindNearestPassableNavcell(entity_pos_t x, entity_pos_t z, u16& outI, u16& outJ, pass_class_t passClass); + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass); + + virtual bool NavcellIsReachable(u16 i0, u16 j0, u16 i1, u16 j1, pass_class_t passClass); + virtual void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& goal, pass_class_t passClass, WaypointPath& ret) { m_LongPathfinder.ComputePath(x0, z0, goal, passClass, ret); Index: source/simulation2/components/ICmpPathfinder.h =================================================================== --- source/simulation2/components/ICmpPathfinder.h +++ source/simulation2/components/ICmpPathfinder.h @@ -88,6 +88,24 @@ */ virtual Grid ComputeShoreGrid(bool expandOnWater = false) = 0; + /** + * Transform an arbitrary PathGoal into a reachable Point PathGoal, see Hierarchical Pathfinder for details + * Return true if the goal was reachable originally, false otherwise. + */ + virtual bool MakeGoalReachable(entity_pos_t x0, entity_pos_t z0, PathGoal &goal, pass_class_t passClass) = 0; + + /** + * Gives the closest passable navcell from the given position. + * Returns how many navcells away (manhattan) that navcell is. + */ + virtual u32 FindNearestPassableNavcell(entity_pos_t x, entity_pos_t z, u16& outI, u16& outJ, pass_class_t passClass) = 0; + + /** + * Returns true if navcell (i0, j0) has the same global region ID as navcell (i1, j1). + * i.e. you can reach one from the other. + */ + virtual bool NavcellIsReachable(u16 i0, u16 j0, u16 i1, u16 j1, pass_class_t passClass) = 0; + /** * Compute a tile-based path from the given point to the goal, and return the set of waypoints. * The waypoints correspond to the centers of horizontally/vertically adjacent tiles Index: source/simulation2/components/tests/test_HierPathfinder.h =================================================================== --- /dev/null +++ source/simulation2/components/tests/test_HierPathfinder.h @@ -0,0 +1,477 @@ +/* Copyright (C) 2019 Wildfire Games. + * This file is part of 0 A.D. + * + * 0 A.D. 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, either version 2 of the License, or + * (at your option) any later version. + * + * 0 A.D. 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 0 A.D. If not, see . + */ + +#include "simulation2/system/ComponentTest.h" + +#define TEST + +#include "maths/Vector2D.h" +#include "simulation2/helpers/HierarchicalPathfinder.h" + +class TestHierarchicalPathfinder : public CxxTest::TestSuite +{ +public: + void setUp() + { + } + + void tearDown() + { + } + + const pass_class_t PASS_1 = 1; + const pass_class_t PASS_2 = 2; + const pass_class_t NON_PASS_1 = 4; + + const u16 mapSize = 240; + + std::map pathClassMask; + std::map nonPathClassMask; + + void debug_grid(Grid& grid) { + for (size_t i = 0; i < grid.m_W; ++i) + { + for (size_t j = 0; j < grid.m_H; ++j) + printf("%i", grid.get(i,j)); + printf("\n"); + } + } + + void debug_grid_points(Grid& grid, u16 i1, u16 j1, u16 i2, u16 j2) { + for (size_t i = 0; i < grid.m_W; ++i) + { + for (size_t j = 0; j < grid.m_H; ++j) + { + if (i == i1 && j == j1) + printf("A"); + else if (i == i2 && j == j2) + printf("B"); + else + printf("%i", grid.get(i,j)); + } + printf("\n"); + } + } + + void assert_blank(HierarchicalPathfinder& hierPath) + { + // test that the map has the same global region everywhere + auto globalRegionID = hierPath.GetGlobalRegion(35, 23, PASS_1); + for (size_t i = 0; i < mapSize; ++i) + for (size_t j = 0; j < mapSize; ++j) + { + TS_ASSERT(globalRegionID == hierPath.GetGlobalRegion(i, j, PASS_1)); + TS_ASSERT(hierPath.GetGlobalRegion(i, j, PASS_2) == 0); + } + + u16 i = 89; + u16 j = 34; + hierPath.FindNearestPassableNavcell(i, j, PASS_1); + TS_ASSERT(i == 89 && j == 34); + + for (auto& chunk : hierPath.m_Chunks[PASS_1]) + TS_ASSERT(chunk.m_RegionsID.size() == 1); + + // number of connected regions: 4 in the middle, 2 in the corners. + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(120, 120, PASS_1)].size() == 4); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(20, 20, PASS_1)].size() == 2); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(220, 220, PASS_1)].size() == 2); + + std::set reachables; + hierPath.FindReachableRegions(hierPath.Get(120, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 9); + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(20, 20, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 9); + } + + void test_reachability_and_update() + { + pathClassMask = { + { "1", 1 }, + { "2", 2 }, + }; + nonPathClassMask = { + { "3", 4 } + }; + + HierarchicalPathfinder hierPath; + Grid grid(mapSize, mapSize); + Grid dirtyGrid(mapSize, mapSize); + + // Entirely passable for PASS_1, not for others; + for (size_t i = 0; i < mapSize; ++i) + for (size_t j = 0; j < mapSize; ++j) + grid.set(i, j, 6); + + hierPath.Recompute(&grid, nonPathClassMask, pathClassMask); + + assert_blank(hierPath); + + ////////////////////////////////////////////////////// + // Split the map in two in the middle. + for (u16 j = 0; j < mapSize; ++j) + { + grid.set(125, j, 7); + dirtyGrid.set(125, j, 1); + } + + hierPath.Update(&grid, dirtyGrid); + + // Global region: check we are now split in two. + TS_ASSERT(hierPath.GetGlobalRegion(50, 50, PASS_1) != hierPath.GetGlobalRegion(150, 50, PASS_1)); + for (size_t j = 0; j < mapSize; ++j) + { + TS_ASSERT(hierPath.Get(125, j, PASS_1).r == 0); + TS_ASSERT(hierPath.GetGlobalRegion(125, j, PASS_1) == 0); + } + for (size_t i = 0; i < 125; ++i) + for (size_t j = 0; j < mapSize; ++j) + { + TS_ASSERT(hierPath.GetGlobalRegion(50, 50, PASS_1) == hierPath.GetGlobalRegion(i, j, PASS_1)); + TS_ASSERT(hierPath.GetGlobalRegion(i, j, PASS_2) == 0); + } + for (size_t i = 126; i < mapSize; ++i) + for (size_t j = 0; j < mapSize; ++j) + { + TS_ASSERT(hierPath.GetGlobalRegion(150, 50, PASS_1) == hierPath.GetGlobalRegion(i, j, PASS_1)); + TS_ASSERT(hierPath.GetGlobalRegion(i, j, PASS_2) == 0); + } + + // number of connected regions: 3 in the middle (both sides), 2 in the corners. + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(120, 120, PASS_1)].size() == 3); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(170, 120, PASS_1)].size() == 3); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(20, 20, PASS_1)].size() == 2); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(220, 220, PASS_1)].size() == 2); + + std::set reachables; + hierPath.FindReachableRegions(hierPath.Get(120, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 6); + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(170, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 6); + + ////////////////////////////////////////////////////// + // Un-split the map in two in the middle. + for (u16 j = 0; j < mapSize; ++j) + { + grid.set(125, j, 6); + dirtyGrid.set(125, j, 1); + } + hierPath.Update(&grid, dirtyGrid); + assert_blank(hierPath); + + ////////////////////////////////////////////////////// + // Partial split in the middle chunk - no actual connectivity change + for (u16 j = 120; j < 150; ++j) + { + grid.set(125, j, 7); + dirtyGrid.set(125, j, 1); + } + hierPath.Update(&grid, dirtyGrid); + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(170, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 9); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(170, 120, PASS_1)].size() == 4); + + ////////////////////////////////////////////////////// + // Block a strip along the edge, but regions are still connected. + for (u16 j = 70; j < 200; ++j) + { + grid.set(96, j, 7); + dirtyGrid.set(96, j, 1); + } + hierPath.Update(&grid, dirtyGrid); + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(170, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 9); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(20, 120, PASS_1)].size() == 2); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(170, 120, PASS_1)].size() == 3); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(200, 120, PASS_1)].size() == 3); + + ////////////////////////////////////////////////////// + // Block the other edge + for (u16 j = 70; j < 200; ++j) + { + grid.set(192, j, 7); + dirtyGrid.set(192, j, 1); + } + hierPath.Update(&grid, dirtyGrid); + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(170, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 9); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(20, 120, PASS_1)].size() == 2); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(170, 120, PASS_1)].size() == 2); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(200, 120, PASS_1)].size() == 2); + + ////////////////////////////////////////////////////// + // Create an isolated region in the middle chunk + for (u16 i = 96; i < 140; ++i) + { + grid.set(i, 110, 7); + dirtyGrid.set(i, 110, 1); + } + for (u16 i = 96; i < 140; ++i) + { + grid.set(i, 140, 7); + dirtyGrid.set(i, 140, 1); + } + for (u16 j = 110; j < 141; ++j) + { + grid.set(140, j, 7); + dirtyGrid.set(140, j, 1); + } + hierPath.Update(&grid, dirtyGrid); + + TS_ASSERT(hierPath.GetGlobalRegion(120, 120, PASS_1) != hierPath.GetGlobalRegion(150, 50, PASS_1)); + + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(170, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 9); + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(120, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 1); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(120, 120, PASS_1)].size() == 0); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(20, 120, PASS_1)].size() == 2); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(170, 120, PASS_1)].size() == 2); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(200, 120, PASS_1)].size() == 2); + + ////////////////////////////////////////////////////// + // Open it + for (u16 j = 110; j < 141; ++j) + { + grid.set(140, j, 6); + dirtyGrid.set(140, j, 1); + } + hierPath.Update(&grid, dirtyGrid); + + TS_ASSERT(hierPath.GetGlobalRegion(120, 120, PASS_1) == hierPath.GetGlobalRegion(150, 50, PASS_1)); + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(170, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 9); + reachables.clear(); + hierPath.FindReachableRegions(hierPath.Get(120, 120, PASS_1), reachables, PASS_1); + TS_ASSERT(reachables.size() == 9); + TS_ASSERT(hierPath.m_Edges[PASS_1][hierPath.Get(120, 120, PASS_1)].size() == 2); + } + + u16 manhattan(u16 i, u16 j, u16 gi, u16 gj) { + return abs(i - gi) + abs(j - gj); + } + + double euclidian(u16 i, u16 j, u16 gi, u16 gj) { + return sqrt((i - gi)*(i - gi) + (j - gj)*(j - gj)); + } + void test_passability() + { + pathClassMask = { + { "1", 1 }, + { "2", 2 }, + }; + nonPathClassMask = { + { "3", 4 } + }; + + // 0 is passable, 1 is not. + // i is vertical, j is horizontal; +#define _ 0 +#define X 1 + NavcellData gridDef[40][40] = { + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,X,X,X,X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,X,X,X,X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,X,X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,X,X,X,X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,X,X,X,X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,X,X,X,X,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,X,_,_,X,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,X,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,X,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,X,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,X,X,X,_,_,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,X,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,X,X,X,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,X,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,_,X,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,X,X,X,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}, + {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_} + }; +#undef _ +#undef X + // upscaled 5 times + HierarchicalPathfinder hierPath; + Grid grid(40*5, 40*5); + Grid dirtyGrid(40*5, 40*5); + + for (size_t i = 0; i < 40; ++i) + for (size_t j = 0; j < 40; ++j) + for (size_t ii = 0; ii < 5; ++ii) + for (size_t jj = 0; jj < 5; ++jj) + grid.set(i * 5 + ii, j * 5 + jj, gridDef[i][j]); + + hierPath.Recompute(&grid, nonPathClassMask, pathClassMask); + + u16 i = 5, j = 5; + hierPath.FindNearestPassableNavcell(i, j, PASS_1); + TS_ASSERT(i == 5 && j == 5); + hierPath.FindNearestPassableNavcell(i, j, PASS_1, true); + TS_ASSERT(i == 5 && j == 5); + + // use a macro so the lines reported by tests are accurate +#define check_closest_passable(i, j, expected_manhattan, limited) \ + oi = i; oj = j; \ + pi = i; pj = j; \ + TS_ASSERT(!IS_PASSABLE(grid.get(pi, pj), PASS_1)); \ + hierPath.FindNearestPassableNavcell(pi, pj, PASS_1, limited); \ +\ + if (expected_manhattan == -1) \ + { \ + TS_ASSERT(oi == pi && oj == pj); \ + } else { \ + TS_ASSERT(IS_PASSABLE(grid.get(pi, pj), PASS_1)); \ + TS_ASSERT(manhattan(pi, pj, oi, oj) == expected_manhattan); \ + } + u16 oi, oj, pi, pj; + + check_closest_passable(4 * 5, 4 * 5, 1, false); + check_closest_passable(4 * 5, 4 * 5, 1, true); + + check_closest_passable(4 * 5 + 1, 4 * 5 + 1, 2, false); + check_closest_passable(4 * 5 + 1, 4 * 5 + 1, 2, true); + + check_closest_passable(14 * 5 + 2, 7 * 5 + 2, 8, false); + check_closest_passable(14 * 5 + 2, 7 * 5 + 2, -1, true); + check_closest_passable(14 * 5 + 2, 7 * 5 + 4, 6, false); + check_closest_passable(14 * 5 + 2, 7 * 5 + 4, -1, true); + check_closest_passable(14 * 5 + 2, 7 * 5 + 5, 5, false); + check_closest_passable(14 * 5 + 2, 7 * 5 + 5, 5, true); + check_closest_passable(14 * 5 + 2, 7 * 5 + 6, 4, false); + check_closest_passable(14 * 5 + 2, 7 * 5 + 6, 4, true); + + check_closest_passable(5 * 5 + 3, 7 * 5 + 2, 2, false); + check_closest_passable(5 * 5 + 3, 7 * 5 + 2, 2, true); +#undef check_closest_passable + + PathGoal goal; + goal.type = PathGoal::POINT; + + // from the left of the C, goal is unreachable, expect closest navcell to goal + oi = 5 * 5 + 3; oj = 3 * 5 + 3; + pi = 5 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // random reachable point. + oi = 5 * 5 + 3; oj = 3 * 5 + 3; + pi = 26 * 5 + 3; pj = 5 * 5 + 2; goal.x = fixed::FromInt(pi) + fixed::FromInt(1)/3; goal.z = fixed::FromInt(pj) + fixed::FromInt(1)/3; + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // top-left corner + goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // near bottom-right corner + goal.x = fixed::FromInt(pi) + fixed::FromInt(3)/4; goal.z = fixed::FromInt(pj) + fixed::FromInt(3)/4; + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // Circle + goal.type = PathGoal::CIRCLE; + goal.hw = fixed::FromInt(1) / 2; + + // from the left of the C, goal is unreachable, expect closest navcell to goal + oi = 5 * 5 + 3; oj = 3 * 5 + 3; + pi = 5 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // same position, goal is reachable, expect closest navcell to goal + goal.hw = fixed::FromInt(3); + goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // Square + goal.type = PathGoal::SQUARE; + goal.hw = fixed::FromInt(1) / 2; + goal.hh = fixed::FromInt(1) / 2; + + // from the left of the C, goal is unreachable, expect closest navcell to goal + oi = 5 * 5 + 3; oj = 3 * 5 + 3; + pi = 5 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // same position, goal is reachable, expect closest navcell to goal + goal.hw = fixed::FromInt(3); + goal.hh = fixed::FromInt(3); + goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // Goal is reachable diagonally (1 cell away) + goal.hw = fixed::FromInt(1); + goal.hh = fixed::FromInt(1); + oi = 5 * 5 + 3; oj = 3 * 5 + 3; + pi = 5 * 5 - 1; pj = 7 * 5 + 3; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); + TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity()); + + // Huge Circle goal, expect point closest to us. + goal.type = PathGoal::CIRCLE; + goal.hw = fixed::FromInt(20); + + oi = 5 * 5 + 3; oj = 3 * 5 + 3; + pi = 36 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj); + + hierPath.MakeGoalReachable(oi, oj, goal, PASS_1); + // bit of leeway for cell placement + TS_ASSERT(abs(euclidian(goal.x.ToInt_RoundToNegInfinity(), goal.z.ToInt_RoundToNegInfinity(), pi, pj)-20) < 1.5f); + TS_ASSERT(abs(euclidian(goal.x.ToInt_RoundToNegInfinity(), goal.z.ToInt_RoundToNegInfinity(), oi, oj) - euclidian(pi, pj, oi, oj)) < 22.0f); + } +}; Index: source/simulation2/components/tests/test_Pathfinder.h =================================================================== --- source/simulation2/components/tests/test_Pathfinder.h +++ source/simulation2/components/tests/test_Pathfinder.h @@ -17,8 +17,11 @@ #include "simulation2/system/ComponentTest.h" +#define TEST + #include "simulation2/components/ICmpObstructionManager.h" #include "simulation2/components/ICmpPathfinder.h" +#include "simulation2/components/CCmpPathfinder_Common.h" #include "graphics/MapReader.h" #include "graphics/Terrain.h" @@ -111,6 +114,160 @@ } } + void hierarchical_globalRegions_testmap(std::wstring map) + { + CTerrain terrain; + + CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); + sim2.LoadDefaultScripts(); + sim2.ResetState(); + + CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself + + LDR_BeginRegistering(); + mapReader->LoadMap(map, + sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, + &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, + &sim2, &sim2.GetSimContext(), -1, false); + LDR_EndRegistering(); + TS_ASSERT_OK(LDR_NonprogressiveLoad()); + + sim2.PreInitGame(); + sim2.InitGame(); + sim2.Update(0); + + CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); + + pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default"); + HierarchicalPathfinder& hier = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.GetHierarchicalPathfinder(); + + std::map globalRegions = hier.m_GlobalRegions[obstructionsMask]; + + for (u8 cj = 0; cj < hier.m_ChunksH; cj += 2) + for (u8 ci = 0; ci < hier.m_ChunksW; ci += 2) + for(u16 i : hier.GetChunk(ci, cj, obstructionsMask).m_RegionsID) + { + // Assert that all reachable regions are indeed the same region ID + // This does not highlight that unreachable regions might (wrongly) have the same ID). + std::set reachables; + hier.FindReachableRegions(HierarchicalPathfinder::RegionID{ci, cj, i}, reachables, obstructionsMask); + HierarchicalPathfinder::GlobalRegionID ID = globalRegions[HierarchicalPathfinder::RegionID{ci, cj, i}]; + for (HierarchicalPathfinder::RegionID region : reachables) + TS_ASSERT_EQUALS(ID, globalRegions[region]); + } + } + + void test_hierarchical_globalRegions() + { + // This test validates that the hierarchical's pathfinder global regions are in accordance with its regions + // IE it asserts that, for any two regions A and B of the hierarchical pathfinder, if one can find a path from A to B + // then A and B have the same global region. + std::vector maps = { L"maps/scenarios/Peloponnese.pmp", L"maps/skirmishes/Corinthian Isthmus (2).pmp", L"maps/skirmishes/Greek Acropolis (2).pmp" }; + +// disable in debug mode, creating the simulation and running the initial turn is too slow and tends to OOM in debug mode. +#ifndef DEBUG + for (std::wstring t : maps) + hierarchical_globalRegions_testmap(t); +#endif + } + void test_reachability_sanity_test() + { + // Goal of this test: validate that we stay on the same cell. This isn't 100% coverage but if the point goal gets broken, things will be _bad_. + PathGoal tgoal{PathGoal::POINT, fixed::FromInt(2), fixed::FromInt(2), fixed::Zero(), fixed::Zero(), CFixedVector2D(fixed::FromInt(1), fixed::Zero()),CFixedVector2D(fixed::Zero(), fixed::FromInt(1)), fixed::Zero()}; + + PathGoal ogoal = tgoal; + + // End up on the top left edge. + CreatePointGoalAt(5, 6, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(5)); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(6)); + tgoal = ogoal; + // End up as close as possible from (2,2) on the same cell. + CreatePointGoalAt(1, 1, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(2) - fixed::Epsilon()); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2) - fixed::Epsilon()); + tgoal = ogoal; + // End up on the goal. + CreatePointGoalAt(2, 2, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(2)); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2)); + tgoal = ogoal; + tgoal.x = fixed::FromInt(5)/2; + // End up on the goal. + CreatePointGoalAt(2, 2, tgoal); + TS_ASSERT_EQUALS(tgoal.x, fixed::FromInt(5)/2); + TS_ASSERT_EQUALS(tgoal.z, fixed::FromInt(2)); + } + + void hierarchical_update_testmap(std::wstring map) + { + CTerrain terrain; + + CSimulation2 sim2(NULL, g_ScriptRuntime, &terrain); + sim2.LoadDefaultScripts(); + sim2.ResetState(); + + CMapReader* mapReader = new CMapReader(); // it'll call "delete this" itself + + LDR_BeginRegistering(); + mapReader->LoadMap(map, + sim2.GetScriptInterface().GetJSRuntime(), JS::UndefinedHandleValue, + &terrain, NULL, NULL, NULL, NULL, NULL, NULL, NULL, + &sim2, &sim2.GetSimContext(), -1, false); + LDR_EndRegistering(); + TS_ASSERT_OK(LDR_NonprogressiveLoad()); + + sim2.PreInitGame(); + sim2.InitGame(); + sim2.Update(0); + + CmpPtr cmpPathfinder(sim2, SYSTEM_ENTITY); + + pass_class_t obstructionsMask = cmpPathfinder->GetPassabilityClass("default"); + HierarchicalPathfinder& hier = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.GetHierarchicalPathfinder(); + + // make copies + const auto pristine_GR = hier.m_GlobalRegions; + const auto pristine_Chunks = hier.m_Chunks; + const HierarchicalPathfinder::EdgesMap pristine_Edges = hier.m_Edges.at(obstructionsMask); + + Grid* pathfinderGrid = ((CCmpPathfinder*)cmpPathfinder.operator->())->m_LongPathfinder.m_Grid; + + Grid dirtyGrid(hier.m_ChunksW * HierarchicalPathfinder::CHUNK_SIZE,hier.m_ChunksH * HierarchicalPathfinder::CHUNK_SIZE); + srand(1234); + + size_t tries = 20; + for (size_t i = 0; i < tries; ++i) + { + // Dirty a random one + dirtyGrid.reset(); + u8 ci = rand() % (hier.m_ChunksW-10) + 8; + u8 cj = rand() % (hier.m_ChunksH-10) + 8; + dirtyGrid.set(ci * HierarchicalPathfinder::CHUNK_SIZE + 4, cj * HierarchicalPathfinder::CHUNK_SIZE + 4, 1); + + hier.Update(pathfinderGrid, dirtyGrid); + + // Formally speaking we should rather validate that regions exist with the same pixels, but so far + // re-initing regions will keep the same IDs for the same pixels so this is OK. + TS_ASSERT_EQUALS(hier.m_Chunks.at(obstructionsMask), pristine_Chunks.at(obstructionsMask)); + // same here + TS_ASSERT_EQUALS(pristine_Edges, hier.m_Edges.at(obstructionsMask)); + } + } + + void test_hierarchical_update() + { + // This test validates that the "Update" function of the hierarchical pathfinder + // ends up in a correct state (by comparing it with the clean, "Recompute"-d state). + std::vector maps = { L"maps/scenarios/Peloponnese.pmp", L"maps/skirmishes/Corinthian Isthmus (2).pmp", L"maps/skirmishes/Greek Acropolis (2).pmp" }; + +// disable in debug mode, creating the simulation and running the initial turn is too slow and tends to OOM in debug mode. +#ifndef DEBUG + for (std::wstring t : maps) + hierarchical_update_testmap(t); +#endif + } + void test_performance_DISABLED() { CTerrain terrain; @@ -129,6 +286,8 @@ LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); + sim2.PreInitGame(); + sim2.InitGame(); sim2.Update(0); CmpPtr cmp(sim2, SYSTEM_ENTITY); @@ -240,6 +399,8 @@ LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); + sim2.PreInitGame(); + sim2.InitGame(); sim2.Update(0); std::ofstream stream(OsString("perf2.html").c_str(), std::ofstream::out | std::ofstream::trunc); @@ -295,6 +456,8 @@ LDR_EndRegistering(); TS_ASSERT_OK(LDR_NonprogressiveLoad()); + sim2.PreInitGame(); + sim2.InitGame(); sim2.Update(0); std::ofstream stream(OsString("perf3.html").c_str(), std::ofstream::out | std::ofstream::trunc); Index: source/simulation2/helpers/HierarchicalPathfinder.h =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.h +++ source/simulation2/helpers/HierarchicalPathfinder.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2015 Wildfire Games. +/* Copyright (C) 2018 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -27,7 +27,7 @@ /** * Hierarchical pathfinder. * - * It doesn't find shortest paths, but deals with connectivity. + * Deals with connectivity (can point A reach point B?). * * The navcell-grid representation of the map is split into fixed-size chunks. * Within a chunk, each maximal set of adjacently-connected passable navcells @@ -35,18 +35,33 @@ * Each region is a vertex in the hierarchical pathfinder's graph. * When two regions in adjacent chunks are connected by passable navcells, * the graph contains an edge between the corresponding two vertexes. - * (There will never be an edge between two regions in the same chunk.) + * By design, there can never be an edge between two regions in the same chunk. + * + * Those fixed-size chunks are used to efficiently compute "global regions" by effectively flood-filling. + * Those can then be used to immediately determine if two reachables points are connected. + * + * The main use of this class is to convert an arbitrary PathGoal to a reachable navcell. + * This happens in MakeGoalReachable. * - * Since regions are typically fairly large, it is possible to determine - * connectivity between any two navcells by mapping them onto their appropriate - * region and then doing a relatively small graph search. */ +#ifdef TEST +class TestCmpPathfinder; +class TestHierarchicalPathfinder; +void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal); +#endif + class HierarchicalOverlay; class HierarchicalPathfinder { +#ifdef TEST + friend class TestCmpPathfinder; + friend class TestHierarchicalPathfinder; +#endif public: + typedef u32 GlobalRegionID; + struct RegionID { u8 ci, cj; // chunk ID @@ -54,7 +69,7 @@ RegionID(u8 ci, u8 cj, u16 r) : ci(ci), cj(cj), r(r) { } - bool operator<(RegionID b) const + bool operator<(const RegionID& b) const { // Sort by chunk ID, then by per-chunk region ID if (ci < b.ci) @@ -68,7 +83,7 @@ return r < b.r; } - bool operator==(RegionID b) const + bool operator==(const RegionID& b) const { return ((ci == b.ci) && (cj == b.cj) && (r == b.r)); } @@ -86,30 +101,37 @@ void Update(Grid* grid, const Grid& dirtinessGrid); - RegionID Get(u16 i, u16 j, pass_class_t passClass); + RegionID Get(u16 i, u16 j, pass_class_t passClass) const; + GlobalRegionID GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const; /** - * Updates @p goal so that it's guaranteed to be reachable from the navcell + * Updates @p goal to a point goal guaranteed to be reachable from the original navcell * @p i0, @p j0 (which is assumed to be on a passable navcell). * - * If the goal is not reachable, it is replaced with a point goal nearest to - * the goal center. + * If the goal is not reachable, it is replaced with an acceptable point goal + * This function does not necessarily return the closest navcell to the goal + * but the one with the lowest f score of the A* algorithm. + * This means it is usually a tradeoff between walking time and distance to the goal. * * In the case of a non-point reachable goal, it is replaced with a point goal * at the reachable navcell of the goal which is nearest to the starting navcell. + * TODO: since A* is used, it could return the reachable navcell nearest to the penultimate region visited. + * which is probably better (imagine a path that must bend around). + * + * @returns true if the goal was reachable, false otherwise. */ - void MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass); + bool MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const; /** - * Updates @p i, @p j (which is assumed to be an impassable navcell) - * to the nearest passable navcell. + * Updates @p i, @p j to the nearest passable navcell. + * @param limited: only search a few tiles around (i, j) */ - void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass); + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass, bool limited = false) const; /** * Generates the connectivity grid associated with the given pass_class */ - Grid GetConnectivityGrid(pass_class_t passClass); + Grid GetConnectivityGrid(pass_class_t passClass) const; pass_class_t GetPassabilityClass(const std::string& name) const { @@ -123,15 +145,15 @@ private: static const u8 CHUNK_SIZE = 96; // number of navcells per side - // TODO PATHFINDER: figure out best number. Probably 64 < n < 128 + // TODO: figure out best number. Probably 64 < n < 128 struct Chunk { u8 m_ChunkI, m_ChunkJ; // chunk ID - u16 m_NumRegions; // number of local region IDs (starting from 1) + std::vector m_RegionsID; // IDs of local region, without 0 u16 m_Regions[CHUNK_SIZE][CHUNK_SIZE]; // local region ID per navcell - cassert(CHUNK_SIZE*CHUNK_SIZE/2 < 65536); // otherwise we could overflow m_NumRegions with a checkerboard pattern + cassert(CHUNK_SIZE*CHUNK_SIZE/2 < 65536); // otherwise we could overflow m_RegionsID with a checkerboard pattern void InitRegions(int ci, int cj, Grid* grid, pass_class_t passClass); @@ -139,39 +161,55 @@ void RegionCenter(u16 r, int& i, int& j) const; + void NearestNavcell(int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const; + void RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const; bool RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const; + +#ifdef TEST + bool operator==(const Chunk& b) const + { + return (m_ChunkI == b.m_ChunkI && m_ChunkJ == b.m_ChunkJ && m_RegionsID == b.m_RegionsID && memcmp(&m_Regions, &b.m_Regions, sizeof(u16) * CHUNK_SIZE * CHUNK_SIZE) == 0); + } +#endif }; typedef std::map > EdgesMap; - void FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges); + void RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges); + void UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges); - void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass); + void UpdateGlobalRegions(const std::map>& needNewGlobalRegionMap); - void FindPassableRegions(std::set& regions, pass_class_t passClass); + template + void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const; + + void FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const; /** * Updates @p iGoal and @p jGoal to the navcell that is the nearest to the * initial goal coordinates, in one of the given @p regions. * (Assumes @p regions is non-empty.) */ - void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass); + void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const; - Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) + const Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) const { - return m_Chunks[passClass].at(cj * m_ChunksW + ci); + return m_Chunks.at(passClass).at(cj * m_ChunksW + ci); } - void FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid); + void FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const; u16 m_W, m_H; - u16 m_ChunksW, m_ChunksH; + u8 m_ChunksW, m_ChunksH; std::map > m_Chunks; std::map m_Edges; + std::map> m_GlobalRegions; + GlobalRegionID m_NextGlobalRegionID; + // Passability classes for which grids will be updated when calling Update std::map m_PassClassMasks; Index: source/simulation2/helpers/HierarchicalPathfinder.cpp =================================================================== --- source/simulation2/helpers/HierarchicalPathfinder.cpp +++ source/simulation2/helpers/HierarchicalPathfinder.cpp @@ -1,4 +1,4 @@ -/* Copyright (C) 2016 Wildfire Games. +/* Copyright (C) 2018 Wildfire Games. * This file is part of 0 A.D. * * 0 A.D. is free software: you can redistribute it and/or modify @@ -22,6 +22,8 @@ #include "graphics/Overlay.h" #include "ps/Profile.h" +#include "Geometry.h" + // Find the root ID of a region, used by InitRegions inline u16 RootID(u16 x, const std::vector& v) { @@ -39,6 +41,8 @@ memset(m_Regions, 0, sizeof(m_Regions)); + m_RegionsID.clear(); + int i0 = ci * CHUNK_SIZE; int j0 = cj * CHUNK_SIZE; int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W); @@ -110,14 +114,13 @@ } } - // Directly point the root ID - m_NumRegions = 0; + // Mark connected regions as being the same ID (i.e. the lowest) for (u16 i = 1; i < regionID+1; ++i) { - if (connect[i] == i) - ++m_NumRegions; - else + if (connect[i] != i) connect[i] = RootID(i, connect); + else + m_RegionsID.push_back(connect[i]); } // Replace IDs by the root ID @@ -171,6 +174,53 @@ j_out = m_ChunkJ * CHUNK_SIZE + sj / n; } +/** + * Returns the global navcell coords, and the squared distance to the goal + * navcell, of whichever navcell inside the given chunk is closest to + * that goal. + */ +void HierarchicalPathfinder::Chunk::NearestNavcell(int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const +{ + iBest = 0; + jBest = 0; + dist2Best = std::numeric_limits::max(); + u32 rowBest = std::numeric_limits::max(); + + // TODO: it might be faster to try the best navcell first, since we can compute that. + for (int j = 0; j < CHUNK_SIZE; ++j) + { + for (int i = 0; i < CHUNK_SIZE; ++i) + { + if (m_Regions[j][i] == 0) + continue; + + /** + * Line-point distance. Over a single row, dist2 is first monotonically decreasing, then monotonically increasing + * Thus once we stop improving, we can safely break. + * However, we cannot compare across lines because obstructed chunks may skew numbers. + * Imagine a situation like this (G is the goal, X a passable cell, - an impassable one): + * ----G- + * -X---- + * X---X- + * Despite the first X on the third row being farther, the third row has a better navcell. + */ + u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal) + + (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal); + if (dist2 < dist2Best) + { + iBest = i + m_ChunkI*CHUNK_SIZE; + jBest = j + m_ChunkJ*CHUNK_SIZE; + dist2Best = dist2; + rowBest = dist2; + } + else if (dist2 < rowBest) + rowBest = dist2; + else // dist2 >= rowBest: we can carry on on the next line. + continue; + } + } +} + /** * Returns the global navcell coords, and the squared distance to the goal * navcell, of whichever navcell inside the given region is closest to @@ -191,7 +241,6 @@ u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal) + (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal); - if (dist2 < dist2Best) { iBest = i + m_ChunkI*CHUNK_SIZE; @@ -361,15 +410,18 @@ m_W = grid->m_W; m_H = grid->m_H; + ENSURE((grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE < 256 && (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE < 256); // else the u8 Chunk::m_ChunkI will overflow + // Divide grid into chunks with round-to-positive-infinity m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE; m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE; - ENSURE(m_ChunksW < 256 && m_ChunksH < 256); // else the u8 Chunk::m_ChunkI will overflow - m_Chunks.clear(); m_Edges.clear(); + // reset global regions + m_NextGlobalRegionID = 1; + for (auto& passClassMask : allPassClasses) { pass_class_t passClass = passClassMask.second; @@ -385,16 +437,32 @@ } // Construct the search graph over the regions - EdgesMap& edges = m_Edges[passClass]; - - for (int cj = 0; cj < m_ChunksH; ++cj) - { - for (int ci = 0; ci < m_ChunksW; ++ci) - { - FindEdges(ci, cj, passClass, edges); - } - } + RecomputeAllEdges(passClass, edges); + + // Spread global regions. + std::map& globalRegion = m_GlobalRegions[passClass]; + globalRegion.clear(); + for (u8 cj = 0; cj < m_ChunksH; ++cj) + for (u8 ci = 0; ci < m_ChunksW; ++ci) + for (u16 rid : GetChunk(ci, cj, passClass).m_RegionsID) + { + RegionID reg{ci,cj,rid}; + if (globalRegion.find(reg) == globalRegion.end()) + { + GlobalRegionID ID = m_NextGlobalRegionID++; + + globalRegion.insert({ reg, ID }); + // avoid creating an empty link if possible, FindReachableRegions uses [] which calls the default constructor + if (edges.find(reg) != edges.end()) + { + std::set reachable; + FindReachableRegions(reg, reachable, passClass); + for (const RegionID& region : reachable) + globalRegion.insert({ region, ID }); + } + } + } } if (m_DebugOverlay) @@ -409,11 +477,27 @@ { PROFILE3("Hierarchical Update"); - for (int cj = 0; cj < m_ChunksH; ++cj) + ASSERT(m_NextGlobalRegionID < std::numeric_limits::max()); + + std::map> needNewGlobalRegionMap; + + // Algorithm for the partial update: + // 1. Loop over chunks. + // 2. For any dirty chunk: + // - remove all regions from the global region map + // - remove all edges, by removing the neighbor connection with them and then deleting us + // - recreate regions inside the chunk + // - reconnect the regions. We may do too much work if we reconnect with a dirty chunk, but that's fine. + // 3. Recreate global regions. + // This means that if any chunk changes, we may need to flood (at most once) the whole map. + // That's quite annoying, but I can't think of an easy way around it. + // If we could be sure that a region's topology hasn't changed, we could skip removing its global region + // but that's non trivial as we have no easy way to determine said topology (regions could "switch" IDs on update for now). + for (u8 cj = 0; cj < m_ChunksH; ++cj) { int j0 = cj * CHUNK_SIZE; int j1 = std::min(j0 + CHUNK_SIZE, (int)dirtinessGrid.m_H); - for (int ci = 0; ci < m_ChunksW; ++ci) + for (u8 ci = 0; ci < m_ChunksW; ++ci) { // Skip chunks where no navcells are dirty. int i0 = ci * CHUNK_SIZE; @@ -425,27 +509,35 @@ { pass_class_t passClass = passClassMask.second; Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW); + + // Clean up edges and global region ID + EdgesMap& edgeMap = m_Edges[passClass]; + for (u16 i : a.m_RegionsID) + { + RegionID reg{ci, cj, i}; + m_GlobalRegions[passClass].erase(reg); + for (const RegionID& neighbor : edgeMap[reg]) + { + edgeMap[neighbor].erase(reg); + if (edgeMap[neighbor].empty()) + edgeMap.erase(neighbor); + } + edgeMap.erase(reg); + } + + // recompute local regions (not global regions) a.InitRegions(ci, cj, grid, passClass); - } - } - } - // TODO: Also be clever with edges - m_Edges.clear(); - for (const std::pair& passClassMask : m_PassClassMasks) - { - pass_class_t passClass = passClassMask.second; - EdgesMap& edges = m_Edges[passClass]; + for (u16 i : a.m_RegionsID) + needNewGlobalRegionMap[passClass].push_back(RegionID{ci, cj, i}); - for (int cj = 0; cj < m_ChunksH; ++cj) - { - for (int ci = 0; ci < m_ChunksW; ++ci) - { - FindEdges(ci, cj, passClass, edges); + UpdateEdges(ci, cj, passClass, edgeMap); } } } + UpdateGlobalRegions(needNewGlobalRegionMap); + if (m_DebugOverlay) { PROFILE("debug overlay"); @@ -455,22 +547,15 @@ } /** - * Find edges between regions in this chunk and the adjacent below/left chunks. + * Connect a chunk's regions to their neighbors. Not optimised for global recomputing. + * TODO: reduce code duplication with below */ -void HierarchicalPathfinder::FindEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) +void HierarchicalPathfinder::UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges) { std::vector& chunks = m_Chunks[passClass]; Chunk& a = chunks.at(cj*m_ChunksW + ci); - // For each edge between chunks, we loop over every adjacent pair of - // navcells in the two chunks. If they are both in valid regions - // (i.e. are passable navcells) then add a graph edge between those regions. - // (We don't need to test for duplicates since EdgesMap already uses a - // std::set which will drop duplicate entries.) - // But as set.insert can be quite slow on large collection, and that we usually - // try to insert the same values, we cache the previous one for a fast test. - if (ci > 0) { Chunk& b = chunks.at(cj*m_ChunksW + (ci-1)); @@ -492,6 +577,27 @@ } } + if (ci < m_ChunksW-1) + { + Chunk& b = chunks.at(cj*m_ChunksW + (ci+1)); + RegionID raPrev(0,0,0); + RegionID rbPrev(0,0,0); + for (int j = 0; j < CHUNK_SIZE; ++j) + { + RegionID ra = a.Get(CHUNK_SIZE-1, j); + RegionID rb = b.Get(0, j); + if (ra.r && rb.r) + { + if (ra == raPrev && rb == rbPrev) + continue; + edges[ra].insert(rb); + edges[rb].insert(ra); + raPrev = ra; + rbPrev = rb; + } + } + } + if (cj > 0) { Chunk& b = chunks.at((cj-1)*m_ChunksW + ci); @@ -513,6 +619,94 @@ } } + if (cj < m_ChunksH - 1) + { + Chunk& b = chunks.at((cj+1)*m_ChunksW + ci); + RegionID raPrev(0,0,0); + RegionID rbPrev(0,0,0); + for (int i = 0; i < CHUNK_SIZE; ++i) + { + RegionID ra = a.Get(i, CHUNK_SIZE-1); + RegionID rb = b.Get(i, 0); + if (ra.r && rb.r) + { + if (ra == raPrev && rb == rbPrev) + continue; + edges[ra].insert(rb); + edges[rb].insert(ra); + raPrev = ra; + rbPrev = rb; + } + } + } +} + +/** + * Find edges between regions in all chunks, in an optimised manner (only look at top/left) + */ +void HierarchicalPathfinder::RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges) +{ + std::vector& chunks = m_Chunks[passClass]; + + edges.clear(); + + for (int cj = 0; cj < m_ChunksH; ++cj) + { + for (int ci = 0; ci < m_ChunksW; ++ci) + { + Chunk& a = chunks.at(cj*m_ChunksW + ci); + + // For each edge between chunks, we loop over every adjacent pair of + // navcells in the two chunks. If they are both in valid regions + // (i.e. are passable navcells) then add a graph edge between those regions. + // (We don't need to test for duplicates since EdgesMap already uses a + // std::set which will drop duplicate entries.) + // But as set.insert can be quite slow on large collection, and that we usually + // try to insert the same values, we cache the previous one for a fast test. + + if (ci > 0) + { + Chunk& b = chunks.at(cj*m_ChunksW + (ci-1)); + RegionID raPrev(0,0,0); + RegionID rbPrev(0,0,0); + for (int j = 0; j < CHUNK_SIZE; ++j) + { + RegionID ra = a.Get(0, j); + RegionID rb = b.Get(CHUNK_SIZE-1, j); + if (ra.r && rb.r) + { + if (ra == raPrev && rb == rbPrev) + continue; + edges[ra].insert(rb); + edges[rb].insert(ra); + raPrev = ra; + rbPrev = rb; + } + } + } + + if (cj > 0) + { + Chunk& b = chunks.at((cj-1)*m_ChunksW + ci); + RegionID raPrev(0,0,0); + RegionID rbPrev(0,0,0); + for (int i = 0; i < CHUNK_SIZE; ++i) + { + RegionID ra = a.Get(i, 0); + RegionID rb = b.Get(i, CHUNK_SIZE-1); + if (ra.r && rb.r) + { + if (ra == raPrev && rb == rbPrev) + continue; + edges[ra].insert(rb); + edges[rb].insert(ra); + raPrev = ra; + rbPrev = rb; + } + } + } + } + } } /** @@ -550,97 +744,322 @@ xz.push_back(b.Y.ToFloat()); m_DebugOverlayLines.emplace_back(); - m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0); + m_DebugOverlayLines.back().m_Color = CColor(1.0, 0.0, 0.0, 1.0); SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true); } } } -HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) +void HierarchicalPathfinder::UpdateGlobalRegions(const std::map>& needNewGlobalRegionMap) +{ + // Use FindReachableRegions because we cannot be sure, even if we find a non-dirty chunk nearby, + // that we weren't the only bridge connecting that chunk to the rest of the global region. + for (const std::pair>& regionsInNeed : needNewGlobalRegionMap) + for (const RegionID& reg : regionsInNeed.second) + { + std::map& globalRegions = m_GlobalRegions[regionsInNeed.first]; + // if we have already been given a region, skip us. + if (globalRegions.find(reg) != globalRegions.end()) + continue; + + std::set reachable; + FindReachableRegions(reg, reachable, regionsInNeed.first); + + GlobalRegionID ID = m_NextGlobalRegionID++; + + for (const RegionID& reg : reachable) + globalRegions[reg] = ID; + } +} + +HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) const { int ci = i / CHUNK_SIZE; int cj = j / CHUNK_SIZE; ENSURE(ci < m_ChunksW && cj < m_ChunksH); - return m_Chunks[passClass][cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE); + return m_Chunks.at(passClass)[cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE); +} + +HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const +{ + RegionID region = Get(i, j, passClass); + if (region.r == 0) + return (GlobalRegionID)0; + return m_GlobalRegions.at(passClass).at(region); +} + +void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal) +{ + // recreate the goal as a point-goal. + PathGoal newGoal; + newGoal.type = PathGoal::POINT; + Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z); + + CFixedVector2D relativePos(goal.x - newGoal.x, goal.z - newGoal.z); + CFixedVector2D u(fixed::FromInt(1), fixed::FromInt(0)); + CFixedVector2D v(fixed::FromInt(0), fixed::FromInt(1)); + CFixedVector2D halfsize(fixed::FromInt(1)/2, fixed::FromInt(1)/2); + + CFixedVector2D bottomRight = CFixedVector2D(newGoal.x, newGoal.z) + halfsize; + + // Try to get close to the goal. If the goal center is in the navcell, pick that, else choose an edge. + if (Geometry::PointIsInSquare(relativePos, u, v, halfsize)) + { + newGoal.x = goal.x; + newGoal.z = goal.z; + } + else + { + relativePos = Geometry::NearestPointOnSquare(relativePos, u, v, halfsize); + newGoal.x += relativePos.X; + newGoal.z += relativePos.Y; + } + // We must not change the navcell we're on, and this could (since we could be on the bottom/right edge). + // NavcellContainsGoal returns true only for the left/upper edges, + // so we can just epsilon to the left/up. + if (newGoal.x >= bottomRight.X) + newGoal.x -= fixed::Epsilon(); + if (newGoal.z >= bottomRight.Y) + newGoal.z -= fixed::Epsilon(); + + goal = newGoal; } -void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) +bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const { PROFILE2("MakeGoalReachable"); - RegionID source = Get(i0, j0, passClass); - // Find everywhere that's reachable - std::set reachableRegions; - FindReachableRegions(source, reachableRegions, passClass); + // Make the goal reachable. + // In case the goal is a passable navcell, we'll just compare global regions and return immediately. + // If the goal isn't a passable navcell, we'll try to make it one using the limited FindNearestPassableNavcell + // This optimises for a common case where we want to go to a tree and similar things. + // If the goal is decidedly not passable, we'll revert to a flood-fill. - // Check whether any reachable region contains the goal - // And get the navcell that's the closest to the point + //////////////////////////////// + // determine if we will be able to reach the goal. + u16 gi, gj; + Pathfinding::NearestNavcell(goal.x, goal.z, gi, gj, m_W, m_H); - u16 bestI = 0; - u16 bestJ = 0; - u32 dist2Best = std::numeric_limits::max(); + std::set goalRegions; + FindGoalRegions(gi, gj, goal, goalRegions, passClass); - for (const RegionID& region : reachableRegions) + std::vector targetRegions; + + GlobalRegionID startID = GetGlobalRegion(i0, j0, passClass); + bool reachable = false; + for (const RegionID& r : goalRegions) + if (m_GlobalRegions.at(passClass).at(r) == startID) + { + reachable = true; + targetRegions.push_back(r); + } + //////////////////////////////// + // if the goal is a point-goal and reachable, just return early. + if (reachable && goal.type == PathGoal::POINT) { - // Skip region if its chunk doesn't contain the goal area - entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (region.ci * CHUNK_SIZE); - entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (region.cj * CHUNK_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE; - if (!goal.RectContainsGoal(x0, z0, x1, z1)) - continue; + CreatePointGoalAt(gi, gj, goal); + return reachable; + } - u16 i,j; - u32 dist2; + //////////////////////////////// + // If the goal is unreachable, we'd still like to get at least somewhere on the closest reachable region. + // Use a limited FindNearestPassableNavcell search, for the 5 neighboring navcells (common case: a tree…) + // if that returns a passable, reachable navcell, go there instead. + if (!reachable) + { + u16 ngi = gi, ngj = gj; + FindNearestPassableNavcell(ngi, ngj, passClass, true); - // If the region contains the goal area, the goal is reachable - // Remember the best point for optimization. - if (GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, i, j, dist2)) + if (GetGlobalRegion(ngi, ngj, passClass) == startID) { - // If it's a point, no need to move it, we're done - if (goal.type == PathGoal::POINT) - return; - if (dist2 < dist2Best) + gi = ngi; + gj = ngj; + CreatePointGoalAt(gi, gj, goal); + return reachable; + } + } + + //////////////////////////////// + // Loop through regions and get the best navcell. + u16 bestI = 0, bestJ = 0; + u32 bestDist = std::numeric_limits::max(); + if (reachable) + { + // If we have target regions, we can just check them individually, which is reasonably fast. + for (const RegionID& region : targetRegions) + { + u16 ri, rj; + u32 dist; + GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, ri, rj, dist); + if (dist < bestDist) { - bestI = i; - bestJ = j; - dist2Best = dist2; + bestI = ri; + bestJ = rj; + bestDist = dist; } } } - - // If the goal area wasn't reachable, - // find the navcell that's nearest to the goal's center - if (dist2Best == std::numeric_limits::max()) + else { - u16 iGoal, jGoal; - Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H); + // Otherwise we should try all reachable regions. + // To avoid doing too much work, we'll sort them by distance from chunk center + // If we've already found a point from a region and our current chunk center is at least √2*region_width farther + // we can be sure any navcell in this region will not be better, so we can exit. - FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass); + struct RegionComparator + { + RegionComparator() = delete; + RegionComparator(u16 i, u16 j): gi (i), gj(j) {}; - // Construct a new point goal at the nearest reachable navcell - PathGoal newGoal; - newGoal.type = PathGoal::POINT; - Pathfinding::NavcellCenter(iGoal, jGoal, newGoal.x, newGoal.z); - goal = newGoal; - return; + u32 dist(const HierarchicalPathfinder::RegionID& region) const + { + return (region.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) * (region.ci * CHUNK_SIZE + CHUNK_SIZE/2 - gi) + + (region.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj) * (region.cj * CHUNK_SIZE + CHUNK_SIZE/2 - gj); + } + bool operator()(const HierarchicalPathfinder::RegionID& a, const HierarchicalPathfinder::RegionID& b) const + { + if (dist(a) < dist(b)) + return true; + if (dist(a) > dist(b)) + return false; + return a.r < b.r; + } + u16 gi, gj; + }; + + // TODO: if this becomes too slow, perhaps roll-out an optimised insert-sort here. + RegionComparator comparator(gi, gj); + std::set reachableRegions(comparator); + FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass); + + u32 bestChunkCenterDist = std::numeric_limits::max(); + u32 maxDistFromChunkCenter = (fixed::FromInt(3) / 2 * HierarchicalPathfinder::CHUNK_SIZE).ToInt_RoundToInfinity(); + ENSURE(maxDistFromChunkCenter < std::numeric_limits::max()); + maxDistFromChunkCenter *= maxDistFromChunkCenter; + for (const RegionID& region : reachableRegions) + { + u32 chunkDist = comparator.dist(region); + if (bestDist < std::numeric_limits::max() && (ssize_t)chunkDist > (ssize_t)bestChunkCenterDist + maxDistFromChunkCenter) + break; // break instead of continue as the set is ordered in increased distance. + + int ri, rj; + u32 dist; + GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, gi, gj, ri, rj, dist); + if (dist < bestDist) + { + bestI = ri; + bestJ = rj; + bestDist = dist; + bestChunkCenterDist = chunkDist; + } + } } + CreatePointGoalAt(bestI, bestJ, goal); - ENSURE(dist2Best != std::numeric_limits::max()); - PathGoal newGoal; - newGoal.type = PathGoal::POINT; - Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z); - goal = newGoal; + return reachable; +} + +template +void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const +{ + // Flood-fill the region graph, starting at 'from', + // collecting all the regions that are reachable via edges + reachable.insert(from); + + const EdgesMap& edgeMap = m_Edges.at(passClass); + if (edgeMap.find(from) == edgeMap.end()) + return; + + std::vector open; + open.reserve(64); + open.push_back(from); + + while (!open.empty()) + { + RegionID curr = open.back(); + open.pop_back(); + for (const RegionID& region : edgeMap.at(curr)) + // Add to the reachable set; if this is the first time we added + // it then also add it to the open list + if (reachable.insert(region).second) + open.push_back(region); + } } -void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) +void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass, bool limited) const { - std::set regions; - FindPassableRegions(regions, passClass); - FindNearestNavcellInRegions(regions, i, j, passClass); + // If our current region is not 0, then this is passable, return immediately. + if (Get(i, j, passClass).r != 0) + return; + + u32 dist2Best = std::numeric_limits::max(); + + u16 oi = i, oj = j; + // in most cases we're < 5 cells away so go for this first + for (u16 tj = std::max(0, oj-5); tj <= std::min(m_H-1, oj+5); ++tj) + for (u16 ti = std::max(0, oi-5); ti <= std::min(m_W-1, oi+5); ++ti) + if (Get(ti, tj, passClass).r != 0) + { + u32 dist = (ti-oi)*(ti-oi) + (tj-oj)*(tj-oj); + if (dist < dist2Best) + { + i = ti; + j = tj; + dist2Best = dist; + } + } + if (dist2Best < std::numeric_limits::max() || limited) + return; + + // We are on an impassable area, so we cannot rely on our accessibility. + // This function does not necessarily need to return the absolute closest navcell, + // since standing on impassable terrain is already a little wonky + // We'll expand in a cross-wise way, returning the best passable cell after each expansion (if any) + // this will ensure that we return an acceptable close navcell in general. + // since the first time we could be close to an edge, we'll expand at least once. + + u16 iBest = i; + u16 jBest = j; + dist2Best = std::numeric_limits::max(); + + u16 si = i/CHUNK_SIZE, sj = j/CHUNK_SIZE; + std::set> visited = { { si, sj } }; + std::vector> openList = { { si, sj } }, openListTemp; + static const int expander[4][2] = { { 1, 0 },{ -1, 0 },{ 0, 1 },{ 0, -1 } }; + for (size_t step = 0;;++step) + { + openListTemp.clear(); + for (std::pair chunk : openList) + { + // go through all regions of a chunk + int tempi, tempj; + u32 dist2; + GetChunk(chunk.first, chunk.second, passClass).NearestNavcell(i, j, tempi, tempj, dist2); + + if (dist2 < dist2Best) + { + iBest = tempi; + jBest = tempj; + dist2Best = dist2; + } + // expand cross + for (const int* dir : expander) + { + u8 x = (u8)std::min(std::max((int)si + dir[0], 0), (int)(m_ChunksW-1)); + u8 z = (u8)std::min(std::max((int)sj + dir[1], 0), (int)(m_ChunksH-1)); + if (visited.insert({ x, z }).second) + openListTemp.push_back({ x, z }); + } + } + if (openListTemp.empty() || (step > 0 && dist2Best < std::numeric_limits::max())) + break; + openList.swap(openListTemp); + } + i = iBest; + j = jBest; } -void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) +void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const { // Find the navcell in the given regions that's nearest to the goal navcell: // * For each region, record the (squared) minimal distance to the goal point @@ -697,47 +1116,46 @@ jGoal = jBest; } -void HierarchicalPathfinder::FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) +void HierarchicalPathfinder::FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const { - // Flood-fill the region graph, starting at 'from', - // collecting all the regions that are reachable via edges - - std::vector open; - open.push_back(from); - reachable.insert(from); - - while (!open.empty()) + if (goal.type == PathGoal::POINT) { - RegionID curr = open.back(); - open.pop_back(); - - for (const RegionID& region : m_Edges[passClass][curr]) - // Add to the reachable set; if this is the first time we added - // it then also add it to the open list - if (reachable.insert(region).second) - open.push_back(region); + RegionID region = Get(gi, gj, passClass); + if (region.r > 0) + regions.insert(region); + return; } -} -void HierarchicalPathfinder::FindPassableRegions(std::set& regions, pass_class_t passClass) -{ - // Construct a set of all regions of all chunks for this pass class - for (const Chunk& chunk : m_Chunks[passClass]) - { - // region 0 is impassable tiles - for (int r = 1; r <= chunk.m_NumRegions; ++r) - regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r)); - } + // For non-point cases, we'll test each region inside the bounds of the goal. + // we might occasionally test a few too many for circles but it's not too bad. + // Note that this also works in the Inverse-circle / Inverse-square case + // Since our ranges are inclusive, we will necessarily test at least the perimeter/outer bound of the goal. + // If we find a navcell, great, if not, well then we'll be surrounded by an impassable barrier. + // Since in the Inverse-XX case we're supposed to start inside, then we can't ever reach the goal so it's good enough. + // It's not worth it to skip the "inner" regions since we'd need ranges above CHUNK_SIZE for that to start mattering + // (and even then not always) and that just doesn't happen for Inverse-XX goals + int size = (std::max(goal.hh, goal.hw) * 3 / 2).ToInt_RoundToInfinity(); + + u16 a,b; u32 c; // unused params for RegionNearestNavcellInGoal + + for (u8 sz = std::max(0,(gj - size) / CHUNK_SIZE); sz <= std::min(m_ChunksH-1, (gj + size + 1) / CHUNK_SIZE); ++sz) + for (u8 sx = std::max(0,(gi - size) / CHUNK_SIZE); sx <= std::min(m_ChunksW-1, (gi + size + 1) / CHUNK_SIZE); ++sx) + { + const Chunk& chunk = GetChunk(sx, sz, passClass); + for (u16 i : chunk.m_RegionsID) + if (chunk.RegionNearestNavcellInGoal(i, 0, 0, goal, a, b, c)) + regions.insert(RegionID{sx, sz, i}); + } } -void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) +void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const { ENSURE(grid.m_W == m_W && grid.m_H == m_H); int i0 = region.ci * CHUNK_SIZE; int j0 = region.cj * CHUNK_SIZE; - const Chunk& c = m_Chunks[passClass][region.cj * m_ChunksW + region.ci]; + const Chunk& c = m_Chunks.at(passClass)[region.cj * m_ChunksW + region.ci]; for (int j = 0; j < CHUNK_SIZE; ++j) for (int i = 0; i < CHUNK_SIZE; ++i) @@ -745,7 +1163,7 @@ grid.set(i0 + i, j0 + j, value); } -Grid HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) +Grid HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) const { Grid connectivityGrid(m_W, m_H); connectivityGrid.reset(); Index: source/simulation2/helpers/LongPathfinder.h =================================================================== --- source/simulation2/helpers/LongPathfinder.h +++ source/simulation2/helpers/LongPathfinder.h @@ -162,6 +162,10 @@ LongPathfinder(); ~LongPathfinder(); +#ifdef TEST + HierarchicalPathfinder& GetHierarchicalPathfinder() { return m_PathfinderHier; } +#endif + void SetDebugOverlay(bool enabled); void SetHierDebugOverlay(bool enabled, const CSimContext *simContext) @@ -237,6 +241,16 @@ void ComputePath(entity_pos_t x0, entity_pos_t z0, const PathGoal& origGoal, pass_class_t passClass, std::vector excludedRegions, WaypointPath& path); + bool MakeGoalReachable(u16 i0, u16 j0, PathGoal &goal, pass_class_t passClass) { return m_PathfinderHier.MakeGoalReachable(i0, j0, goal, passClass); }; + + void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) { return m_PathfinderHier.FindNearestPassableNavcell(i, j, passClass); }; + + bool NavcellIsReachable(u16 i0, u16 j0, u16 i1, u16 j1, pass_class_t passClass) + { + return m_PathfinderHier.GetGlobalRegion(i0, j0, passClass) == m_PathfinderHier.GetGlobalRegion(i1, j1, passClass); + }; + + Grid GetConnectivityGrid(pass_class_t passClass) { return m_PathfinderHier.GetConnectivityGrid(passClass); Index: source/simulation2/helpers/PathGoal.cpp =================================================================== --- source/simulation2/helpers/PathGoal.cpp +++ source/simulation2/helpers/PathGoal.cpp @@ -27,13 +27,13 @@ static bool NavcellContainsCircle(int i, int j, fixed x, fixed z, fixed r, bool inside) { // Accept any navcell (i,j) that contains a point which is inside[/outside] - // (or on the edge of) the circle + // (or on the left/upper edge of) the circle. + // The reason for picking only two edges is that an edge should only belong to one navcell. - // Get world-space bounds of navcell entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE - fixed::Epsilon(); + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE - fixed::Epsilon(); if (inside) { @@ -62,13 +62,13 @@ bool inside) { // Accept any navcell (i,j) that contains a point which is inside[/outside] - // (or on the edge of) the square + // (or on the left/upper edge of) the square. + // The reason for picking only two edges is that an edge should only belong to one navcell. - // Get world-space bounds of navcell entity_pos_t x0 = entity_pos_t::FromInt(i).Multiply(Pathfinding::NAVCELL_SIZE); entity_pos_t z0 = entity_pos_t::FromInt(j).Multiply(Pathfinding::NAVCELL_SIZE); - entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE; - entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE; + entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE - fixed::Epsilon(); + entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE - fixed::Epsilon(); if (inside) {