added new sanity checks and compile-time assertions to prevent errors

fixed stair-building issue
new test-cases
added elevator support
fixed/improved some walker modules
This commit is contained in:
2016-09-10 15:12:39 +02:00
parent 7baeecb3f9
commit 82f8828a04
26 changed files with 996 additions and 198 deletions

View File

@@ -0,0 +1,99 @@
#ifndef GRID_ELEVATORS_H
#define GRID_ELEVATORS_H
#include <vector>
#include <set>
#include "../../Grid.h"
#include "Helper.h"
#include "../../../floorplan/v2/Floorplan.h"
#include <fstream>
template <typename T> class Elevators {
private:
/** the grid to build into */
Grid<T>& grid;
/** calculation helper */
Helper<T> helper;
public:
/** ctor */
Elevators(Grid<T>& grid) : grid(grid), helper(grid) {
;
}
~Elevators() {
;
}
/** build the given elevator */
void build(const Floorplan::Floor* floor, const Floorplan::Elevator* elevator) {
const int gs_cm = grid.getGridSize_cm();
struct IntPos {
int x_cm;
int y_cm;
IntPos(int x_cm, int y_cm) : x_cm(x_cm), y_cm(y_cm) {;}
};
// identify all grid-aligned nodes that belong to the elevator
std::vector<IntPos> nodesWithin;
const HelperPoly poly(elevator->getPoints());
auto callback = [&] (const int x_cm, const int y_cm) {
const GridPoint gp1(x_cm, y_cm, floor->getStartingZ()*100); // starting floor
const GridPoint gp2(x_cm, y_cm, floor->getEndingZ()*100); // the floor above
// ensure such a node is present in both floors (and thus a connection is possible)
if (grid.hasNodeFor(gp1) && grid.hasNodeFor(gp2)) {
nodesWithin.push_back(IntPos(x_cm, y_cm));
}
};
poly.forEachGridPoint(gs_cm, callback);
// now create the interconnection in z-direction
const int z1_cm = std::ceil((floor->getStartingZ()*100+1) / gs_cm) * gs_cm; // the next node above the current flor
const int z2_cm = std::floor((floor->getEndingZ()*100-1) / gs_cm) * gs_cm; // the last node below the next floor
for (const IntPos nodePos : nodesWithin) {
// create nodes BETWEEN the two floors (skip the floors themselves! -> floor1+gridSize <-> floor2-gridSize
for (int z_cm = z1_cm; z_cm <= z2_cm; z_cm += gs_cm) {
const GridPoint gp1(nodePos.x_cm, nodePos.y_cm, z_cm); // the to-be-added node
Assert::isFalse(grid.hasNodeFor(gp1), "elevator collission"); // such a node must not yet exist! otherwise we e.g. collide with a stari
const int idx = grid.add(T(gp1.x_cm, gp1.y_cm, gp1.z_cm)); // create the node
grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type
}
// connect each of the new nodes with the node below it. NOW ALSO EXAMINE THE floor above (z2_cm + gs_cm)
for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) {
const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm);
const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
Assert::isTrue(grid.hasNodeFor(gpBelow), "missing node");
Assert::isTrue(grid.hasNodeFor(gp), "missing node");
T& n1 = (T&) grid.getNodeFor(gpBelow);
T& n2 = (T&) grid.getNodeFor(gp);
grid.connectBiDir(n1, n2);
}
}
}
};
#endif // GRID_ELEVATORS_H

View File

@@ -8,7 +8,8 @@
#include "../../../floorplan/v2/Floorplan.h"
#include "Helper.h"
#include "Stairs.h"
#include "Stairs2.h"
#include "Elevators.h"
#include "../../../geo/Units.h"
#include "../../GridNodeBBox.h"
@@ -37,6 +38,9 @@ private:
/** stair builder */
Stairs<T> stairs;
/** elevator builder */
Elevators<T> elevators;
bool _buildStairs = true;
bool _removeIsolated = true;
@@ -44,7 +48,9 @@ private:
public:
/** ctor with the grid to fill */
GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid) {;}
explicit GridFactory(Grid<T>& grid) : grid(grid), helper(grid), stairs(grid), elevators(grid) {
}
/** whether or not to build stairs */
@@ -59,7 +65,7 @@ public:
Log::add(name, "building grid from IndoorMap", true);
const int total = map->floors.size()*2 + 1;
const int total = map->floors.size()*3 + 1;
int cur = 0;
// build all the floors
@@ -78,6 +84,15 @@ public:
}
}
// build all elevators
if (listener) {listener->onGridBuildUpdateMajor("adding elevators");}
if (_buildStairs) {
for (Floorplan::Floor* f : map->floors) {
buildElevators(f, listener);
if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
}
}
// remove isolated nodes
if (_removeIsolated) {
if (listener) {listener->onGridBuildUpdateMajor("removing isolated nodes");}
@@ -180,78 +195,23 @@ public:
}
void buildElevators(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
const int total = floor->elevators.size();
int cur = 0;
// process each elevator within the floor
for (const Floorplan::Elevator* elevator : floor->elevators) {
if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " elevator " + std::to_string(cur+1));}
elevators.build(floor, elevator);
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
}
// void addWithin(const Floorplan::Floor* floor, std::vector<Entry>& nodeList) {
// const int fz1_cm = floor->atHeight*100;
// const int fz2_cm = (floor->atHeight+floor->height)*100;
// for (Entry& e : nodeList) {
// if ( (e.pos.z_cm <= fz1_cm) || (e.pos.z_cm >= fz2_cm) ) {
// e.idx = grid.getNearestNode(e.pos).getIdx();
// e.part = {9999};
// } else {
// const T t(e.pos.x_cm, e.pos.y_cm, e.pos.z_cm);
// e.idx = grid.addUnaligned(t, false);
// }
// }
// }
// void filter(std::vector<Entry>& nodeList) {
// const int gs_cm = grid.getGridSize_cm();
// const int limit_cm = gs_cm * 0.50;
// // remove duplicate nodes or nodes, that are too close to each other
// for(auto it = nodeList.begin(); it != nodeList.end(); ) {
// // currently examined node
// const Entry& e1 = *it;
// // matches for nodes that are NOT the same instance AND nearby (< grid-size)
// auto matches = [&] (const Entry& e2) { return (e1.pos != e2.pos) && (e1.pos.getDistanceInCM(e2.pos) <= limit_cm); };
// auto match = std::find_if(nodeList.begin(), nodeList.end(), matches);
// // remove if this node has a nearby neighbor
// if (match != nodeList.end()) {
// // combine both nodes within one:
// // the node belongs to more than one stair-part
// Entry& e2 = *match;
// e2.part.insert(e2.part.end(), e1.part.begin(), e1.part.end());
// it = nodeList.erase(it);
// } else {
// ++it;
// }
// }
// }
// std::vector<Entry> minOnly(const std::vector<Entry>& lst) {
// auto comp = [&] (const Entry& a, const Entry& b) {return grid[a.idx].z_cm < grid[b.idx].z_cm;};
// auto it = std::min_element(lst.begin(), lst.end(), comp);
// T& min = grid[it->idx];
// std::vector<Entry> res;
// for (const Entry& e : lst) {
// if (grid[e.idx].z_cm == min.z_cm) {res.push_back(e);}
// }
// return res;
// }
/** connect all neighboring nodes part of the given index-vector */
void connectAdjacent(const std::vector<int>& indices) {

View File

@@ -26,7 +26,12 @@ struct HelperPoly {
/** ctor from floorplan-quad */
HelperPoly(const Floorplan::Quad3& quad) {
add(quad.p1); add(quad.p2); add(quad.p3); add(quad.p4);
add(quad.p1*100); add(quad.p2*100); add(quad.p3*100); add(quad.p4*100);
}
/** ctor from floorplan-polygon */
HelperPoly(const Floorplan::Polygon2& poly) {
for (Point2 p : poly.points) { add(p * 100); }
}
void add(const Point2 p) {
@@ -67,6 +72,30 @@ struct HelperPoly {
}
/** call a user-function for each GRID-ALIGNED point within the polygon */
void forEachGridPoint(const int gridSize_cm, std::function<void(int x_cm, int y_cm)> callback) const {
int x1 = std::floor(bbox_cm.getMin().x / gridSize_cm) * gridSize_cm;
int x2 = std::ceil(bbox_cm.getMax().x / gridSize_cm) * gridSize_cm;
int y1 = std::floor(bbox_cm.getMin().y / gridSize_cm) * gridSize_cm;
int y2 = std::ceil(bbox_cm.getMax().y / gridSize_cm) * gridSize_cm;
// process each point within the (aligned) bbox
for (int y = y1; y <= y2; y += gridSize_cm) {
for (int x = x1; x <= x2; x += gridSize_cm) {
// does this point belong to the polygon?
if (!contains(Point2(x,y))) {continue;}
// call the callback
callback(x,y);
}
}
}
};
@@ -94,7 +123,7 @@ public:
}
/** connect the given node to all its neighbors */
/** connect the given node to all its neighbors (x,y) */
void connectToNeighbors(T& n1) {
const int gs_cm = grid.getGridSize_cm();
@@ -120,6 +149,32 @@ public:
}
/** connect the given node to all its neighbors )x,y,z) */
void connectToNeighborsXYZ(T& n1) {
const int gs_cm = grid.getGridSize_cm();
for (int z = -gs_cm; z <= +gs_cm; z += gs_cm) {
for (int y = -gs_cm; y <= +gs_cm; y += gs_cm) {
for (int x = -gs_cm; x <= +gs_cm; x += gs_cm) {
// skip the node itself
if (x == 0 && y == 0 && z == 0) {continue;}
// try to find a matching neighbor
const GridPoint gp(n1.x_cm + x, n1.y_cm + y, n1.z_cm + z);
const T* n2 = grid.getNodePtrFor(gp);
if (!n2) {continue;}
// connect
if (n1.hasNeighbor(n2->getIdx())) {continue;}
grid.connectUniDir(n1, *n2);
}
}
}
}
int gridSize() const {
return grid.getGridSize_cm();;

View File

@@ -72,6 +72,7 @@ public:
switch(n1.getType()) {
case GridNode::TYPE_DOOR: doors.push_back(n1); break;
case GridNode::TYPE_STAIR: stairs.push_back(n1); break;
case GridNode::TYPE_ELEVATOR: stairs.push_back(n1); break;
}
}
@@ -87,7 +88,7 @@ public:
// probability adjustments
Distribution::Normal<float> avoidWalls(0.0, 0.35);
Distribution::Normal<float> favorDoors(0.0f, 0.5f);
Distribution::Normal<float> favorStairs(0.0f, 3.5f);
Distribution::Normal<float> favorStairs(0.0f, 1.5f);
if (l) {
l->onGridBuildUpdateMajor(2, 1);
@@ -124,7 +125,7 @@ public:
// final probability
n1.navImportance = 1.0f;
n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 2.5f;
n1.navImportance += favorStairs.getProbability(distToStair_m) * 30.5f;
// use wall avoidance
if (useNormal) {

207
grid/factory/v2/Stairs2.h Normal file
View File

@@ -0,0 +1,207 @@
#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;
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);}
});
}
// 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) {
// 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)) {
z_cm = p1.z*u + p2.z*v + p3.z*w;
} else {
helper.bary(p, p1.xy(), p3.xy(), p4.xy(), u, v, w);
z_cm = p1.z*u + p3.z*v + p4.z*w;
}
// the to-be-added position
GridPoint gp(sn.x_cm, sn.y_cm, 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();
} 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);}
}
}
}
// 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