began putting everything together

code/particles/MyInitializer.h

@@ -0,0 +1,59 @@
+#ifndef MYINITIALIZER3_H
+#define MYINITIALIZER3_H
+
+#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
+#include "MyState.h"
+
+#include <Indoor/grid/Grid.h>
+
+class MyInitializer : public K::ParticleFilterInitializer<MyState> {
+
+private:
+	int x_cm;
+	int y_cm;
+	int z_cm;
+	int heading;
+
+	Grid<MyGridNode>& grid;
+
+public:
+
+	/** q0 = random */
+	MyInitializer(Grid<MyGridNode>& grid) : grid(grid), heading(0) {
+
+	}
+
+	/** q0 = given */
+	MyInitializer(Grid<MyGridNode>& grid, int x_cm, int y_cm, int z_cm, int heading) :
+		grid(grid), x_cm(x_cm), y_cm(y_cm), z_cm(z_cm), heading(heading) {
+
+	}
+
+	virtual void initialize(std::vector<K::Particle<MyState>>& particles) override {
+
+		std::minstd_rand gen;
+		std::uniform_int_distribution<> dist(0, grid.getNumNodes());
+
+		for (K::Particle<MyState>& p : particles) {
+
+			MyGridNode& n = grid[dist(gen)];
+
+			//p.state.pCur = Point3(x_cm, y_cm, z_cm);
+			//GridPoint gp(p.state.pCur.x, p.state.pCur.y, p.state.pCur.z);
+			//p.state.walkState.node = &grid.getNodeFor(gp);
+
+			p.state.pCur = (Point3) n;
+			p.state.walkState.node = &n;
+
+			p.state.pOld = p.state.pCur;
+			p.state.walkState.heading = Heading::rnd();
+			p.state.distanceWalkedCM = 0;
+			p.state.hPa = 0;
+
+		}
+
+	}
+
+};
+
+#endif // MYINITIALIZER_H