348 lines
12 KiB
C++
348 lines
12 KiB
C++
#ifndef STAIRS_H
|
|
#define STAIRS_H
|
|
|
|
#include <vector>
|
|
#include <set>
|
|
|
|
#include "../../Grid.h"
|
|
#include "Helper.h"
|
|
#include "../../../floorplan/v2/Floorplan.h"
|
|
|
|
#include <fstream>
|
|
#include <unordered_set>
|
|
|
|
/**
|
|
* this one should be both, simpler and more robus than v1.
|
|
* - prevents duplicate (x,y) nodes
|
|
* - slightly grows the stair-quads to ensure they overlap (sometimes the do not by a few mm)
|
|
* - only connects the corener nodes between adjacent quads (seems better for most stair-situations)
|
|
*
|
|
*/
|
|
template <typename T> class Stairs {
|
|
|
|
private:
|
|
|
|
/** the grid to build into */
|
|
Grid<T>& grid;
|
|
|
|
/** calculation helper */
|
|
Helper<T> helper;
|
|
|
|
// keep a list of all vertices below stairwells and remove them hereafter
|
|
std::vector<T*> toDelete;
|
|
|
|
bool tryImproveStairConnections = true;
|
|
bool abortOnError = true;
|
|
|
|
private:
|
|
|
|
/** helper struct */
|
|
struct StairNode {
|
|
|
|
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) {;}
|
|
|
|
};
|
|
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
|
|
|
|
}
|
|
|
|
~Stairs() {
|
|
finalize();
|
|
}
|
|
|
|
/** whether to abort when errors are detected */
|
|
void setAbortOnError(const bool abort) {
|
|
this->abortOnError = abort;
|
|
}
|
|
|
|
|
|
|
|
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<int> 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();
|
|
|
|
// get all of the parts for the stair
|
|
const std::vector<Floorplan::StairPart> parts = stair->getParts();
|
|
|
|
// convert each part to a quad. hereby, slightly grow each quad, to ensure stair-parts are connected without gaps!
|
|
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor, 1.05);
|
|
|
|
std::vector<StairNode> stairNodes;
|
|
|
|
// process each quad to get a list of all stair-nodes to add
|
|
for (int i = 0; i < (int) quads.size(); ++i) {
|
|
|
|
// create a 2D polygon (ignore z) for the quad
|
|
const Floorplan::Quad3& quad = quads[i];
|
|
HelperPoly poly(quad);
|
|
|
|
// get a callback for each node that belongs to the quad
|
|
poly.forEachGridPoint(gs_cm, [&] (int x_cm, int y_cm) {
|
|
|
|
const StairNode sn(x_cm, y_cm, i);
|
|
|
|
// IMPORTANT
|
|
// skip nodes that belong to more than one quad -> skip duplicates -> much more stable!
|
|
// NOTE: currently this will kill circular stairs that are above themselves
|
|
// FIX: skip dupliactes only between adjacent quads? this should help
|
|
auto comp = [&] (const StairNode& sn1) { return sn1.x_cm == sn.x_cm && sn1.y_cm == sn.y_cm; };
|
|
auto it = std::find_if(stairNodes.begin(), stairNodes.end(), comp);
|
|
if (it == stairNodes.end()) {stairNodes.push_back(sn);}
|
|
|
|
});
|
|
|
|
}
|
|
|
|
// 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
|
|
const Point2 p(sn.x_cm, sn.y_cm);
|
|
|
|
// the stair-quads centimeter position
|
|
const Floorplan::Quad3& quad = quads[sn.belongsToQuadIdx];
|
|
const Point3 p1 = quad.p1 * 100;
|
|
const Point3 p2 = quad.p2 * 100;
|
|
const Point3 p3 = quad.p3 * 100;
|
|
const Point3 p4 = quad.p4 * 100;
|
|
|
|
// get the z-value from one of the both triangles
|
|
// int z_cm;
|
|
float u,v,w;
|
|
if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, 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);
|
|
sn.z_cm = p1.z*u + p3.z*v + p4.z*w;
|
|
}
|
|
|
|
}
|
|
|
|
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 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
|
|
// // 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;};
|
|
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) {
|
|
if (abortOnError) {
|
|
throw Exception("stair's start or end is not directly connectable to a floor");
|
|
} else{
|
|
std::cout << "stair's start or end is not directly connectable to a floor" << std::endl;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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<float> 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, 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);
|
|
|
|
// 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 - 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, stairAbsStart_cm));//floor->getStartingZ()*100));
|
|
if (n) {toDelete.push_back(n);}
|
|
//} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
|
|
}
|
|
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);}
|
|
}
|
|
|
|
// mark the node as stair-node
|
|
grid[sn.gridIdx].setType(GridNode::TYPE_STAIR);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
// 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
|
|
// 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
|
|
// this allows for some additional checks/criteria to be used
|
|
for (const StairNode& sn1 : stairNodes) {
|
|
T& n1 = (T&) grid[sn1.gridIdx];
|
|
|
|
for (const StairNode& sn2 : stairNodes) {
|
|
|
|
// node full?
|
|
if (n1.fullyConnected()) {continue;}
|
|
|
|
T& n2 = (T&) grid[sn2.gridIdx];
|
|
|
|
// do not connect node with itself
|
|
if (n2.getIdx() == n1.getIdx()) {continue;}
|
|
|
|
if (n1.hasNeighbor(n2.getIdx())) {continue;}
|
|
|
|
// connect adjacent stair-nodes
|
|
// but only if their stair-parts are also adjacent
|
|
// this addresses several error-situations with round stairs (end connected to the start) , ...
|
|
const int dx = sn1.x_cm - sn2.x_cm;
|
|
const int dy = sn1.y_cm - sn2.y_cm;
|
|
const int dz = sn1.belongsToQuadIdx - sn2.belongsToQuadIdx;
|
|
|
|
// connect
|
|
if (std::abs(dx) <= gs_cm && std::abs(dy) <= gs_cm && std::abs(dz) <= 1) {
|
|
grid.connectUniDir(n1, n2);
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
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);}
|
|
toDelete.clear();
|
|
grid.cleanup();
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
};
|
|
|
|
#endif // STAIRS_H
|