FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

Merge branch 'master' of https://git.frank-ebner.de/FHWS/Indoor

Browse Source
This commit is contained in:
k-a-z-u
2017-11-15 16:42:10 +01:00
parent 7af5131ccf 72932ad90f
commit 89c6b07e68
8 changed files with 164 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
floorplan/v2/Floorplan.h
View File

@@ -508,6 +508,9 @@ namespace Floorplan {
/** the elevator's rotation (in radians) */ /** the elevator's rotation (in radians) */
float rotation; float rotation;
/** the elevator's height (from its starting position) */
float height_m;
/** get the 4 corner points for the elevator */ /** get the 4 corner points for the elevator */
Polygon2 getPoints() const { Polygon2 getPoints() const {
const Point2 p1 = Point2(+width/2, +depth/2).rotated(rotation) + center; const Point2 p1 = Point2(+width/2, +depth/2).rotated(rotation) + center;

25
floorplan/v2/FloorplanLINT.h
View File

@@ -99,7 +99,7 @@ namespace Floorplan {
// check elevators // check elevators
for (const Elevator* e : floor->elevators) { for (const Elevator* e : floor->elevators) {
checkElevator(res, floor, e); checkElevator(res, map, floor, e);
} }
} }
@@ -222,14 +222,31 @@ namespace Floorplan {
} }
static void checkElevator(Issues& res, const Floor* floor, const Elevator* e) { static void checkElevator(Issues& res, const IndoorMap* map, const Floor* floor, const Elevator* e) {
if (e->depth < 0.5) { if (e->depth < 0.5) {
res.push_back(Issue(Type::ERROR, floor, "elevator's depth @" + e->center.asString() + " is too small: " + std::to_string(e->depth) + "m")); res.push_back(Issue(Type::ERROR, floor, "elevator's @" + e->center.asString() + ": depth is too small: " + std::to_string(e->depth) + "m"));
} }
if (e->width < 0.5) { if (e->width < 0.5) {
res.push_back(Issue(Type::ERROR, floor, "elevator's width @" + e->center.asString() + " is too small: " + std::to_string(e->width) + "m")); res.push_back(Issue(Type::ERROR, floor, "elevator's @" + e->center.asString() + ": width is too small: " + std::to_string(e->width) + "m"));
}
if (e->height_m < 0.1) {
res.push_back(Issue(Type::ERROR, floor, "elevator's @" + e->center.asString() + ": height is too small: " + std::to_string(e->height_m) + "m"));
}
// list of all heights where there is a floor;
std::vector<int> floorAtHeight_cm;
for (const Floor* f : map->floors) {
const int floorZ_cm = std::round(f->atHeight * 100);
floorAtHeight_cm.push_back(floorZ_cm); // integer height in cm
}
// disconnected end? (must be long to ANY other floor within the map)
const int elevEndZ_cm = std::round( (floor->getStartingZ() + e->height_m) * 100 );
if(std::find(floorAtHeight_cm.begin(), floorAtHeight_cm.end(), elevEndZ_cm) == floorAtHeight_cm.end()) {
res.push_back(Issue(Type::ERROR, floor, "elevator @" + e->center.asString() + " is not connected to the ending floor's ground! [open elevator end]"));
} }
} }

1
floorplan/v2/FloorplanReader.h
View File

@@ -168,6 +168,7 @@ namespace Floorplan {
elev->center = Point2(el->FloatAttribute("cx"), el->FloatAttribute("cy")); elev->center = Point2(el->FloatAttribute("cx"), el->FloatAttribute("cy"));
elev->depth = el->FloatAttribute("depth"); elev->depth = el->FloatAttribute("depth");
elev->width = el->FloatAttribute("width"); elev->width = el->FloatAttribute("width");
elev->height_m = el->FloatAttribute("height");
elev->rotation = el->FloatAttribute("rotation"); elev->rotation = el->FloatAttribute("rotation");
return elev; return elev;
} }

1
floorplan/v2/FloorplanWriter.h
View File

@@ -115,6 +115,7 @@ namespace Floorplan {
elem->SetAttribute("cy", elevator->center.y); elem->SetAttribute("cy", elevator->center.y);
elem->SetAttribute("width", elevator->width); elem->SetAttribute("width", elevator->width);
elem->SetAttribute("depth", elevator->depth); elem->SetAttribute("depth", elevator->depth);
elem->SetAttribute("height", elevator->height_m);
elem->SetAttribute("rotation", elevator->rotation); elem->SetAttribute("rotation", elevator->rotation);
elevators->InsertEndChild(elem); elevators->InsertEndChild(elem);
} }

15
grid/Grid.h
View File

@@ -231,14 +231,23 @@ public:
const uint64_t center = 1 << 19; const uint64_t center = 1 << 19;
// build // build
const uint64_t x = center + (int64_t) std::round((p.x_cm) / (float)gridSize_cm); const uint64_t x = center + (int64_t) idxX(p.x_cm);
const uint64_t y = center + (int64_t) std::round((p.y_cm) / (float)gridSize_cm); const uint64_t y = center + (int64_t) idxY(p.y_cm);
const uint64_t z = center + (int64_t) std::round((p.z_cm) / (float)gridSize_cm * 5); // z is usually much lower and not always aligned -> allow more room for hashes const uint64_t z = center + (int64_t) idxZ(p.z_cm);
return (z << 40) | (y << 20) | (x << 0); return (z << 40) | (y << 20) | (x << 0);
} }
inline int idxX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm);}
inline int idxY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm);}
inline int idxZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm);} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
inline int snapX(const int x_cm) const {return std::round(x_cm / (float)gridSize_cm) * gridSize_cm;}
inline int snapY(const int y_cm) const {return std::round(y_cm / (float)gridSize_cm) * gridSize_cm;}
inline int snapZ(const int z_cm) const {return std::round(z_cm / (float)gridSize_cm) * gridSize_cm;} // * 5?? // z is usually much lower and not always aligned -> allow more room for hashes
/** array access */ /** array access */
T& operator [] (const int idx) { T& operator [] (const int idx) {
Assert::isBetween(idx, 0, getNumNodes()-1, "index out of bounds"); Assert::isBetween(idx, 0, getNumNodes()-1, "index out of bounds");

94
grid/factory/v2/Elevators.h
View File

@@ -50,10 +50,15 @@ public:
std::vector<IntPos> nodesWithin; std::vector<IntPos> nodesWithin;
const HelperPoly poly(elevator->getPoints()); const HelperPoly poly(elevator->getPoints());
// elevator starts at the current floor, but where does the elevator end?
const int f1_cm = grid.snapZ(floor->getStartingZ()*100);
// elevator's end is given by its height
const int f2_cm = grid.snapZ( (floor->getStartingZ() + elevator->height_m) * 100);
auto callback = [&] (const int x_cm, const int y_cm) { auto callback = [&] (const int x_cm, const int y_cm) {
const GridPoint gp1(x_cm, y_cm, floor->getStartingZ()*100); // starting floor const GridPoint gp1(x_cm, y_cm, f1_cm); // starting floor
const GridPoint gp2(x_cm, y_cm, floor->getEndingZ()*100); // the floor above const GridPoint gp2(x_cm, y_cm, f2_cm); // the floor above
// ensure such a node is present in both floors (and thus a connection is possible) // ensure such a node is present in both floors (and thus a connection is possible)
if (grid.hasNodeFor(gp1) && grid.hasNodeFor(gp2)) { if (grid.hasNodeFor(gp1) && grid.hasNodeFor(gp2)) {
@@ -63,34 +68,85 @@ public:
}; };
poly.forEachGridPoint(gs_cm, callback); poly.forEachGridPoint(gs_cm, callback);
if (nodesWithin.empty()) {
throw Exception("faild to determine starting and ending nodes for elevator. disconnected?");
}
// now create the interconnection in z-direction // 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 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 //const int z2_cm = std::floor((floor->getEndingZ()*100-1) / gs_cm) * gs_cm; // the last node below the next floor
int z1_cm = std::ceil((f1_cm+1.0f) / gs_cm) * gs_cm;
int z2_cm = std::floor((f2_cm-1.0f) / gs_cm) * gs_cm;
// ensure the nodes between (z1,z2) are not directly the floor (f1,f2)
//if (grid.snapZ(z1_cm) == grid.snapZ(f1_cm)) {z1_cm += gs_cm;}
//if (grid.snapZ(z2_cm) == grid.snapZ(f2_cm)) {z2_cm -= gs_cm;}
for (const IntPos nodePos : nodesWithin) { for (const IntPos nodePos : nodesWithin) {
// create nodes BETWEEN the two floors (skip the floors themselves! -> floor1+gridSize <-> floor2-gridSize // 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) { 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 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 //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 if (grid.hasNodeFor(gp1)) {
grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type const int idx = grid.getNodeFor(gp1).getIdx();
grid[idx].setType(GridNode::TYPE_ELEVATOR); // set the node-type
} else {
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) // connect each of the new nodes with the node below it
for (int z_cm = z1_cm; z_cm <= z2_cm + gs_cm; z_cm += gs_cm) { // also connect the elevator to the starting and ending floor
GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, z_cm-gs_cm); for (int z_cm = z1_cm; z_cm <= z2_cm; z_cm += gs_cm) {
GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
// above the ending floor? -> snap to ending floor // directly above starting floor
// note: this one is needed if the floor-heights are not dividable by the grid-size if (z_cm == z1_cm) {
if (gp.z_cm > floor->getEndingZ()*100) {gp.z_cm = floor->getEndingZ()*100;}
// connect with floor below
const GridPoint gpBelow(nodePos.x_cm, nodePos.y_cm, f1_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");
const T& n1 = grid.getNodeFor(gpBelow);
const T& n2 = grid.getNodeFor(gp);
grid.connectBiDir(n1.getIdx(), n2.getIdx());
grid[n1.getIdx()].setType(GridNode::TYPE_ELEVATOR);
} else {
// connect with node below
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");
const T& n1 = grid.getNodeFor(gpBelow);
const T& n2 = grid.getNodeFor(gp);
grid.connectBiDir(n1.getIdx(), n2.getIdx());
}
// directly below ending floor
if (z_cm == z2_cm) {
// connect with floor above
const GridPoint gpAbove(nodePos.x_cm, nodePos.y_cm, f2_cm);
const GridPoint gp(nodePos.x_cm, nodePos.y_cm, z_cm);
Assert::isTrue(grid.hasNodeFor(gpAbove), "missing node");
Assert::isTrue(grid.hasNodeFor(gp), "missing node");
const T& n1 = grid.getNodeFor(gpAbove);
const T& n2 = grid.getNodeFor(gp);
//if (n1.getIdx() == n2.getIdx()) {continue;}
grid.connectBiDir(n1.getIdx(), n2.getIdx());
grid[n1.getIdx()].setType(GridNode::TYPE_ELEVATOR);
}
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);
} }
} }

2
grid/factory/v2/GridFactory.h
View File

@@ -198,7 +198,7 @@ public:
const int x2 = helper.align(bbox.getMax().x); const int x2 = helper.align(bbox.getMax().x);
const int y1 = helper.align(bbox.getMin().y); const int y1 = helper.align(bbox.getMin().y);
const int y2 = helper.align(bbox.getMax().y); const int y2 = helper.align(bbox.getMax().y);
const int z_cm = (floor->atHeight*100); const int z_cm = std::round(floor->atHeight*100);
const int total = (x2-x1) / helper.gridSize(); const int total = (x2-x1) / helper.gridSize();
int cur = 0; int cur = 0;

63
grid/factory/v2/Stairs2.h
View File

@@ -155,18 +155,24 @@ public:
} }
for (StairNode& sn : stairNodes) { for (StairNode& sn : stairNodes) {
const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ); // current percentage from minZ to maxZ const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ); // current percentage from minZ to maxZ WHICH ONE IS CORRECT?
//sn.z_cm = floor->getStartingZ()*100 + zPercent * floor->height*100; // apply percentage to floorStartZ <-> floorEndZ //const float zPercent = (sn.z_cm - stairAbsStart_cm) / (stairAbsEnd_cm - stairAbsStart_cm); // current percentage from minZ to maxZ WHICH ONE IS CORRECT?
sn.z_cm = std::round(stairAbsStart_cm + zPercent * stairHeight_cm); // apply percentage to floorStartZ <-> floorEndZ //sn.z_cm = floor->getStartingZ()*100 + zPercent * floor->height*100; // apply percentage to floorStartZ <-> floorEndZ
sn.z_cm = std::round(stairAbsStart_cm + zPercent * stairHeight_cm); // apply percentage to floorStartZ <-> floorEndZ
} }
// snap stair-nodes to nearby grid nodes, if possible
if (tryImproveStairConnections) {
for (StairNode& sn : stairNodes) { // // snap stair-nodes to nearby grid nodes, if possible
if (std::abs(sn.z_cm-stairAbsStart_cm) < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)) ) {sn.z_cm = stairAbsStart_cm;} // // we try to improve the number of connections between stair and starting/ending floor
if (std::abs(sn.z_cm-stairAbsEnd_cm) < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsEnd_cm))) {sn.z_cm = stairAbsEnd_cm;} // if (tryImproveStairConnections) {
} // for (StairNode& sn : stairNodes) {
} // //if (std::abs(sn.z_cm-stairAbsStart_cm) < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)) ) {sn.z_cm = stairAbsStart_cm;}
// //if (std::abs(sn.z_cm-stairAbsEnd_cm) < gs_cm*0.75 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsEnd_cm))) {sn.z_cm = stairAbsEnd_cm;}
// //if (std::abs(sn.z_cm-stairAbsStart_cm) < gs_cm*1.5 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)) ) {auxcon.push_back(AuxConnection(sn, GridPoint(sn.x_cm, sn.y_cm, stairAbsStart_cm)));}
// //if (std::abs(sn.z_cm-stairAbsEnd_cm) < gs_cm*1.5 && grid.hasNodeFor(GridPoint(sn.x_cm, sn.y_cm, stairAbsEnd_cm))) {auxcon.push_back(AuxConnection(sn, GridPoint(sn.x_cm, sn.y_cm, stairAbsEnd_cm)));}
// }
// }
// stort all stair-nodes by z (ascending) // stort all stair-nodes by z (ascending)
const auto comp = [] (const StairNode& sn1, const StairNode& sn2) {return sn1.z_cm < sn2.z_cm;}; const auto comp = [] (const StairNode& sn1, const StairNode& sn2) {return sn1.z_cm < sn2.z_cm;};
@@ -235,6 +241,40 @@ public:
} }
// for larger grid-sizes stair connections to starting/ending floors are quite bad if the stairs are not 90degree aligned
// to improve the number of connections between floor and stair, we brute-force search for connectable nodes (between stair and floor, given threshold)
if (tryImproveStairConnections) {
// connect all stair-nodes to the floor if their distance is below this threshold
const int maxDist_cm = gs_cm * 1.2;
for (StairNode& sn : stairNodes) {
const auto& gn1 = grid[sn.gridIdx];
// all stair-nodes that are near to a floor
const bool lower = std::abs(sn.z_cm-stairAbsStart_cm) < maxDist_cm;
const bool upper = std::abs(sn.z_cm-stairAbsEnd_cm) < maxDist_cm;
if (lower || upper) {
// cross-check with each grid node (slow...)
for (const auto& gn2 : grid.getNodes()) {
if (gn2.getDistanceInCM(gn1) > maxDist_cm) {continue;} // connect with a floor-node near the stair-node
if (gn1.hasNeighbor(gn2.getIdx())) {continue;} // already connected?
//if (gn2.hasNeighbor(gn1.getIdx())) {continue;}
if (gn2.getIdx() == gn1.getIdx()) {continue;} // do not connect with myself
if (gn2.fullyConnected()) {continue;} // skip full nodes
if (gn1.fullyConnected()) {continue;} // skip full nodes
grid.connectBiDir(gn1.getIdx(), gn2.getIdx()); // connect
}
}
}
}
// sanity check // sanity check
// connectedWithHeights should contain 2 entries: // connectedWithHeights should contain 2 entries:
// one for the starting floor // one for the starting floor
@@ -274,11 +314,8 @@ public:
} }
} }
} }