Indoor/grid/factory/v2/Importance.h

#ifndef IMPORTANCE_H
#define IMPORTANCE_H

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

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

#include "../../../math/MiniMat2.h"
#include "../../../math/Distributions.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) {

		Log::add(name, "adding importance information to all nodes");// at height " + std::to_string(z_cm));

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

		// the number of neighbors to use
		static constexpr int numNeighbors = 12;

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

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

			// is the current node a door?
			//if (isDoor(n1, neighbors)) {doors.push_back(n1);}					// OLD
			if (n1.getType() == GridNode::TYPE_DOOR) {doors.push_back(n1);}		// NEW!

			// favor stairs just like doors
			//if (isStaircase(g, n1)) {doors.push_back(n1);}					// OLD
			if (n1.getType() == GridNode::TYPE_STAIR) {doors.push_back(n1);}	// NEW

		}

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

		Distribution::Normal<float> favorDoors(0.0f, 0.5f);

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

			// skip nodes on other than the requested floor-level
			//if (n1.z_cm != z_cm) {continue;}

			// get the 10 nearest neighbors and their distance
			size_t indices[numNeighbors];
			float squaredDist[numNeighbors];
			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
			knn.get(point, numNeighbors, indices, squaredDist);

			// get the neighbors
			std::vector<T*> neighbors;
			for (int i = 0; i < numNeighbors; ++i) {
				neighbors.push_back(&inv[indices[i]]);
			}

			n1.navImportance = 1.0f;

			//if (n1.getType() == GridNode::TYPE_FLOOR) {

			// get the distance to the nearest door
			const float distToWall_m = Units::cmToM(std::sqrt(squaredDist[0]) + g.getGridSize_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} ));

			n1.navImportance =
					1 +
					getWallImportance( distToWall_m ) +
					favorDoors.getProbability(distToDoor_m) * 1.5f;


			//}
			//addDoor(n1, neighbors);

			// importance for this node (based on the distance from the next door)
			//n1.navImportance += favorDoors.getProbability(dist_m) * 0.30;


			//n1.navImportance = (dist_m < 0.2) ? (1) : (0.5);
		}

	}

	/** is the given node connected to a staircase? */
	template <typename T> static bool isStaircase(Grid<T>& g, T& node) {

		return node.getType() == GridNode::TYPE_STAIR;

//		// if this node has a neighbor with a different z, this is a stair
//		for (T& neighbor : g.neighbors(node)) {
//			if (neighbor.z_cm != node.z_cm) {return true;}
//		}
//		return false;

	}

//	/** is the given node (and its inverted neighbors) a door? */
//	template <typename T> static bool isDoor( T& nSrc, std::vector<T*> neighbors ) {

//		if (nSrc.getType() != GridNode::TYPE_FLOOR) {return false;}

//		MiniMat2 m1;
////		MiniMat2 m2;
//		Point3 center = nSrc;

//		// calculate the centroid of the nSrc's nearest-neighbors
//		Point3 centroid(0,0,0);
//		for (const T* n : neighbors) {
//			centroid = centroid + (Point3)*n;
//		}
//		centroid /= neighbors.size();

//		// if nSrc is too far from the centroid, this does not make sense
//		if ((centroid-center).length() > 40) {return false;}

//		// build covariance of the nearest-neighbors
//		int used = 0;
//		for (const T* n : neighbors) {

//			const Point3 d1 = (Point3)*n - centroid;
//			if (d1.length() > 100) {continue;}	// radius search
//			m1.addSquared(d1.x, d1.y);

////			const Point3 d2 = (Point3)*n - center;
////			if (d2.length() > 100) {continue;}	// radius search
////			m2.addSquared(d2.x, d2.y);

//			++used;

//		}

//		// we need at least two points for the covariance
//		if (used < 6) {return false;}

//		// check eigenvalues
//		MiniMat2::EV ev1 = m1.getEigenvalues();
////		MiniMat2::EV ev2 = m2.getEigenvalues();

//		// ensure e1 > e2
//		if (ev1.e1 < ev1.e2) {std::swap(ev1.e1, ev1.e2);}
////		if (ev2.e1 < ev2.e2) {std::swap(ev2.e1, ev2.e2);}

//		// door?
//		const float ratio1 = (ev1.e2/ev1.e1);
////		const float ratio2 = (ev2.e2/ev2.e1);
////		const float ratio3 = std::max(ratio1, ratio2) / std::min(ratio1, ratio2);

//		return (ratio1 < 0.30 && ratio1 > 0.05) ;

//	}

	/** get the importance of the given node depending on its nearest wall */
	static float getWallImportance(float dist_m) {

		// avoid sticking too close to walls (unlikely)
		static Distribution::Normal<float> avoidWalls(0.0, 0.35);

		// favour walking near walls (likely)
		//static Distribution::Normal<float> stickToWalls(0.9, 0.7);

		// favour walking far away (likely)
		//static Distribution::Normal<float> farAway(2.2, 0.5);
		//if (dist_m > 2.0) {dist_m = 2.0;}

		// overall importance
//		return	- avoidWalls.getProbability(dist_m) * 0.30		// avoid walls
//				+ stickToWalls.getProbability(dist_m) * 0.15	// walk near walls
//				+ farAway.getProbability(dist_m) * 0.15			// walk in the middle
		return	- avoidWalls.getProbability(dist_m)		// avoid walls
				//+ stickToWalls.getProbability(dist_m)	// walk near walls
				//+ farAway.getProbability(dist_m)			// walk in the middle
		;


	}


};

#endif // IMPORTANCE_H