FrankE
382a046df1
added new sanity checks fixed minor errors added corresponding test-cases added moving-median
404 lines
10 KiB
C++
Executable File
404 lines
10 KiB
C++
Executable File
#ifndef GRIDFACTORY_H
|
|
#define GRIDFACTORY_H
|
|
|
|
#include <string>
|
|
#include <unordered_set>
|
|
|
|
#include "../../floorplan/Floor.h"
|
|
#include "../../floorplan/Stairs.h"
|
|
#include "../../floorplan/PlatformStair.h"
|
|
|
|
#include "../../geo/Units.h"
|
|
#include "../GridNodeBBox.h"
|
|
#include "../Grid.h"
|
|
|
|
#include "../../misc/Debug.h"
|
|
|
|
template <typename T> class GridFactory {
|
|
|
|
/** logging name */
|
|
static constexpr const char* name = "GridFac";
|
|
|
|
private:
|
|
|
|
/** the grid to build into */
|
|
Grid<T>& grid;
|
|
|
|
|
|
public:
|
|
|
|
/** ctor with the grid to fill */
|
|
GridFactory(Grid<T>& grid) : grid(grid) {;}
|
|
|
|
/** add the given floor at the provided height (in cm) */
|
|
void addFloor(const Floor& floor, const float z_cm) {
|
|
|
|
Log::add(name, "adding floor at height " + std::to_string(z_cm), false);
|
|
Log::tick();
|
|
|
|
const int gridSize_cm = grid.getGridSize_cm();
|
|
|
|
// build grid-points
|
|
for(int x_cm = 0; x_cm < floor.getWidth_cm(); x_cm += gridSize_cm) {
|
|
for (int y_cm = 0; y_cm < floor.getDepth_cm(); y_cm += gridSize_cm) {
|
|
|
|
// check intersection with the floorplan
|
|
const GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
|
|
if (intersects(bbox, floor)) {continue;}
|
|
|
|
// add to the grid
|
|
grid.add(T(x_cm, y_cm, z_cm));
|
|
|
|
}
|
|
}
|
|
|
|
Log::tock();
|
|
|
|
connectAdjacent(z_cm);
|
|
|
|
}
|
|
|
|
/** connect all neighboring nodes part of the given index-vector */
|
|
void connectAdjacent(const std::vector<int>& indices) {
|
|
|
|
for (const int idx : indices) {
|
|
|
|
// connect the node with its neighbors
|
|
connectAdjacent(grid[idx]);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
/** connect all neighboring nodes located on the given height-plane */
|
|
void connectAdjacent(const float z_cm) {
|
|
|
|
Log::add(name, "connecting all adjacent nodes at height " + std::to_string(z_cm), false);
|
|
Log::tick();
|
|
|
|
// connect adjacent grid-points
|
|
for (T& n1 : grid) {
|
|
|
|
// not the floor we are looking for? -> skip (ugly.. slow(er))
|
|
if (n1.z_cm != z_cm) {continue;}
|
|
|
|
// connect the node with its neighbors
|
|
connectAdjacent(n1);
|
|
|
|
}
|
|
|
|
Log::tock();
|
|
|
|
}
|
|
|
|
/** connect the given node with its neighbors */
|
|
void connectAdjacent(T& n1) {
|
|
|
|
const int gridSize_cm = grid.getGridSize_cm();
|
|
|
|
// square around the node
|
|
for (int x = -gridSize_cm; x <= gridSize_cm; x += gridSize_cm) {
|
|
for (int y = -gridSize_cm; y <= gridSize_cm; y += gridSize_cm) {
|
|
|
|
// skip the center (node itself)
|
|
if ((x == y) && (x == 0)) {continue;}
|
|
|
|
// position of the potential neighbor
|
|
const int ox = n1.x_cm + x;
|
|
const int oy = n1.y_cm + y;
|
|
const GridPoint p(ox, oy, n1.z_cm);
|
|
|
|
// does the grid contain the potential neighbor?
|
|
const T* n2 = grid.getNodePtrFor(p);
|
|
if (n2 != nullptr) {
|
|
grid.connectUniDir(n1, *n2); // UNI-dir connection as EACH node is processed!
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
float align(const float val) {
|
|
const float gridSize_cm = grid.getGridSize_cm();
|
|
return std::round(val/gridSize_cm) * gridSize_cm;
|
|
}
|
|
|
|
/** shrink the given bbox to be grid-aligned */
|
|
BBox2 shrinkAlign(const BBox2& bb) {
|
|
const float gridSize_cm = grid.getGridSize_cm();
|
|
Point2 p1 = bb.getMin();
|
|
Point2 p2 = bb.getMax();
|
|
p1.x = std::ceil(p1.x/gridSize_cm)*gridSize_cm;
|
|
p1.y = std::ceil(p1.y/gridSize_cm)*gridSize_cm;
|
|
p2.x = std::floor(p2.x/gridSize_cm)*gridSize_cm;
|
|
p2.y = std::floor(p2.y/gridSize_cm)*gridSize_cm;
|
|
BBox2 res; res.add(p1); res.add(p2); return res;
|
|
}
|
|
|
|
/** add a new platform-stair between the two given floors */
|
|
void buildPlatformStair(const PlatformStair& s, const float z1_cm, const float z2_cm) {
|
|
|
|
const float zCenter_cm = align((z2_cm + z1_cm) / 2);
|
|
std::vector<int> indices;
|
|
|
|
// add the platform in the middle
|
|
BBox2 bb = shrinkAlign(s.platform);
|
|
const int gridSize_cm = grid.getGridSize_cm();
|
|
for (int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
|
|
for (int y_cm = bb.getMin().y; y_cm <= bb.getMax().y; y_cm += gridSize_cm) {
|
|
int idx = grid.add(T(x_cm, y_cm, zCenter_cm));
|
|
indices.push_back(idx);
|
|
}
|
|
}
|
|
|
|
// connect the plattform in the middle
|
|
connectAdjacent(indices);
|
|
|
|
// TODO: interconnect (x-y) the stair lines???
|
|
buildStair(s.s1, z1_cm, zCenter_cm);
|
|
buildStair(s.s2, z2_cm, zCenter_cm);
|
|
|
|
}
|
|
|
|
void addStairs(const Stairs& stairs, const float z1_cm, const float z2_cm) {
|
|
|
|
Log::add(name, "adding stairs between " + std::to_string(z1_cm) + " and " + std::to_string(z2_cm), false);
|
|
Log::tick();
|
|
|
|
for (const Stair& s : stairs) { buildStair(s, z1_cm, z2_cm); }
|
|
|
|
Log::tock();
|
|
|
|
}
|
|
|
|
void buildStair(const Stair& s, const float z1_cm, const float z2_cm) {
|
|
|
|
// potential starting-point for the stair
|
|
for (T& n : grid) {
|
|
|
|
// node lies on the stair's starting edge?
|
|
if (n.z_cm == z1_cm && grid.getBBox(n).intersects(s.start)) {
|
|
|
|
// construct end-point by using the stair's direction
|
|
const Point3 end = Point3(n.x_cm, n.y_cm, z2_cm) + Point3(s.dir.x, s.dir.y, 0);
|
|
GridPoint gp(end.x, end.y, end.z);
|
|
|
|
// does such and end-point exist within the grid? -> construct stair-line
|
|
if (grid.hasNodeFor(gp)) {
|
|
T& n2 = (T&) grid.getNodeFor(gp);
|
|
buildStairLine(n, n2);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
/** build a stair (z-transition) from n1 to n2 */
|
|
void buildStairLine(T& _n1, T& _n2) {
|
|
|
|
// half the grid size = small steps
|
|
const int gridSize_cm = grid.getGridSize_cm();// / std::sqrt(2);
|
|
|
|
// local copies, needed for std::swap to work
|
|
T n1 = _n1; T n2 = _n2;
|
|
|
|
// ensure we work from lower to upper levels
|
|
if (n2.z_cm < n1.z_cm) { std::swap(n1, n2); }
|
|
|
|
const float zDiff = n2.z_cm - n1.z_cm;
|
|
const float xDiff = n2.x_cm - n1.x_cm;
|
|
const float yDiff = n2.y_cm - n1.y_cm;
|
|
|
|
int idx1 = n1.getIdx(); // starting node
|
|
int idx2 = -1; // next node
|
|
const int idx3 = n2.getIdx(); // final node
|
|
|
|
// move upards in gridSize steps
|
|
for (int _z = gridSize_cm; _z < zDiff - gridSize_cm; _z+= gridSize_cm) {
|
|
|
|
// calculate the percentage of reached upwards-distance
|
|
const float percent = _z/zDiff;
|
|
|
|
// adjust (x,y) accordingly (interpolate)
|
|
int x = n1.x_cm + xDiff * percent;
|
|
int y = n1.y_cm + yDiff * percent;
|
|
int z = n1.z_cm + _z;
|
|
|
|
// snap (x,y) to the grid???
|
|
//x = std::round(x / gridSize_cm) * gridSize_cm;
|
|
//y = std::round(y / gridSize_cm) * gridSize_cm;
|
|
|
|
// create a new node add it to the grid, and connect it with the previous one
|
|
idx2 = grid.addUnaligned(T(x,y,z));
|
|
grid.connectBiDir(idx1, idx2);
|
|
idx1 = idx2;
|
|
|
|
}
|
|
|
|
// add the last segment
|
|
Assert::isTrue(idx2 != -1, "strange stair issue?!");
|
|
grid.connectBiDir(idx2, idx3);
|
|
|
|
}
|
|
|
|
/** add the inverted version of the given z-layer */
|
|
void addInverted(const Grid<T>& gIn, const float z_cm) {
|
|
|
|
// get the original grid's bbox
|
|
BBox3 bb = gIn.getBBox();
|
|
|
|
const int gridSize_cm = grid.getGridSize_cm();
|
|
|
|
// build new grid-points
|
|
for(int x_cm = bb.getMin().x; x_cm <= bb.getMax().x; x_cm += gridSize_cm) {
|
|
for (int y_cm = bb.getMin().y; y_cm < bb.getMax().y; y_cm += gridSize_cm) {
|
|
|
|
// does the input-grid contain such a point?
|
|
GridPoint gp(x_cm, y_cm, z_cm);
|
|
if (gIn.hasNodeFor(gp)) {continue;}
|
|
|
|
// add to the grid
|
|
grid.add(T(x_cm, y_cm, z_cm));
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// TODO: how to determine the starting index?!
|
|
|
|
// IDEAS: find all segments:
|
|
// start at a random point, add all connected points to the set
|
|
// start at a NEW random point ( not part of the already processed points), add connected points to a new set
|
|
// repeat until all points processed
|
|
// how to handle multiple floor layers?!?!
|
|
// run after all floors AND staircases were added??
|
|
// OR: random start, check segment size, < 50% of all nodes? start again
|
|
|
|
void removeIsolated() {
|
|
|
|
Log::add(name, "searching for isolated nodes");
|
|
|
|
// get largest connected region
|
|
std::unordered_set<int> set;
|
|
do {
|
|
const int idxStart = rand() % grid.getNumNodes();
|
|
set.clear();
|
|
Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
|
|
getConnected(grid[idxStart], set);
|
|
Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");
|
|
|
|
} while (set.size() < 0.5 * grid.getNumNodes());
|
|
|
|
// remove all other
|
|
Log::add(name, "removing the isolated nodes");
|
|
for (int i = 0; i < grid.getNumNodes(); ++i) {
|
|
if (set.find(i) == set.end()) {grid.remove(i);}
|
|
}
|
|
|
|
// clean the grid
|
|
grid.cleanup();
|
|
|
|
}
|
|
|
|
/** remove all nodes not connected to n1 */
|
|
void removeIsolated(T& n1) {
|
|
|
|
// get the connected region around n1
|
|
Log::add(name, "getting set of all nodes connected to " + (std::string) n1, false);
|
|
Log::tick();
|
|
std::unordered_set<int> set;
|
|
getConnected(n1, set);
|
|
Log::tock();
|
|
|
|
// remove all other
|
|
Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
|
|
Log::tick();
|
|
for (T& n2 : grid) {
|
|
if (set.find(n2.getIdx()) == set.end()) {grid.remove(n2);}
|
|
}
|
|
Log::tock();
|
|
|
|
// clean the grid (physically delete the removed nodes)
|
|
grid.cleanup();
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
/** recursively get all connected nodes and add them to the set */
|
|
void getConnected(T& n1, std::unordered_set<int>& visited) {
|
|
|
|
std::unordered_set<int> toVisit;
|
|
toVisit.insert(n1.getIdx());
|
|
|
|
// run while there are new nodes to visit
|
|
while(!toVisit.empty()) {
|
|
|
|
// get the next node
|
|
int nextIdx = *toVisit.begin();
|
|
toVisit.erase(nextIdx);
|
|
visited.insert(nextIdx);
|
|
T& next = grid[nextIdx];
|
|
|
|
// get all his (unprocessed) neighbors and add them to the region
|
|
for (const T& n2 : grid.neighbors(next)) {
|
|
if (visited.find(n2.getIdx()) == visited.end()) {
|
|
toVisit.insert(n2.getIdx());
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
// /** recursively get all connected nodes and add them to the set */
|
|
// void getConnected(const int idx, std::unordered_set<int>& set) {
|
|
|
|
// // get the node behind idx
|
|
// const T& n1 = (T&) grid[idx];
|
|
|
|
// // add him to the current region
|
|
// set.insert(n1.getIdx());
|
|
|
|
// // get all his (unprocessed) neighbors and add them to the region
|
|
// for (const T& n2 : grid.neighbors(n1)) {
|
|
// if (set.find(n2.getIdx()) == set.end()) {
|
|
// getConnected(n2.getIdx(), set);
|
|
// }
|
|
// }
|
|
|
|
// }
|
|
|
|
// /** recursively get all connected nodes and add them to the set */
|
|
// void getConnected(const T& n1, std::unordered_set<int>& set) {
|
|
|
|
// // add him to the current region
|
|
// set.insert(n1.getIdx());
|
|
|
|
// // get all his (unprocessed) neighbors and add them to the region
|
|
// for (const T& n2 : grid.neighbors(n1)) {
|
|
// if (set.find(n2.getIdx()) == set.end()) {
|
|
// getConnected(n2, set);
|
|
// }
|
|
// }
|
|
|
|
// }
|
|
|
|
private:
|
|
|
|
/** does the bbox intersect with any of the floor's walls? */
|
|
static inline bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
|
|
for (const Line2& l : floor.getObstacles()) {
|
|
if (bbox.intersects(l)) {return true;}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
};
|
|
|
|
#endif // GRIDFACTORY_H
|