LCOV - code coverage report
Current view: top level - source/simulation2/helpers - HierarchicalPathfinder.cpp (source / functions) Hit Total Coverage
Test: 0 A.D. test coverage report Lines: 303 430 70.5 %
Date: 2023-01-19 00:18:29 Functions: 21 28 75.0 %

          Line data    Source code
       1             : /* Copyright (C) 2021 Wildfire Games.
       2             :  * This file is part of 0 A.D.
       3             :  *
       4             :  * 0 A.D. is free software: you can redistribute it and/or modify
       5             :  * it under the terms of the GNU General Public License as published by
       6             :  * the Free Software Foundation, either version 2 of the License, or
       7             :  * (at your option) any later version.
       8             :  *
       9             :  * 0 A.D. is distributed in the hope that it will be useful,
      10             :  * but WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      12             :  * GNU General Public License for more details.
      13             :  *
      14             :  * You should have received a copy of the GNU General Public License
      15             :  * along with 0 A.D.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : 
      18             : #include "precompiled.h"
      19             : 
      20             : #include "HierarchicalPathfinder.h"
      21             : 
      22             : #include "graphics/Overlay.h"
      23             : #include "ps/Profile.h"
      24             : #include "renderer/Scene.h"
      25             : 
      26             : #include "simulation2/helpers/Grid.h"
      27             : 
      28             : // Find the root ID of a region, used by InitRegions
      29          86 : inline u16 RootID(u16 x, const std::vector<u16>& v)
      30             : {
      31         112 :     while (v[x] < x)
      32          26 :         x = v[x];
      33             : 
      34          60 :     return x;
      35             : }
      36             : 
      37         117 : void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid<NavcellData>* grid, pass_class_t passClass)
      38             : {
      39         117 :     ENSURE(ci < 256 && cj < 256); // avoid overflows
      40         117 :     m_ChunkI = ci;
      41         117 :     m_ChunkJ = cj;
      42             : 
      43         117 :     memset(m_Regions, 0, sizeof(m_Regions));
      44             : 
      45         117 :     int i0 = ci * CHUNK_SIZE;
      46         117 :     int j0 = cj * CHUNK_SIZE;
      47         117 :     int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W);
      48         117 :     int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H);
      49             : 
      50             :     // Efficiently flood-fill the m_Regions grid
      51             : 
      52         117 :     int regionID = 0;
      53         234 :     std::vector<u16> connect;
      54             : 
      55         117 :     u16* pCurrentID = NULL;
      56         117 :     u16 LeftID = 0;
      57         117 :     u16 DownID = 0;
      58         117 :     bool Checked = false; // prevent some unneccessary RootID calls
      59             : 
      60         117 :     connect.reserve(32); // TODO: What's a sensible number?
      61         117 :     connect.push_back(0); // connect[0] = 0
      62             : 
      63             :     // Start by filling the grid with 0 for blocked,
      64             :     // and regionID for unblocked
      65        9252 :     for (int j = j0; j < j1; ++j)
      66             :     {
      67      746490 :         for (int i = i0; i < i1; ++i)
      68             :         {
      69      737355 :             pCurrentID = &m_Regions[j-j0][i-i0];
      70     1054694 :             if (!IS_PASSABLE(grid->get(i, j), passClass))
      71             :             {
      72      317339 :                 *pCurrentID = 0;
      73      317339 :                 continue;
      74             :             }
      75             : 
      76      420016 :             if (j > j0)
      77      414693 :                 DownID = m_Regions[j-1-j0][i-i0];
      78             : 
      79      420016 :             if (i == i0)
      80        4323 :                 LeftID = 0;
      81             :             else
      82      415693 :                 LeftID = m_Regions[j-j0][i-1-i0];
      83             : 
      84      420016 :             if (LeftID > 0)
      85             :             {
      86      414206 :                 *pCurrentID = LeftID;
      87      414206 :                 if (*pCurrentID != DownID && DownID > 0 && !Checked)
      88             :                 {
      89          20 :                     u16 id0 = RootID(DownID, connect);
      90          20 :                     u16 id1 = RootID(LeftID, connect);
      91          20 :                     Checked = true; // this avoids repeatedly connecting the same IDs
      92             : 
      93          20 :                     if (id0 < id1)
      94          16 :                         connect[id1] = id0;
      95           4 :                     else if (id0 > id1)
      96           4 :                         connect[id0] = id1;
      97             :                 }
      98      414186 :                 else if (DownID == 0)
      99        5658 :                     Checked = false;
     100             :             }
     101        5810 :             else if (DownID > 0)
     102             :             {
     103        5714 :                 *pCurrentID = DownID;
     104        5714 :                 Checked = false;
     105             :             }
     106             :             else
     107             :             {
     108             :                 // New ID
     109          96 :                 *pCurrentID = ++regionID;
     110          96 :                 connect.push_back(regionID);
     111          96 :                 Checked = false;
     112             :             }
     113             :         }
     114             :     }
     115             : 
     116             :     // Mark connected regions as being the same ID (i.e. the lowest)
     117         117 :     m_RegionsID.clear();
     118         213 :     for (u16 i = 1; i < regionID+1; ++i)
     119             :     {
     120          96 :         if (connect[i] != i)
     121          20 :             connect[i] = RootID(i, connect);
     122             :         else
     123          76 :             m_RegionsID.push_back(connect[i]);
     124             :     }
     125             : 
     126             :     // Replace IDs by the root ID
     127       11349 :     for (int j = 0; j < CHUNK_SIZE; ++j)
     128     1089504 :         for (int i = 0; i < CHUNK_SIZE; ++i)
     129     1078272 :             m_Regions[j][i] = connect[m_Regions[j][i]];
     130         117 : }
     131             : 
     132             : /**
     133             :  * Returns a RegionID for the given global navcell coords
     134             :  * (which must be inside this chunk);
     135             :  */
     136      451790 : HierarchicalPathfinder::RegionID HierarchicalPathfinder::Chunk::Get(int i, int j) const
     137             : {
     138      451790 :     ENSURE(i < CHUNK_SIZE && j < CHUNK_SIZE);
     139      451790 :     return RegionID(m_ChunkI, m_ChunkJ, m_Regions[j][i]);
     140             : }
     141             : 
     142             : /**
     143             :  * Return the global navcell coords that correspond roughly to the
     144             :  * center of the given region in this chunk.
     145             :  * (This is not guaranteed to be actually inside the region.)
     146             :  */
     147           0 : void HierarchicalPathfinder::Chunk::RegionCenter(u16 r, int& i_out, int& j_out) const
     148             : {
     149             :     // Find the mean of i,j coords of navcells in this region:
     150             : 
     151           0 :     u32 si = 0, sj = 0; // sum of i,j coords
     152           0 :     u32 n = 0; // number of navcells in region
     153             : 
     154             :     cassert(CHUNK_SIZE < 256); // conservative limit to ensure si and sj don't overflow
     155             : 
     156           0 :     for (int j = 0; j < CHUNK_SIZE; ++j)
     157             :     {
     158           0 :         for (int i = 0; i < CHUNK_SIZE; ++i)
     159             :         {
     160           0 :             if (m_Regions[j][i] == r)
     161             :             {
     162           0 :                 si += i;
     163           0 :                 sj += j;
     164           0 :                 n += 1;
     165             :             }
     166             :         }
     167             :     }
     168             : 
     169             :     // Avoid divide-by-zero
     170           0 :     if (n == 0)
     171           0 :         n = 1;
     172             : 
     173           0 :     i_out = m_ChunkI * CHUNK_SIZE + si / n;
     174           0 :     j_out = m_ChunkJ * CHUNK_SIZE + sj / n;
     175           0 : }
     176             : 
     177             : /**
     178             :  * Returns the global navcell coords, and the squared distance to the goal
     179             :  * navcell, of whichever navcell inside the given region is closest to
     180             :  * that goal.
     181             :  */
     182          93 : void HierarchicalPathfinder::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const
     183             : {
     184          93 :     iBest = 0;
     185          93 :     jBest = 0;
     186          93 :     dist2Best = std::numeric_limits<u32>::max();
     187             : 
     188        9021 :     for (int j = 0; j < CHUNK_SIZE; ++j)
     189             :     {
     190      866016 :         for (int i = 0; i < CHUNK_SIZE; ++i)
     191             :         {
     192      857088 :             if (m_Regions[j][i] != r)
     193      298332 :                 continue;
     194             : 
     195     1117512 :             u32 dist2 = (i + m_ChunkI*CHUNK_SIZE - iGoal)*(i + m_ChunkI*CHUNK_SIZE - iGoal)
     196      558756 :                         + (j + m_ChunkJ*CHUNK_SIZE - jGoal)*(j + m_ChunkJ*CHUNK_SIZE - jGoal);
     197             : 
     198      558756 :             if (dist2 < dist2Best)
     199             :             {
     200        7926 :                 iBest = i + m_ChunkI*CHUNK_SIZE;
     201        7926 :                 jBest = j + m_ChunkJ*CHUNK_SIZE;
     202        7926 :                 dist2Best = dist2;
     203             :             }
     204             :         }
     205             :     }
     206          93 : }
     207             : 
     208             : /**
     209             :  * Gives the global navcell coords, and the squared distance to the (i0,j0)
     210             :  * navcell, of whichever navcell inside the given region and inside the given goal
     211             :  * is closest to (i0,j0)
     212             :  * Returns true if the goal is inside the region, false otherwise.
     213             :  */
     214           4 : bool HierarchicalPathfinder::Chunk::RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const
     215             : {
     216             :     // TODO: this should be optimized further.
     217             :     // Most used cases empirically seem to be SQUARE, INVERTED_CIRCLE and then POINT and CIRCLE somehwat equally
     218           4 :     iOut = 0;
     219           4 :     jOut = 0;
     220           4 :     dist2Best = std::numeric_limits<u32>::max();
     221             : 
     222             :     // Calculate the navcell that contains the center of the goal.
     223           4 :     int gi = (goal.x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
     224           4 :     int gj = (goal.z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
     225             : 
     226           4 :     switch(goal.type)
     227             :     {
     228           0 :     case PathGoal::POINT:
     229             :     {
     230             :         // i and j can be equal to CHUNK_SIZE on the top and right borders of the map,
     231             :         // specially when mapSize is a multiple of CHUNK_SIZE
     232           0 :         int i = std::min((int)CHUNK_SIZE - 1, gi - m_ChunkI * CHUNK_SIZE);
     233           0 :         int j = std::min((int)CHUNK_SIZE - 1, gj - m_ChunkJ * CHUNK_SIZE);
     234           0 :         if (m_Regions[j][i] == r)
     235             :         {
     236           0 :             iOut = gi;
     237           0 :             jOut = gj;
     238           0 :             dist2Best = (gi - i0)*(gi - i0)
     239           0 :                       + (gj - j0)*(gj - j0);
     240           0 :             return true;
     241             :         }
     242           0 :         return false;
     243             :     }
     244           4 :     case PathGoal::CIRCLE:
     245             :     case PathGoal::SQUARE:
     246             :     {
     247             :         // restrict ourselves to a square surrounding the goal.
     248           4 :         int radius = (std::max(goal.hw*3/2,goal.hh*3/2) >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToInfinity();
     249           4 :         int imin = std::max(0, gi-m_ChunkI*CHUNK_SIZE-radius);
     250           4 :         int imax = std::min((int)CHUNK_SIZE, gi-m_ChunkI*CHUNK_SIZE+radius+1);
     251           4 :         int jmin = std::max(0, gj-m_ChunkJ*CHUNK_SIZE-radius);
     252           4 :         int jmax = std::min((int)CHUNK_SIZE, gj-m_ChunkJ*CHUNK_SIZE+radius+1);
     253           4 :         bool found = false;
     254           4 :         u32 dist2 = std::numeric_limits<u32>::max();
     255         132 :         for (u16 j = jmin; j < jmax; ++j)
     256             :         {
     257        3867 :             for (u16 i = imin; i < imax; ++i)
     258             :             {
     259        3739 :                 if (m_Regions[j][i] != r)
     260        1447 :                     continue;
     261             : 
     262        2292 :                 if (found)
     263             :                 {
     264        3534 :                     dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
     265        1767 :                         + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
     266        1767 :                     if (dist2 >= dist2Best)
     267        1607 :                         continue;
     268             :                 }
     269             : 
     270         685 :                 if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
     271             :                 {
     272          11 :                     if (!found)
     273             :                     {
     274           1 :                         found = true;
     275           2 :                         dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
     276           1 :                             + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
     277             :                     }
     278          11 :                     iOut = i + m_ChunkI*CHUNK_SIZE;
     279          11 :                     jOut = j + m_ChunkJ*CHUNK_SIZE;
     280          11 :                     dist2Best = dist2;
     281             :                 }
     282             :             }
     283             :         }
     284           4 :         return found;
     285             :     }
     286           0 :     case PathGoal::INVERTED_CIRCLE:
     287             :     case PathGoal::INVERTED_SQUARE:
     288             :     {
     289           0 :         bool found = false;
     290           0 :         u32 dist2 = std::numeric_limits<u32>::max();
     291             :         // loop over all navcells.
     292           0 :         for (u16 j = 0; j < CHUNK_SIZE; ++j)
     293             :         {
     294           0 :             for (u16 i = 0; i < CHUNK_SIZE; ++i)
     295             :             {
     296           0 :                 if (m_Regions[j][i] != r)
     297           0 :                     continue;
     298             : 
     299           0 :                 if (found)
     300             :                 {
     301           0 :                     dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
     302           0 :                         + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
     303           0 :                     if (dist2 >= dist2Best)
     304           0 :                         continue;
     305             :                 }
     306             : 
     307           0 :                 if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
     308             :                 {
     309           0 :                     if (!found)
     310             :                     {
     311           0 :                         found = true;
     312           0 :                         dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
     313           0 :                             + (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
     314             :                     }
     315           0 :                     iOut = i + m_ChunkI*CHUNK_SIZE;
     316           0 :                     jOut = j + m_ChunkJ*CHUNK_SIZE;
     317           0 :                     dist2Best = dist2;
     318             :                 }
     319             :             }
     320             :         }
     321           0 :         return found;
     322             :     }
     323             :     }
     324           0 :     return false;
     325             : }
     326             : 
     327           6 : HierarchicalPathfinder::HierarchicalPathfinder() : m_DebugOverlay(NULL)
     328             : {
     329           6 : }
     330             : 
     331          12 : HierarchicalPathfinder::~HierarchicalPathfinder()
     332             : {
     333           6 :     SAFE_DELETE(m_DebugOverlay);
     334           6 : }
     335             : 
     336           0 : void HierarchicalPathfinder::SetDebugOverlay(bool enabled, const CSimContext* simContext)
     337             : {
     338           0 :     if (enabled && !m_DebugOverlay)
     339             :     {
     340           0 :         m_DebugOverlay = new HierarchicalOverlay(*this);
     341           0 :         m_DebugOverlayLines.clear();
     342           0 :         m_SimContext = simContext;
     343           0 :         AddDebugEdges(GetPassabilityClass("default"));
     344             :     }
     345           0 :     else if (!enabled && m_DebugOverlay)
     346             :     {
     347           0 :         SAFE_DELETE(m_DebugOverlay);
     348           0 :         m_DebugOverlayLines.clear();
     349           0 :         m_SimContext = NULL;
     350             :     }
     351           0 : }
     352             : 
     353           0 : void HierarchicalPathfinder::RenderSubmit(SceneCollector& collector)
     354             : {
     355           0 :     if (!m_DebugOverlay)
     356           0 :         return;
     357             : 
     358           0 :     for (size_t i = 0; i < m_DebugOverlayLines.size(); ++i)
     359           0 :         collector.Submit(&m_DebugOverlayLines[i]);
     360             : }
     361             : 
     362           3 : void HierarchicalPathfinder::Recompute(Grid<NavcellData>* grid,
     363             :     const std::map<std::string, pass_class_t>& nonPathfindingPassClassMasks,
     364             :     const std::map<std::string, pass_class_t>& pathfindingPassClassMasks)
     365             : {
     366           6 :     PROFILE2("Hierarchical Recompute");
     367             : 
     368           3 :     m_PassClassMasks = pathfindingPassClassMasks;
     369             : 
     370           6 :     std::map<std::string, pass_class_t> allPassClasses = m_PassClassMasks;
     371           3 :     allPassClasses.insert(nonPathfindingPassClassMasks.begin(), nonPathfindingPassClassMasks.end());
     372             : 
     373           3 :     m_W = grid->m_W;
     374           3 :     m_H = grid->m_H;
     375             : 
     376           3 :     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
     377             : 
     378             :     // Divide grid into chunks with round-to-positive-infinity
     379           3 :     m_ChunksW = (grid->m_W + CHUNK_SIZE - 1) / CHUNK_SIZE;
     380           3 :     m_ChunksH = (grid->m_H + CHUNK_SIZE - 1) / CHUNK_SIZE;
     381             : 
     382           3 :     m_Chunks.clear();
     383           3 :     m_Edges.clear();
     384             : 
     385             :     // Reset global regions.
     386           3 :     m_NextGlobalRegionID = 1;
     387             : 
     388          12 :     for (auto& passClassMask : allPassClasses)
     389             :     {
     390           9 :         pass_class_t passClass = passClassMask.second;
     391             : 
     392             :         // Compute the regions within each chunk
     393           9 :         m_Chunks[passClass].resize(m_ChunksW*m_ChunksH);
     394          30 :         for (int cj = 0; cj < m_ChunksH; ++cj)
     395             :         {
     396          78 :             for (int ci = 0; ci < m_ChunksW; ++ci)
     397             :             {
     398          57 :                 m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass);
     399             :             }
     400             :         }
     401             : 
     402             :         // Construct the search graph over the regions.
     403           9 :         EdgesMap& edges = m_Edges[passClass];
     404           9 :         RecomputeAllEdges(passClass, edges);
     405             : 
     406             :         // Spread global regions.
     407           9 :         std::map<RegionID, GlobalRegionID>& globalRegion = m_GlobalRegions[passClass];
     408           9 :         globalRegion.clear();
     409          30 :         for (u8 cj = 0; cj < m_ChunksH; ++cj)
     410          78 :             for (u8 ci = 0; ci < m_ChunksW; ++ci)
     411          99 :                 for (u16 rid : GetChunk(ci, cj, passClass).m_RegionsID)
     412             :                 {
     413          42 :                     RegionID reg{ci,cj,rid};
     414          42 :                     if (globalRegion.find(reg) == globalRegion.end())
     415             :                     {
     416           8 :                         GlobalRegionID ID = m_NextGlobalRegionID++;
     417             : 
     418           8 :                         globalRegion.insert({ reg, ID });
     419             :                         // Avoid creating an empty link if possible, FindReachableRegions uses [] which calls the default constructor.
     420           8 :                         if (edges.find(reg) != edges.end())
     421             :                         {
     422           8 :                             std::set<RegionID> reachable;
     423           4 :                             FindReachableRegions(reg, reachable, passClass);
     424          42 :                             for (const RegionID& region : reachable)
     425          38 :                                 globalRegion.insert({ region, ID });
     426             :                         }
     427             :                     }
     428             :                 }
     429             :     }
     430             : 
     431           3 :     if (m_DebugOverlay)
     432             :     {
     433           0 :         m_DebugOverlayLines.clear();
     434           0 :         AddDebugEdges(GetPassabilityClass("default"));
     435             :     }
     436           3 : }
     437             : 
     438           7 : void HierarchicalPathfinder::Update(Grid<NavcellData>* grid, const Grid<u8>& dirtinessGrid)
     439             : {
     440          14 :     PROFILE3("Hierarchical Update");
     441             : 
     442           7 :     ASSERT(m_NextGlobalRegionID < std::numeric_limits<GlobalRegionID>::max());
     443             : 
     444          14 :     std::map<pass_class_t, std::vector<RegionID> > needNewGlobalRegionMap;
     445             : 
     446             :     // Algorithm for the partial update:
     447             :     // 1. Loop over chunks.
     448             :     // 2. For any dirty chunk:
     449             :     //      - remove all regions from the global region map
     450             :     //      - remove all edges, by removing the neighbor connection with them and then deleting us
     451             :     //      - recreate regions inside the chunk
     452             :     //      - reconnect the regions. We may do too much work if we reconnect with a dirty chunk, but that's fine.
     453             :     // 3. Recreate global regions.
     454             :     // This means that if any chunk changes, we may need to flood (at most once) the whole map.
     455             :     // That's quite annoying, but I can't think of an easy way around it.
     456             :     // If we could be sure that a region's topology hasn't changed, we could skip removing its global region
     457             :     // but that's non trivial as we have no easy way to determine said topology (regions could "switch" IDs on update for now).
     458          28 :     for (u8 cj = 0; cj <  m_ChunksH; ++cj)
     459             :     {
     460          21 :         int j0 = cj * CHUNK_SIZE;
     461          21 :         int j1 = std::min(j0 + CHUNK_SIZE, (int)dirtinessGrid.m_H);
     462          84 :         for (u8 ci = 0; ci < m_ChunksW; ++ci)
     463             :         {
     464             :             // Skip chunks where no navcells are dirty.
     465          63 :             int i0 = ci * CHUNK_SIZE;
     466          63 :             int i1 = std::min(i0 + CHUNK_SIZE, (int)dirtinessGrid.m_W);
     467          63 :             if (!dirtinessGrid.any_set_in_square(i0, j0, i1, j1))
     468          33 :                 continue;
     469             : 
     470          90 :             for (const std::pair<const std::string, pass_class_t>& passClassMask : m_PassClassMasks)
     471             :             {
     472          60 :                 pass_class_t passClass = passClassMask.second;
     473          60 :                 Chunk& a = m_Chunks[passClass].at(ci + cj*m_ChunksW);
     474             : 
     475             :                 // Clean up edges and global region ID
     476          60 :                 EdgesMap& edgeMap = m_Edges[passClass];
     477          94 :                 for (u16 i : a.m_RegionsID)
     478             :                 {
     479          34 :                     RegionID reg{ci, cj, i};
     480          34 :                     m_GlobalRegions[passClass].erase(reg);
     481         124 :                     for (const RegionID& neighbor : edgeMap[reg])
     482             :                     {
     483          90 :                         edgeMap[neighbor].erase(reg);
     484          90 :                         if (edgeMap[neighbor].empty())
     485           0 :                             edgeMap.erase(neighbor);
     486             :                     }
     487          34 :                     edgeMap.erase(reg);
     488             :                 }
     489             : 
     490             :                 // Recompute regions inside this chunk.
     491          60 :                 a.InitRegions(ci, cj, grid, passClass);
     492             : 
     493          94 :                 for (u16 i : a.m_RegionsID)
     494          34 :                     needNewGlobalRegionMap[passClass].push_back(RegionID{ci, cj, i});
     495             : 
     496          60 :                 UpdateEdges(ci, cj, passClass, edgeMap);
     497             :             }
     498             :         }
     499             :     }
     500             : 
     501           7 :     UpdateGlobalRegions(needNewGlobalRegionMap);
     502             : 
     503           7 :     if (m_DebugOverlay)
     504             :     {
     505           0 :         m_DebugOverlayLines.clear();
     506           0 :         AddDebugEdges(GetPassabilityClass("default"));
     507             :     }
     508           7 : }
     509             : 
     510         254 : void HierarchicalPathfinder::ComputeNeighbors(EdgesMap& edges, Chunk& a, Chunk& b, bool transpose, bool opposite) const
     511             : {
     512             :     // For each edge between chunks, we loop over every adjacent pair of
     513             :     // navcells in the two chunks. If they are both in valid regions
     514             :     // (i.e. are passable navcells) then add a graph edge between those regions.
     515             :     // (We don't need to test for duplicates since EdgesMap already uses a
     516             :     // std::set which will drop duplicate entries.)
     517             :     // But as set.insert can be quite slow on large collection, and that we usually
     518             :     // try to insert the same values, we cache the previous one for a fast test.
     519         254 :     RegionID raPrev(0,0,0);
     520         254 :     RegionID rbPrev(0,0,0);
     521       24638 :     for (int k = 0; k < CHUNK_SIZE; ++k)
     522             :     {
     523       24384 :         u8 aSide = opposite ? CHUNK_SIZE - 1 : 0;
     524       24384 :         u8 bSide = CHUNK_SIZE - 1 - aSide;
     525       24384 :         RegionID ra = transpose ? a.Get(k, aSide) : a.Get(aSide, k);
     526       24384 :         RegionID rb = transpose ? b.Get(k, bSide) : b.Get(bSide, k);
     527       24384 :         if (ra.r && rb.r)
     528             :         {
     529        9890 :             if (ra == raPrev && rb == rbPrev)
     530        9751 :                 continue;
     531         139 :             edges[ra].insert(rb);
     532         139 :             edges[rb].insert(ra);
     533         139 :             raPrev = ra;
     534         139 :             rbPrev = rb;
     535             :         }
     536             :     }
     537         254 : }
     538             : 
     539             : /**
     540             :  * Connect a chunk's regions to their neighbors. Not optimised for global recomputing.
     541             :  */
     542          60 : void HierarchicalPathfinder::UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges)
     543             : {
     544          60 :     std::vector<Chunk>& chunks = m_Chunks[passClass];
     545             : 
     546          60 :     Chunk& a = chunks.at(cj*m_ChunksW + ci);
     547             : 
     548          60 :     if (ci > 0)
     549          60 :         ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci-1)), false, false);
     550             : 
     551          60 :     if (ci < m_ChunksW-1)
     552          42 :         ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci+1)), false, true);
     553             : 
     554          60 :     if (cj > 0)
     555          40 :         ComputeNeighbors(edges, a, chunks.at((cj-1)*m_ChunksW + ci), true, false);
     556             : 
     557          60 :     if (cj < m_ChunksH - 1)
     558          40 :         ComputeNeighbors(edges, a, chunks.at((cj+1)*m_ChunksW + ci), true, true);
     559          60 : }
     560             : 
     561             : /**
     562             :  * Find edges between regions in all chunks, in an optimised manner (only look at top/left)
     563             :  */
     564           9 : void HierarchicalPathfinder::RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges)
     565             : {
     566           9 :     std::vector<Chunk>& chunks = m_Chunks[passClass];
     567             : 
     568           9 :     edges.clear();
     569             : 
     570          30 :     for (int cj = 0; cj < m_ChunksH; ++cj)
     571             :     {
     572          78 :         for (int ci = 0; ci < m_ChunksW; ++ci)
     573             :         {
     574          57 :             Chunk& a = chunks.at(cj*m_ChunksW + ci);
     575             : 
     576          57 :             if (ci > 0)
     577          36 :                 ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci-1)), false, false);
     578             : 
     579          57 :             if (cj > 0)
     580          36 :                 ComputeNeighbors(edges, a, chunks.at((cj-1)*m_ChunksW + ci), true, false);
     581             :         }
     582             :     }
     583           9 : }
     584             : 
     585             : /**
     586             :  * Debug visualisation of graph edges between regions.
     587             :  */
     588           0 : void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass)
     589             : {
     590           0 :     const EdgesMap& edges = m_Edges[passClass];
     591           0 :     const std::vector<Chunk>& chunks = m_Chunks[passClass];
     592             : 
     593           0 :     for (auto& edge : edges)
     594             :     {
     595           0 :         for (const RegionID& region: edge.second)
     596             :         {
     597             :             // Draw a line between the two regions' centers
     598             : 
     599             :             int i0, j0, i1, j1;
     600           0 :             chunks[edge.first.cj * m_ChunksW + edge.first.ci].RegionCenter(edge.first.r, i0, j0);
     601           0 :             chunks[region.cj * m_ChunksW + region.ci].RegionCenter(region.r, i1, j1);
     602             : 
     603           0 :             CFixedVector2D a, b;
     604           0 :             Pathfinding::NavcellCenter(i0, j0, a.X, a.Y);
     605           0 :             Pathfinding::NavcellCenter(i1, j1, b.X, b.Y);
     606             : 
     607             :             // Push the endpoints inwards a little to avoid overlaps
     608           0 :             CFixedVector2D d = b - a;
     609           0 :             d.Normalize(entity_pos_t::FromInt(1));
     610           0 :             a += d;
     611           0 :             b -= d;
     612             : 
     613           0 :             std::vector<float> xz;
     614           0 :             xz.push_back(a.X.ToFloat());
     615           0 :             xz.push_back(a.Y.ToFloat());
     616           0 :             xz.push_back(b.X.ToFloat());
     617           0 :             xz.push_back(b.Y.ToFloat());
     618             : 
     619           0 :             m_DebugOverlayLines.emplace_back();
     620           0 :             m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);
     621           0 :             SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);
     622             :         }
     623             :     }
     624           0 : }
     625             : 
     626           7 : void HierarchicalPathfinder::UpdateGlobalRegions(const std::map<pass_class_t, std::vector<RegionID> >& needNewGlobalRegionMap)
     627             : {
     628             :     // Use FindReachableRegions because we cannot be sure, even if we find a non-dirty chunk nearby,
     629             :     // that we weren't the only bridge connecting that chunk to the rest of the global region.
     630          14 :     for (const std::pair<const pass_class_t, std::vector<RegionID>>& regionsInNeed : needNewGlobalRegionMap)
     631             :     {
     632          41 :         for (const RegionID& reg : regionsInNeed.second)
     633             :         {
     634          34 :             std::map<RegionID, GlobalRegionID>& globalRegions = m_GlobalRegions[regionsInNeed.first];
     635             :             // If we have already been given a region, skip us.
     636          34 :             if (globalRegions.find(reg) != globalRegions.end())
     637          25 :                 continue;
     638             : 
     639          18 :             std::set<RegionID> reachable;
     640           9 :             FindReachableRegions(reg, reachable, regionsInNeed.first);
     641             : 
     642           9 :             GlobalRegionID ID = m_NextGlobalRegionID++;
     643             : 
     644          76 :             for (const RegionID& regionId : reachable)
     645          67 :                 globalRegions[regionId] = ID;
     646             :         }
     647             :     }
     648           7 : }
     649             : 
     650      403022 : HierarchicalPathfinder::RegionID HierarchicalPathfinder::Get(u16 i, u16 j, pass_class_t passClass) const
     651             : {
     652      403022 :     int ci = i / CHUNK_SIZE;
     653      403022 :     int cj = j / CHUNK_SIZE;
     654      403022 :     ENSURE(ci < m_ChunksW && cj < m_ChunksH);
     655      403022 :     return m_Chunks.at(passClass)[cj*m_ChunksW + ci].Get(i % CHUNK_SIZE, j % CHUNK_SIZE);
     656             : }
     657             : 
     658      402734 : HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const
     659             : {
     660      402734 :     return GetGlobalRegion(Get(i, j, passClass), passClass);
     661             : }
     662             : 
     663      402740 : HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(RegionID region, pass_class_t passClass) const
     664             : {
     665      402740 :     return region.r == 0 ? GlobalRegionID(0) : m_GlobalRegions.at(passClass).at(region);
     666             : }
     667             : 
     668           6 : void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)
     669             : {
     670           6 :     PathGoal newGoal;
     671           6 :     newGoal.type = PathGoal::POINT;
     672           6 :     Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z);
     673           6 :     goal = newGoal;
     674           6 : }
     675             : 
     676          11 : bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const
     677             : {
     678          22 :     PROFILE2("MakeGoalReachable");
     679             : 
     680             :     u16 iGoal, jGoal;
     681          11 :     Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
     682             : 
     683          22 :     std::set<InterestingRegion, SortByBestToPoint> goalRegions(SortByBestToPoint(i0, j0));
     684             :     // This returns goal regions ordered by distance from the best navcell in each region.
     685          11 :     FindGoalRegionsAndBestNavcells(i0, j0, iGoal, jGoal, goal, goalRegions, passClass);
     686             : 
     687             :     // Because of the sorting above, we can stop as soon as the first reachable goal region is found.
     688          11 :     for (const InterestingRegion& region : goalRegions)
     689           6 :         if (GetGlobalRegion(region.region, passClass) == GetGlobalRegion(i0, j0, passClass))
     690             :         {
     691           6 :             iGoal = region.bestI;
     692           6 :             jGoal = region.bestJ;
     693             : 
     694             :             // No need to move reachable point goals.
     695           6 :             if (goal.type != PathGoal::POINT)
     696           1 :                 CreatePointGoalAt(iGoal, jGoal, goal);
     697           6 :             return true;
     698             :         }
     699             : 
     700             :     // Goal wasn't reachable - get the closest navcell in the nearest reachable region.
     701          10 :     std::set<RegionID, SortByCenterToPoint> reachableRegions(SortByCenterToPoint(iGoal, jGoal));
     702           5 :     FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass);
     703             : 
     704           5 :     FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass);
     705           5 :     CreatePointGoalAt(iGoal, jGoal, goal);
     706           5 :     return false;
     707             : }
     708             : 
     709             : 
     710           0 : bool HierarchicalPathfinder::IsGoalReachable(u16 i0, u16 j0, const PathGoal& goal, pass_class_t passClass) const
     711             : {
     712           0 :     PROFILE2("IsGoalReachable");
     713             : 
     714             :     u16 iGoal, jGoal;
     715           0 :     Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
     716             : 
     717           0 :     std::set<InterestingRegion, SortByBestToPoint> goalRegions(SortByBestToPoint(i0, j0));
     718             :     // This returns goal regions ordered by distance from the best navcell in each region.
     719           0 :     FindGoalRegionsAndBestNavcells(i0, j0, iGoal, jGoal, goal, goalRegions, passClass);
     720             : 
     721             :     // Because of the sorting above, we can stop as soon as the first reachable goal region is found.
     722           0 :     for (const InterestingRegion& region : goalRegions)
     723           0 :         if (GetGlobalRegion(region.region, passClass) == GetGlobalRegion(i0, j0, passClass))
     724           0 :             return true;
     725           0 :     return false;
     726             : }
     727             : 
     728          17 : void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const
     729             : {
     730          34 :     std::set<RegionID, SortByCenterToPoint> regions(SortByCenterToPoint(i, j));
     731             : 
     732             :     // Construct a set of all regions of all chunks for this pass class
     733         170 :     for (const Chunk& chunk : m_Chunks.at(passClass))
     734         336 :         for (int r : chunk.m_RegionsID)
     735         183 :             regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r));
     736             : 
     737          17 :     FindNearestNavcellInRegions(regions, i, j, passClass);
     738          17 : }
     739             : 
     740          22 : void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set<RegionID, SortByCenterToPoint>& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const
     741             : {
     742          22 :     u16 bestI = iGoal, bestJ = jGoal; // Somewhat sensible default-values should regions() be passed empty.
     743          22 :     u32 bestDist = std::numeric_limits<u32>::max();
     744             : 
     745             :     // Because regions are sorted by increasing distance, we can ignore regions that are obviously farther than the current best point.
     746             :     // Since regions are squares, that happens when the center of a region is at least sqrt(2) * CHUNK_SIZE farther than the current best point.
     747             :     // Add one to avoid cases where the center navcell is actually slightly off-center (= CHUNK_SIZE is even)
     748          22 :     u32 maxDistFromBest = (fixed::FromInt(3) / 2 * CHUNK_SIZE).ToInt_RoundToInfinity() + 1;
     749             :     // TODO: update to static_assert with constexpr
     750          22 :     ENSURE(maxDistFromBest < std::numeric_limits<u16>::max());
     751          22 :     maxDistFromBest *= maxDistFromBest;
     752             : 
     753         115 :     for (const RegionID& region : regions)
     754             :     {
     755         115 :         u32 chunkDist = region.DistanceTo(iGoal, jGoal);
     756             :         // This might overflow, but only if we are already close to the maximal possible distance, so the condition would probably be false anyways.
     757             :         // It's also a bit pessimistic, so we'll still consider a few too many regions.
     758         115 :         if (bestDist < std::numeric_limits<u32>::max() && chunkDist > maxDistFromBest + bestDist)
     759          22 :             break; // Break, the set is ordered by increased distance so a closer region will not be found.
     760             : 
     761             :         int ri, rj;
     762             :         u32 dist;
     763          93 :         GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, ri, rj, dist);
     764          93 :         if (dist < bestDist)
     765             :         {
     766          22 :             bestI = ri;
     767          22 :             bestJ = rj;
     768          22 :             bestDist = dist;
     769             :         }
     770             :     }
     771          22 :     iGoal = bestI;
     772          22 :     jGoal = bestJ;
     773          22 : }
     774             : 
     775          11 : void HierarchicalPathfinder::FindGoalRegionsAndBestNavcells(u16 i0, u16 j0, u16 gi, u16 gj, const PathGoal& goal, std::set<InterestingRegion, SortByBestToPoint>& regions, pass_class_t passClass) const
     776             : {
     777          11 :     if (goal.type == PathGoal::POINT)
     778             :     {
     779           8 :         RegionID region = Get(gi, gj, passClass);
     780           8 :         if (region.r > 0)
     781           5 :             regions.insert({region, gi, gj});
     782           8 :         return;
     783             :     }
     784             : 
     785             :     // For non-point cases, we'll test each region inside the bounds of the goal.
     786             :     // we might occasionally test a few too many for circles but it's not too bad.
     787             :     // Note that this also works in the Inverse-circle / Inverse-square case
     788             :     // Since our ranges are inclusive, we will necessarily test at least the perimeter/outer bound of the goal.
     789             :     // If we find a navcell, great, if not, well then we'll be surrounded by an impassable barrier.
     790             :     // 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.
     791             :     // It's not worth it to skip the "inner" regions since we'd need ranges above CHUNK_SIZE for that to start mattering
     792             :     // (and even then not always) and that just doesn't happen for Inverse-XX goals
     793           3 :     int size = (std::max(goal.hh, goal.hw) * 3 / 2).ToInt_RoundToInfinity();
     794             : 
     795             :     u16 bestI, bestJ;
     796             :     u32 c; // Unused.
     797             : 
     798           6 :     for (u8 sz = std::max(0,(gj - size) / CHUNK_SIZE); sz <= std::min(m_ChunksH-1, (gj + size + 1) / CHUNK_SIZE); ++sz)
     799           7 :         for (u8 sx = std::max(0,(gi - size) / CHUNK_SIZE); sx <= std::min(m_ChunksW-1, (gi + size + 1) / CHUNK_SIZE); ++sx)
     800             :         {
     801           4 :             const Chunk& chunk = GetChunk(sx, sz, passClass);
     802           8 :             for (u16 i : chunk.m_RegionsID)
     803           4 :                 if (chunk.RegionNearestNavcellInGoal(i, i0, j0, goal, bestI, bestJ, c))
     804           1 :                     regions.insert({RegionID{sx, sz, i}, bestI, bestJ});
     805             :         }
     806             : }
     807             : 
     808           0 : void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid<u16>& grid) const
     809             : {
     810           0 :     ENSURE(grid.m_W == m_W && grid.m_H == m_H);
     811             : 
     812           0 :     int i0 = region.ci * CHUNK_SIZE;
     813           0 :     int j0 = region.cj * CHUNK_SIZE;
     814             : 
     815           0 :     const Chunk& c = m_Chunks.at(passClass)[region.cj * m_ChunksW + region.ci];
     816             : 
     817           0 :     for (int j = 0; j < CHUNK_SIZE; ++j)
     818           0 :         for (int i = 0; i < CHUNK_SIZE; ++i)
     819           0 :             if (c.m_Regions[j][i] == region.r)
     820           0 :                 grid.set(i0 + i, j0 + j, value);
     821           0 : }
     822             : 
     823           0 : Grid<u16> HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) const
     824             : {
     825           0 :     Grid<u16> connectivityGrid(m_W, m_H);
     826           0 :     connectivityGrid.reset();
     827             : 
     828           0 :     u16 idx = 1;
     829             : 
     830           0 :     for (u16 i = 0; i < m_W; ++i)
     831             :     {
     832           0 :         for (u16 j = 0; j < m_H; ++j)
     833             :         {
     834           0 :             if (connectivityGrid.get(i, j) != 0)
     835           0 :                 continue;
     836             : 
     837           0 :             RegionID from = Get(i, j, passClass);
     838           0 :             if (from.r == 0)
     839           0 :                 continue;
     840             : 
     841           0 :             std::set<RegionID> reachable;
     842           0 :             FindReachableRegions(from, reachable, passClass);
     843             : 
     844           0 :             for (const RegionID& region : reachable)
     845           0 :                 FillRegionOnGrid(region, passClass, idx, connectivityGrid);
     846             : 
     847           0 :             ++idx;
     848             :         }
     849             :     }
     850             : 
     851           0 :     return connectivityGrid;
     852           0 : }

Generated by: LCOV version 1.13