diff --git a/grid/Grid.h b/grid/Grid.h index 4b9a9ed..4a789c1 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -14,20 +14,21 @@ #include "../misc/Debug.h" /** - * grid of the given grid-size, storing some value which - * extends GridPoint and GridNode + * grid of a given-size, storing some user-data-value which + * - extends GridPoint and GridNode * * Usage: * for (Node& n : grid) {...} * for (Node& n2 : grid.neighbors(n)) {...} * */ -template class Grid { +template class Grid { static constexpr const char* name = "Grid"; #include "GridNeighborIterator.h" + /** UID for nodes */ typedef uint64_t UID; private: @@ -38,10 +39,13 @@ private: /** UID -> index mapping */ std::unordered_map hashes; + /** the user-given grid-size */ + const int gridSize_cm; + public: - /** ctor */ - Grid() { + /** ctor with the grid's size (in cm) */ + Grid(const int gridSize_cm) : gridSize_cm(gridSize_cm) { static_assert((sizeof(T::_idx) > 0), "T must inherit from GridNode!"); static_assert((sizeof(T::x_cm) > 0), "T must inherit from GridPoint!"); } @@ -52,12 +56,15 @@ public: /** no-assign */ void operator = (const Grid& o) = delete; + /** allows for-each iteration over all included nodes */ decltype(nodes.begin()) begin() {return nodes.begin();} /** allows for-each iteration over all included nodes */ decltype(nodes.end()) end() {return nodes.end();} + /** get the grid's size */ + int getGridSize_cm() const {return gridSize_cm;} /** * add the given element to the grid. diff --git a/grid/GridNeighborIterator.h b/grid/GridNeighborIterator.h index a3ffdf4..0e7d3f7 100644 --- a/grid/GridNeighborIterator.h +++ b/grid/GridNeighborIterator.h @@ -8,7 +8,7 @@ class NeighborIter : std::iterator { private: /** the grid the src-node belongs to */ - Grid* grid; + Grid* grid; /** index of the source-node within its grid */ int srcNodeIdx; @@ -19,8 +19,8 @@ private: public: /** ctor */ - NeighborIter(const Grid& grid, const int srcNodeIdx, const int nIdx) : - grid((Grid*)&grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;} + NeighborIter(const Grid& grid, const int srcNodeIdx, const int nIdx) : + grid((Grid*)&grid), srcNodeIdx(srcNodeIdx), nIdx(nIdx) {;} /** next neighbor */ NeighborIter& operator++() {++nIdx; return *this;} @@ -44,7 +44,7 @@ class NeighborForEach { private: /** the grid the src-node belongs to */ - const Grid& grid; + const Grid& grid; /** index of the source-node within its grid */ const int srcNodeIdx; @@ -52,7 +52,7 @@ private: public: /** ctor */ - NeighborForEach(const Grid& grid, const int srcNodeIdx) : + NeighborForEach(const Grid& grid, const int srcNodeIdx) : grid(grid), srcNodeIdx(srcNodeIdx) {;} /** starting point */ diff --git a/grid/GridNode.h b/grid/GridNode.h index a4529cd..76c172f 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -4,7 +4,8 @@ #include "GridNodeBBox.h" #include "GridPoint.h" -template class Grid; +/** forward decl. */ +template class Grid; /** @@ -17,7 +18,8 @@ struct GridNode { private: - template friend class Grid; + /** grant full access to the grid */ + template friend class Grid; /** INTERNAL: array-index */ int _idx; diff --git a/grid/factory/GridFactory.h b/grid/factory/GridFactory.h index 92bc4aa..44d97a7 100755 --- a/grid/factory/GridFactory.h +++ b/grid/factory/GridFactory.h @@ -11,7 +11,7 @@ #include "../../misc/Debug.h" -template class GridFactory { +template class GridFactory { /** logging name */ static constexpr const char* name = "GridFac"; @@ -19,19 +19,21 @@ template class GridFactory { private: /** the grid to build into */ - Grid& grid; + Grid& grid; public: /** ctor with the grid to fill */ - GridFactory(Grid& grid) : grid(grid) {;} + GridFactory(Grid& grid) : grid(grid) {;} /** add the given floor at the provided height (in cm) */ void addFloor(const Floor& floor, const float z_cm) { Log::add(name, "adding floor at height " + std::to_string(z_cm)); + const float gridSize_cm = grid.getGridSize_cm(); + // build grid-points for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) { for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) { @@ -55,6 +57,8 @@ public: Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm)); + const int gridSize_cm = grid.getGridSize_cm(); + // connect adjacent grid-points for (int idx = 0; idx < grid.getNumNodes(); ++idx) { @@ -132,6 +136,8 @@ public: int idx2 = -1; const int idx3 = n2.getIdx(); + const int gridSize_cm = grid.getGridSize_cm(); + // move upards in gridSize steps for (int z = gridSize_cm; z < zDiff; z+= gridSize_cm) { @@ -161,11 +167,13 @@ public: } /** add the inverted version of the given z-layer */ - void addInverted(const Grid& gIn, const float z_cm) { + void addInverted(const Grid& gIn, const float z_cm) { // get the original grid's bbox BBox3 bb = gIn.getBBox(); + const int gridSize_cm = grid.getGridSize_cm(); + // build new grid-points for(int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) { for (int y_cm = bb.getMin().y; y_cm < bb.getMax().y; y_cm += gridSize_cm) { diff --git a/grid/factory/GridImportance.h b/grid/factory/GridImportance.h index 0fccf33..044d0a3 100644 --- a/grid/factory/GridImportance.h +++ b/grid/factory/GridImportance.h @@ -31,25 +31,26 @@ private: public: /** attach importance-factors to the grid */ - template void addImportance(Grid& g, const float z_cm) { + template void addImportance(Grid& g, const float z_cm) { Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm)); // get an inverted version of the grid - Grid inv; - GridFactory fac(inv); + Grid inv(g.getGridSize_cm()); + GridFactory fac(inv); fac.addInverted(g, z_cm); // construct KNN search - KNN, 3> knn(inv); + KNN, 3> knn(inv); // the number of neighbors to use static constexpr int numNeighbors = 8; - for (int idx = 0; idx < g.getNumNodes(); ++idx) { + // create list of all doors + std::vector doors; - // process each point - T& n1 = (T&) g[idx]; + // process each node + for (T& n1 : g) { // get the 10 nearest neighbors and their distance size_t indices[numNeighbors]; @@ -64,14 +65,51 @@ public: } addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) ); - addDoor(n1, neighbors); + //addDoor(n1, neighbors); + + // is the current node a door? + if (isDoor(n1, neighbors)) {doors.push_back(n1);} + + // favor stairs just like doors + if (isStaircase(g, n1)) {doors.push_back(n1);} } + KNNArray> knnArrDoors(doors); + KNN>, 3> knnDoors(knnArrDoors); + + // process each node again + for (T& n1 : g) { + + static K::NormalDistribution favorDoors(0.0, 0.6); + + // get the distance to the nearest door + const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} )); + + // importance for this node (based on the distance from the next door) + const float imp = 1.0 + favorDoors.getProbability(dist_m) * 0.35; + + // adjust + n1.imp *= imp; + + } + + + } + + /** is the given node connected to a staircase? */ + template bool isStaircase(Grid& g, T& node) { + + // if this node has a neighbor with a different z, this is a stair + for (T& neighbor : g.neighbors(node)) { + if (neighbor.z_cm != node.z_cm) {return true;} + } + return false; + } /** attach importance-factors to the grid */ - template void addDistanceToTarget(Grid& g, Dijkstra& d) { + template void addDistanceToTarget(Grid& g, Dijkstra& d) { //Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm)); @@ -87,7 +125,7 @@ public: } - template void addImportance(Grid& g, DijkstraNode* start, DijkstraNode* end) { + template void addImportance(Grid& g, DijkstraNode* start, DijkstraNode* end) { // routing path DijkstraPath path(end, start); @@ -112,8 +150,8 @@ public: } - /** add importance to nSrc if it is part of a door */ - template void addDoor( T& nSrc, std::vector neighbors ) { + /** is the given node (and its inverted neighbors) a door? */ + template bool isDoor( T& nSrc, std::vector neighbors ) { MiniMat2 m; Point3 center = nSrc; @@ -126,7 +164,7 @@ public: centroid /= neighbors.size(); // if nSrc is too far from the centroid, this does not make sense - if ((centroid-center).length() > 60) {return;} + if ((centroid-center).length() > 20) {return false;} // build covariance of the nearest-neighbors int used = 0; @@ -138,7 +176,7 @@ public: } // we need at least two points for the covariance - if (used < 2) {return;} + if (used < 2) {return false;} // check eigenvalues MiniMat2::EV ev = m.getEigenvalues(); @@ -147,7 +185,7 @@ public: if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);} // door? - if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.3; } + return ((ev.e2/ev.e1) < 0.15) ; } diff --git a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h index 56b33b2..8580fb4 100644 --- a/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h +++ b/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h @@ -41,7 +41,7 @@ private: public: /** ctor with the target you want to reach */ - template GridWalkLightAtTheEndOfTheTunnel(Grid& grid, const Access& acc, const T& target) { + template GridWalkLightAtTheEndOfTheTunnel(Grid& grid, const Access& acc, const T& target) { // build all shortest path to reach th target dijkstra.build(target, target, acc); @@ -60,7 +60,7 @@ public: } - template GridWalkState getDestination(Grid& grid, GridWalkState start, float distance_m) { + GridWalkState getDestination(Grid& grid, GridWalkState start, float distance_m) { int retries = 2; GridWalkState res; @@ -84,7 +84,7 @@ public: private: - template GridWalkState walk(Grid& grid, GridWalkState cur, float distRest_m) { + GridWalkState walk(Grid& grid, GridWalkState cur, float distRest_m) { drawer.reset();; diff --git a/grid/walk/GridWalkWeighted.h b/grid/walk/GridWalkWeighted.h index f38d0a8..7c12a1b 100644 --- a/grid/walk/GridWalkWeighted.h +++ b/grid/walk/GridWalkWeighted.h @@ -45,7 +45,7 @@ private: public: - template State getDestination(Grid& grid, State start, float distance_m) { + template State getDestination(Grid& grid, State start, float distance_m) { int retries = 2; State res; @@ -73,7 +73,7 @@ private: return Heading(from.x_cm, from.y_cm, to.x_cm, to.y_cm); } - template State walk(Grid& grid, State cur, float distRest_m) { + State walk(Grid& grid, State cur, float distRest_m) { drawer.reset();; diff --git a/main.cpp b/main.cpp index 1a20c74..c04b597 100755 --- a/main.cpp +++ b/main.cpp @@ -15,7 +15,9 @@ int main(int argc, char** argv) { #ifdef WITH_TESTS ::testing::InitGoogleTest(&argc, argv); + //::testing::GTEST_FLAG(filter) = "*Importance*"; ::testing::GTEST_FLAG(filter) = "*Walk*"; + return RUN_ALL_TESTS(); #endif diff --git a/tests/grid/GridImportance.cpp b/tests/grid/GridImportance.cpp index d0ffeec..908bea1 100644 --- a/tests/grid/GridImportance.cpp +++ b/tests/grid/GridImportance.cpp @@ -11,8 +11,8 @@ TEST(GridImportance, a) { - Grid<20, GP> g; - GridFactory<20, GP> gf(g); + Grid g(20); + GridFactory gf(g); FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6); Floor f1 = fpf.getFloor("1"); diff --git a/tests/grid/Plot.h b/tests/grid/Plot.h index 6d29a8a..d7acf38 100644 --- a/tests/grid/Plot.h +++ b/tests/grid/Plot.h @@ -51,13 +51,13 @@ public: } - template Plot& showGrid(Grid& g) { + template Plot& showGrid(Grid& g) { addEdges(g); addNodes(g); return *this; } - template Plot& addEdges(Grid& g) { + template Plot& addEdges(Grid& g) { // prevent adding edges twice std::set done; @@ -78,7 +78,7 @@ public: } - template Plot& addNodes(Grid& g) { + template Plot& addNodes(Grid& g) { for (GP& n1 : g) { K::GnuplotPoint3 p1(n1.x_cm, n1.y_cm, n1.z_cm); @@ -89,7 +89,7 @@ public: } - template Plot& build(Grid& g) { + template Plot& build(Grid& g) { std::set done; diff --git a/tests/grid/TestAll.cpp b/tests/grid/TestAll.cpp index ddea5ec..ca83c00 100644 --- a/tests/grid/TestAll.cpp +++ b/tests/grid/TestAll.cpp @@ -13,13 +13,13 @@ TEST(TestAll, Nav) { - Grid<20, GP> g; + Grid g(20); // dijkstra mapper class TMP { - Grid<20, GP>& grid; + Grid& grid; public: - TMP(Grid<20, GP>& grid) : grid(grid) {;} + TMP(Grid& grid) : grid(grid) {;} int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();} const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);} float getWeightBetween(const GP& n1, const GP& n2) const { @@ -29,7 +29,7 @@ TEST(TestAll, Nav) { } } tmp(g); - GridFactory<20, GP> gf(g); + GridFactory gf(g); // load floorplan FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6); diff --git a/tests/grid/TestGrid.cpp b/tests/grid/TestGrid.cpp index aed5222..c1b00d4 100755 --- a/tests/grid/TestGrid.cpp +++ b/tests/grid/TestGrid.cpp @@ -8,7 +8,7 @@ TEST(Grid, add) { - Grid<20, GP> grid; + Grid grid(20); ASSERT_EQ(0, grid.add(GP())); ASSERT_EQ(1, grid.add(GP())); ASSERT_EQ(2, grid.add(GP())); @@ -28,7 +28,7 @@ TEST(Grid, add) { TEST(Grid, BBox) { - Grid<20, GP> grid; + Grid grid(20); int idx = grid.add(GP(40,40,40)); ASSERT_EQ(30, grid.getBBox(idx).getMin().x); ASSERT_EQ(50, grid.getBBox(idx).getMax().x); @@ -40,7 +40,7 @@ TEST(Grid, BBox) { TEST(Grid, connectBiDir) { - Grid<1, GP> grid; + Grid grid(1); int idx1 = grid.add(GP( 0, 0, 0)); int idx2 = grid.add(GP( 0, 1, 0)); @@ -68,7 +68,7 @@ TEST(Grid, connectBiDir) { TEST(Grid, disconnectBiDir) { - Grid<1, GP> grid; + Grid grid(1); int idx1 = grid.add(GP( 0, 0, 0)); int idx2 = grid.add(GP( 0, 1, 0)); @@ -111,7 +111,7 @@ TEST(Grid, disconnectBiDir) { TEST(Grid, uid) { - Grid<20, GP> grid; + Grid grid(20); GP gp(20,40,60); uint64_t uid = grid.getUID(gp); @@ -125,7 +125,7 @@ TEST(Grid, uid) { TEST(Grid, remove) { - Grid<1, GP> grid; + Grid grid(1); GP gp1( 0, 0, 0); GP gp2( 0, 1, 0); @@ -162,7 +162,7 @@ TEST(Grid, remove) { TEST(Grid, neighborIter) { - Grid<1, GP> grid; + Grid grid(1); int idx1 = grid.add(GP( 0, 0, 0)); int idx2 = grid.add(GP( 0, 1, 0)); @@ -183,7 +183,7 @@ TEST(Grid, neighborIter) { TEST(Grid, bbox) { - Grid<1, GP> grid; + Grid grid(1); grid.add(GP( 0, 0, 0)); grid.add(GP( 0, 1, 0)); @@ -206,7 +206,7 @@ TEST(Grid, bbox) { TEST(Grid, nearest) { - Grid<20, GP> grid; + Grid grid(20); GP c1(20,20,20); GP c2(40,40,40); diff --git a/tests/grid/TestGridFactory.cpp b/tests/grid/TestGridFactory.cpp index 744e784..dde31be 100755 --- a/tests/grid/TestGridFactory.cpp +++ b/tests/grid/TestGridFactory.cpp @@ -10,10 +10,10 @@ TEST(GridFactory, create) { - Grid<20, GP> g; - Grid<20, GP> gInv; + Grid g(20); + Grid gInv(20); - GridFactory<20, GP> gf(g); + GridFactory gf(g); FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 4); Floor f1 = fpf.getFloor("1"); Floor f2 = fpf.getFloor("2"); @@ -24,7 +24,7 @@ TEST(GridFactory, create) { gf.addStairs(s1_2, 20, 340); gf.removeIsolated(); - GridFactory<20, GP> gfInv(gInv); + GridFactory gfInv(gInv); gfInv.addInverted(g, 20); gfInv.addInverted(g, 340); diff --git a/tests/grid/TestImportance.cpp b/tests/grid/TestImportance.cpp new file mode 100644 index 0000000..cd91aa6 --- /dev/null +++ b/tests/grid/TestImportance.cpp @@ -0,0 +1,44 @@ +#ifdef WITH_TESTS + +#include "../Tests.h" + +#include "../../grid/factory/GridImportance.h" +#include "../../grid/factory/GridFactory.h" +#include "../../floorplan/FloorplanFactorySVG.h" +#include "../../nav/dijkstra/Dijkstra.h" +#include "../../grid/walk/GridWalkWeighted.h" + +#include "Plot.h" + +TEST(Importance, Doors) { + + + Grid g(20); + GridFactory gf(g); + + // load floorplan + FloorplanFactorySVG fpf(getDataFile("fp1.svg"), 6); + Floor f1 = fpf.getFloor("1"); + Floor f2 = fpf.getFloor("2"); + Stairs s1_2 = fpf.getStairs("1_2"); + + // build the grid + gf.addFloor(f1, 20); + //gf.addFloor(f2, 340); + gf.addStairs(s1_2, 20, 340); + gf.removeIsolated(); + + // calculate node importance based on the floorplan (walls, ...) + GridImportance gi; + gi.addImportance(g, 20); + + + Plot p; + p.gp << "set view 0,0\n"; + p.build(g); + p.addFloor(f1, 20); + p.fire(); + +} + +#endif diff --git a/tests/grid/TestWalk.cpp b/tests/grid/TestWalk.cpp index 19e7455..efedae9 100644 --- a/tests/grid/TestWalk.cpp +++ b/tests/grid/TestWalk.cpp @@ -14,8 +14,8 @@ TEST(Walk, plot) { - Grid<20, GP> g; - GridFactory<20, GP> gf(g); + Grid g(20); + GridFactory gf(g); bool use3D = true; @@ -40,9 +40,9 @@ TEST(Walk, plot) { // dijkstra mapper class TMP { - Grid<20, GP>& grid; + Grid& grid; public: - TMP(Grid<20, GP>& grid) : grid(grid) {;} + TMP(Grid& grid) : grid(grid) {;} int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();} const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);} float getWeightBetween(const GP& n1, const GP& n2) const { diff --git a/tests/nav/dijkstra/TestDijkstra.cpp b/tests/nav/dijkstra/TestDijkstra.cpp index 21cf18d..ec19753 100644 --- a/tests/nav/dijkstra/TestDijkstra.cpp +++ b/tests/nav/dijkstra/TestDijkstra.cpp @@ -8,7 +8,7 @@ TEST(Dijkstra, build) { - Grid<1, GP> grid; + Grid grid(1); int idx1 = grid.add(GP( 0, 0, 0)); int idx2 = grid.add(GP( 0, 1, 0)); @@ -22,9 +22,9 @@ TEST(Dijkstra, build) { grid.connectBiDir(idx1, idx5); class TMP { - Grid<1, GP>& grid; + Grid& grid; public: - TMP(Grid<1, GP>& grid) : grid(grid) {;} + TMP(Grid& grid) : grid(grid) {;} int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();} const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);} float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}