Fusion2016/code/particles/MyTransition.h

#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 distWalk;
	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), distWalk(1.5, 0.5), distStop(0.0, 0.1), distBaro(0.3, 0.05) {

		distWalkStop.setSeed(1234);
		distWalk.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(distWalk.draw() * deltaMS / 1000.0);
			}

			// update cumulative distance
			p.state.distanceWalkedCM += std::abs(dist_m * 100.0);

			// get new destination
			//const Node3* dst = choice->getTarget(src, p.state, dist_m);
			p.state.walkState = walker.getDestination(grid, p.state.walkState, dist_m );

			// 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;


            // --- ATTENTION HORRIBLE CODE INCOMING. ---
			p.state.hPa += (p.state.pOld.z - p.state.pCur.z) / 100.0f * 0.105f;
//            //how many floors are changed? and in what direction (given by the sign)
//            double numFloorChanged = p.state.z_nr_old - p.state.z_nr;

//            //The height of the single floor levels.
//            const static double floor_height[3] = {4.1, 3.4, 3.4};

//            //update barometer
//            if(USE_BAROMETRIC_FORMULAR){
//                //height the particle has climbed.
//                double h_1 = 0.0;
//                double mu = 0.0;

//                //we need only the sign of the floors changed, since the pressure change between the floors
//                //is calculated within s_getAtmosphericPressure
//                numFloorChanged = sgn(numFloorChanged);

//                for(int i = std::min(p.state.z_nr_old, p.state.z_nr); i < std::max(p.state.z_nr_old, p.state.z_nr); i++){
//                   h_1 += floor_height[i];
//                }

//                {
//                    // use the barometric formular to calculate the relative pressure
//                    // the calculation is done assuming sea level height at every floor.
//                    double mslp = BarometricFormular::s_getSeaLevelPressure();
//                    double pressure = BarometricFormular::s_getAtmosphericPressure(h_1, 297.0);
//                    mu = std::abs(mslp - pressure);
//                }

//                if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
//					p.state.hPa += numFloorChanged * K::NormalDistribution::draw(mu, 0.005);
//                else
//                    p.state.hPa = numFloorChanged * K::NormalDistribution::draw(mu, 0.15);
//            }
//            else{
//                if (!USE_STATIC_CIRCULAR_BUFFERING && !USE_DYNAMIC_CIRCULAR_BUFFERING)
//                    p.state.hPa += numFloorChanged * distBaro.draw();
//                else
 //                   p.state.hPa = numFloorChanged * distBaro.draw();
//            }

//			// sanity check
//			if (p.state.heading != p.state.heading) {throw "detected NaN";}
//            if (p.state.z_nr != p.state.z_nr) {throw "detected NaN";}
//			if (p.state.x_cm != p.state.x_cm) {throw "detected NaN";}
//			if (p.state.y_cm != p.state.y_cm) {throw "detected NaN";}

//			// ensure p.state.z_nr IS discreet
//            if ( std::abs(p.state.z_nr - std::round(p.state.z_nr)) > 0.01) {throw "detected continuous z_nr!";}

		}

	}


};

#endif // MYTRANSITION_H