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

dijkstra is now bleching fast

Browse Source 
deleting from the grid is now bleaching fast
added new helper methods
many new test-cases
many new methods for geo classes and others
added a bunch of new grid-walkers
This commit is contained in:
FrankE
2016-01-26 18:13:30 +01:00
parent b503fb9bdc
commit e6329e1db4
26 changed files with 824 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
grid/factory/GridFactory.h
View File

@@ -42,7 +42,7 @@ public:
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);
const GridNodeBBox bbox(GridPoint(x_cm, y_cm, z_cm), gridSize_cm);
if (intersects(bbox, floor)) {continue;}
// add to the grid
@@ -101,20 +101,19 @@ public:
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));
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) {
for (int i = 0; i < grid.getNumNodes(); ++i) {
// potential starting-point for the stair
for (T& n : grid) {
// 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) )) {
// 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, n.z_cm) + Point3(s.dir.x, s.dir.y, (z2_cm-z1_cm));
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 grap? -> construct stair
@@ -130,6 +129,8 @@ public:
}
Log::tock();
}
/** build a stair (z-transition) from n1 to n2 */
@@ -147,14 +148,15 @@ public:
const int gridSize_cm = grid.getGridSize_cm();
// move upards in gridSize steps
for (int z = gridSize_cm; z < zDiff; z+= gridSize_cm) {
for (int _z = gridSize_cm; _z < zDiff; _z+= gridSize_cm) {
// calculate the percentage of reached upwards-distance
const float percent = z/zDiff;
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;
@@ -218,7 +220,7 @@ public:
const int idxStart = rand() % grid.getNumNodes();
set.clear();
Log::add(name, "getting connected region starting at " + (std::string) grid[idxStart]);
getConnected(idxStart, set);
getConnected(grid[idxStart], set);
Log::add(name, "region size is " + std::to_string(set.size()) + " nodes");
} while (set.size() < 0.5 * grid.getNumNodes());
@@ -234,30 +236,94 @@ public:
}
private:
/** remove all nodes not connected to n1 */
void removeIsolated(T& n1) {
/** recursively get all connected nodes and add them to the set */
void getConnected(const int idx, std::unordered_set<int>& set) {
// 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();
// 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);
}
// 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? */
bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
static inline bool intersects(const GridNodeBBox& bbox, const Floor& floor) {
for (const Line2& l : floor.getObstacles()) {
if (bbox.intersects(l)) {return true;}
}