358 lines
12 KiB
C++
358 lines
12 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#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
|