Indoor/navMesh/walk/NavMeshWalkKLD.h

#ifndef NAVMESHWALKKLD_H
#define NAVMESHWALKKLD_H

#include <vector>
#include <algorithm>
#include <random>

#include "../NavMesh.h"
#include "../NavMeshLocation.h"
#include "../../geo/Heading.h"

#include "NavMeshSub.h"
#include "NavMeshWalkParams.h"
#include "NavMeshWalkEval.h"

namespace NM {

    /**
     * simple walker that gives particles a slight chance to walk
     * thru walls depending upon a given delta
     */
    template <typename Tria> class NavMeshWalkKLD {

    private:

        const NavMesh<Tria>& mesh;

        std::vector<NavMeshWalkEval<Tria>*> evals;
        Distribution::Uniform<float> distNewOne = Distribution::Uniform<float>(0,1);

        int hits = 0;
        int misses = 0;
        int walls = 0;
        int noTria = 0;

    public:


        /** single result */
        struct ResultEntry {
            NavMeshLocation<Tria> location;
            Heading heading;
            double probability;
            ResultEntry() : heading(0) {;}
        };

        /** list of results */
        using ResultList = std::vector<ResultEntry>;

    public:

        /** ctor */
        NavMeshWalkKLD(const NavMesh<Tria>& mesh) : mesh(mesh) {

        }

        /** add a new evaluator to the walker */
        void addEvaluator(NavMeshWalkEval<Tria>* eval) {
            this->evals.push_back(eval);
        }

        /**
         * @brief getOne
         * @param params - of the current sample
         * @param kld - the kullback-leibler divergence
         * @param lambda - some value to influence kld and wifi
         * @param qualityWifi - current wifi quality between [0,1]
         * @return a newly drawn sample based on the previous sample or random
         */
        ResultEntry getOne(const NavMeshWalkParams<Tria>& params, const double kld, const double lambda, const double qualityWifi) {

            // sanity checks
            params.check();

            ResultEntry re;

            // get kld between [0,1] the lower this value, the more we need to
            // increase the reachable radius and number of particles walking tru walls
            const double wallProb = 1 - std::exp(-lambda * (kld * qualityWifi));

            // to-be-walked distance;
            const float toBeWalkedDist = params.getToBeWalkedDistance();
            const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
            const float toBeWalkedDistKld = (kld * qualityWifi);

            // construct reachable region
            NavMeshSub<Tria> reachable(params.start, toBeWalkedDistKld); //EDIT HERE: ADD TOBEWALKDISTKLD...

            // get the to-be-reached destination's position (using start+distance+heading)
            const Point2 dir = params.heading.asVector();
            const Point2 dst = params.start.pos.xy() + (dir * toBeWalkedDist);

            //3D Destination for finding the location within the complete mesh
            //TODO: better solution for z? seems to be a hack on stairs
            const Point2 dsttt = params.start.pos.xy() + (dir * (toBeWalkedDistSafe + 1.0));
            const Point3 dst3D = Point3(dsttt.x, dsttt.y, params.start.pos.z);

            const Tria* dstTria = reachable.getContainingTriangle(dst);

            // is above destination reachable?
            if (dstTria) {

                re.heading = params.heading;                    // heading was OK -> keep
                re.location.pos = dstTria->toPoint3(dst);       // new destination position
                re.location.tria = dstTria;                     // new destination triangle
                ++hits;
            }
            //give the particle a slight chance to walk thru the wall
            else if(distNewOne.draw() < wallProb){

                try{
                    //check if there is a triangle for dst behind the wall
                    const Tria* dstTriaBehindWall = mesh.getLocation(dst3D).tria;
                    re.heading = params.heading;
                    re.location.pos = dst3D;
                    re.location.tria = dstTriaBehindWall;
                    ++walls;
                    ++misses;

                } catch (...) {
                    NavMeshRandom<Tria> rnd = reachable.getRandom();					// random-helper
                    re.location = rnd.draw();											// get a random destination
                    re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());	// update the heading
                    ++noTria;
                    ++misses;
                }

                ++misses;
            } else {

                NavMeshRandom<Tria> rnd = reachable.getRandom();					// random-helper
                re.location = rnd.draw();											// get a random destianation
                re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());	// update the heading

                ++misses;
            }

            const int total = (hits + misses);
            const int totalWalls = (walls + noTria);
            if (total % 10000 == 0) {
                //std::cout << "hits: " << (hits*100/total) << "%" << std::endl;
                //std::cout << "walls: " << (walls*100/totalWalls) << "%" << std::endl;
            }

            // calculate probability
            const NavMeshPotentialWalk<Tria> pwalk(params, re.location);
            re.probability = 1.0;
            for (const NavMeshWalkEval<Tria>* eval : evals) {
                const double p1 = eval->getProbability(pwalk);
                re.probability *= p1;
            }

            // done
            return re;

        }

        ResultList getMany(const NavMeshWalkParams<Tria>& params) {

            // sanity checks
            params.check();

            return {getOne(params)};

        }

        const NavMesh<Tria>& getMesh(){
            return mesh;
        }

    };

}


#endif // NAVMESHWALKKLD_H