FrankE
d283d9b326
new grid walkers + fixes new test-cases worked on step/and turn detection android offline-data-reader worked on vap-grouping
323 lines
8.6 KiB
C++
323 lines
8.6 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>
|
|
|
|
|
|
|
|
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
|