new bbox features (2D, 3D)

added stair-sanitizing code
adjusted walkers

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

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<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;

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<float>::getProbability(0, Angle::degToRad(15), diff);
+		const double angleProb = Distribution::Normal<float>::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;

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
 		}