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

worked on grid creation

Browse Source 
fixed some issues with stairs
fixed/added LINT
This commit is contained in:
k-a-z-u
2017-07-27 18:40:56 +02:00
parent 3a92199777
commit c21925e86f
5 changed files with 114 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
grid/Grid.h
View File

@@ -268,7 +268,7 @@ public:
}
/** remove the connection from n1 to n2 (not the other way round!) */
void disconnectUniDir(T& n1, T& n2) {
void disconnectUniDir(T& n1, const T& n2) {
for (int n = 0; n < n1._numNeighbors; ++n) {
if (n1._neighbors[n] == n2._idx) {
arrayRemove(n1._neighbors, n, n1._numNeighbors);
@@ -301,12 +301,16 @@ public:
void remove(T& node) {
// disconnect from all neighbors
int cnt = 0;
while (node._numNeighbors) {
// by removing one neighbor, all others are shifted to close the gap within the array
// its therefor ok to always delete array index [0]
disconnectBiDir(node._idx, node._neighbors[0]);
if (++cnt > node.MAX_NEIGHBORS) {
Log::add(name, "WARNING! found unsolveable neighboring! " + node.asString(), true);
break;
}
}
// remove from hash-list

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

@@ -89,7 +89,7 @@ public:
if (listener) {listener->onGridBuildUpdateMajor("adding stairs");}
if (_buildStairs) {
for (Floorplan::Floor* f : map->floors) {
buildStairs(f, listener);
buildStairs(map, f, listener);
if (listener) {listener->onGridBuildUpdateMajor(total, ++cur);}
}
}
@@ -215,7 +215,7 @@ public:
GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), helper.gridSize());
// slightly grow the bbox to ensure even obstacles that are directly aligned to the bbox are hit
bbox.grow(0.42345);
bbox.grow(0.1337);
if (intersects(bbox, floor)) {continue;}
// add to the grid [once]
@@ -246,7 +246,7 @@ public:
}
void buildStairs(const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
void buildStairs(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, GridFactoryListener* listener = nullptr) {
const int total = floor->stairs.size();
int cur = 0;
@@ -254,7 +254,7 @@ public:
// process each stair within the floor
for (const Floorplan::Stair* stair : floor->stairs) {
if (listener) {listener->onGridBuildUpdateMinor("adding " + floor->name + " stair " + std::to_string(cur+1));}
stairs.build(floor, stair);
stairs.build(map, floor, stair);
if (listener) {listener->onGridBuildUpdateMinor(total, ++cur);}
}
@@ -419,6 +419,9 @@ public:
void removeIsolatedNodes() {
//std::cout << "todo: remove" << std::endl;
//return;
// try to start at the first stair
for (T& n : grid) {
if (n.getType() == GridNode::TYPE_STAIR) {removeIsolatedNodes(n); return;}
@@ -439,6 +442,9 @@ public:
getConnected(n1, set);
Log::tock();
//const int numToRemove = grid.getNumNodes() - set.size();
//int numRemoved = 0;
// remove all other
Log::add(name, "removing all nodes NOT connected to " + (std::string) n1, false);
Log::tick();
@@ -455,6 +461,9 @@ public:
// proceed ;)
grid.remove(n2);
//++numRemoved;
//std::cout << numRemoved << ":" << numToRemove << std::endl;
}
}
Log::tock();

14
grid/factory/v2/Helper.h
View File

@@ -254,13 +254,13 @@ public:
}
static bool bary(Point2 p, Point2 a, Point2 b, Point2 c, float &u, float &v, float &w) {
Point2 v0 = b - a, v1 = c - a, v2 = p - a;
float d00 = dot(v0, v0);
float d01 = dot(v0, v1);
float d11 = dot(v1, v1);
float d20 = dot(v2, v0);
float d21 = dot(v2, v1);
float denom = d00 * d11 - d01 * d01;
const Point2 v0 = b - a, v1 = c - a, v2 = p - a;
double d00 = dot(v0, v0);
double d01 = dot(v0, v1);
double d11 = dot(v1, v1);
double d20 = dot(v2, v0);
double d21 = dot(v2, v1);
double denom = d00 * d11 - d01 * d01;
v = (d11 * d20 - d01 * d21) / denom;
w = (d00 * d21 - d01 * d20) / denom;
u = 1.0f - v - w;

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

@@ -31,19 +31,23 @@ private:
// keep a list of all vertices below stairwells and remove them hereafter
std::vector<T*> toDelete;
bool tryImproveStairConnections = true;
private:
/** helper struct */
struct StairNode {
const int x_cm;
const int y_cm;
const int belongsToQuadIdx;
int x_cm;
int y_cm;
int belongsToQuadIdx;
float z_cm; // interpolated
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) {;}
};
@@ -62,7 +66,15 @@ public:
void build(const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
void build(const Floorplan::IndoorMap* map, const Floorplan::Floor* floor, const Floorplan::Stair* stair) {
// starting-height of all available floors (in cm)
// needed to ensure that stairs start and end at floors
std::vector<int> floorsAtHeight_cm;
for (const Floorplan::Floor* f : map->floors) {
const int floorAtHeight_cm = std::round(f->atHeight*100);
floorsAtHeight_cm.push_back(floorAtHeight_cm);
}
const int gs_cm = grid.getGridSize_cm();
@@ -123,20 +135,56 @@ public:
}
const float stairAbsStart_m = floor->atHeight + stair->getParts().front().start.z;
const float stairAbsEnd_m = floor->atHeight + stair->getParts().back().end.z;
//const float stairHeight_m = stairAbsEnd_m - stairAbsStart_m;
const float stairAbsStart_cm = std::round(stairAbsStart_m*100);
const float stairAbsEnd_cm = std::round(stairAbsEnd_m*100);
const float stairHeight_cm = stairAbsEnd_cm - stairAbsStart_cm;
// this might lead to stairs the start slightly above the starting-floor
// or ending slightly below the ending floor. this would lead to DISCONNECTION!
// therefore re-scale the z-values to ensure they start at floor1 and end at floor 2
float minZ = +9999999;
float maxZ = -9999999;
for (const StairNode& sn : stairNodes) {
if (sn.z_cm < minZ) {minZ = sn.z_cm;}
if (sn.z_cm > maxZ) {maxZ = sn.z_cm;}
}
for (StairNode& sn : stairNodes) {
const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ); // current percentage from minZ to maxZ
sn.z_cm = floor->getStartingZ()*100 + zPercent * floor->height*100; // apply percentage to floorStartZ <-> floorEndZ
{
float minZ = +9999999;
float maxZ = -9999999;
for (const StairNode& sn : stairNodes) {
if (sn.z_cm < minZ) {minZ = sn.z_cm;}
if (sn.z_cm > maxZ) {maxZ = sn.z_cm;}
}
for (StairNode& sn : stairNodes) {
const float zPercent = (sn.z_cm - minZ) / (maxZ - minZ); // current percentage from minZ to maxZ
//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) {
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;}
}
}
// stort all stair-nodes by z (ascending)
const auto comp = [] (const StairNode& sn1, const StairNode& sn2) {return sn1.z_cm < sn2.z_cm;};
std::sort(stairNodes.begin(), stairNodes.end(), comp);
// get first and last stair note
const bool end1OK = std::find(floorsAtHeight_cm.begin(), floorsAtHeight_cm.end(), stairNodes.front().z_cm) != floorsAtHeight_cm.end();
const bool end2OK = std::find(floorsAtHeight_cm.begin(), floorsAtHeight_cm.end(), stairNodes.back().z_cm) != floorsAtHeight_cm.end();
// be sure both are connected to a floor
if (!end1OK || !end2OK) {
throw Exception("stair's start or end is not directly connectable to a floor");
}
}
// track z-positions of already-existing grid nodes we connected the stair to
// if this list contains 2 distinctive values, the stair is successfully connected to starting and ending floor!
std::unordered_set<float> connectedWithHeights;
@@ -167,15 +215,16 @@ public:
// check if there is a nearby floor-node to delete
// -> remove nodes directly above/below the stair
const int deleteDist_cm = 100;
const float distToBelow = gp.z_cm - floor->getStartingZ()*100;
const float distToAbove = floor->getEndingZ()*100 - gp.z_cm;
const float distToBelow = gp.z_cm - stairAbsStart_cm;//floor->getStartingZ()*100;
const float distToAbove = stairAbsEnd_cm - gp.z_cm;//floor->getEndingZ()*100 - gp.z_cm;
//if (distToBelow > gs_cm && distToBelow < deleteDist_cm) {
if (distToBelow > 0 && distToBelow < deleteDist_cm) {
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getStartingZ()*100));
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, stairAbsStart_cm));//floor->getStartingZ()*100));
if (n) {toDelete.push_back(n);}
//} else if (distToAbove > gs_cm && distToAbove < deleteDist_cm) {
} else if (distToAbove > 0 && distToAbove < deleteDist_cm) {
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, floor->getEndingZ()*100));
}
if (distToAbove > 0 && distToAbove < deleteDist_cm) {
T* n = (T*) grid.getNodePtrFor(GridPoint(gp.x_cm, gp.y_cm, stairAbsEnd_cm));//floor->getEndingZ()*100));
if (n) {toDelete.push_back(n);}
}
@@ -191,7 +240,7 @@ public:
// one for the starting floor
// one for the ending floor
// this mainly fails, when there is a REMOVING outline-area that removes to many nodes and the stair can not be connected
Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!");
Assert::isTrue(connectedWithHeights.size() == 2, "stair is not correctly connected to starting and ending floor!");
// now connect all new nodes with their neighbors
// do not perform normal grid-connection but examine the nodes within the generated vector