added several grid-walks

added new helper methods/classes (e.g. for heading)
new test cases
optimize the dijkstra
cleanups/refactoring
added timed-benchmarks to the log
many more...

misc/Debug.h

@@ -4,23 +4,37 @@
 #include <string>
 #include <iostream>
 #include <iomanip>
+#include "Time.h"
 
 class Log {
 
 public:
 
+
+
 	static void add(const char* comp, const std::string what) {
 		addComp(comp);
-		std::cout << what << std::endl;
+		std::cout << what;
+		addTime();
+		std::cout << std::endl;
 	}
 
 	static void add(const std::string& component, const std::string what) {
 		addComp(component.c_str());
-		std::cout << what << std::endl;
+		std::cout << what;
+		addTime();
+		std::cout << std::endl;
 	}
 
 private:
 
+	static void addTime() {
+		static auto last = Time::tick();
+		const auto cur = Time::tick();
+		std::cout << " (+" << Time::diffMS(last, cur) << "ms)";
+		last = cur;
+	}
+
 	static void addComp(const char* component) {
 		std::cout << "[" << std::setw(12) << std::setfill(' ') << component << "] ";
 	}

misc/KNN.h

@@ -2,6 +2,7 @@
 #define KNN_H
 
 #include "../lib/nanoflann/nanoflann.hpp"
+#include "Debug.h"
 
 /**
  * helper class to extract k-nearest-neighbors
@@ -9,14 +10,16 @@
  * uses nanoflann
  *
  * usage:
- *	KNN<float, Grid<20, T>, T, 3> knn(theGrid);
- *	float search[] = {0,0,0};
- *	std::vector<T> elems = knn.get(search, 3);
+ *		Grid<30, T> theGrid;
+ *		KNN<Grid<20, T>, 3, float> knn(theGrid);
+ *		std::vector<T> elems = knn.get({0,0,0}, 10);
  */
-template <typename Scalar, typename DataStructure, typename Element, int dim> class KNN {
+template <typename DataStructure, int dim, typename Scalar = float> class KNN {
 
 private:
 
+	static constexpr const char* name = "KNN";
+
 	/** type-definition for the nanoflann KD-Tree used for searching */
 	typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<Scalar, DataStructure>, DataStructure, dim> Tree;
 
@@ -33,11 +36,15 @@ public:
 
 	/** ctor */
 	KNN(DataStructure& data) : tree(dim, data, nanoflann::KDTreeSingleIndexAdaptorParams(maxLeafs)), data(data) {
+
+		Log::add(name, "building kd-tree for " + std::to_string(data.kdtree_get_point_count()) +  " elements");
 		tree.buildIndex();
+		Log::add(name, "done");
+
 	}
 
 	/** get the k-nearest-neighbors for the given input point */
-	std::vector<Element> get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const {
+	template <typename Element> std::vector<Element> get(const Scalar* point, const int numNeighbors, const float maxDistSquared = 99999) const {
 
 		// buffer for to-be-fetched neighbors
 		size_t indices[numNeighbors];
@@ -56,6 +63,11 @@ public:
 
 	}
 
+	/** get the k-nearest-neighbors for the given input point */
+	template <typename Element> std::vector<Element> get(std::initializer_list<Scalar> point, const int numNeighbors, const float maxDistSquared = 99999) const {
+		return get(point.begin(), numNeighbors, maxDistSquared);
+	}
+
 	/** get the nearest neighbor and its distance */
 	void getNearest(const Scalar* point, size_t& idx, float& distSquared) {
 
@@ -64,6 +76,30 @@ public:
 
 	}
 
+	/** get the index of the element nearest to the given point */
+	size_t getNearestIndex(const Scalar* point) {
+		size_t idx;
+		float distSquared;
+		tree.knnSearch(point, 1, &idx, &distSquared);
+		return idx;
+	}
+
+	/** get the index of the element nearest to the given point */
+	size_t getNearestIndex(const std::initializer_list<Scalar> lst) {
+		size_t idx;
+		float distSquared;
+		tree.knnSearch(lst.begin(), 1, &idx, &distSquared);
+		return idx;
+	}
+
+	/** get the distance to the element nearest to the given point */
+	float getNearestDistance(const std::initializer_list<Scalar> lst) {
+		size_t idx;
+		float distSquared;
+		tree.knnSearch(lst.begin(), 1, &idx, &distSquared);
+		return std::sqrt(distSquared);
+	}
+
 	void get(const Scalar* point, const int numNeighbors, size_t* indices, float* squaredDist) {
 
 		// find k-nearest-neighbors

misc/KNNArray.h

@@ -0,0 +1,59 @@
+#ifndef KNNARRAY_H
+#define KNNARRAY_H
+
+/**
+ * this wrapper class provides all methods needed for nanoflanns KNN-search.
+ * in order for this wrapper class to work, your data-structure must provide
+ * the following methods:
+ *
+ * PointList:
+ *		size() - return the number of contained points
+ *		operator [] - access points via their index
+ * Point
+ *		operator [] - access each dimension via its index
+ *
+ * example:
+ *		std::vector<Point3> points;
+ *		KNNArray<std::vector<Point3>> arr(points);
+ *		KNN<KNNArray<std::vector<Point3>>, 3> knn(arr);
+ */
+template <typename T> class KNNArray {
+
+private:
+
+	/** the underlying data structure */
+	const T& elem;
+
+public:
+
+	/** ctor with the underlying data structure */
+	KNNArray(const T& elem) : elem(elem) {
+		;
+	}
+
+	/** get the number of elements to search throrugh */
+	inline int kdtree_get_point_count() const {
+		return elem.size();
+	}
+
+	/** use nanoflanns default bbox */
+	template <class BBOX> inline bool kdtree_get_bbox(BBOX& bb) const {
+		(void) bb; return false;
+	}
+
+	/** get the idx-th element's dim-th coordinate */
+	inline float kdtree_get_pt(const size_t idx, const int dim) const {
+		return elem[idx][dim];
+	}
+
+	/** get the SQUARED distance between the given coordinates and the provided element */
+	inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
+		const float d0 = p1[0] - elem[idx_p2][0];
+		const float d1 = p1[1] - elem[idx_p2][1];
+		const float d2 = p1[2] - elem[idx_p2][2];
+		return (d0*d0) + (d1*d1) + (d2*d2);
+	}
+
+};
+
+#endif // KNNARRAY_H

misc/Time.h

@@ -0,0 +1,21 @@
+#ifndef TIME_H
+#define TIME_H
+
+#include <chrono>
+
+class Time {
+
+public:
+
+	static std::chrono::system_clock::time_point tick() {
+		return std::chrono::system_clock::now();
+
+	}
+
+	static int diffMS(std::chrono::system_clock::time_point tick1, std::chrono::system_clock::time_point tick2) {
+		return std::chrono::duration_cast<std::chrono::milliseconds>(tick2 - tick1).count();
+	}
+
+};
+
+#endif // TIME_H