Index: ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h
===================================================================
--- ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h (revision 22652)
+++ ps/trunk/source/simulation2/components/tests/test_HierPathfinder.h (revision 22653)
@@ -1,480 +1,509 @@
/* 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,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_},
{_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_,_}
};
#undef _
#undef X
// upscaled 5 times
HierarchicalPathfinder hierPath;
Grid grid(40*5, 40*5);
Grid dirtyGrid(40*5, 40*5);
for (size_t i = 0; i < 40; ++i)
for (size_t j = 0; j < 40; ++j)
for (size_t ii = 0; ii < 5; ++ii)
for (size_t jj = 0; jj < 5; ++jj)
grid.set(i * 5 + ii, j * 5 + jj, gridDef[i][j]);
hierPath.Recompute(&grid, nonPathClassMask, pathClassMask);
u16 i = 5, j = 5;
hierPath.FindNearestPassableNavcell(i, j, PASS_1);
TS_ASSERT(i == 5 && j == 5);
// 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); \
}
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);
#undef check_closest_passable
PathGoal goal;
goal.type = PathGoal::POINT;
// from the left of the C, goal is unreachable, expect closest navcell to goal
oi = 5 * 5 + 3; oj = 3 * 5 + 3;
pi = 5 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
hierPath.FindNearestPassableNavcell(pi, pj, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// random reachable point.
oi = 5 * 5 + 3; oj = 3 * 5 + 3;
pi = 26 * 5 + 3; pj = 5 * 5 + 2; goal.x = fixed::FromInt(pi) + fixed::FromInt(1)/3; goal.z = fixed::FromInt(pj) + fixed::FromInt(1)/3;
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// top-left corner
goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// near bottom-right corner
goal.x = fixed::FromInt(pi) + fixed::FromInt(3)/4; goal.z = fixed::FromInt(pj) + fixed::FromInt(3)/4;
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// Circle
goal.type = PathGoal::CIRCLE;
goal.hw = fixed::FromInt(1) / 2;
// from the left of the C, goal is unreachable, expect closest navcell to goal
oi = 5 * 5 + 3; oj = 3 * 5 + 3;
pi = 5 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
hierPath.FindNearestPassableNavcell(pi, pj, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// same position, goal is reachable, expect closest navcell to goal
goal.hw = fixed::FromInt(3);
goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
hierPath.FindNearestPassableNavcell(pi, pj, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// Square
goal.type = PathGoal::SQUARE;
goal.hw = fixed::FromInt(1) / 2;
goal.hh = fixed::FromInt(1) / 2;
// from the left of the C, goal is unreachable, expect closest navcell to goal
oi = 5 * 5 + 3; oj = 3 * 5 + 3;
pi = 5 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
hierPath.FindNearestPassableNavcell(pi, pj, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// same position, goal is reachable, expect closest navcell to goal
goal.hw = fixed::FromInt(3);
goal.hh = fixed::FromInt(3);
goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
hierPath.FindNearestPassableNavcell(pi, pj, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// Goal is reachable diagonally (1 cell away)
goal.hw = fixed::FromInt(1);
goal.hh = fixed::FromInt(1);
oi = 5 * 5 + 3; oj = 3 * 5 + 3;
pi = 5 * 5 - 1; pj = 7 * 5 + 3; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
hierPath.FindNearestPassableNavcell(pi, pj, PASS_1);
TS_ASSERT(pi == goal.x.ToInt_RoundToNegInfinity() && pj == goal.z.ToInt_RoundToNegInfinity());
// Huge Circle goal, expect point closest to us.
goal.type = PathGoal::CIRCLE;
goal.hw = fixed::FromInt(20);
oi = 5 * 5 + 3; oj = 3 * 5 + 3;
pi = 36 * 5 + 3; pj = 7 * 5 + 2; goal.x = fixed::FromInt(pi); goal.z = fixed::FromInt(pj);
hierPath.MakeGoalReachable(oi, oj, goal, PASS_1);
// bit of leeway for cell placement
TS_ASSERT(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 22652)
+++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.cpp (revision 22653)
@@ -1,803 +1,898 @@
/* 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
+ // 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 ID
+ // 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);
}
-void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const
+HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const
{
- PROFILE2("MakeGoalReachable");
- RegionID source = Get(i0, j0, passClass);
+ return GetGlobalRegion(Get(i, j, passClass), passClass);
+}
- // Find everywhere that's reachable
- std::set reachableRegions;
- FindReachableRegions(source, reachableRegions, passClass);
+HierarchicalPathfinder::GlobalRegionID HierarchicalPathfinder::GetGlobalRegion(RegionID region, pass_class_t passClass) const
+{
+ return region.r == 0 ? GlobalRegionID(0) : m_GlobalRegions.at(passClass).at(region);
+}
- // Check whether any reachable region contains the goal
- // And get the navcell that's the closest to the point
+void CreatePointGoalAt(u16 i, u16 j, PathGoal& goal)
+{
+ PathGoal newGoal;
+ newGoal.type = PathGoal::POINT;
+ Pathfinding::NavcellCenter(i, j, newGoal.x, newGoal.z);
+ goal = newGoal;
+}
- u16 bestI = 0;
- u16 bestJ = 0;
- u32 dist2Best = std::numeric_limits::max();
+void HierarchicalPathfinder::MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const
+{
+ PROFILE2("MakeGoalReachable");
- for (const RegionID& region : reachableRegions)
- {
- // Skip region if its chunk doesn't contain the goal area
- entity_pos_t x0 = Pathfinding::NAVCELL_SIZE * (region.ci * CHUNK_SIZE);
- entity_pos_t z0 = Pathfinding::NAVCELL_SIZE * (region.cj * CHUNK_SIZE);
- entity_pos_t x1 = x0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE;
- entity_pos_t z1 = z0 + Pathfinding::NAVCELL_SIZE * CHUNK_SIZE;
- if (!goal.RectContainsGoal(x0, z0, x1, z1))
- continue;
+ u16 iGoal, jGoal;
+ Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
- u16 i,j;
- u32 dist2;
+ bool reachable = false;
+ std::set goalRegions;
+ FindGoalRegions(iGoal, jGoal, goal, goalRegions, passClass);
- // If the region contains the goal area, the goal is reachable
- // Remember the best point for optimization.
- if (GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, i, j, dist2))
- {
- // If it's a point, no need to move it, we're done
- if (goal.type == PathGoal::POINT)
- return;
- if (dist2 < dist2Best)
- {
- bestI = i;
- bestJ = j;
- dist2Best = dist2;
- }
+ // Add all reachable goal regions to the set of regions we want to look at.
+ std::set interestingRegions;
+ for (const RegionID& r : goalRegions)
+ if (GetGlobalRegion(r, passClass) == GetGlobalRegion(i0, j0, passClass))
+ {
+ reachable = true;
+ interestingRegions.insert(r);
}
- }
- // If the goal area wasn't reachable,
- // find the navcell that's nearest to the goal's center
- if (dist2Best == std::numeric_limits::max())
- {
- u16 iGoal, jGoal;
- Pathfinding::NearestNavcell(goal.x, goal.z, iGoal, jGoal, m_W, m_H);
-
- FindNearestNavcellInRegions(reachableRegions, iGoal, jGoal, passClass);
-
- // Construct a new point goal at the nearest reachable navcell
- PathGoal newGoal;
- newGoal.type = PathGoal::POINT;
- Pathfinding::NavcellCenter(iGoal, jGoal, newGoal.x, newGoal.z);
- goal = newGoal;
+ // If the goal is unreachable, expand to all reachable regions.
+ if (!reachable)
+ {
+ FindReachableRegions(Get(i0, j0, passClass), interestingRegions, passClass);
+ FindNearestNavcellInRegions(interestingRegions, iGoal, jGoal, passClass);
+ CreatePointGoalAt(iGoal, jGoal, goal);
return;
}
- ENSURE(dist2Best != std::numeric_limits::max());
- PathGoal newGoal;
- newGoal.type = PathGoal::POINT;
- Pathfinding::NavcellCenter(bestI, bestJ, newGoal.x, newGoal.z);
- goal = newGoal;
+ if (goal.type == PathGoal::POINT)
+ return; // Reachable point goal, no need to move it.
+
+ u16 bestI = 0, bestJ = 0;
+ u32 dist2Best = std::numeric_limits::max();
+
+ // Test each reachable goal region and return the navcell closest to the cneter.
+ for (const RegionID& region : interestingRegions)
+ {
+ u16 ri, rj;
+ u32 dist;
+ GetChunk(region.ci, region.cj, passClass).RegionNearestNavcellInGoal(region.r, i0, j0, goal, ri, rj, dist);
+ if (dist < dist2Best)
+ {
+ bestI = ri;
+ bestJ = rj;
+ dist2Best = dist;
+ }
+ }
+ return CreatePointGoalAt(bestI, bestJ, goal);
}
void HierarchicalPathfinder::FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const
{
std::set regions;
FindPassableRegions(regions, passClass);
FindNearestNavcellInRegions(regions, i, j, passClass);
}
void HierarchicalPathfinder::FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const
{
// Find the navcell in the given regions that's nearest to the goal navcell:
// * For each region, record the (squared) minimal distance to the goal point
// * Sort regions by that underestimated distance
// * For each region, find the actual nearest navcell
// * Stop when the underestimated distances are worse than the best real distance
std::vector > regionDistEsts; // pair of (distance^2, region)
for (const RegionID& region : regions)
{
int i0 = region.ci * CHUNK_SIZE;
int j0 = region.cj * CHUNK_SIZE;
int i1 = i0 + CHUNK_SIZE - 1;
int j1 = j0 + CHUNK_SIZE - 1;
// Pick the point in the chunk nearest the goal
int iNear = Clamp((int)iGoal, i0, i1);
int jNear = Clamp((int)jGoal, j0, j1);
int dist2 = (iNear - iGoal)*(iNear - iGoal)
+ (jNear - jGoal)*(jNear - jGoal);
regionDistEsts.emplace_back(dist2, region);
}
// Sort by increasing distance (tie-break on RegionID)
std::sort(regionDistEsts.begin(), regionDistEsts.end());
int iBest = iGoal;
int jBest = jGoal;
u32 dist2Best = std::numeric_limits::max();
for (auto& pair : regionDistEsts)
{
if (pair.first >= dist2Best)
break;
RegionID region = pair.second;
int i, j;
u32 dist2;
GetChunk(region.ci, region.cj, passClass).RegionNavcellNearest(region.r, iGoal, jGoal, i, j, dist2);
if (dist2 < dist2Best)
{
iBest = i;
jBest = j;
dist2Best = dist2;
}
}
iGoal = iBest;
jGoal = jBest;
}
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::FindPassableRegions(std::set& regions, pass_class_t passClass) const
{
// 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));
}
+void HierarchicalPathfinder::FindGoalRegions(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);
+ 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 a,b; u32 c; // unused params for RegionNearestNavcellInGoal
+
+ for (u8 sz = std::max(0,(gj - size) / CHUNK_SIZE); sz <= std::min(m_ChunksH-1, (gj + size + 1) / CHUNK_SIZE); ++sz)
+ for (u8 sx = std::max(0,(gi - size) / CHUNK_SIZE); sx <= std::min(m_ChunksW-1, (gi + size + 1) / CHUNK_SIZE); ++sx)
+ {
+ const Chunk& chunk = GetChunk(sx, sz, passClass);
+ for (u16 i : chunk.m_RegionsID)
+ if (chunk.RegionNearestNavcellInGoal(i, 0, 0, goal, a, b, c))
+ regions.insert(RegionID{sx, sz, i});
+ }
+}
+
void HierarchicalPathfinder::FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) 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;
}
Index: ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h
===================================================================
--- ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h (revision 22652)
+++ ps/trunk/source/simulation2/helpers/HierarchicalPathfinder.h (revision 22653)
@@ -1,245 +1,260 @@
/* 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 .
*/
#ifndef INCLUDED_HIERPATHFINDER
#define INCLUDED_HIERPATHFINDER
#include "Pathfinding.h"
#include "renderer/TerrainOverlay.h"
#include "Render.h"
#include "graphics/SColor.h"
/**
* Hierarchical pathfinder.
*
* Deals with connectivity (can point A reach point B?).
*
* The navcell-grid representation of the map is split into fixed-size chunks.
* Within a chunk, each maximal set of adjacently-connected passable navcells
* is defined as a region.
* Each region is a vertex in the hierarchical pathfinder's graph.
* When two regions in adjacent chunks are connected by passable navcells,
* the graph contains an edge between the corresponding two vertexes.
- * (There will never be an edge between two regions in the same chunk.)
+ * By design, there can never be an edge between two regions in the same chunk.
+ *
+ * Those fixed-size chunks are used to efficiently compute "global regions" by effectively flood-filling.
+ * Those can then be used to immediately determine if two reachables points are connected.
+ *
+ * The main use of this class is to convert an arbitrary PathGoal to a reachable navcell.
+ * This happens in MakeGoalReachable.
*
- * Since regions are typically fairly large, it is possible to determine
- * connectivity between any two navcells by mapping them onto their appropriate
- * region and then doing a relatively small graph search.
*/
#ifdef TEST
class TestCmpPathfinder;
class TestHierarchicalPathfinder;
#endif
class HierarchicalOverlay;
class SceneCollector;
class HierarchicalPathfinder
{
#ifdef TEST
friend class TestCmpPathfinder;
friend class TestHierarchicalPathfinder;
#endif
public:
+ typedef u32 GlobalRegionID;
+
struct RegionID
{
u8 ci, cj; // chunk ID
u16 r; // unique-per-chunk local region ID
RegionID(u8 ci, u8 cj, u16 r) : ci(ci), cj(cj), r(r) { }
bool operator<(const RegionID& b) const
{
// Sort by chunk ID, then by per-chunk region ID
if (ci < b.ci)
return true;
if (b.ci < ci)
return false;
if (cj < b.cj)
return true;
if (b.cj < cj)
return false;
return r < b.r;
}
bool operator==(const RegionID& b) const
{
return ((ci == b.ci) && (cj == b.cj) && (r == b.r));
}
};
HierarchicalPathfinder();
~HierarchicalPathfinder();
void SetDebugOverlay(bool enabled, const CSimContext* simContext);
// Non-pathfinding grids will never be recomputed on calling HierarchicalPathfinder::Update
void Recompute(Grid* passabilityGrid,
const std::map& nonPathfindingPassClassMasks,
const std::map& pathfindingPassClassMasks);
void Update(Grid* grid, const Grid& dirtinessGrid);
RegionID Get(u16 i, u16 j, pass_class_t passClass) const;
+ GlobalRegionID GetGlobalRegion(u16 i, u16 j, pass_class_t passClass) const;
+ GlobalRegionID GetGlobalRegion(RegionID region, pass_class_t passClass) const;
+
/**
* Updates @p goal so that it's guaranteed to be reachable from the navcell
* @p i0, @p j0 (which is assumed to be on a passable navcell).
*
* If the goal is not reachable, it is replaced with a point goal nearest to
* the goal center.
*
* In the case of a non-point reachable goal, it is replaced with a point goal
* at the reachable navcell of the goal which is nearest to the starting navcell.
*/
void MakeGoalReachable(u16 i0, u16 j0, PathGoal& goal, pass_class_t passClass) const;
/**
* Updates @p i, @p j (which is assumed to be an impassable navcell)
* to the nearest passable navcell.
*/
void FindNearestPassableNavcell(u16& i, u16& j, pass_class_t passClass) const;
/**
* Generates the connectivity grid associated with the given pass_class
*/
Grid GetConnectivityGrid(pass_class_t passClass) const;
pass_class_t GetPassabilityClass(const std::string& name) const
{
auto it = m_PassClassMasks.find(name);
if (it != m_PassClassMasks.end())
return it->second;
LOGERROR("Invalid passability class name '%s'", name.c_str());
return 0;
}
void RenderSubmit(SceneCollector& collector);
private:
static const u8 CHUNK_SIZE = 96; // number of navcells per side
// TODO: figure out best number. Probably 64 < n < 128
struct Chunk
{
u8 m_ChunkI, m_ChunkJ; // chunk ID
std::vector m_RegionsID; // IDs of local regions, 0 (impassable) excluded
u16 m_Regions[CHUNK_SIZE][CHUNK_SIZE]; // local region ID per navcell
cassert(CHUNK_SIZE*CHUNK_SIZE/2 < 65536); // otherwise we could overflow m_RegionsID with a checkerboard pattern
void InitRegions(int ci, int cj, Grid* grid, pass_class_t passClass);
RegionID Get(int i, int j) const;
void RegionCenter(u16 r, int& i, int& j) const;
void RegionNavcellNearest(u16 r, int iGoal, int jGoal, int& iBest, int& jBest, u32& dist2Best) const;
bool RegionNearestNavcellInGoal(u16 r, u16 i0, u16 j0, const PathGoal& goal, u16& iOut, u16& jOut, u32& dist2Best) const;
#ifdef TEST
bool operator==(const Chunk& b) const
{
return (m_ChunkI == b.m_ChunkI && m_ChunkJ == b.m_ChunkJ && m_RegionsID.size() == b.m_RegionsID.size() && memcmp(&m_Regions, &b.m_Regions, sizeof(u16) * CHUNK_SIZE * CHUNK_SIZE) == 0);
}
#endif
};
typedef std::map > EdgesMap;
void ComputeNeighbors(EdgesMap& edges, Chunk& a, Chunk& b, bool transpose, bool opposite) const;
void RecomputeAllEdges(pass_class_t passClass, EdgesMap& edges);
void UpdateEdges(u8 ci, u8 cj, pass_class_t passClass, EdgesMap& edges);
+ void UpdateGlobalRegions(const std::map >& needNewGlobalRegionMap);
+
void FindReachableRegions(RegionID from, std::set& reachable, pass_class_t passClass) const;
void FindPassableRegions(std::set& regions, pass_class_t passClass) const;
+ void FindGoalRegions(u16 gi, u16 gj, const PathGoal& goal, std::set& regions, pass_class_t passClass) const;
+
/**
* Updates @p iGoal and @p jGoal to the navcell that is the nearest to the
* initial goal coordinates, in one of the given @p regions.
* (Assumes @p regions is non-empty.)
*/
void FindNearestNavcellInRegions(const std::set& regions, u16& iGoal, u16& jGoal, pass_class_t passClass) const;
const Chunk& GetChunk(u8 ci, u8 cj, pass_class_t passClass) const
{
return m_Chunks.at(passClass).at(cj * m_ChunksW + ci);
}
void FillRegionOnGrid(const RegionID& region, pass_class_t passClass, u16 value, Grid& grid) const;
u16 m_W, m_H;
u8 m_ChunksW, m_ChunksH;
std::map > m_Chunks;
std::map m_Edges;
+ std::map > m_GlobalRegions;
+ GlobalRegionID m_NextGlobalRegionID;
+
// Passability classes for which grids will be updated when calling Update
std::map m_PassClassMasks;
void AddDebugEdges(pass_class_t passClass);
HierarchicalOverlay* m_DebugOverlay;
const CSimContext* m_SimContext; // Used for drawing the debug lines
public:
std::vector m_DebugOverlayLines;
};
class HierarchicalOverlay : public TerrainTextureOverlay
{
public:
HierarchicalPathfinder& m_PathfinderHier;
HierarchicalOverlay(HierarchicalPathfinder& pathfinderHier) :
TerrainTextureOverlay(Pathfinding::NAVCELLS_PER_TILE), m_PathfinderHier(pathfinderHier)
{
}
virtual void BuildTextureRGBA(u8* data, size_t w, size_t h)
{
pass_class_t passClass = m_PathfinderHier.GetPassabilityClass("default");
for (size_t j = 0; j < h; ++j)
{
for (size_t i = 0; i < w; ++i)
{
SColor4ub color;
HierarchicalPathfinder::RegionID rid = m_PathfinderHier.Get(i, j, passClass);
if (rid.r == 0)
color = SColor4ub(0, 0, 0, 0);
else if (rid.r == 0xFFFF)
color = SColor4ub(255, 0, 255, 255);
else
color = GetColor(rid.r + rid.ci*5 + rid.cj*7, 127);
*data++ = color.R;
*data++ = color.G;
*data++ = color.B;
*data++ = color.A;
}
}
}
};
#endif // INCLUDED_HIERPATHFINDER