Index: ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h
===================================================================
--- ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h (revision 22877)
+++ ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h (revision 22878)
@@ -1,511 +1,522 @@
/* 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
HierarchicalPathfinder::GlobalRegionID 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 = std::map {
{ "1", 1 },
{ "2", 2 },
};
nonPathClassMask = std::map {
{ "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 = std::map {
{ "1", 1 },
{ "2", 2 },
};
nonPathClassMask = std::map {
{ "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,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
- {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
- {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
- {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
- {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
- {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
- {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
- {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
- {_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
+ {_,_,_,_,_,_,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
+ // upscaled n times
+ const int scale = 6;
+
HierarchicalPathfinder hierPath;
- Grid grid(40*5, 40*5);
- Grid dirtyGrid(40*5, 40*5);
+ Grid grid(40*scale, 40*scale);
+ Grid dirtyGrid(40*scale, 40*scale);
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]);
+ for (size_t ii = 0; ii < scale; ++ii)
+ for (size_t jj = 0; jj < scale; ++jj)
+ grid.set(i * scale + ii, j * scale + 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);
// use a macro so the lines reported by tests are accurate
#define check_closest_passable(i, j, expected_manhattan) \
oi = i; oj = j; \
pi = i; pj = j; \
TS_ASSERT(!IS_PASSABLE(grid.get(pi, pj), PASS_1)); \
hierPath.FindNearestPassableNavcell(pi, pj, PASS_1); \
\
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); \
+ TS_ASSERT_EQUALS(manhattan(pi, pj, oi, oj), expected_manhattan); \
}
u16 oi, oj, pi, pj;
- check_closest_passable(4 * 5, 4 * 5, 1);
- check_closest_passable(4 * 5 + 1, 4 * 5 + 1, 2);
- check_closest_passable(14 * 5 + 2, 7 * 5 + 2, 8);
- check_closest_passable(14 * 5 + 2, 7 * 5 + 4, 6);
- check_closest_passable(14 * 5 + 2, 7 * 5 + 5, 5);
- check_closest_passable(14 * 5 + 2, 7 * 5 + 6, 4);
- check_closest_passable(5 * 5 + 3, 7 * 5 + 2, 2);
+ check_closest_passable(4 * scale, 4 * scale, 1);
+ check_closest_passable(4 * scale + 1, 4 * scale + 1, 2);
+ check_closest_passable(14 * scale + 2, 7 * scale + 2, 9);
+ check_closest_passable(14 * scale + 2, 7 * scale + 4, 8);
+ check_closest_passable(14 * scale + 2, 7 * scale + 5, 7);
+ check_closest_passable(14 * scale + 2, 7 * scale + 6, 6);
+ check_closest_passable(5 * scale + 3, 7 * scale + 2, 3);
#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 = 6 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
+ oi = 5 * scale + 3; oj = 3 * scale + 3;
+ pi = 5 * scale + 3; pj = 6 * scale + 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_EQUALS(pi, goal.x.ToInt_RoundToNegInfinity());
TS_ASSERT_EQUALS(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;
+ oi = 5 * scale + 3; oj = 3 * scale + 3;
+ pi = 26 * scale + 3; pj = 5 * scale + 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);
+ oi = 5 * scale + 3; oj = 3 * scale + 3;
+ pi = 5 * scale + 3; pj = 7 * scale + 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());
+ // Same position, but goal is unreachable and much farther away.
+ goal.type = PathGoal::POINT;
+ oi = 5 * scale + 3; oj = 3 * scale + 3;
+ pi = 34 * scale + 3; pj = 6 * scale + 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_EQUALS(pi, goal.x.ToInt_RoundToNegInfinity())
+ TS_ASSERT_EQUALS(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);
+ oi = 5 * scale + 3; oj = 3 * scale + 3;
+ pi = 5 * scale + 3; pj = 7 * scale + 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);
+ oi = 5 * scale + 3; oj = 3 * scale + 3;
+ pi = 5 * scale - 1; pj = 7 * scale + 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);
+ oi = 5 * scale + 3; oj = 3 * scale + 3;
+ pi = 36 * scale + 3; pj = 7 * scale + 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(std::fabs(euclidian(goal.x.ToInt_RoundToNegInfinity(), goal.z.ToInt_RoundToNegInfinity(), pi, pj)-20) < 1.5f);
TS_ASSERT(std::fabs(euclidian(goal.x.ToInt_RoundToNegInfinity(), goal.z.ToInt_RoundToNegInfinity(), oi, oj) - euclidian(pi, pj, oi, oj)) < 22.0f);
}
void test_regions_flood_fill()
{
// Partial test of region inner flood filling.
// This highlights that internal region IDs can become higher than the number of regions.
pathClassMask = std::map {
{ "1", 1 },
{ "2", 2 },
};
nonPathClassMask = std::map {
{ "3", 4 }
};
// 0 is passable, 1 is not.
// i is vertical, j is horizontal;
#define _ 0
#define X 1
NavcellData gridDef[5][5] = {
{X,_,X,_,_},
{_,_,X,X,_},
{X,_,X,_,_},
{_,_,X,X,_},
{X,_,X,_,_}
};
#undef _
#undef X
HierarchicalPathfinder hierPath;
Grid grid(5, 5);
Grid dirtyGrid(5, 5);
for (size_t i = 0; i < 5; ++i)
for (size_t j = 0; j < 5; ++j)
grid.set(i, j, gridDef[i][j]);
hierPath.Recompute(&grid, nonPathClassMask, pathClassMask);
TS_ASSERT_EQUALS(hierPath.m_Chunks[pathClassMask["1"]][0].m_RegionsID.size(), 2);
TS_ASSERT_EQUALS(hierPath.m_Chunks[pathClassMask["1"]][0].m_RegionsID.back(), 4);
}
};
Index: ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp
===================================================================
--- ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp (revision 22877)
+++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp (revision 22878)
@@ -1,855 +1,857 @@
/* 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 "precompiled.h"
#include "HierarchicalPathfinder.h"
#include "graphics/Overlay.h"
#include "ps/Profile.h"
#include "renderer/Scene.h"
// Find the root ID of a region, used by InitRegions
inline u16 RootID(u16 x, const std::vector& v)
{
while (v[x] < x)
x = v[x];
return x;
}
void HierarchicalPathfinder::Chunk::InitRegions(int ci, int cj, Grid* grid, pass_class_t passClass)
{
ENSURE(ci < 256 && cj < 256); // avoid overflows
m_ChunkI = ci;
m_ChunkJ = cj;
memset(m_Regions, 0, sizeof(m_Regions));
int i0 = ci * CHUNK_SIZE;
int j0 = cj * CHUNK_SIZE;
int i1 = std::min(i0 + CHUNK_SIZE, (int)grid->m_W);
int j1 = std::min(j0 + CHUNK_SIZE, (int)grid->m_H);
// Efficiently flood-fill the m_Regions grid
int regionID = 0;
std::vector connect;
u16* pCurrentID = NULL;
u16 LeftID = 0;
u16 DownID = 0;
bool Checked = false; // prevent some unneccessary RootID calls
connect.reserve(32); // TODO: What's a sensible number?
connect.push_back(0); // connect[0] = 0
// Start by filling the grid with 0 for blocked,
// and regionID for unblocked
for (int j = j0; j < j1; ++j)
{
for (int i = i0; i < i1; ++i)
{
pCurrentID = &m_Regions[j-j0][i-i0];
if (!IS_PASSABLE(grid->get(i, j), passClass))
{
*pCurrentID = 0;
continue;
}
if (j > j0)
DownID = m_Regions[j-1-j0][i-i0];
if (i == i0)
LeftID = 0;
else
LeftID = m_Regions[j-j0][i-1-i0];
if (LeftID > 0)
{
*pCurrentID = LeftID;
if (*pCurrentID != DownID && DownID > 0 && !Checked)
{
u16 id0 = RootID(DownID, connect);
u16 id1 = RootID(LeftID, connect);
Checked = true; // this avoids repeatedly connecting the same IDs
if (id0 < id1)
connect[id1] = id0;
else if (id0 > id1)
connect[id0] = id1;
}
else if (DownID == 0)
Checked = false;
}
else if (DownID > 0)
{
*pCurrentID = DownID;
Checked = false;
}
else
{
// New ID
*pCurrentID = ++regionID;
connect.push_back(regionID);
Checked = false;
}
}
}
// Mark connected regions as being the same ID (i.e. the lowest)
m_RegionsID.clear();
for (u16 i = 1; i < regionID+1; ++i)
{
if (connect[i] != i)
connect[i] = RootID(i, connect);
else
m_RegionsID.push_back(connect[i]);
}
// Replace IDs by the root ID
for (int j = 0; j < CHUNK_SIZE; ++j)
for (int i = 0; i < CHUNK_SIZE; ++i)
m_Regions[j][i] = connect[m_Regions[j][i]];
}
/**
* Returns a RegionID for the given global navcell coords
* (which must be inside this chunk);
*/
HierarchicalPathfinder::RegionID HierarchicalPathfinder::Chunk::Get(int i, int j) const
{
ENSURE(i < CHUNK_SIZE && j < CHUNK_SIZE);
return RegionID(m_ChunkI, m_ChunkJ, m_Regions[j][i]);
}
/**
* Return the global navcell coords that correspond roughly to the
* center of the given region in this chunk.
* (This is not guaranteed to be actually inside the region.)
*/
void HierarchicalPathfinder::Chunk::RegionCenter(u16 r, int& i_out, int& j_out) const
{
// Find the mean of i,j coords of navcells in this region:
u32 si = 0, sj = 0; // sum of i,j coords
u32 n = 0; // number of navcells in region
cassert(CHUNK_SIZE < 256); // conservative limit to ensure si and sj don't overflow
for (int j = 0; j < CHUNK_SIZE; ++j)
{
for (int i = 0; i < CHUNK_SIZE; ++i)
{
if (m_Regions[j][i] == r)
{
si += i;
sj += j;
n += 1;
}
}
}
// Avoid divide-by-zero
if (n == 0)
n = 1;
i_out = m_ChunkI * CHUNK_SIZE + si / n;
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 region is closest to
* that goal.
*/
void HierarchicalPathfinder::Chunk::RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const
{
iBest = 0;
jBest = 0;
dist2Best = std::numeric_limits::max();
for (int j = 0; j < CHUNK_SIZE; ++j)
{
for (int i = 0; i < CHUNK_SIZE; ++i)
{
if (m_Regions[j][i] != r)
continue;
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;
}
}
}
}
/**
* Gives the global navcell coords, and the squared distance to the (i0,j0)
* navcell, of whichever navcell inside the given region and inside the given goal
* is closest to (i0,j0)
* Returns true if the goal is inside the region, false otherwise.
*/
bool HierarchicalPathfinder::Chunk::RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const
{
// TODO: this should be optimized further.
// Most used cases empirically seem to be SQUARE, INVERTED_CIRCLE and then POINT and CIRCLE somehwat equally
iOut = 0;
jOut = 0;
dist2Best = std::numeric_limits::max();
// Calculate the navcell that contains the center of the goal.
int gi = (goal.x >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
int gj = (goal.z >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToNegInfinity();
switch(goal.type)
{
case PathGoal::POINT:
{
// i and j can be equal to CHUNK_SIZE on the top and right borders of the map,
// specially when mapSize is a multiple of CHUNK_SIZE
int i = std::min((int)CHUNK_SIZE - 1, gi - m_ChunkI * CHUNK_SIZE);
int j = std::min((int)CHUNK_SIZE - 1, gj - m_ChunkJ * CHUNK_SIZE);
if (m_Regions[j][i] == r)
{
iOut = gi;
jOut = gj;
dist2Best = (gi - i0)*(gi - i0)
+ (gj - j0)*(gj - j0);
return true;
}
return false;
}
case PathGoal::CIRCLE:
case PathGoal::SQUARE:
{
// restrict ourselves to a square surrounding the goal.
int radius = (std::max(goal.hw*3/2,goal.hh*3/2) >> Pathfinding::NAVCELL_SIZE_LOG2).ToInt_RoundToInfinity();
int imin = std::max(0, gi-m_ChunkI*CHUNK_SIZE-radius);
int imax = std::min((int)CHUNK_SIZE, gi-m_ChunkI*CHUNK_SIZE+radius+1);
int jmin = std::max(0, gj-m_ChunkJ*CHUNK_SIZE-radius);
int jmax = std::min((int)CHUNK_SIZE, gj-m_ChunkJ*CHUNK_SIZE+radius+1);
bool found = false;
u32 dist2 = std::numeric_limits::max();
for (u16 j = jmin; j < jmax; ++j)
{
for (u16 i = imin; i < imax; ++i)
{
if (m_Regions[j][i] != r)
continue;
if (found)
{
dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
if (dist2 >= dist2Best)
continue;
}
if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
{
if (!found)
{
found = true;
dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
}
iOut = i + m_ChunkI*CHUNK_SIZE;
jOut = j + m_ChunkJ*CHUNK_SIZE;
dist2Best = dist2;
}
}
}
return found;
}
case PathGoal::INVERTED_CIRCLE:
case PathGoal::INVERTED_SQUARE:
{
bool found = false;
u32 dist2 = std::numeric_limits::max();
// loop over all navcells.
for (u16 j = 0; j < CHUNK_SIZE; ++j)
{
for (u16 i = 0; i < CHUNK_SIZE; ++i)
{
if (m_Regions[j][i] != r)
continue;
if (found)
{
dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
if (dist2 >= dist2Best)
continue;
}
if (goal.NavcellContainsGoal(m_ChunkI * CHUNK_SIZE + i, m_ChunkJ * CHUNK_SIZE + j))
{
if (!found)
{
found = true;
dist2 = (i + m_ChunkI*CHUNK_SIZE - i0)*(i + m_ChunkI*CHUNK_SIZE - i0)
+ (j + m_ChunkJ*CHUNK_SIZE - j0)*(j + m_ChunkJ*CHUNK_SIZE - j0);
}
iOut = i + m_ChunkI*CHUNK_SIZE;
jOut = j + m_ChunkJ*CHUNK_SIZE;
dist2Best = dist2;
}
}
}
return found;
}
}
return false;
}
HierarchicalPathfinder::HierarchicalPathfinder() : m_DebugOverlay(NULL)
{
}
HierarchicalPathfinder::~HierarchicalPathfinder()
{
SAFE_DELETE(m_DebugOverlay);
}
void HierarchicalPathfinder::SetDebugOverlay(bool enabled, const CSimContext* simContext)
{
if (enabled && !m_DebugOverlay)
{
m_DebugOverlay = new HierarchicalOverlay(*this);
m_DebugOverlayLines.clear();
m_SimContext = simContext;
AddDebugEdges(GetPassabilityClass("default"));
}
else if (!enabled && m_DebugOverlay)
{
SAFE_DELETE(m_DebugOverlay);
m_DebugOverlayLines.clear();
m_SimContext = NULL;
}
}
void HierarchicalPathfinder::RenderSubmit(SceneCollector& collector)
{
if (!m_DebugOverlay)
return;
for (size_t i = 0; i < m_DebugOverlayLines.size(); ++i)
collector.Submit(&m_DebugOverlayLines[i]);
}
void HierarchicalPathfinder::Recompute(Grid* grid,
const std::map& nonPathfindingPassClassMasks,
const std::map& pathfindingPassClassMasks)
{
PROFILE2("Hierarchical Recompute");
m_PassClassMasks = pathfindingPassClassMasks;
std::map allPassClasses = m_PassClassMasks;
allPassClasses.insert(nonPathfindingPassClassMasks.begin(), nonPathfindingPassClassMasks.end());
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;
m_Chunks.clear();
m_Edges.clear();
// Reset global regions.
m_NextGlobalRegionID = 1;
for (auto& passClassMask : allPassClasses)
{
pass_class_t passClass = passClassMask.second;
// Compute the regions within each chunk
m_Chunks[passClass].resize(m_ChunksW*m_ChunksH);
for (int cj = 0; cj < m_ChunksH; ++cj)
{
for (int ci = 0; ci < m_ChunksW; ++ci)
{
m_Chunks[passClass].at(cj*m_ChunksW + ci).InitRegions(ci, cj, grid, passClass);
}
}
// Construct the search graph over the regions.
EdgesMap& edges = m_Edges[passClass];
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)
{
m_DebugOverlayLines.clear();
AddDebugEdges(GetPassabilityClass("default"));
}
}
void HierarchicalPathfinder::Update(Grid* grid, const Grid& dirtinessGrid)
{
PROFILE3("Hierarchical Update");
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 (u8 ci = 0; ci < m_ChunksW; ++ci)
{
// Skip chunks where no navcells are dirty.
int i0 = ci * CHUNK_SIZE;
int i1 = std::min(i0 + CHUNK_SIZE, (int)dirtinessGrid.m_W);
if (!dirtinessGrid.any_set_in_square(i0, j0, i1, j1))
continue;
for (const std::pair& passClassMask : m_PassClassMasks)
{
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 regions inside this chunk.
a.InitRegions(ci, cj, grid, passClass);
for (u16 i : a.m_RegionsID)
needNewGlobalRegionMap[passClass].push_back(RegionID{ci, cj, i});
UpdateEdges(ci, cj, passClass, edgeMap);
}
}
}
UpdateGlobalRegions(needNewGlobalRegionMap);
if (m_DebugOverlay)
{
m_DebugOverlayLines.clear();
AddDebugEdges(GetPassabilityClass("default"));
}
}
void HierarchicalPathfinder::ComputeNeighbors(EdgesMap& edges, Chunk& a, Chunk& b, bool transpose, bool opposite) const
{
// 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.
RegionID raPrev(0,0,0);
RegionID rbPrev(0,0,0);
for (int k = 0; k < CHUNK_SIZE; ++k)
{
u8 aSide = opposite ? CHUNK_SIZE - 1 : 0;
u8 bSide = CHUNK_SIZE - 1 - aSide;
RegionID ra = transpose ? a.Get(k, aSide) : a.Get(aSide, k);
RegionID rb = transpose ? b.Get(k, bSide) : b.Get(bSide, k);
if (ra.r && rb.r)
{
if (ra == raPrev && rb == rbPrev)
continue;
edges[ra].insert(rb);
edges[rb].insert(ra);
raPrev = ra;
rbPrev = rb;
}
}
}
/**
* Connect a chunk's regions to their neighbors. Not optimised for global recomputing.
*/
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);
if (ci > 0)
ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci-1)), false, false);
if (ci < m_ChunksW-1)
ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci+1)), false, true);
if (cj > 0)
ComputeNeighbors(edges, a, chunks.at((cj-1)*m_ChunksW + ci), true, false);
if (cj < m_ChunksH - 1)
ComputeNeighbors(edges, a, chunks.at((cj+1)*m_ChunksW + ci), true, true);
}
/**
* 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);
if (ci > 0)
ComputeNeighbors(edges, a, chunks.at(cj*m_ChunksW + (ci-1)), false, false);
if (cj > 0)
ComputeNeighbors(edges, a, chunks.at((cj-1)*m_ChunksW + ci), true, false);
}
}
}
/**
* Debug visualisation of graph edges between regions.
*/
void HierarchicalPathfinder::AddDebugEdges(pass_class_t passClass)
{
const EdgesMap& edges = m_Edges[passClass];
const std::vector& chunks = m_Chunks[passClass];
for (auto& edge : edges)
{
for (const RegionID& region: edge.second)
{
// Draw a line between the two regions' centers
int i0, j0, i1, j1;
chunks[edge.first.cj * m_ChunksW + edge.first.ci].RegionCenter(edge.first.r, i0, j0);
chunks[region.cj * m_ChunksW + region.ci].RegionCenter(region.r, i1, j1);
CFixedVector2D a, b;
Pathfinding::NavcellCenter(i0, j0, a.X, a.Y);
Pathfinding::NavcellCenter(i1, j1, b.X, b.Y);
// Push the endpoints inwards a little to avoid overlaps
CFixedVector2D d = b - a;
d.Normalize(entity_pos_t::FromInt(1));
a += d;
b -= d;
std::vector xz;
xz.push_back(a.X.ToFloat());
xz.push_back(a.Y.ToFloat());
xz.push_back(b.X.ToFloat());
xz.push_back(b.Y.ToFloat());
m_DebugOverlayLines.emplace_back();
m_DebugOverlayLines.back().m_Color = CColor(1.0, 1.0, 1.0, 1.0);
SimRender::ConstructLineOnGround(*m_SimContext, xz, m_DebugOverlayLines.back(), true);
}
}
}
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.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
{
return GetGlobalRegion(Get(i, j, passClass), passClass);
}
HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(RegionID region, pass_class_t passClass) const
{
return region.r == 0 ? GlobalRegionID(0) : m_GlobalRegions.at(passClass).at(region);
}
void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)
{
PathGoal newGoal;
newGoal.type = PathGoal::POINT;
Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z);
goal = newGoal;
}
bool HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const
{
PROFILE2("MakeGoalReachable");
u16 iGoal, jGoal;
Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
std::set goalRegions(SortByBestToPoint(i0, j0));
// This returns goal regions ordered by distance from the best navcell in each region.
FindGoalRegionsAndBestNavcells(i0, j0, iGoal, jGoal, goal, goalRegions, passClass);
// Because of the sorting above, we can stop as soon as the first reachable goal region is found.
for (const InterestingRegion& region : goalRegions)
if (GetGlobalRegion(region.region, passClass) == GetGlobalRegion(i0, j0, passClass))
{
iGoal = region.bestI;
jGoal = region.bestJ;
// No need to move reachable point goals.
if (goal.type != PathGoal::POINT)
CreatePointGoalAt(iGoal, jGoal, goal);
return true;
}
// Goal wasn't reachable - get the closest navcell in the nearest reachable region.
- std::set reachableRegions(SortByCenterToPoint(i0, j0));
+ std::set reachableRegions(SortByCenterToPoint(iGoal, jGoal));
FindReachableRegions(Get(i0, j0, passClass), reachableRegions, passClass);
FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass);
CreatePointGoalAt(iGoal, jGoal, goal);
return false;
}
void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const
{
std::set regions(SortByCenterToPoint(i, j));
// Construct a set of all regions of all chunks for this pass class
for (const Chunk& chunk : m_Chunks.at(passClass))
for (int r : chunk.m_RegionsID)
regions.insert(RegionID(chunk.m_ChunkI, chunk.m_ChunkJ, r));
FindNearestNavcellInRegions(regions, i, j, passClass);
}
void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const
{
u16 bestI = iGoal, bestJ = jGoal; // Somewhat sensible default-values should regions() be passed empty.
u32 bestDist = std::numeric_limits::max();
// Because regions are sorted by increasing distance, we can ignore regions that are obviously farther than the current best point.
// Since regions are squares, that happens when the center of a region is at least √2 * CHUNK_SIZE farther than the current best point.
// Add one to avoid cases where the center navcell is actually slightly off-center (= CHUNK_SIZE is even)
u32 maxDistFromBest = (fixed::FromInt(3) / 2 * CHUNK_SIZE).ToInt_RoundToInfinity() + 1;
+ // TODO: update to static_assert with constexpr
ENSURE(maxDistFromBest < std::numeric_limits::max());
maxDistFromBest *= maxDistFromBest;
for (const RegionID& region : regions)
{
u32 chunkDist = region.DistanceTo(iGoal, jGoal);
// This might overflow, but only if we are already close to the maximal possible distance, so the condition would probably be false anyways.
+ // It's also a bit pessimistic, so we'll still consider a few too many regions.
if (bestDist < std::numeric_limits::max() && chunkDist > maxDistFromBest + bestDist)
break; // Break, the set is ordered by increased distance so a closer region will not be found.
int ri, rj;
u32 dist;
GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, ri, rj, dist);
if (dist < bestDist)
{
bestI = ri;
bestJ = rj;
bestDist = dist;
}
}
iGoal = bestI;
jGoal = bestJ;
}
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::FindGoalRegionsAndBestNavcells(u16 i0, u16 j0, u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const
{
if (goal.type == PathGoal::POINT)
{
RegionID region = Get(gi, gj, passClass);
if (region.r > 0)
regions.insert({region, gi, gj});
return;
}
// 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 bestI, bestJ;
u32 c; // Unused.
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, i0, j0, goal, bestI, bestJ, c))
regions.insert({RegionID{sx, sz, i}, bestI, bestJ});
}
}
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.at(passClass)[region.cj * m_ChunksW + region.ci];
for (int j = 0; j < CHUNK_SIZE; ++j)
for (int i = 0; i < CHUNK_SIZE; ++i)
if (c.m_Regions[j][i] == region.r)
grid.set(i0 + i, j0 + j, value);
}
Grid HierarchicalPathfinder::GetConnectivityGrid(pass_class_t passClass) const
{
Grid connectivityGrid(m_W, m_H);
connectivityGrid.reset();
u16 idx = 1;
for (size_t i = 0; i < m_W; ++i)
{
for (size_t j = 0; j < m_H; ++j)
{
if (connectivityGrid.get(i, j) != 0)
continue;
RegionID from = Get(i, j, passClass);
if (from.r == 0)
continue;
std::set reachable;
FindReachableRegions(from, reachable, passClass);
for (const RegionID& region : reachable)
FillRegionOnGrid(region, passClass, idx, connectivityGrid);
++idx;
}
}
return connectivityGrid;
}