added code for advanced sample impoverishment using the mesh

navMesh/walk/NavMeshWalkWifi.h

@@ -0,0 +1,152 @@
+#ifndef NAVMESHWALKWIFI_H
+#define NAVMESHWALKWIFI_H
+
+#include <vector>
+
+#include "../NavMesh.h"
+#include "../NavMeshLocation.h"
+#include "../../geo/Heading.h"
+
+#include "../../sensors/radio/WiFiMeasurements.h"
+#include <Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h>
+#include <Indoor/sensors/radio/WiFiProbabilityFree.h>
+
+#include "NavMeshSub.h"
+#include "NavMeshWalkParams.h"
+#include "NavMeshWalkEval.h"
+
+namespace NM {
+
+    /** simple walker extended by a wifi anti impoverishment method
+     * draw 10000 random particles within the building equaly
+     * evaluate them with wifi and then draw cumulative from a list
+     * instead of killing the particles who walk against walls
+     * the number of new particles is restricted by ??? */
+    template <typename Tria> class NavMeshWalkWifi {
+
+    public:
+        /** single result */
+        struct ResultEntry {
+            NavMeshLocation<Tria> location;
+            Heading heading;
+            double probability;
+            ResultEntry() : heading(0) {;}
+        };
+
+    private:
+
+        const NavMesh<Tria>& mesh;
+
+        std::vector<NavMeshWalkEval<Tria>*> evals;
+
+        std::vector<ResultEntry> wifiSamples;
+        WiFiModel& wifiModel;
+        WiFiObserverFree wifiProbability;
+        double wifiCumWeight;
+        DrawList<NavMeshLocation<Tria>> wifiSamplesDrawList;
+
+        int hits = 0;
+        int misses = 0;
+
+    public:
+
+        /** list of results */
+        using ResultList = std::vector<ResultEntry>;
+
+        /** ctor */
+        NavMeshWalkWifi(const NavMesh<Tria>& mesh, WiFiModel& wifiModel) : mesh(mesh),
+            wifiModel(wifiModel),
+            wifiProbability(Settings::WiFiModel::sigma, wifiModel){
+
+        }
+
+        /** add a new evaluator to the walker */
+        void addEvaluator(NavMeshWalkEval<Tria>* eval) {
+            this->evals.push_back(eval);
+        }
+
+        /** update every transition step the WiFi */
+        void updateWiFi(const WiFiMeasurements& wifiObs, Timestamp currentTime){
+
+            this->wifiSamplesDrawList.reset();
+
+            //todo: restrict this to specific region
+            NavMeshRandom<Tria> rnd = mesh.getRandom();
+            wifiCumWeight = 0;
+            for(int i = 0; i < 10000; ++i){
+
+                NavMeshLocation<Tria> tmpLocation = rnd.draw();
+                double weight = wifiProbability.getProbability(tmpLocation.pos, currentTime, wifiObs);
+
+                this->wifiSamplesDrawList.add(tmpLocation, weight);
+            }
+        }
+
+        ResultEntry getOne(const NavMeshWalkParams<Tria>& params) {
+
+            // sanity checks
+            params.check();
+
+            ResultEntry re;
+
+            // to-be-walked distance;
+            const float toBeWalkedDist = params.getToBeWalkedDistance();
+            const float toBeWalkedDistSafe = 0.75 + toBeWalkedDist * 1.1;
+
+            // construct reachable region
+            NavMeshSub<Tria> reachable(params.start, toBeWalkedDistSafe);
+
+            // 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);
+
+            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
+                re.probability = 1.0;
+                ++hits;
+
+            } else {
+                re.location = wifiSamplesDrawList.get();
+                re.heading = Heading(params.start.pos.xy(), re.location.pos.xy());
+                re.probability = 0.1;
+                ++misses;
+            }
+
+            const int total = (hits + misses);
+            if (total % 10000 == 0) {
+                //std::cout << "hits: " << (hits*100/total) << "%" << 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)};
+        }
+    };
+
+}
+
+#endif // NAVMESHWALKWIFI_H