From 5b35b5d3e0cdbf4042dba72e99f7ff9dfda8de86 Mon Sep 17 00:00:00 2001 From: FrankE Date: Sat, 6 Feb 2016 15:02:41 +0100 Subject: [PATCH] new bbox features (2D, 3D) added stair-sanitizing code adjusted walkers --- geo/BBox3.h | 22 +++++++++++++ grid/factory/GridFactory.h | 54 ++++++++++++++++++++++++++++--- grid/walk/GridWalkPathControl.h | 4 +-- grid/walk/GridWalkSimpleControl.h | 2 +- 4 files changed, 74 insertions(+), 8 deletions(-) diff --git a/geo/BBox3.h b/geo/BBox3.h index e270978..7b9c02d 100644 --- a/geo/BBox3.h +++ b/geo/BBox3.h @@ -49,6 +49,28 @@ public: (p2.z == o.p2.z); } + /** shrink the bbox in each dimension by the given amount */ + void shrink(const float v) { + shrink(Point3(v,v,v)); + } + + /** shrink the bbox by the amount given for each dimension */ + void shrink(const Point3& p) { + p1 += p; // increase minimum + p2 -= p; // decrease maximum + } + + /** does the bbox contain the given point? */ + bool contains(const Point3& p) const { + if (p.x < p1.x) {return false;} + if (p.x > p2.x) {return false;} + if (p.y < p1.y) {return false;} + if (p.y > p2.y) {return false;} + if (p.z < p1.z) {return false;} + if (p.z > p2.z) {return false;} + return true; + } + }; #endif // BBOX3_H diff --git a/grid/factory/GridFactory.h b/grid/factory/GridFactory.h index 71dd254..96cb3cb 100755 --- a/grid/factory/GridFactory.h +++ b/grid/factory/GridFactory.h @@ -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& 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& 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; diff --git a/grid/walk/GridWalkPathControl.h b/grid/walk/GridWalkPathControl.h index 4ac88bf..fcabec3 100644 --- a/grid/walk/GridWalkPathControl.h +++ b/grid/walk/GridWalkPathControl.h @@ -91,11 +91,11 @@ private: const float diff = possibleHead.getDiffHalfRAD(head); // // compare this heading with the requested one - const double angleProb = Distribution::Normal::getProbability(0, Angle::degToRad(15), diff); + const double angleProb = Distribution::Normal::getProbability(0, Angle::degToRad(25), diff); // const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1; // favor best 3 angles equally // nodes own importance - const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.0; + const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025; // bring it together return angleProb * nodeProb; diff --git a/grid/walk/GridWalkSimpleControl.h b/grid/walk/GridWalkSimpleControl.h index 333dbe0..3552851 100644 --- a/grid/walk/GridWalkSimpleControl.h +++ b/grid/walk/GridWalkSimpleControl.h @@ -64,7 +64,7 @@ private: // TODO: WHY?! not only when going back to the start? if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) { if (start.z_cm == possible.z_cm) {return 0;} // back to the start - throw 1; + //throw 1; return 0.5;// stair start/end TODO: fix }