added missing code changes

started working on refactoring of sensors
new test-cases

grid/walk/GridWalkShortestPathControl.h

@@ -17,6 +17,8 @@
 #include "GridWalkHelper.h"
 #include "GridWalk.h"
 
+#include <KLib/math/statistics/Statistics.h>
+
 template <typename T> class GridWalkShortestPathControl : public GridWalk<T> {
 
 
@@ -82,14 +84,17 @@ protected:
 	Dijkstra<T> dijkstra;
 	const T& target;
 
-	Point3 centerOfMass = Point3(0,0,0);
+	//Point3 centerOfMass = Point3(0,0,0);
 	Wrapper wrapper;
 	DijkstraPath<T>* path = nullptr;
-	KNN<Wrapper,3>* knn = nullptr;
+	//KNN<Wrapper,3>* knn = nullptr;
 
 
 	DrawList<T&> drawer;
 
+	std::vector<Point3> points;
+	Point3 centerOfMass;
+	float stdDevDist;
 
 public:
 
@@ -109,17 +114,51 @@ public:
 
 	int recalc = 0;
 
+	int times = 3;
+	float pOther = 0.10;
+
 	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad) {
 
-		// update the center-of-mass
-		centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
-		if (path == nullptr) {rebuildPath(grid);}
 
-		if (++recalc > 100 * 1000) {
+		// update the center-of-mass
+		points.push_back( (Point3)*start.node );
+
+		//centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
+		//if (path == nullptr) {rebuildPath(grid);}
+
+
+
+		if (++recalc >= 7500) {
+
+			// center of mass
+			Point3 sum(0,0,0);
+			for (const Point3& p : points) {sum += p;}
+			centerOfMass = sum/points.size();
+
+			// deviation from the center of mass
+			float dSum = 0;
+			float dSum2 = 0;
+			for (const Point3& p : points) {
+				const float d = p.getDistance(centerOfMass);
+				dSum += d;
+				dSum2 += d*d;
+			}
+			dSum /= points.size();
+			dSum2 /= points.size();
+			stdDevDist = std::sqrt( (dSum2) - (dSum * dSum) ) * times;
+
+			// restart
+			points.clear();
 			recalc = 0;
-			rebuildPath(grid);
+
+			// update
+			rebuildPath(grid, centerOfMass);
+
 		}
 
+
+
+
 //		if (knn != nullptr) {
 //			const float dist = knn->getNearestDistance( {(float)start.node->x_cm, (float)start.node->y_cm, (float)start.node->z_cm} );
 //			if (dist > 10000) {
@@ -133,9 +172,11 @@ public:
 		// proportional change of the to-be-walked distance
 		static 	Distribution::Normal<float> dWalk(1, 0.10);
 
-		distance_m = distance_m*dWalk.draw()*2;		// TODO: why *2?
+		distance_m = distance_m*dWalk.draw()*1.5;		// TODO: why *2?
 		headChange_rad = headChange_rad*dHead.draw();
 
+		static 	Distribution::Normal<float> sWalk(0, 0.10);
+		if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }
 
 		return walk(grid, start, distance_m, headChange_rad);
 
@@ -143,12 +184,12 @@ public:
 
 private:
 
-	double getProbability(const T& start, const T& possible, const Heading head) const {
+	double getProbability(Grid<T>& grid, const T& start, const T& prev, const T& possible, const Heading head) const {
 
 		// 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
 		}
 
@@ -165,11 +206,16 @@ private:
 		// nodes own importance
 		double nodeProb = 1;//(possible.distToTarget < start.distToTarget) ? 1 : 0.025;
 
-		if (knn != nullptr) {
-			const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
+		if (path != nullptr) {
+			//const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
+			int steps = stdDevDist / grid.getGridSize_cm();
+			const float pToTarget = possible.getDistanceInMeter(*path->getFromStart(steps).element);
+			const float sToTarget = prev.getDistanceInMeter(*path->getFromStart(steps).element);
+			nodeProb = (pToTarget < sToTarget) ? (1.0) : (pOther);
 			//const float sd = knn->getNearestDistance( {(float)start.x_cm, (float)start.y_cm, (float)start.z_cm} );
 			//nodeProb = (pd < sd) ? 1 : 0.0;
-			nodeProb = Distribution::Exponential<float>::getProbability(0.15, pd_m);
+			//nodeProb = Distribution::Exponential<float>::getProbability(0.9, pToTarget);
+			//nodeProb = Distribution::Exponential<float>::getProbability(1.0, tDist_m);
 		}
 
 		// bring it together
@@ -198,7 +244,7 @@ private:
 			drawer.reset();
 			for (T& neighbor : grid.neighbors(*cur.node)) {
 
-				const double prob = getProbability(*start.node, neighbor, reqHeading);
+				const double prob = getProbability(grid, *start.node, *cur.node, neighbor, reqHeading);
 				drawer.add(neighbor, prob);
 
 			}
@@ -230,14 +276,14 @@ private:
 	}
 
 	/** rebuild the path for the given center point */
-	void rebuildPath(Grid<T>& grid) {
+	void rebuildPath(Grid<T>& grid, const Point3 centerOfMass) {
 
 		// find the grid node nearest to the current center-of-mass
 		auto nearestGridNode = [&] (const T& n1, const T& n2) { return ((Point3)n1).getDistance(centerOfMass) < ((Point3)n2).getDistance(centerOfMass); };
 		const T& currentMass = *std::min_element(grid.begin(), grid.end(), nearestGridNode);
 
 		delete path; path = nullptr;
-		delete knn; knn = nullptr;
+		//delete knn; knn = nullptr;
 
 		DijkstraNode<T>* dnTarget = dijkstra.getNode(target);
 		DijkstraNode<T>* dnStart =  dijkstra.getNode(currentMass);
@@ -252,9 +298,9 @@ private:
 
 			// create k-nn lookup
 			wrapper = Wrapper(path);
-			knn = new KNN<Wrapper, 3>(wrapper);
+			//knn = new KNN<Wrapper, 3>(wrapper);
 		} catch (...) {
-			knn = nullptr;
+			//knn = nullptr;
 			path = nullptr;
 		}