worked on nav mesh walk

navMesh/walk/NavMeshWalkEval.h

@@ -131,7 +131,8 @@ namespace NM {
 			const float requestedDistance_m = walk.requested.getToBeWalkedDistance();
 			const float walkedDistance_m = walk.requested.start.pos.getDistance(walk.end.pos);
 			const float diff = walkedDistance_m - requestedDistance_m;
-			return dist.getProbability(diff);
+			const double res = dist.getProbability(diff);
+			return res;
 			//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
 
 		}

navMesh/walk/NavMeshWalkSinkOrSwim.h

@@ -0,0 +1,156 @@
+#ifndef NAVMESHWALKSINKORSWIM_H
+#define NAVMESHWALKSINKORSWIM_H
+
+#include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+#include "../../geo/Heading.h"
+#include "../../math/distribution/Normal.h"
+#include "../../math/distribution/Uniform.h"
+
+#include "NavMeshSub.h"
+#include "NavMeshWalkParams.h"
+#include "NavMeshWalkEval.h"
+
+namespace NM {
+
+	/**
+	 * try to move to the requested location
+	 * and, if not, return null
+	 */
+	template <typename Tria> class NavMeshWalkSinkOrSwim {
+
+	public:
+
+		struct Config {
+			Distribution::Uniform<float>* distanceVariation = nullptr;
+			Distribution::Uniform<float>* headingVariation = nullptr;
+			void check() {
+				Assert::isNotNull(distanceVariation,	"distanceVariation must not be null");
+				Assert::isNotNull(headingVariation,		"headingVariation must not be null");
+			}
+		};
+
+	private:
+
+		const NavMesh<Tria>& mesh;
+
+		std::vector<NavMeshWalkEval<Tria>*> evals;
+
+		Config cfg;
+
+		int hits = 0;
+		int misses = 0;
+
+	public:
+
+
+		/** single result */
+		struct ResultEntry {
+			NavMeshLocation<Tria> location;
+			Heading heading;
+			double probability;
+			ResultEntry() : heading(0) {;}
+		};
+
+		ResultEntry lastRes;
+
+		/** list of results */
+		using ResultList = std::vector<ResultEntry>;
+
+	public:
+
+		/** ctor without config */
+		NavMeshWalkSinkOrSwim(const NavMesh<Tria>& mesh) : mesh(mesh), cfg() {
+
+		}
+
+		/** ctor with config */
+		NavMeshWalkSinkOrSwim(const NavMesh<Tria>& mesh, Config cfg) : mesh(mesh), cfg(cfg) {
+			cfg.check();
+		}
+
+		/** add a new evaluator to the walker */
+		void addEvaluator(NavMeshWalkEval<Tria>* eval) {
+			this->evals.push_back(eval);
+		}
+
+		ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
+
+			// sanity checks
+			params.check();
+
+			ResultEntry re;
+
+			// variation?
+			const float distVar =		(cfg.distanceVariation) ? (cfg.distanceVariation->draw()) : (0);
+			const float headingVar =	(cfg.headingVariation) ? (cfg.headingVariation->draw()) : (0);
+
+			// to-be-walked distance;
+			const float toBeWalkedDist = params.getToBeWalkedDistance() + distVar;
+			const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
+
+			// construct reachable region
+			NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+			// get the to-be-reached destination's position (using start+distance+heading)
+			const Heading heading = params.heading + headingVar;
+			const Point2 dir = heading.asVector();
+			const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
+
+			const Tria* dstTria = reachable.getContainingTriangle(dst);
+
+			// is above destination reachable?
+			if (dstTria) {
+
+				re.heading = params.heading;					// heading was OK -> keep
+				re.location.pos = dstTria->toPoint3(dst);		// new destination position
+				re.location.tria = dstTria;						// new destination triangle
+				re.probability = 1;
+				++hits;
+
+				// calculate probability
+				const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
+				re.probability = 1.0;
+				for (const NavMeshWalkEval<Tria>* eval : evals) {
+					const double p1 = eval->getProbability(pwalk);
+					re.probability *= p1;
+				}
+
+				lastRes = re;
+
+			} else {
+
+//				re.heading = params.heading;					// keep
+//				re.location = params.start;						// keep
+//				re.probability = 0;								// kill
+				re = lastRes;
+				//re.probability *= 0.1;
+				++misses;
+
+			}
+
+			const int total = (hits + misses);
+			if (total % 10000 == 0) {
+				//std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
+			}
+
+			// done
+			return re;
+
+		}
+
+		ResultList getMany(const NavMeshWalkParams<Tria>& params) {
+
+			// sanity checks
+			params.check();
+
+			return {getOne(params)};
+
+		}
+
+
+	};
+
+}
+
+#endif // NAVMESHWALKSINKORSWIM_H