worked on grid-walker v3

grid/walk/v3/WalkEvaluator.h

@@ -0,0 +1,91 @@
+#ifndef INDOOR_GW3_WALKEVALUATOR_H
+#define INDOOR_GW3_WALKEVALUATOR_H
+
+#include "Structs.h"
+#include "Helper.h"
+
+#include "../../../math/Distributions.h"
+#include "../../../grid/Grid.h"
+
+namespace GW3 {
+
+	/** interface for all evaluators that return a probability for a given walk */
+	template <typename Node> class WalkEvaluator {
+
+	public:
+
+		/** get the probability for the given walk */
+		//virtual double getProbability(const Walk<Node>& walk) const = 0;
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const = 0;
+
+	};
+
+
+	/** evaluate the grid-node-importance importance(end) */
+	template <typename Node> class WalkEvalEndNodeProbability : public WalkEvaluator<Node> {
+
+		Grid<Node>* grid;
+
+	public:
+
+		WalkEvalEndNodeProbability(Grid<Node>* grid) : grid(grid) {;}
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+
+			(void) params;
+			(void) pStart;
+
+			const GridPoint gp = Helper<Node>::p3ToGp(pEnd);
+			const Node& node = grid->getNodeFor(gp);
+			const double p = node.getWalkImportance();
+			return std::pow(p,10);
+
+		}
+
+	};
+
+
+
+	/** evaluate the difference between head(start,end) and the requested heading */
+	template <typename Node> class WalkEvalHeadingStartEnd : public WalkEvaluator<Node> {
+
+	public:
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+
+			(void) params;
+
+			if (pStart == pEnd) {
+				std::cout << "warn! start-position == end-positon" << std::endl;
+				return 0;
+			}
+
+			const Heading head(pStart.xy(), pEnd.xy());
+			const float diff = head.getDiffHalfRAD(params.heading);
+			//const float diff = Heading::getSignedDiff(params.heading, head);
+			return Distribution::Normal<double>::getProbability(0, 0.04, diff);
+
+		}
+
+	};
+
+	/** evaluate the difference between distance(start, end) and the requested distance */
+	template <typename Node> class WalkEvalDistance : public WalkEvaluator<Node> {
+
+		const double sigma = 0.1f;
+
+	public:
+
+		virtual double getProbability(const Point3 pStart, const Point3 pEnd, const WalkParams& params) const override {
+
+			const float walkedDistance_m = pStart.getDistance(pEnd);
+			return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
+
+		}
+
+	};
+
+}
+
+#endif // INDOOR_GW3_WALKEVALUATOR_H