kazu
9947dced15
added new helper methods/classes (e.g. for heading) new test cases optimize the dijkstra cleanups/refactoring added timed-benchmarks to the log many more...
255 lines
6.3 KiB
C++
Executable File
255 lines
6.3 KiB
C++
Executable File
#ifndef GRIDFACTORY_H
|
|
#define GRIDFACTORY_H
|
|
|
|
#include <string>
|
|
#include "../../floorplan/Floor.h"
|
|
#include "../../floorplan/Stairs.h"
|
|
|
|
#include "../../geo/Units.h"
|
|
#include "../GridNodeBBox.h"
|
|
#include "../Grid.h"
|
|
|
|
#include "../../misc/Debug.h"
|
|
|
|
template <int gridSize_cm, typename T> class GridFactory {
|
|
|
|
/** logging name */
|
|
static constexpr const char* name = "GridFac";
|
|
|
|
private:
|
|
|
|
/** the grid to build into */
|
|
Grid<gridSize_cm, T>& grid;
|
|
|
|
|
|
public:
|
|
|
|
/** ctor with the grid to fill */
|
|
GridFactory(Grid<gridSize_cm, 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));
|
|
|
|
// 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
|
|
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));
|
|
|
|
}
|
|
}
|
|
|
|
connectAdjacent(z_cm);
|
|
|
|
}
|
|
|
|
/** 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));
|
|
|
|
// connect adjacent grid-points
|
|
for (int idx = 0; idx < grid.getNumNodes(); ++idx) {
|
|
|
|
T& n1 = (T&) grid[idx];
|
|
if (n1.z_cm != z_cm) {continue;} // ugly... different floor -> skip
|
|
|
|
// square around each point
|
|
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
|
|
int ox = n1.x_cm + x;
|
|
int oy = n1.y_cm + y;
|
|
GridPoint p(ox, oy, n1.z_cm);
|
|
|
|
// does the grid contain the potential neighbor?
|
|
if (grid.hasNodeFor(p)) {
|
|
T& n2 = (T&) grid.getNodeFor(p);
|
|
grid.connectUniDir(n1, n2);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
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));
|
|
|
|
for (const Stair& s : stairs) {
|
|
|
|
for (int i = 0; i < grid.getNumNodes(); ++i) {
|
|
|
|
// potential starting-point for the stair
|
|
T& n = (T&) grid[i];
|
|
|
|
// real starting point for the stair?
|
|
if (s.from.contains( Point2(n.x_cm, n.y_cm) )) {
|
|
|
|
// construct end-point by using the stair's direction
|
|
const Point3 end = Point3(n.x_cm, n.y_cm, n.z_cm) + Point3(s.dir.x, s.dir.y, (z2_cm-z1_cm));
|
|
GridPoint gp(end.x, end.y, end.z);
|
|
|
|
// does such and end-point exist within the grap? -> construct stair
|
|
if (grid.hasNodeFor(gp)) {
|
|
T& n2 = (T&) grid.getNodeFor(gp);
|
|
|
|
buildStair(n, n2);
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
/** build a stair (z-transition) from n1 to n2 */
|
|
void buildStair(T& n1, T& n2) {
|
|
|
|
//TODO: ensure n1 is below 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();
|
|
int idx2 = -1;
|
|
const int idx3 = n2.getIdx();
|
|
|
|
// move upards in gridSize steps
|
|
for (int z = gridSize_cm; z < zDiff; 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;
|
|
|
|
// 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
|
|
if (idx2 != -1) {
|
|
grid.connectBiDir(idx2, idx3);
|
|
}
|
|
|
|
}
|
|
|
|
/** add the inverted version of the given z-layer */
|
|
void addInverted(const Grid<gridSize_cm, T>& gIn, const float z_cm) {
|
|
|
|
// get the original grid's bbox
|
|
BBox3 bb = gIn.getBBox();
|
|
|
|
// 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::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(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();
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
/** recursively get all connected nodes and add them to the set */
|
|
void getConnected(const int idx, std::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);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
/** does the bbox intersect with any of the floor's walls? */
|
|
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
|