From 34271b5cb7de5691ff3346acd71563f3cfcb80d2 Mon Sep 17 00:00:00 2001 From: kazu Date: Wed, 24 May 2017 17:51:29 +0200 Subject: [PATCH] fixed grid factory issue with stairs added some sanity checks --- grid/GridNode.h | 1 + grid/factory/v2/GridFactory.h | 79 +++++++++++++++++++++++++++++------ grid/factory/v2/Stairs2.h | 19 +++++++-- 3 files changed, 83 insertions(+), 16 deletions(-) diff --git a/grid/GridNode.h b/grid/GridNode.h index 863ee43..6b21e87 100755 --- a/grid/GridNode.h +++ b/grid/GridNode.h @@ -54,6 +54,7 @@ public: static const uint8_t TYPE_OUTDOOR = 100; + public: /** ctor */ diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 589dd5d..a42ed81 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -42,6 +42,8 @@ private: Elevators elevators; + std::vector withinRemovePolygon; + bool _buildStairs = true; bool _removeIsolated = true; @@ -60,6 +62,14 @@ public: void setRemoveIsolated(const bool remove) {this->_removeIsolated = remove;} + /** does the given grid-node have a stair-neighbor? */ + bool hasStairNeighbor(const T& node) const { + for (const T& n : grid.neighbors(node)) { + if (n.getType() == GridNode::TYPE_STAIR) {return true;} + } + return false; + } + /** build using the given map */ void build(const Floorplan::IndoorMap* map, GridFactoryListener* listener = nullptr) { @@ -93,6 +103,27 @@ public: } } + // remove nodes within remove-OutlinePolygons + // this must happen AFTER stairs have been added, otherwise stairs might not be connectable due to removed/missing nodes. + // also, within this loop we prevent the deltion of nodes that are a direct neighbor of a stair! + // [otherwise the same thing would happen again!] + if (true) { + + for (const GridPoint& gp : withinRemovePolygon) { + T* n = (T*) grid.getNodePtrFor(gp); + + // delete if node is present and is no direct neighbor of a stair + if (n && !hasStairNeighbor(*n)) { + grid.remove(*n); + } + + } + + // remove all nodes that were just marked for removal + grid.cleanup(); + + } + // remove isolated nodes if (_removeIsolated) { if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");} @@ -114,31 +145,36 @@ public: return bb; } - enum class PartOfOutline { - NO, - INDOOR, - OUTDOOR, + struct PartOfOutline { + bool contained = false; // contained within at-least one polygon? + bool markedForRemove = false; // part of a "to-be-removed" polygon? + bool outdoor = false; // otherwise: indoor }; /** get the part of outline the given location belongs to. currently: none, indoor, outdoor */ static PartOfOutline isPartOfFloorOutline(const int x_cm, const int y_cm, const Floorplan::FloorOutline& outline) { // assume the point is not part of the outline - PartOfOutline res = PartOfOutline::NO; + PartOfOutline res; + res.contained = false; + res.markedForRemove = false; // process every outline polygon for (Floorplan::FloorOutlinePolygon* poly : outline) { HelperPoly pol(*poly); if (pol.contains(Point2(x_cm, y_cm))) { + // mark as "contained" + res.contained = true; + // belongs to a "remove" polygon? -> directly ignore this location! if (poly->method == Floorplan::OutlineMethod::REMOVE) { - return PartOfOutline::NO; + res.markedForRemove = true; } // belongs to a "add" polygon? -> remember until all polygons were checked // [might still belong to a "remove" polygon] - res = poly->outdoor ? PartOfOutline::OUTDOOR : PartOfOutline::INDOOR; + res.outdoor = poly->outdoor; } } @@ -173,7 +209,7 @@ public: // does the outline-polygon contain this position? const PartOfOutline part = isPartOfFloorOutline(x_cm, y_cm, floor->outline); - if (part == PartOfOutline::NO) {continue;} + if (!part.contained) {continue;} // check intersection with the floorplan GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize()); @@ -188,6 +224,11 @@ public: updateType(t, part, bbox, floor); grid.add(t); + // node part of remove-region? + // if so, remove >>AFTER<< stairs have been added + if (part.markedForRemove) { + withinRemovePolygon.push_back(t); + } // debug ++numNodes; @@ -395,7 +436,19 @@ public: Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false); Log::tick(); for (T& n2 : grid) { - if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);} + if (set.find(n2.getIdx()) == set.end()) { + + // sanity check + // wouldn't make sense that a stair-node is removed.. + // maybe something went wrong elsewhere??? + Assert::notEqual(n2.getType(), GridNode::TYPE_STAIR, "detected an isolated stair?!"); + Assert::notEqual(n2.getType(), GridNode::TYPE_ELEVATOR, "detected an isolated elevator?!"); + //Assert::notEqual(n2.getType(), GridNode::TYPE_DOOR, "detected an isolated door?!"); + + // proceed ;) + grid.remove(n2); + + } } Log::tock(); @@ -510,10 +563,10 @@ private: static inline void updateType(T& t, const PartOfOutline part, const GridNodeBBox& bbox, const Floorplan::Floor* floor) { // first, assume the type of the outline polygon - switch (part) { - case PartOfOutline::OUTDOOR: t.setType(GridNode::TYPE_OUTDOOR); break; - case PartOfOutline::INDOOR: t.setType(GridNode::TYPE_FLOOR); break; - default: throw Exception("should not happen"); + if (part.outdoor) { + t.setType(GridNode::TYPE_OUTDOOR); + } else { + //t.setType(GridNode::TYPE_FLOOR); } // hereafter, process each obstacle and mark doors diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h index 1e4e6cc..2372d03 100644 --- a/grid/factory/v2/Stairs2.h +++ b/grid/factory/v2/Stairs2.h @@ -157,18 +157,24 @@ public: // remember the z-position of the already-existing grid-node we connected the stair to connectedWithHeights.insert(grid[sn.gridIdx].z_cm); + // mark the node as stair-node + //grid[sn.gridIdx].setType(GridNode::TYPE_STAIR); + } else { sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm)); // check if there is a nearby floor-node to delete + // -> remove nodes directly above/below the stair const int deleteDist_cm = 100; const float distToBelow = gp.z_cm - floor->getStartingZ()*100; const float distToAbove = floor->getEndingZ()*100 - gp.z_cm; - if (distToBelow > gs_cm && distToBelow < deleteDist_cm) { + //if (distToBelow > gs_cm && distToBelow < deleteDist_cm) { + if (distToBelow > 0 && distToBelow < deleteDist_cm) { T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100)); if (n) {toDelete.push_back(n);} - } else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) { + //} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) { + } else if (distToAbove > 0 && distToAbove < deleteDist_cm) { T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100)); if (n) {toDelete.push_back(n);} } @@ -181,7 +187,11 @@ public: } // sanity check - Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!"); + // connectedWithHeights should contain 2 entries: + // one for the starting floor + // one for the ending floor + // this mainly fails, when there is a REMOVING outline-area that removes to many nodes and the stair can not be connected + Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!"); // now connect all new nodes with their neighbors // do not perform normal grid-connection but examine the nodes within the generated vector @@ -225,6 +235,9 @@ public: void finalize() { + //std::cout << "stairs.finalize() crashes!" << std::endl; + //return; + // delete all pending nodes and perform a cleanup if (!toDelete.empty()) { for (T* n : toDelete) {grid.remove(*n);}