From c21925e86f3fadadd92bb06ab10fe6fa703ee40e Mon Sep 17 00:00:00 2001 From: k-a-z-u Date: Thu, 27 Jul 2017 18:40:56 +0200 Subject: [PATCH] worked on grid creation fixed some issues with stairs fixed/added LINT --- floorplan/v2/FloorplanLINT.h | 26 ++++++++--- grid/Grid.h | 8 +++- grid/factory/v2/GridFactory.h | 17 +++++-- grid/factory/v2/Helper.h | 14 +++--- grid/factory/v2/Stairs2.h | 87 +++++++++++++++++++++++++++-------- 5 files changed, 114 insertions(+), 38 deletions(-) diff --git a/floorplan/v2/FloorplanLINT.h b/floorplan/v2/FloorplanLINT.h index 719fb65..9b93edc 100644 --- a/floorplan/v2/FloorplanLINT.h +++ b/floorplan/v2/FloorplanLINT.h @@ -60,7 +60,7 @@ namespace Floorplan { if (i.type == Type::ERROR) {++err;} } if (err > 0) { - throw Exception("detected floorplan errors"); + // throw Exception("detected floorplan errors"); } } @@ -94,7 +94,7 @@ namespace Floorplan { // check stairs for (const Stair* s : floor->stairs) { - checkStair(res, floor, s); + checkStair(res, map, floor, s); } // check elevators @@ -169,7 +169,14 @@ namespace Floorplan { } - static void checkStair(Issues& res, const Floor* floor, const Stair* stair) { + static void checkStair(Issues& res, const IndoorMap* map, const Floor* floor, const Stair* stair) { + + // list of all heights where there is a floor; + std::vector floorAtHeight_cm; + for (const Floor* f : map->floors) { + const int floorZ_cm = std::round(f->atHeight * 100); + floorAtHeight_cm.push_back(floorZ_cm); // integer height in cm + } if (stair->getParts().empty()) { res.push_back(Issue(Type::ERROR, floor, "stair does not contain any parts! [empty stair]")); @@ -182,13 +189,20 @@ namespace Floorplan { const Floorplan::Quad3& quadS = quads.front(); const Floorplan::Quad3& quadE = quads.back(); - // disconnected start? + // start == end? + if (quadS.p1.z == quadE.p3.z) { + res.push_back(Issue(Type::ERROR, floor, "stair start and end must not belong to the same floor!")); + } + + // disconnected start? (MUST belong to the floor the stair is attached to) if (quadS.p1.z != floor->getStartingZ()) { res.push_back(Issue(Type::ERROR, floor, "stair is not connected to the starting floor's ground! [open stair start]")); } - // disconnected end? - if (quadE.p3.z != floor->getEndingZ()) { + // disconnected end? (must be long to ANY other floor within the map) + //if (quadE.p3.z != floor->getEndingZ()) { + const int stairEndingZ_cm = std::round( quadE.p3.z * 100 ); + if(std::find(floorAtHeight_cm.begin(), floorAtHeight_cm.end(), stairEndingZ_cm) == floorAtHeight_cm.end()) { res.push_back(Issue(Type::ERROR, floor, "stair is not connected to the ending floor's ground! [open stair end]")); } diff --git a/grid/Grid.h b/grid/Grid.h index 0443635..c908674 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -268,7 +268,7 @@ public: } /** remove the connection from n1 to n2 (not the other way round!) */ - void disconnectUniDir(T& n1, T& n2) { + void disconnectUniDir(T& n1, const T& n2) { for (int n = 0; n < n1._numNeighbors; ++n) { if (n1._neighbors[n] == n2._idx) { arrayRemove(n1._neighbors, n, n1._numNeighbors); @@ -301,12 +301,16 @@ public: void remove(T& node) { // disconnect from all neighbors + int cnt = 0; while (node._numNeighbors) { // by removing one neighbor, all others are shifted to close the gap within the array // its therefor ok to always delete array index [0] disconnectBiDir(node._idx, node._neighbors[0]); - + if (++cnt > node.MAX_NEIGHBORS) { + Log::add(name, "WARNING! found unsolveable neighboring! " + node.asString(), true); + break; + } } // remove from hash-list diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index 9ebd0c0..d79abc8 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -89,7 +89,7 @@ public: if (listener) {listener->onGridBuildUpdateMajor("adding stairs");} if (_buildStairs) { for (Floorplan::Floor* f : map->floors) { - buildStairs(f, listener); + buildStairs(map, f, listener); if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);} } } @@ -215,7 +215,7 @@ public: GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize()); // slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit - bbox.grow(0.42345); + bbox.grow(0.1337); if (intersects(bbox, floor)) {continue;} // add to the grid [once] @@ -246,7 +246,7 @@ public: } - void buildStairs(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) { + void buildStairs(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) { const int total = floor->stairs.size(); int cur = 0; @@ -254,7 +254,7 @@ public: // process each stair within the floor for (const Floorplan::Stair* stair : floor->stairs) { if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " stair " + std::to_string(cur+1));} - stairs.build(floor, stair); + stairs.build(map, floor, stair); if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);} } @@ -419,6 +419,9 @@ public: void removeIsolatedNodes() { + //std::cout << "todo: remove" << std::endl; + //return; + // try to start at the first stair for (T& n : grid) { if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;} @@ -439,6 +442,9 @@ public: getConnected(n1, set); Log::tock(); + //const int numToRemove = grid.getNumNodes() - set.size(); + //int numRemoved = 0; + // remove all other Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false); Log::tick(); @@ -455,6 +461,9 @@ public: // proceed ;) grid.remove(n2); + //++numRemoved; + //std::cout << numRemoved << ":" << numToRemove << std::endl; + } } Log::tock(); diff --git a/grid/factory/v2/Helper.h b/grid/factory/v2/Helper.h index ebf3c50..e0e51bb 100644 --- a/grid/factory/v2/Helper.h +++ b/grid/factory/v2/Helper.h @@ -254,13 +254,13 @@ public: } static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) { - Point2 v0 = b - a, v1 = c - a, v2 = p - a; - float d00 = dot(v0, v0); - float d01 = dot(v0, v1); - float d11 = dot(v1, v1); - float d20 = dot(v2, v0); - float d21 = dot(v2, v1); - float denom = d00 * d11 - d01 * d01; + const Point2 v0 = b - a, v1 = c - a, v2 = p - a; + double d00 = dot(v0, v0); + double d01 = dot(v0, v1); + double d11 = dot(v1, v1); + double d20 = dot(v2, v0); + double d21 = dot(v2, v1); + double denom = d00 * d11 - d01 * d01; v = (d11 * d20 - d01 * d21) / denom; w = (d00 * d21 - d01 * d20) / denom; u = 1.0f - v - w; diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h index 2372d03..12c2600 100644 --- a/grid/factory/v2/Stairs2.h +++ b/grid/factory/v2/Stairs2.h @@ -31,19 +31,23 @@ private: // keep a list of all vertices below stairwells and remove them hereafter std::vector toDelete; + bool tryImproveStairConnections = true; + private: /** helper struct */ struct StairNode { - const int x_cm; - const int y_cm; - const int belongsToQuadIdx; + int x_cm; + int y_cm; + int belongsToQuadIdx; float z_cm; // interpolated int gridIdx = -1; StairNode(const int x_cm, const int y_cm, const int quadIdx) : x_cm(x_cm), y_cm(y_cm), belongsToQuadIdx(quadIdx) {;} + + }; @@ -62,7 +66,15 @@ public: - void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) { + void build(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, const Floorplan::Stair* stair) { + + // starting-height of all available floors (in cm) + // needed to ensure that stairs start and end at floors + std::vector floorsAtHeight_cm; + for (const Floorplan::Floor* f : map->floors) { + const int floorAtHeight_cm = std::round(f->atHeight*100); + floorsAtHeight_cm.push_back(floorAtHeight_cm); + } const int gs_cm = grid.getGridSize_cm(); @@ -123,20 +135,56 @@ public: } + const float stairAbsStart_m = floor->atHeight + stair->getParts().front().start.z; + const float stairAbsEnd_m = floor->atHeight + stair->getParts().back().end.z; + //const float stairHeight_m = stairAbsEnd_m - stairAbsStart_m; + const float stairAbsStart_cm = std::round(stairAbsStart_m*100); + const float stairAbsEnd_cm = std::round(stairAbsEnd_m*100); + const float stairHeight_cm = stairAbsEnd_cm - stairAbsStart_cm; + // this might lead to stairs the start slightly above the starting-floor // or ending slightly below the ending floor. this would lead to DISCONNECTION! // therefore re-scale the z-values to ensure they start at floor1 and end at floor 2 - float minZ = +9999999; - float maxZ = -9999999; - for (const StairNode& sn : stairNodes) { - if (sn.z_cm < minZ) {minZ = sn.z_cm;} - if (sn.z_cm > maxZ) {maxZ = sn.z_cm;} - } - for (StairNode& sn : stairNodes) { - const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ); // current percentage from minZ to maxZ - sn.z_cm = floor->getStartingZ()*100 + zPercent * floor->height*100; // apply percentage to floorStartZ <-> floorEndZ + { + + float minZ = +9999999; + float maxZ = -9999999; + for (const StairNode& sn : stairNodes) { + if (sn.z_cm < minZ) {minZ = sn.z_cm;} + if (sn.z_cm > maxZ) {maxZ = sn.z_cm;} + } + + for (StairNode& sn : stairNodes) { + const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ); // current percentage from minZ to maxZ + //sn.z_cm = floor->getStartingZ()*100 + zPercent * floor->height*100; // apply percentage to floorStartZ <-> floorEndZ + sn.z_cm = std::round(stairAbsStart_cm + zPercent * stairHeight_cm); // apply percentage to floorStartZ <-> floorEndZ + } + + // snap stair-nodes to nearby grid nodes, if possible + if (tryImproveStairConnections) { + for (StairNode& sn : stairNodes) { + if (std::abs(sn.z_cm-stairAbsStart_cm) < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)) ) {sn.z_cm = stairAbsStart_cm;} + if (std::abs(sn.z_cm-stairAbsEnd_cm) < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsEnd_cm))) {sn.z_cm = stairAbsEnd_cm;} + } + } + + // stort all stair-nodes by z (ascending) + const auto comp = [] (const StairNode& sn1, const StairNode& sn2) {return sn1.z_cm < sn2.z_cm;}; + std::sort(stairNodes.begin(), stairNodes.end(), comp); + + // get first and last stair note + const bool end1OK = std::find(floorsAtHeight_cm.begin(), floorsAtHeight_cm.end(), stairNodes.front().z_cm) != floorsAtHeight_cm.end(); + const bool end2OK = std::find(floorsAtHeight_cm.begin(), floorsAtHeight_cm.end(), stairNodes.back().z_cm) != floorsAtHeight_cm.end(); + + // be sure both are connected to a floor + if (!end1OK || !end2OK) { + throw Exception("stair's start or end is not directly connectable to a floor"); + } + } + + // track z-positions of already-existing grid nodes we connected the stair to // if this list contains 2 distinctive values, the stair is successfully connected to starting and ending floor! std::unordered_set connectedWithHeights; @@ -167,15 +215,16 @@ public: // 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; + const float distToBelow = gp.z_cm - stairAbsStart_cm;//floor->getStartingZ()*100; + const float distToAbove = stairAbsEnd_cm - gp.z_cm;//floor->getEndingZ()*100 - gp.z_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)); + T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, stairAbsStart_cm));//floor->getStartingZ()*100)); if (n) {toDelete.push_back(n);} //} 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 (distToAbove > 0 && distToAbove < deleteDist_cm) { + T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, stairAbsEnd_cm));//floor->getEndingZ()*100)); if (n) {toDelete.push_back(n);} } @@ -191,7 +240,7 @@ public: // 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!"); + 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