many changes :P

grid/factory/GridImportance.h

@@ -0,0 +1,84 @@
+#ifndef GRIDIMPORTANCE_H
+#define GRIDIMPORTANCE_H
+
+#include "../Grid.h"
+#include "GridFactory.h"
+#include "../../misc/KNN.h"
+
+#include <KLib/math/distribution/Normal.h>
+
+class GridImportance {
+
+public:
+
+	/** attach importance-factors to the grid */
+	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {
+
+		// get an inverted version of the grid
+		Grid<gridSize_cm, T> inv;
+		GridFactory<gridSize_cm, T> fac(inv);
+		fac.addInverted(g, z_cm);
+
+		// construct KNN search
+		KNN<float, Grid<gridSize_cm, T>, T, 3> knn(inv);
+
+		for (int idx = 0; idx < g.getNumNodes(); ++idx) {
+
+			// process each point
+			T& n1 = (T&) g[idx];
+
+//			// get its nearest neighbor
+//			size_t idxNear;
+//			float distSquared;
+//			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
+//			knn.getNearest(point, idxNear, distSquared);
+
+//			// calculate importante
+//			const float imp = importance( Units::cmToM(std::sqrt(distSquared)) );
+//			n1.imp = imp;
+
+			size_t indices[10];
+			float squaredDist[10];
+			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
+			knn.get(point, 10, indices, squaredDist);
+
+			const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
+			const float imp2 = door( indices );
+			n1.imp = (imp1 + imp2)/2;
+
+		}
+
+
+
+	}
+
+	float door( size_t* indices ) {
+
+		// build covariance
+
+		//if (dist1_m > 1.0) {return 1;}
+		//return 1.0 - std::abs(dist1_m - dist2_m);
+		return 1;
+	}
+
+	float importance(float dist_m) {
+
+		static K::NormalDistribution d1(0.0, 0.5);
+		//if (dist_m > 1.5) {dist_m = 1.5;}
+		return 1.0 - d1.getProbability(dist_m) * 0.5;
+
+//		static K::NormalDistribution d1(1.0, 0.75);
+//		//static K::NormalDistribution d2(3.0, 0.75);
+
+//		if (dist_m > 3.0) {dist_m = 3.0;}
+//		return 0.8 + d1.getProbability(dist_m);// + d2.getProbability(dist_m);
+
+//		if (dist_m < 0.5)	{return 0.8;}
+//		if (dist_m < 1.5)	{return 1.2;}
+//		if (dist_m < 2.5)	{return 0.8;}
+//		else				{return 1.2;}
+	}
+
+};
+
+#endif // GRIDIMPORTANCE_H