diff --git a/floorplan/v2/Floorplan.h b/floorplan/v2/Floorplan.h index 8732839..d03f30d 100644 --- a/floorplan/v2/Floorplan.h +++ b/floorplan/v2/Floorplan.h @@ -508,6 +508,9 @@ namespace Floorplan { /** the elevator's rotation (in radians) */ float rotation; + /** the elevator's height (from its starting position) */ + float height_m; + /** get the 4 corner points for the elevator */ Polygon2 getPoints() const { const Point2 p1 = Point2(+width/2, +depth/2).rotated(rotation) + center; diff --git a/floorplan/v2/FloorplanLINT.h b/floorplan/v2/FloorplanLINT.h index 9b93edc..52ff9be 100644 --- a/floorplan/v2/FloorplanLINT.h +++ b/floorplan/v2/FloorplanLINT.h @@ -99,7 +99,7 @@ namespace Floorplan { // check elevators for (const Elevator* e : floor->elevators) { - checkElevator(res, floor, e); + checkElevator(res, map, floor, e); } } @@ -222,14 +222,31 @@ namespace Floorplan { } - static void checkElevator(Issues& res, const Floor* floor, const Elevator* e) { + static void checkElevator(Issues& res, const IndoorMap* map, const Floor* floor, const Elevator* e) { if (e->depth < 0.5) { - res.push_back(Issue(Type::ERROR, floor, "elevator's depth @" + e->center.asString() + " is too small: " + std::to_string(e->depth) + "m")); + res.push_back(Issue(Type::ERROR, floor, "elevator's @" + e->center.asString() + ": depth is too small: " + std::to_string(e->depth) + "m")); } if (e->width < 0.5) { - res.push_back(Issue(Type::ERROR, floor, "elevator's width @" + e->center.asString() + " is too small: " + std::to_string(e->width) + "m")); + res.push_back(Issue(Type::ERROR, floor, "elevator's @" + e->center.asString() + ": width is too small: " + std::to_string(e->width) + "m")); + } + + if (e->height_m < 0.1) { + res.push_back(Issue(Type::ERROR, floor, "elevator's @" + e->center.asString() + ": height is too small: " + std::to_string(e->height_m) + "m")); + } + + // 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 + } + + // disconnected end? (must be long to ANY other floor within the map) + const int elevEndZ_cm = std::round( (floor->getStartingZ() + e->height_m) * 100 ); + if(std::find(floorAtHeight_cm.begin(), floorAtHeight_cm.end(), elevEndZ_cm) == floorAtHeight_cm.end()) { + res.push_back(Issue(Type::ERROR, floor, "elevator @" + e->center.asString() + " is not connected to the ending floor's ground! [open elevator end]")); } } diff --git a/floorplan/v2/FloorplanReader.h b/floorplan/v2/FloorplanReader.h index b9cb84e..462b78e 100644 --- a/floorplan/v2/FloorplanReader.h +++ b/floorplan/v2/FloorplanReader.h @@ -168,6 +168,7 @@ namespace Floorplan { elev->center = Point2(el->FloatAttribute("cx"), el->FloatAttribute("cy")); elev->depth = el->FloatAttribute("depth"); elev->width = el->FloatAttribute("width"); + elev->height_m = el->FloatAttribute("height"); elev->rotation = el->FloatAttribute("rotation"); return elev; } diff --git a/floorplan/v2/FloorplanWriter.h b/floorplan/v2/FloorplanWriter.h index e8eff0e..82cdfd0 100644 --- a/floorplan/v2/FloorplanWriter.h +++ b/floorplan/v2/FloorplanWriter.h @@ -115,6 +115,7 @@ namespace Floorplan { elem->SetAttribute("cy", elevator->center.y); elem->SetAttribute("width", elevator->width); elem->SetAttribute("depth", elevator->depth); + elem->SetAttribute("height", elevator->height_m); elem->SetAttribute("rotation", elevator->rotation); elevators->InsertEndChild(elem); } diff --git a/grid/Grid.h b/grid/Grid.h index c908674..aa4da03 100755 --- a/grid/Grid.h +++ b/grid/Grid.h @@ -231,14 +231,23 @@ public: const uint64_t center = 1 << 19; // build - const uint64_t x = center + (int64_t) std::round((p.x_cm) / (float)gridSize_cm); - const uint64_t y = center + (int64_t) std::round((p.y_cm) / (float)gridSize_cm); - const uint64_t z = center + (int64_t) std::round((p.z_cm) / (float)gridSize_cm * 5); // z is usually much lower and not always aligned -> allow more room for hashes + const uint64_t x = center + (int64_t) idxX(p.x_cm); + const uint64_t y = center + (int64_t) idxY(p.y_cm); + const uint64_t z = center + (int64_t) idxZ(p.z_cm); return (z << 40) | (y << 20) | (x << 0); } + inline int idxX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm);} + inline int idxY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm);} + inline int idxZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm);} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes + + inline int snapX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm) * gridSize_cm;} + inline int snapY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm) * gridSize_cm;} + inline int snapZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm) * gridSize_cm;} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes + + /** array access */ T& operator [] (const int idx) { Assert::isBetween(idx, 0, getNumNodes()-1, "index out of bounds"); diff --git a/grid/factory/v2/Elevators.h b/grid/factory/v2/Elevators.h index a10f0de..f4b84c4 100644 --- a/grid/factory/v2/Elevators.h +++ b/grid/factory/v2/Elevators.h @@ -50,10 +50,15 @@ public: std::vector nodesWithin; const HelperPoly poly(elevator->getPoints()); + // elevator starts at the current floor, but where does the elevator end? + const int f1_cm = grid.snapZ(floor->getStartingZ()*100); + // elevator's end is given by its height + const int f2_cm = grid.snapZ( (floor->getStartingZ() + elevator->height_m) * 100); + auto callback = [&] (const int x_cm, const int y_cm) { - const GridPoint gp1(x_cm, y_cm, floor->getStartingZ()*100); // starting floor - const GridPoint gp2(x_cm, y_cm, floor->getEndingZ()*100); // the floor above + const GridPoint gp1(x_cm, y_cm, f1_cm); // starting floor + const GridPoint gp2(x_cm, y_cm, f2_cm); // the floor above // ensure such a node is present in both floors (and thus a connection is possible) if (grid.hasNodeFor(gp1) && grid.hasNodeFor(gp2)) { @@ -63,34 +68,85 @@ public: }; poly.forEachGridPoint(gs_cm, callback); + if (nodesWithin.empty()) { + throw Exception("faild to determine starting and ending nodes for elevator. disconnected?"); + } + // now create the interconnection in z-direction - const int z1_cm = std::ceil((floor->getStartingZ()*100+1) / gs_cm) * gs_cm; // the next node above the current flor - const int z2_cm = std::floor((floor->getEndingZ()*100-1) / gs_cm) * gs_cm; // the last node below the next floor + //const int z1_cm = std::ceil((floor->getStartingZ()*100+1) / gs_cm) * gs_cm; // the next node above the current flor + //const int z2_cm = std::floor((floor->getEndingZ()*100-1) / gs_cm) * gs_cm; // the last node below the next floor + + int z1_cm = std::ceil((f1_cm+1.0f) / gs_cm) * gs_cm; + int z2_cm = std::floor((f2_cm-1.0f) / gs_cm) * gs_cm; + + // ensure the nodes between (z1,z2) are not directly the floor (f1,f2) + //if (grid.snapZ(z1_cm) == grid.snapZ(f1_cm)) {z1_cm += gs_cm;} + //if (grid.snapZ(z2_cm) == grid.snapZ(f2_cm)) {z2_cm -= gs_cm;} + for (const IntPos nodePos : nodesWithin) { // create nodes BETWEEN the two floors (skip the floors themselves! -> floor1+gridSize <-> floor2-gridSize for (int z_cm = z1_cm; z_cm <= z2_cm; z_cm += gs_cm) { const GridPoint gp1(nodePos.x_cm, nodePos.y_cm, z_cm); // the to-be-added node - Assert::isFalse(grid.hasNodeFor(gp1), "elevator collission"); // such a node must not yet exist! otherwise we e.g. collide with a stari - const int idx = grid.add(T(gp1.x_cm, gp1.y_cm, gp1.z_cm)); // create the node - grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type + //Assert::isFalse(grid.hasNodeFor(gp1), "elevator collission"); // such a node must not yet exist! otherwise we e.g. collide with a stari + if (grid.hasNodeFor(gp1)) { + const int idx = grid.getNodeFor(gp1).getIdx(); + grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type + } else { + const int idx = grid.add(T(gp1.x_cm, gp1.y_cm, gp1.z_cm)); // create the node + grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type + } } - // connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm) - for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) { - GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm); - GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm); + // connect each of the new nodes with the node below it + // also connect the elevator to the starting and ending floor + for (int z_cm = z1_cm; z_cm <= z2_cm; z_cm += gs_cm) { - // above the ending floor? -> snap to ending floor - // note: this one is needed if the floor-heights are not dividable by the grid-size - if (gp.z_cm > floor->getEndingZ()*100) {gp.z_cm = floor->getEndingZ()*100;} + // directly above starting floor + if (z_cm == z1_cm) { + + // connect with floor below + const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, f1_cm); + const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm); + Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node"); + Assert::isTrue(grid.hasNodeFor(gp), "missing node"); + const T& n1 = grid.getNodeFor(gpBelow); + const T& n2 = grid.getNodeFor(gp); + grid.connectBiDir(n1.getIdx(), n2.getIdx()); + grid[n1.getIdx()].setType(GridNode::TYPE_ELEVATOR); + + } else { + + // connect with node below + const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm); + const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm); + Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node"); + Assert::isTrue(grid.hasNodeFor(gp), "missing node"); + const T& n1 = grid.getNodeFor(gpBelow); + const T& n2 = grid.getNodeFor(gp); + grid.connectBiDir(n1.getIdx(), n2.getIdx()); + + } + + // directly below ending floor + if (z_cm == z2_cm) { + + // connect with floor above + const GridPoint gpAbove(nodePos.x_cm, nodePos.y_cm, f2_cm); + const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm); + Assert::isTrue(grid.hasNodeFor(gpAbove), "missing node"); + Assert::isTrue(grid.hasNodeFor(gp), "missing node"); + const T& n1 = grid.getNodeFor(gpAbove); + const T& n2 = grid.getNodeFor(gp); + + //if (n1.getIdx() == n2.getIdx()) {continue;} + + grid.connectBiDir(n1.getIdx(), n2.getIdx()); + grid[n1.getIdx()].setType(GridNode::TYPE_ELEVATOR); + + } - Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node"); - Assert::isTrue(grid.hasNodeFor(gp), "missing node"); - T& n1 = (T&) grid.getNodeFor(gpBelow); - T& n2 = (T&) grid.getNodeFor(gp); - grid.connectBiDir(n1, n2); } } diff --git a/grid/factory/v2/GridFactory.h b/grid/factory/v2/GridFactory.h index d79abc8..93b4188 100755 --- a/grid/factory/v2/GridFactory.h +++ b/grid/factory/v2/GridFactory.h @@ -197,7 +197,7 @@ public: const int x2 = helper.align(bbox.getMax().x); const int y1 = helper.align(bbox.getMin().y); const int y2 = helper.align(bbox.getMax().y); - const int z_cm = (floor->atHeight*100); + const int z_cm = std::round(floor->atHeight*100); const int total = (x2-x1) / helper.gridSize(); int cur = 0; diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h index 12c2600..f16b772 100644 --- a/grid/factory/v2/Stairs2.h +++ b/grid/factory/v2/Stairs2.h @@ -155,18 +155,24 @@ public: } 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 + const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ); // current percentage from minZ to maxZ WHICH ONE IS CORRECT? + //const float zPercent = (sn.z_cm - stairAbsStart_cm) / (stairAbsEnd_cm - stairAbsStart_cm); // current percentage from minZ to maxZ WHICH ONE IS CORRECT? + //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;} - } - } + + +// // snap stair-nodes to nearby grid nodes, if possible +// // we try to improve the number of connections between stair and starting/ending floor +// 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;} +// //if (std::abs(sn.z_cm-stairAbsStart_cm) < gs_cm*1.5 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)) ) {auxcon.push_back(AuxConnection(sn, GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)));} +// //if (std::abs(sn.z_cm-stairAbsEnd_cm) < gs_cm*1.5 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsEnd_cm))) {auxcon.push_back(AuxConnection(sn, GridPoint(sn.x_cm, sn.y_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;}; @@ -235,6 +241,40 @@ public: } + + // for larger grid-sizes stair connections to starting/ending floors are quite bad if the stairs are not 90degree aligned + // to improve the number of connections between floor and stair, we brute-force search for connectable nodes (between stair and floor, given threshold) + if (tryImproveStairConnections) { + + // connect all stair-nodes to the floor if their distance is below this threshold + const int maxDist_cm = gs_cm * 1.2; + + for (StairNode& sn : stairNodes) { + + const auto& gn1 = grid[sn.gridIdx]; + + // all stair-nodes that are near to a floor + const bool lower = std::abs(sn.z_cm-stairAbsStart_cm) < maxDist_cm; + const bool upper = std::abs(sn.z_cm-stairAbsEnd_cm) < maxDist_cm; + + if (lower || upper) { + + // cross-check with each grid node (slow...) + for (const auto& gn2 : grid.getNodes()) { + if (gn2.getDistanceInCM(gn1) > maxDist_cm) {continue;} // connect with a floor-node near the stair-node + if (gn1.hasNeighbor(gn2.getIdx())) {continue;} // already connected? + //if (gn2.hasNeighbor(gn1.getIdx())) {continue;} + if (gn2.getIdx() == gn1.getIdx()) {continue;} // do not connect with myself + if (gn2.fullyConnected()) {continue;} // skip full nodes + if (gn1.fullyConnected()) {continue;} // skip full nodes + grid.connectBiDir(gn1.getIdx(), gn2.getIdx()); // connect + } + + } + } + + } + // sanity check // connectedWithHeights should contain 2 entries: // one for the starting floor @@ -274,11 +314,8 @@ public: } - } - - }