added nav-mesh support via demo

navMesh/filter.h

@@ -0,0 +1,184 @@
+#ifndef NAV_MESH_FILTER_H
+#define NAV_MESH_FILTER_H
+
+#include "mesh.h"
+
+#include <Indoor/geo/Heading.h>
+#include <Indoor/math/Distributions.h>
+
+#include <KLib/math/filter/particles/Particle.h>
+#include <KLib/math/filter/particles/ParticleFilter.h>
+#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
+#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
+#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
+
+#include <Indoor/navMesh/walk/NavMeshWalkSimple.h>
+#include <Indoor/navMesh/walk/NavMeshWalkEval.h>
+
+struct MyState {
+
+	/** the state's position (within the mesh) */
+	MyNavMeshLocation pos;
+
+	/** the state's heading */
+	Heading heading;
+
+	MyState() : pos(), heading(0) {;}
+
+	MyState& operator += (const MyState& o) {
+		pos.tria = nullptr;	// impossible
+		pos.pos += o.pos.pos;
+		return *this;
+	}
+
+	MyState& operator /= (const double val) {
+		pos.tria = nullptr;	// impossible
+		pos.pos /= val;
+		return *this;
+	}
+
+	MyState operator * (const double val) const {
+		MyState res;
+		res.pos.pos = pos.pos * val;
+		return res;
+	}
+
+};
+
+struct MyControl {
+
+	int numStepsSinceLastEval = 0;
+	float headingChangeSinceLastEval = 0;
+
+	void afterEval() {
+		numStepsSinceLastEval = 0;
+		headingChangeSinceLastEval = 0;
+	}
+
+
+};
+
+struct MyObservation {
+
+
+
+};
+
+class MyPFInitUniform : public K::ParticleFilterInitializer<MyState> {
+
+	const MyNavMesh* mesh;
+
+public:
+
+	MyPFInitUniform(const MyNavMesh* mesh) : mesh(mesh) {
+		;
+	}
+
+	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+
+		/** random position and heading within the mesh */
+		Distribution::Uniform<float> dHead(0, 2*M_PI);
+		MyNavMeshRandom rnd = mesh->getRandom();
+		for (K::Particle<MyState>& p : particles) {
+			p.state.pos = rnd.draw();
+			p.state.heading = dHead.draw();
+			p.weight = 1.0 / particles.size();
+		}
+	}
+
+};
+
+class MyPFInitFixed : public K::ParticleFilterInitializer<MyState> {
+
+	const MyNavMesh* mesh;
+	const Point3 pos;
+
+public:
+
+	MyPFInitFixed(const MyNavMesh* mesh, const Point3 pos) : mesh(mesh), pos(pos) {
+		;
+	}
+
+	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+
+		/** random position and heading within the mesh */
+		Distribution::Uniform<float> dHead(0, 2*M_PI);
+		for (K::Particle<MyState>& p : particles) {
+			p.state.pos = mesh->getLocation(pos);
+			p.state.heading = dHead.draw();
+			p.weight = 1.0 / particles.size();
+		}
+	}
+
+};
+
+
+class MyPFTrans : public K::ParticleFilterTransition<MyState, MyControl> {
+
+	using MyNavMeshWalk = NM::NavMeshWalkSimple<MyNavMeshTriangle>;
+	MyNavMeshWalk walker;
+
+public:
+
+	MyPFTrans(MyNavMesh& mesh) : walker(mesh) {
+
+		// how to evaluate drawn points
+		//walker.addEvaluator(new NM::WalkEvalHeadingStartEndNormal<MyNavMeshTriangle>(0.04));
+		//walker.addEvaluator(new NM::WalkEvalDistance<MyNavMeshTriangle>(0.1));
+		walker.addEvaluator(new NM::WalkEvalApproachesTarget<MyNavMeshTriangle>(0.9));		// 90% for particles moving towards the target
+
+	}
+
+	void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+
+		Distribution::Normal<float> dStepSizeFloor(0.70, 0.1);
+		Distribution::Normal<float> dStepSizeStair(0.35, 0.1);
+		Distribution::Normal<float> dHeading(0.0, 0.10);
+
+
+		for (K::Particle<MyState>& p : particles) {
+
+			// how to walk
+			MyNavMeshWalkParams params;
+			params.heading = p.state.heading + control->headingChangeSinceLastEval + dHeading.draw();
+			params.numSteps = control->numStepsSinceLastEval;
+			params.start = p.state.pos;
+			params.stepSizes.stepSizeFloor_m = dStepSizeFloor.draw();
+			params.stepSizes.stepSizeStair_m = dStepSizeStair.draw();
+
+			// walk
+			MyNavMeshWalk::ResultEntry res = walker.getOne(params);
+
+			// assign back to particle's state
+			p.weight *= res.probability;
+			p.state.pos = res.location;
+			p.state.heading = res.heading;
+
+		}
+
+		// reset the control (0 steps, 0 delta-heading)
+		//control->afterEval();
+
+	}
+
+
+};
+
+class MyPFEval : public K::ParticleFilterEvaluation<MyState, MyObservation> {
+
+public:
+
+	virtual double evaluation(std::vector<K::Particle<MyState>>& particles, const MyObservation& observation) override {
+
+		return 1.0;
+
+	}
+
+};
+
+
+using MyFilter = K::ParticleFilter<MyState, MyControl, MyObservation>;
+
+
+#endif