Indoor/grid/factory/v2/Importance.h

/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef IMPORTANCE_H
#define IMPORTANCE_H



#include "../../../geo/Units.h"
#include "../../Grid.h"
#include "GridFactoryListener.h"

#include "../../../misc/KNN.h"
#include "../../../misc/KNNArray.h"

#include "../../../math/MiniMat2.h"
#include "../../../math/distribution/Normal.h"
#include "../../../math/distribution/Triangle.h"




class Importance {

private:

	static constexpr const char* name = "GridImp";

public:


	template <typename T> static void addOutlineNodes(Grid<T>& dst, Grid<T>& src) {

		for (const T& n : src) {
			if (n.getNumNeighbors() < 8) {
				if (!dst.hasNodeFor(n)) {
					dst.add(n);
				}
			}
		}

	}

	/** attach importance-factors to the grid */
	template <typename T> static void addImportance(Grid<T>& g, GridFactoryListener* l = nullptr) {

		Log::add(name, "adding importance information to all nodes");
		if (l) {
			l->onGridBuildUpdateMajor(2, 0);
			l->onGridBuildUpdateMajor("adding importance information");
			l->onGridBuildUpdateMinor("performing initial setups");
		}

		// get an inverted version of the grid
		Grid<T> inv(g.getGridSize_cm());
		addOutlineNodes(inv, g);
		//GridFactory<T> fac(inv);
		//fac.addInverted(g, z_cm);

		// sanity check
		Assert::isFalse(inv.getNumNodes() == 0, "inverted grid is empty!");

		// construct KNN search
		KNN<Grid<T>, 3> knn(inv);

		// create list of all door-nodes
		std::vector<T> doors;

		// create list of all stair-nodes
		std::vector<T> stairs;


		// process each node
		for (T& n1 : g) {

			switch(n1.getType()) {
			case GridNode::TYPE_DOOR:		doors.push_back(n1);	break;
			case GridNode::TYPE_STAIR:		stairs.push_back(n1);	break;
			case GridNode::TYPE_ELEVATOR:	stairs.push_back(n1);	break;
			}

		}

		// KNN for doors
		KNNArray<std::vector<T>> knnArrDoors(doors);
		KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);

		// KNN for stairs
		KNNArray<std::vector<T>> knnArrStairs(stairs);
		KNN<KNNArray<std::vector<T>>, 3> knnStairs(knnArrStairs);

		// probability adjustments
		Distribution::Triangle<float> avoidWalls(0.0, 0.35f);
		Distribution::Normal<float> favorDoors(0.0f, 0.4f);
		Distribution::Normal<float> favorStairs(0.0f, 1.5f);

		if (l) {
			l->onGridBuildUpdateMajor(2, 1);
			l->onGridBuildUpdateMinor("calculating importance for each node");
		}

		std::cout << "dunno why, but the KNN for stairs searches extremely slow!" << std::endl;

		// process each node again
		for (int i = 0; i < g.getNumNodes(); ++i) {


			// log
			if (i % (g.getNumNodes() / 20) == 0) {
				if (l) {
					l->onGridBuildUpdateMinor(g.getNumNodes(), i);
				}
			}

			// get the node
			T& n1 = g[i];

			// get the distance to the nearest outline node [an outline node is directly adjacent to a wall]
			const float distToOutline_m = Units::cmToM(knn.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));

			// get the distance to the nearest door
			const float distToDoor_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));

			// get the distance to the nearest stair
			const float distToStair_m = Units::cmToM(knnStairs.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));

			// use wall-avoidance?
			//const bool useWallAvoidance = (distToWall_m*6.0 < distToDoor_m && distToWall_m*6.0 < distToStair_m);
			//const bool useWallAvoidance = (distToDoor_m > 0.4f) && (distToStair_m > 0.4f);
			const bool useWallAvoidance =
			        (distToOutline_m < 0.001f) &&				// node is an outline node [outline-nodes are adjacent to a wall]
			        (distToDoor_m > 0.3f) &&					// doors are 30cm away
			        (distToStair_m > 0.6f);						// stairs are 60cm away;

			// final probability
                        n1.walkImportance = 1.0f;
			//n1.walkImportance += favorDoors.getProbability(distToDoor_m) * 0.75f;
			n1.walkImportance += favorStairs.getProbability(distToStair_m) * 1.0f;

			// use wall avoidance
			if (useWallAvoidance) {
				//n1.walkImportance -= avoidWalls.getProbability(distToWall_m) * 0.3f;
				n1.walkImportance = 0.20f;		// only addresses direct outline nodes
			}


			// navigation importance is calculated using other formulae
			n1.navImportance = 1.0f;
			if (useWallAvoidance) {
				n1.navImportance -= avoidWalls.getProbability(distToOutline_m) * 0.3f;

			}
			//n1.navImportance += favorDoors.getProbability(distToDoor_m) * 0.5;
			n1.navImportance += favorStairs.getProbability(distToStair_m) * 1.0f;




			// sanity check
			Assert::isNotNaN(n1.walkImportance, "detected NaN walk importance for " + n1.asString());
			Assert::isNotNaN(n1.navImportance, "detected NaN walk importance for " + n1.asString());
			Assert::isTrue(n1.walkImportance >= 0, "detected negative walk importance. does not make sense!");
			Assert::isTrue(n1.navImportance >= 0, "detected negative nav importance. does not make sense!");

		}

	}


};

#endif // IMPORTANCE_H