diff --git a/grid/factory/v2/Stairs2.h b/grid/factory/v2/Stairs2.h index a73cdd9..5165432 100644 --- a/grid/factory/v2/Stairs2.h +++ b/grid/factory/v2/Stairs2.h @@ -35,10 +35,14 @@ private: /** helper struct */ struct StairNode { + const int x_cm; const int y_cm; const 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) {;} }; @@ -94,8 +98,7 @@ public: } - // add the new nodes to the grid - // nodes that are near to the two floors that stair is within, are replaced by already existing floor-nodes! + // reconstruct the z-value for each node using interpolation for (StairNode& sn : stairNodes) { // use the nodes (x,y) to reconstruct the z-value for this position using barycentric interpolation @@ -112,19 +115,48 @@ public: int z_cm; float u,v,w; if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) { - z_cm = p1.z*u + p2.z*v + p3.z*w; + sn.z_cm = p1.z*u + p2.z*v + p3.z*w; } else { helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w); - z_cm = p1.z*u + p3.z*v + p4.z*w; + sn.z_cm = p1.z*u + p3.z*v + p4.z*w; } + } + + // this might lead to stairs the start slightly above the starting-floor + // or end 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 + } + + // 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; + + // add the new nodes to the grid + // nodes that are near to the two floors that stair is within, are replaced by already existing floor-nodes! + for (StairNode& sn : stairNodes) { + // the to-be-added position - GridPoint gp(sn.x_cm, sn.y_cm, z_cm); + GridPoint gp(sn.x_cm, sn.y_cm, sn.z_cm); // if a node is near an existing one (the floor above/below) use the existing one! // this ensures the stair is connected to the floor above and below if (grid.hasNodeFor(gp)) { + sn.gridIdx = grid.getNodeFor(gp).getIdx(); + + // remember the z-position of the already-existing grid-node we connected the stair to + connectedWithHeights.insert(grid[sn.gridIdx].z_cm); + } else { sn.gridIdx = grid.add(T(gp.x_cm, gp.y_cm, gp.z_cm)); @@ -141,14 +173,16 @@ public: if (n) {toDelete.push_back(n);} } + // mark the node as stair-node + grid[sn.gridIdx].setType(GridNode::TYPE_STAIR); } - - - } + // sanity check + 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 // this allows for some additional checks/criteria to be used