worked on navMesh stuff

- creation
- walking
- helper

navMesh/walk/NavMeshSub.h

@@ -3,54 +3,80 @@
 
 #include "../NavMesh.h"
 #include "../NavMeshLocation.h"
+#include "../NavMeshRandom.h"
 
 #include <vector>
 #include <unordered_set>
 
+namespace NM {
 
-template <typename Tria> class NavMeshSub {
+	template <typename Tria> class NavMeshSub {
 
-	std::vector<const Tria*> toVisit;
+		std::vector<const Tria*> toVisit;
 
-public:
+	public:
 
-	NavMeshSub(const NavMesh<Tria>& nm, const NavMeshLocation<Tria>& loc, float radius_m) {
-		build(nm,loc,radius_m);
-	}
+		NavMeshSub(const NavMeshLocation<Tria>& loc, float radius_m) {
+			build(loc,radius_m);
+		}
 
-private:
+		/** does this submesh contain the given point? */
+		bool contains(const Point2 p2) const {
+			for (const Tria* t : toVisit) {
+				if (t->contains(p2)) {return true;}
+			}
+			return false;
+		}
 
-	void build(const NavMesh<Tria>& nm, const NavMeshLocation<Tria>& loc, float radius_m) {
+		/** get the triangle that contains the given point (if any) */
+		const Tria* getContainingTriangle(const Point2 p2) const {
+			for (const Tria* t : toVisit) {
+				if (t->contains(p2)) {return t;}
+			}
+			return nullptr;
+		}
 
-		// center to start searching
-		const Point3 center = loc.pos;
+		/** perform random operations on the submesh */
+		NavMeshRandom<Tria> getRandom() {
+			return NavMeshRandom<Tria>(toVisit);
+		}
 
-		toVisit.push_back(loc.tria);
+	private:
 
-		std::unordered_set<const Tria*> visited;
+		void build(const NavMeshLocation<Tria>& loc, float radius_m) {
 
-		size_t next = 0;
-		while (next < toVisit.size()) {
+			std::unordered_set<const Tria*> visited;
 
-			// next triangle
-			const Tria* cur = toVisit[next]; ++next;
+			// starting-triangle + all its (max 3) neighbors
+			toVisit.push_back(loc.tria);
+			visited.insert(loc.tria);
+			for (const auto* n : *loc.tria) {
+				toVisit.push_back( (const Tria*)n );
+			}
+
+			size_t next = 1;	// start with the first neighbor (skip starting triangle itself)
+			while (next < toVisit.size()) {
+
+				// next triangle
+				const NavMeshTriangle* cur = toVisit[next]; ++next;
+
+				// neighbors
+				for (const auto* n : *cur) {
+					const Tria* t = (const Tria*) n;
+					const float dist = loc.pos.getDistance(n->getCenter());
+					if (dist > radius_m) {continue;}
+					if (visited.find(t) != visited.end()) {continue;}
+					toVisit.push_back(t);
+					visited.insert(t);
+				}
 
-			// neighbors
-			for (const Tria* n : cur) {
-				const float dist = loc.pos.getDistance(n.getCenter());
-				if (dist > radius_m) {continue;}
-				if (visited.find(n) != visited.end()) {continue;}
-				toVisit.push_back(n);
-				visited.push_back(n);
 			}
 
 		}
 
-		return toVisit;
 
-	}
+	};
 
-
-};
+}
 
 #endif // NAVMESHSUB_H

navMesh/walk/NavMeshWalkEval.h

@@ -0,0 +1,103 @@
+#ifndef NAVMESHWALKEVAL_H
+#define NAVMESHWALKEVAL_H
+
+#include "NavMeshWalkParams.h"
+#include "../NavMeshLocation.h"
+#include "../../math/Distributions.h"
+
+namespace NM {
+
+	template <typename Tria> struct NavMeshPotentialWalk {
+
+		NavMeshWalkParams<Tria> requested;
+
+		NavMeshLocation<Tria> end;
+
+		NavMeshPotentialWalk(const NavMeshWalkParams<Tria>& requested, const NavMeshLocation<Tria>& end) : requested(requested), end(end) {
+			;
+		}
+
+	};
+
+	/**
+	 * evaluate a NavMeshWalk from -> to = probability
+	 */
+	template <typename Tria> class NavMeshWalkEval {
+
+	public:
+
+		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const = 0;
+
+	};
+
+
+
+
+
+
+
+	/**
+	 * evaluate the difference between head(start,end) and the requested heading
+	 */
+	template <typename Tria> class WalkEvalHeadingStartEnd : public NavMeshWalkEval<Tria> {
+
+		const double sigma_rad;
+		const double kappa;
+		Distribution::VonMises<double> _dist;
+		Distribution::LUT<double> dist;
+
+	public:
+
+		// kappa = 1/var = 1/sigma^2
+		// https://en.wikipedia.org/wiki/Von_Mises_distribution
+		WalkEvalHeadingStartEnd(const double sigma_rad = 0.04) :
+			sigma_rad(sigma_rad), kappa(1.0/(sigma_rad*sigma_rad)), _dist(0, kappa), dist(_dist.getLUT()) {
+			;
+		}
+
+		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
+
+			if (walk.requested.start.pos == walk.end.pos) {
+				std::cout << "warn! start-position == end-positon" << std::endl;
+				return 0;
+			}
+
+			const Heading head(walk.requested.start.pos.xy(), walk.end.pos.xy());
+			const float diff = head.getDiffHalfRAD(walk.requested.heading);
+			//const float diff = Heading::getSignedDiff(params.heading, head);
+			//return Distribution::Normal<double>::getProbability(0, sigma, diff);
+			return dist.getProbability(diff);
+
+		}
+
+	};
+
+	/**
+	 * evaluate the difference between distance(start, end) and the requested distance
+	 */
+	template <typename Tria> class WalkEvalDistance : public NavMeshWalkEval<Tria> {
+
+		const double sigma;
+
+		const Distribution::Normal<double> dist;
+
+	public:
+
+		WalkEvalDistance( const double sigma = 0.1) : sigma(sigma), dist(0, sigma) {;}
+
+		virtual double getProbability(const NavMeshPotentialWalk<Tria>& walk) const override {
+
+			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);
+			//return Distribution::Normal<double>::getProbability(params.distance_m, sigma, walkedDistance_m);
+
+		}
+
+	};
+
+
+}
+
+#endif // NAVMESHWALKEVAL_H

navMesh/walk/NavMeshWalkHelper.h

@@ -1,10 +0,0 @@
-#ifndef NAVMESHWALKHELPER_H
-#define NAVMESHWALKHELPER_H
-
-template <typename Tria> class NavMeshWalkHelper {
-
-
-
-}
-
-#endif // NAVMESHWALKHELPER_H

navMesh/walk/NavMeshWalkParams.h

@@ -0,0 +1,64 @@
+#ifndef NAVMESHWALKPARAMS_H
+#define NAVMESHWALKPARAMS_H
+
+#include "../../geo/Heading.h"
+#include "../NavMeshLocation.h"
+
+namespace NM {
+
+	/** configure pedestrian StepSizes */
+	struct StepSizes {
+
+		float stepSizeFloor_m = NAN;
+		float stepSizeStair_m = NAN;
+
+		bool isValid() const {
+			return (stepSizeFloor_m==stepSizeFloor_m) && (stepSizeStair_m==stepSizeStair_m);
+		}
+
+		template <typename Tria> float inMeter(const int steps, const NavMeshLocation<Tria>& start) const {
+
+			Assert::isTrue(isValid(), "invalid step-sizes given");
+
+			if (start.tria->isPlain()) {
+				return stepSizeFloor_m * steps;
+			} else {
+				return stepSizeStair_m * steps;
+			}
+
+		}
+
+	};
+
+	/** configure walking from -> to */
+	template <typename Tria> struct NavMeshWalkParams {
+
+		/** walk starts here (pos/tria) */
+		NavMeshLocation<Tria> start;
+
+//		/** to-be-walked distance */
+//		float distance_m;
+
+		/** direction to walk to */
+		Heading heading;
+
+		/** number of steps to walk */
+		int numSteps;
+
+		/** configuration for pedestrian's step-sizes */
+		StepSizes stepSizes;
+
+
+		/** empty ctor */
+		NavMeshWalkParams() : heading(0) {;}
+
+		/** get the to-be-walked distance (steps vs. current location [stair/floor/..]) */
+		float getToBeWalkedDistance() const {
+			return stepSizes.inMeter(numSteps, start);
+		}
+
+	};
+
+}
+
+#endif // NAVMESHWALKPARAMS_H

navMesh/walk/NavMeshWalkSimple.h

@@ -2,42 +2,105 @@
 #define NAVMESHWALKSIMPLE_H
 
 #include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+#include "../../geo/Heading.h"
 
-template <typename Tria> class NavMeshWalkSimpel {
+#include "NavMeshSub.h"
+#include "NavMeshWalkParams.h"
+#include "NavMeshWalkEval.h"
 
-private:
+namespace NM {
 
-	const NavMesh<Tria>& mesh;
+	template <typename Tria> class NavMeshWalkSimple {
+
+	private:
+
+		const NavMesh<Tria>& mesh;
+
+		std::vector<NavMeshWalkEval<Tria>*> evals;
+
+		int hits = 0;
+		int misses = 0;
+
+	public:
+
+
+		struct Result {
+
+			NavMeshLocation<Tria> location;
+			Heading heading;
+			double probability;
+
+			Result() : heading(0) {;}
+
+		};
+
+	public:
+
+		/** ctor */
+		NavMeshWalkSimple(const NavMesh<Tria>& mesh) : mesh(mesh) {
+
+		}
+
+		/** add a new evaluator to the walker */
+		void addEvaluator(NavMeshWalkEval<Tria>* eval) {
+			this->evals.push_back(eval);
+		}
+
+		Result getDestination(const NavMeshWalkParams<Tria>& params) {
+
+			Result res;
+			res.heading = params.heading;
+
+			// to-be-walked distance;
+			const float toBeWalkedDist = params.getToBeWalkedDistance();
+			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 Point2 dir = res.heading.asVector();
+			const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);
+
+			const Tria* dstTria = reachable.getContainingTriangle(dst);
+
+			// is above destination reachable?
+			if (dstTria) {
+
+				res.location.pos = dstTria->toPoint3(dst);
+				res.location.tria = dstTria;
+				++hits;
+
+			} else {
+
+				NavMeshRandom<Tria> rnd = reachable.getRandom();
+				NavMeshLocation<Tria> rndLoc = rnd.draw();
+				res.location = rndLoc;
+				res.heading = Heading(params.start.pos.xy(), rndLoc.pos.xy());	// update the heading
+				++misses;
+
+			}
+
+			const int total = (hits + misses);
+			if (total % 10000 == 0) {
+				std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
+			}
+
+			const NavMeshPotentialWalk<Tria> pwalk(params, res.location);
+			res.probability = 1.0;
+			for (const NavMeshWalkEval<Tria>* eval : evals) {
+				const double p1 = eval->getProbability(pwalk);
+				res.probability *= p1;
+			}
+
+			return res;
+
+		}
 
-public:
 
-	struct Location {
-		size_t idx;
-		Point3 pos;
 	};
 
-	struct Result {
-		Location loc;
-	};
-
-	struct Params {
-		Location loc;
-		float distance_m;
-		float heading_rad;
-	};
-
-public:
-
-	/** ctor */
-	NavMeshWalkSimpel(const NavMesh<Tria>& mesh) : mesh(mesh) {
-
-	}
-
-	Result walk(const Params& params) {
-
-	}
-
-
 }
 
 #endif // NAVMESHWALKSIMPLE_H