added code from fusion2016

code/particles/MyTransition.h

@@ -0,0 +1,125 @@
+#ifndef MYTRANSITION_H
+#define MYTRANSITION_H
+
+#include <KLib/math/filter/particles/ParticleFilterTransition.h>
+#include <KLib/math/distribution/Normal.h>
+#include <KLib/math/distribution/Uniform.h>
+
+
+#include <Indoor/grid/Grid.h>
+#include <Indoor/grid/walk/GridWalk.h>
+
+#include "MyState.h"
+#include "MyControl.h"
+//#include "Helper.h"
+
+#include "../toni/barometric.h"
+
+#include "../MyGridNode.h"
+
+inline double sgn(double x){
+    return ((x>0)?1 : ((x<0)?-1 : 1));
+}
+
+class MyTransition : public K::ParticleFilterTransition<MyState, MyControl> {
+
+private:
+
+	Grid<MyGridNode>& grid;
+	GridWalk<MyGridNode>& walker;
+
+
+	/** a simple normal distribution */
+	K::UniformDistribution distWalkStop;
+	K::NormalDistribution distWalkPerSec;
+	K::NormalDistribution distStop;
+
+
+    /** normal distribution for barometer */
+	K::NormalDistribution distBaro;
+
+
+public:
+
+	/**
+	 * ctor
+	 * @param choice the choice to use for randomly drawing nodes
+	 * @param fp the underlying floorplan
+	 */
+	MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& walker) :
+		grid(grid), walker(walker),
+		distWalkStop(0.0, 1.0), distWalkPerSec(1.0, 0.3), distStop(0.0, 0.1), distBaro(0.3, 0.05) {
+
+		distWalkStop.setSeed(1234);
+		distWalkPerSec.setSeed(1234);
+		distStop.setSeed(1234);
+
+        distBaro.setSeed(5678);
+
+	}
+
+public:
+
+	uint64_t ts = 0;
+	uint64_t deltaMS = 0;
+
+	/** set the current time in millisconds */
+	void setCurrentTime(const uint64_t ts) {
+		if (this->ts == 0) {
+			this->ts = ts;
+			deltaMS = 0;
+		} else {
+			deltaMS = ts - this->ts;
+			this->ts = ts;
+		}
+	}
+
+	virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {
+
+		for (K::Particle<MyState>& p : particles) {
+
+			// TODO: depending on the time since the last update
+			// random distance to move
+//			const double distance = (distWalkStop.draw() > 0.2) ? (distWalk.draw()) : (distStop.draw());
+//			double dist_m = distance * deltaMS / 1000.0;
+
+//			if (dist_m < 0) {dist_m = -dist_m; p.state.heading = rand() % 360;}
+
+			// update the old heading and the other old values
+			//p.state.walkState.heading = p.state.heading;
+			p.state.pOld = p.state.pCur;
+
+
+//			// 10% stand still, 90% walk
+//			double dist_m;
+//			if (distWalkStop.draw() > 0.9) {
+//				dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
+//			} else {
+//				dist_m = std::abs(distWalkPerSec.draw() * deltaMS / 1000.0);
+//			}
+
+			// get new destination
+			//const Node3* dst = choice->getTarget(src, p.state, dist_m);
+			p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad );
+
+			// randomly move the particle within its target grid (box)
+			// (z remains unchanged!)
+			//const int grid_size_cm = grid.getGridSize_cm();
+
+			// new position (x,y) is randomly distributed within the target node
+			Point3 noise = Point3(0,0,0);		// TODO
+			p.state.pCur = (Point3) *p.state.walkState.node + noise;
+
+
+			// update the baromter
+			p.state.hPa += (p.state.pOld.z - p.state.pCur.z) / 100.0f * 0.105f;
+
+
+		}
+
+	}
+
+
+};
+
+#endif // MYTRANSITION_H