refactoring to add nav mesh

nav/grid/NodeResampling.h

@@ -0,0 +1,123 @@
+#ifndef NODERESAMPLING_H
+#define NODERESAMPLING_H
+
+
+#include <algorithm>
+#include <random>
+
+#include <Indoor/grid/Grid.h>
+#include <KLib/math/filter/particles/resampling/ParticleFilterResampling.h>
+
+
+	/**
+	 * uses simple probability resampling by drawing particles according
+	 * to their current weight.
+	 * HOWEVER: after drawing them, do NOT use them directly, but replace them with a neighbor
+	 * O(log(n)) per particle
+	 */
+	template <typename State, typename Node>
+	class NodeResampling : public K::ParticleFilterResampling<State> {
+
+	private:
+
+		/** this is a copy of the particle-set to draw from it */
+		std::vector<K::Particle<State>> particlesCopy;
+
+		/** random number generator */
+		std::minstd_rand gen;
+
+		Grid<Node>& grid;
+
+	public:
+
+		/** ctor */
+		NodeResampling(Grid<Node>& grid) : grid(grid) {
+			gen.seed(1234);
+		}
+
+		void resample(std::vector<K::Particle<State>>& particles) override {
+
+			// compile-time sanity checks
+			// TODO: this solution requires EXPLICIT overloading which is bad...
+			//static_assert( HasOperatorAssign<State>::value, "your state needs an assignment operator!" );
+
+			const uint32_t cnt = (uint32_t) particles.size();
+
+			// equal weight for all particles. sums up to 1.0
+			const double equalWeight = 1.0 / (double) cnt;
+
+			// ensure the copy vector has the same size as the real particle vector
+			particlesCopy.resize(cnt);
+
+			// swap both vectors
+			particlesCopy.swap(particles);
+
+			// calculate cumulative weight
+			double cumWeight = 0;
+			for (uint32_t i = 0; i < cnt; ++i) {
+				cumWeight += particlesCopy[i].weight;
+				particlesCopy[i].weight = cumWeight;
+			}
+
+			std::uniform_real_distribution<float> distNewOne(0.0, 1.0);
+			std::uniform_int_distribution<int> distRndNode(0, grid.getNumNodes()-1);
+			std::normal_distribution<float> distTurn(0.0, +0.03);
+
+			// now draw from the copy vector and fill the original one
+			// with the resampled particle-set
+			for (uint32_t i = 0; i < cnt; ++i) {
+
+				// slight chance to get a truely random node as particle
+				// mainly for testing
+				if (distNewOne(gen) < 0.005) {
+					particles[i].state.position = grid[distRndNode(gen)];
+					particles[i].weight = equalWeight;
+					continue;
+				}
+
+				// normal redraw procedure
+				particles[i] = draw(cumWeight);
+				particles[i].weight = equalWeight;
+
+				const Node* n = grid.getNodePtrFor(particles[i].state.position);
+				if (n == nullptr) {continue;}	// should not happen!
+
+				for (int j = 0; j < 2; ++j) {
+					std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
+					const int idx = distIdx(gen);
+					n = &grid.getNeighbor(*n, idx);
+				}
+
+
+				particles[i].state.position = *n;
+				particles[i].state.heading.direction += distTurn(gen);
+
+			}
+
+		}
+
+	private:
+
+		/** draw one particle according to its weight from the copy vector */
+		const K::Particle<State>& draw(const double cumWeight) {
+
+			// generate random values between [0:cumWeight]
+			std::uniform_real_distribution<float> dist(0, cumWeight);
+
+			// draw a random value between [0:cumWeight]
+			const float rand = dist(gen);
+
+			// search comparator (cumWeight is ordered -> use binary search)
+			auto comp = [] (const K::Particle<State>& s, const float d) {return s.weight < d;};
+			auto it = std::lower_bound(particlesCopy.begin(), particlesCopy.end(), rand, comp);
+			return *it;
+
+		}
+
+
+
+	};
+
+
+
+#endif // NODERESAMPLING_H