242 lines
6.9 KiB
C++
242 lines
6.9 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;
|
|
|
|
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) {;}
|
|
};
|
|
|
|
|
|
public:
|
|
|
|
/** ctor */
|
|
Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
|
|
|
|
}
|
|
|
|
~Stairs() {
|
|
finalize();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
|
|
|
|
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;
|
|
}
|
|
|
|
}
|
|
|
|
// 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<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);
|
|
|
|
} 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
|
|
const int deleteDist_cm = 100;
|
|
const float distToBelow = gp.z_cm - floor->getStartingZ()*100;
|
|
const float distToAbove = floor->getEndingZ()*100 - gp.z_cm;
|
|
if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
|
|
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100));
|
|
if (n) {toDelete.push_back(n);}
|
|
} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
|
|
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100));
|
|
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
|
|
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() {
|
|
|
|
// 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
|