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

new bbox features (2D, 3D)

Browse Source 
added stair-sanitizing code
adjusted walkers
This commit is contained in:
FrankE
2016-02-06 15:02:41 +01:00
parent 382a046df1
commit 5b35b5d3e0
4 changed files with 74 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
grid/factory/GridFactory.h
View File

@@ -187,7 +187,7 @@ public:
// does such and end-point exist within the grid? -> construct stair-line
if (grid.hasNodeFor(gp)) {
T& n2 = (T&) grid.getNodeFor(gp);
buildStairLine(n, n2);
buildStairLine(grid, n, n2);
}
}
@@ -195,11 +195,54 @@ public:
}
/**
* this method ensures that the start- and end-node of a stair are "terminated":
* they have less than 8 neighbors. it must not be possible to walk on otherwise
* than using the stair.
*
* NOTE: only works for axis aligned stairs!!!!
* NOTE: needs grid.cleanup() during some later timestep!!
*
*/
void removeStairBlocked(Grid<T>& grid, T& _n1, T& _n2) {
Point3 pStart(_n1.x_cm, _n1.y_cm, _n1.z_cm);
Point3 pEnd(_n2.x_cm, _n2.y_cm, _n2.z_cm);
// cleanup the region around the starting node
BBox3 bboxStart;
bboxStart.add( pStart ); // from the start
bboxStart.add( pStart + (pEnd-pStart) * 0.3 ); // 30% to the end
// cleanup the region around the destionation node
BBox3 bboxEnd;
bboxEnd.add( pEnd ); // from the end
bboxEnd.add( pEnd + (pStart-pEnd) * 0.3 ); // 30% to the start
for (T& n : grid) {
if (n == _n1) {continue;} // keep the start itself
if (n == _n2) {continue;} // keep the end itself
Point3 pp(n.x_cm, n.y_cm, n.z_cm);
if (bboxStart.contains(pp)) {grid.remove(n);} // cleanup starting region
else if (bboxEnd.contains(pp)) {grid.remove(n);} // cleanup ending region
}
}
/** build a stair (z-transition) from n1 to n2 */
void buildStairLine(T& _n1, T& _n2) {
void buildStairLine(Grid<T>& grid, T& _n1, T& _n2) {
// remove unneccesary nodes in stair-environments
removeStairBlocked(grid, _n1, _n2);
// does not work, deletion takes place at a later time!
// sanity check: both, the starting and the end node should have less than 8 neighbors, otherwise they are not "leading" into the stair
//if (_n1.getNumNeighbors() >= 8) {throw Exception("invalid number of neighbors for a stair-start-node");}
//if (_n2.getNumNeighbors() >= 8) {throw Exception("invalid number of neighbors for a stair-end-node");}
// half the grid size = small steps
const int gridSize_cm = grid.getGridSize_cm();// / std::sqrt(2);
const int gridSize_cm = grid.getGridSize_cm() / std::sqrt(2);
// local copies, needed for std::swap to work
T n1 = _n1; T n2 = _n2;
@@ -207,6 +250,7 @@ public:
// ensure we work from lower to upper levels
if (n2.z_cm < n1.z_cm) { std::swap(n1, n2); }
// dimensional difference between stairt start and end
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;
@@ -215,8 +259,8 @@ public:
int idx2 = -1; // next node
const int idx3 = n2.getIdx(); // final node
// move upards in gridSize steps
for (int _z = gridSize_cm; _z < zDiff - gridSize_cm; _z+= gridSize_cm) {
// move upards
for (int _z = gridSize_cm; _z <= zDiff - gridSize_cm; _z+= gridSize_cm) {
// calculate the percentage of reached upwards-distance
const float percent = _z/zDiff;