333 lines
8.9 KiB
C++
333 lines
8.9 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>
|
||
|
||
|
||
|
||
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;
|
||
|
||
|
||
std::ofstream outStairs;
|
||
std::ofstream outDelete;
|
||
|
||
|
||
private:
|
||
|
||
// struct Entry {
|
||
// GridPoint pos;
|
||
// std::vector<int> part;
|
||
// int idx;
|
||
// Entry(const GridPoint pos, int part) : pos(pos) {
|
||
// this->part.push_back(part);
|
||
// }
|
||
// };
|
||
|
||
/** 2D integer point */
|
||
struct XY {
|
||
int x;
|
||
int y;
|
||
XY() : x(-1), y(-1) {;}
|
||
XY(const int x, const int y) : x(x), y(y) {;}
|
||
bool operator == (const XY& o) const {return (x == o.x) && (y == o.y);}
|
||
};
|
||
|
||
|
||
/** 3D integer point */
|
||
struct XYZ {
|
||
int x;
|
||
int y;
|
||
int z;
|
||
XYZ() : x(-1), y(-1), z(-1) {;}
|
||
XYZ(const int x, const int y, const int z) : x(x), y(y), z(z) {;}
|
||
bool operator == (const XYZ& o) const {return (x == o.x) && (y == o.y) && (z == o.z);}
|
||
};
|
||
|
||
/** and intermediate grid node */
|
||
struct Intermediate {
|
||
XY xy;
|
||
T node;
|
||
int idx;
|
||
std::vector<XY> con;
|
||
Intermediate(const XY xy, const T node) : xy(xy),node(node), idx(-1) {;}
|
||
};
|
||
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
Stairs(Grid<T>& grid) : grid(grid), helper(grid) {
|
||
outStairs.open("/tmp/quads.dat");
|
||
outDelete.open("/tmp/delete.dat");
|
||
}
|
||
|
||
~Stairs() {
|
||
finalize();
|
||
outStairs.close();
|
||
outDelete.close();
|
||
}
|
||
|
||
|
||
|
||
void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
|
||
|
||
const int gs_cm = grid.getGridSize_cm();
|
||
|
||
std::vector<Intermediate> stairNodes;
|
||
|
||
// process each part
|
||
int partIdx = 0;
|
||
|
||
const std::vector<Floorplan::StairPart> parts = stair->getParts();
|
||
const std::vector<Floorplan::Quad3> quads = Floorplan::getQuads(parts, floor);
|
||
|
||
|
||
for (int i = 0; i < (int)parts.size(); ++i) {
|
||
|
||
//const Floorplan::StairPart& part = parts[i];
|
||
const Floorplan::Quad3& quad = helper.align(quads[i]*100);
|
||
|
||
const Point3 p1 = quad.p1;
|
||
const Point3 p2 = quad.p2;
|
||
const Point3 p3 = quad.p3;
|
||
const Point3 p4 = quad.p4;
|
||
|
||
outStairs << p1.x << " " << p1.y << " " << p1.z << "\n";
|
||
outStairs << p2.x << " " << p2.y << " " << p2.z << "\n";
|
||
outStairs << p3.x << " " << p3.y << " " << p3.z << "\n";
|
||
outStairs << p4.x << " " << p4.y << " " << p4.z << "\n";
|
||
outStairs << p1.x << " " << p1.y << " " << p1.z << "\n\n\n";
|
||
|
||
|
||
HelperPoly poly; poly.add(p1.xy()); poly.add(p2.xy()); poly.add(p3.xy()); poly.add(p4.xy());
|
||
|
||
const int x1 = helper.align(poly.bbox_cm.getMin().x); // ALIGNF?
|
||
const int y1 = helper.align(poly.bbox_cm.getMin().y);
|
||
const int x2 = helper.align(poly.bbox_cm.getMax().x); // ALIGNC?
|
||
const int y2 = helper.align(poly.bbox_cm.getMax().y);
|
||
|
||
std::vector<XYZ> points;
|
||
|
||
// get all points that are part of the stair-part-polygon
|
||
for (int x = x1; x <= x2; x+=gs_cm) {
|
||
for (int y = y1; y <= y2; y+=gs_cm) {
|
||
if (!poly.contains( Point2(x,y) )) {continue;}
|
||
points.push_back(XYZ(x,y,-1));
|
||
if (y < 500) {
|
||
int i = 0; (void) i;
|
||
}
|
||
}
|
||
}
|
||
|
||
// now determine each corresponding z-coordinate
|
||
for (XYZ& xy : points) {
|
||
|
||
const Point2 p(xy.x, xy.y);
|
||
float u,v,w;
|
||
|
||
if (helper.bary(p, p1.xy(), p2.xy(), p3.xy(), u, v, w)) {
|
||
xy.z = p1.z*u + p2.z*v + p3.z*w;
|
||
} else {
|
||
helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
|
||
xy.z = p1.z*u + p3.z*v + p4.z*w;
|
||
}
|
||
|
||
}
|
||
|
||
// stair start, ensure min snaps to the floor
|
||
if (partIdx == 0) {
|
||
const float minz_cm = minZ(points);
|
||
const float maxz_cm = maxZ(points);
|
||
const int mz = floor->atHeight * 100;
|
||
if (minz_cm != mz) {
|
||
for (XYZ& xyz : points) {
|
||
const float percent = ((xyz.z - minz_cm) / (maxz_cm - minz_cm));
|
||
xyz.z = mz + percent * (maxz_cm - mz);
|
||
}
|
||
}
|
||
}
|
||
|
||
// stair end, ensure max snaps to the floor
|
||
if (partIdx == (int) parts.size() - 1) {
|
||
const float minz_cm = minZ(points);
|
||
const float maxz_cm = maxZ(points);
|
||
const int mz = (floor->atHeight+floor->height) * 100;
|
||
if (maxz_cm != mz) {
|
||
for (XYZ& xyz : points) {
|
||
const float percent = ((xyz.z - minz_cm) / (maxz_cm - minz_cm));
|
||
xyz.z = minz_cm + percent * (mz - minz_cm);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
for (XYZ& xy : points) {
|
||
|
||
// construct a grid-node
|
||
T node(xy.x, xy.y, xy.z);
|
||
|
||
// construct an intermediate-node containing all desired connections
|
||
Intermediate iNode(XY(xy.x, xy.y), node);
|
||
for (int ox = -1; ox <= +1; ++ox) {
|
||
for (int oy = -1; oy <= +1; ++oy) {
|
||
if (ox == 0 && oy == 0) {continue;}
|
||
const int nx = xy.x + ox * gs_cm;
|
||
const int ny = xy.y + oy * gs_cm;
|
||
iNode.con.push_back( XY(nx, ny) );
|
||
}
|
||
}
|
||
stairNodes.push_back(iNode);
|
||
|
||
}
|
||
|
||
|
||
++partIdx;
|
||
|
||
}
|
||
|
||
|
||
// add all stair nodes or replace them with already existing ones
|
||
for (Intermediate& iNode : stairNodes) {
|
||
|
||
// nearer than minDiff? -> use existing node
|
||
const float minDiff = gs_cm * 0.49;
|
||
|
||
// nearer than maxDiff? -> delete as this one is unreachable below the stair
|
||
// to prevent errors (e.g. a platform at 1.0 meter) we use 0.98 meter as deleting threshold
|
||
// otherwise we get problems in some places
|
||
const float maxDiff = 98;
|
||
|
||
// distance between the stair node and the floor above / below
|
||
const float zDiff1 = std::abs(iNode.node.z_cm - 100*floor->getStartingZ());
|
||
const float zDiff2 = std::abs(iNode.node.z_cm - 100*floor->getEndingZ());
|
||
|
||
iNode.idx = -1;
|
||
|
||
// try to connect the stair-node to the floor below
|
||
if (zDiff1 < minDiff) {
|
||
const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
|
||
if (n2) {
|
||
iNode.idx = n2->getIdx();
|
||
} else {
|
||
iNode.idx = grid.add(iNode.node); // add a new one
|
||
helper.connectToNeighbors(grid[iNode.idx]); // and connect to the floor's grid nodes
|
||
}
|
||
|
||
// try to connect the stair-node to the floor above
|
||
} else if (zDiff2 < minDiff) {
|
||
const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
|
||
if (n2) {
|
||
iNode.idx = n2->getIdx();
|
||
} else {
|
||
iNode.idx = grid.add(iNode.node); // add a new one
|
||
helper.connectToNeighbors(grid[iNode.idx]); // and connect to the floor's grid nodes
|
||
}
|
||
|
||
// remove nodes directly below the stair
|
||
} else if (zDiff1 < maxDiff) {
|
||
T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getStartingZ()));
|
||
outDelete << n2->x_cm << " " << n2->y_cm << " " << n2->z_cm << "\n";
|
||
if (n2->y_cm < 500) {
|
||
int i = 0; (void) i;
|
||
}
|
||
if (n2) {toDelete.push_back(n2);}
|
||
|
||
// remove nodes directly above the stair
|
||
} else if (zDiff2 < maxDiff) {
|
||
T* n2 = (T*) grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, 100*floor->getEndingZ()));
|
||
outDelete << n2->x_cm << " " << n2->y_cm << " " << n2->z_cm << "\n";
|
||
if (n2->y_cm < 500) {
|
||
int i = 0; (void) i;
|
||
}
|
||
if (n2) {toDelete.push_back(n2);}
|
||
}
|
||
|
||
// no matching node found -> add a new one to the grid
|
||
if (iNode.idx == -1) {
|
||
const T* n2 = grid.getNodePtrFor(GridPoint(iNode.node.x_cm, iNode.node.y_cm, iNode.node.z_cm));
|
||
iNode.idx = (n2) ? (n2->getIdx()) : (grid.add(iNode.node));
|
||
}
|
||
|
||
// add semantic information
|
||
grid[iNode.idx].setType(GridNode::TYPE_STAIR);
|
||
|
||
}
|
||
|
||
// connect to-be-connected stair-nodes (each node with its [previously configured] neighbors)
|
||
for (const Intermediate& t1 : stairNodes) {
|
||
for (const XY& connectTo : t1.con) {
|
||
for (const Intermediate& t2 : stairNodes) {
|
||
if (connectTo == t2.xy) {
|
||
T& n1 = grid[t1.idx]; // use the nodes already added to the grid
|
||
T& n2 = grid[t2.idx]; // use the nodes already added to the grid
|
||
if (n1.hasNeighbor(n2.getIdx())) {continue;}
|
||
if (n1.getIdx() == n2.getIdx()) {continue;}
|
||
if (n1.getNumNeighbors() >= 10) {continue;}
|
||
grid.connectUniDir(n1, n2);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// finalize after ALL stairs were added. much faster and should be safe!
|
||
//finalize();
|
||
|
||
|
||
}
|
||
|
||
void finalize() {
|
||
|
||
// delete all pending nodes and perform a cleanup
|
||
if (!toDelete.empty()) {
|
||
for (T* n : toDelete) {grid.remove(*n);}
|
||
toDelete.clear();
|
||
grid.cleanup();
|
||
}
|
||
|
||
}
|
||
|
||
static float minZ(const std::vector<XYZ>& points) {
|
||
auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
|
||
auto it = std::min_element(points.begin(), points.end(), comp);
|
||
return it->z;
|
||
}
|
||
|
||
static float maxZ(const std::vector<XYZ>& points) {
|
||
auto comp = [] (const XYZ& a, const XYZ& b) {return a.z < b.z;};
|
||
auto it = std::max_element(points.begin(), points.end(), comp);
|
||
return it->z;
|
||
}
|
||
|
||
};
|
||
|
||
#endif // STAIRS_H
|