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...

nav/dijkstra/DijkstraPath.h

@@ -0,0 +1,58 @@
+#ifndef DIJKSTRAPATH_H
+#define DIJKSTRAPATH_H
+
+#include "DijkstraStructs.h"
+
+/**
+ * describes a dijkstra-generated path between end and start.
+ * allows KNN searches for points within this path.
+ *
+ */
+template <typename T> class DijkstraPath {
+
+private:
+
+	/** the constructed path */
+	std::vector<DijkstraNode<T>*> path;
+
+public:
+
+	/** ctor from end- to start-node */
+	DijkstraPath(DijkstraNode<T>* end, DijkstraNode<T>* start) {
+
+		// follow the path from the end to the start
+		DijkstraNode<T>* curNode = end;
+		while (curNode != start) {
+			path.push_back(curNode);
+			curNode = curNode->previous;
+		}
+
+	}
+
+	/** NANOFLANN: number of elements in the path */
+	inline int kdtree_get_point_count() const {
+		return path.size();
+	}
+
+	/** NANOFLANN: use default bbox */
+	template <class BBOX> inline bool kdtree_get_bbox(BBOX& bb) const {
+		(void) bb; return false;
+	}
+
+	/** NANOFLANN: get the idx-th elements dim-th dimension */
+	inline float kdtree_get_pt(const size_t idx, const int dim) const {
+		return (*(path[idx]->element))[dim];
+	}
+
+	/** NANOFLANN: get the distance between the given point and the idx-th element */
+	inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
+		const DijkstraNode<T>* n = path[idx_p2];
+		const float d0 = p1[0] - (*(n->element))[0];
+		const float d1 = p1[1] - (*(n->element))[1];
+		const float d2 = p1[2] - (*(n->element))[2];
+		return (d0*d0) + (d1*d1) + (d2*d2);
+	}
+
+};
+
+#endif // DIJKSTRAPATH_H