dijkstra is now bleching fast

deleting from the grid is now bleaching fast
added new helper methods
many new test-cases
many new methods for geo classes and others
added a bunch of new grid-walkers

nav/dijkstra/Dijkstra.h

@@ -6,10 +6,12 @@
 #include <algorithm>
 #include <unordered_set>
 #include <list>
+#include <set>
 
 #include "DijkstraStructs.h"
 #include "../../misc/Debug.h"
 #include "../../misc/Time.h"
+#include "../../Defines.h"
 
 #include <KLib/Assertions.h>
 
@@ -18,12 +20,6 @@ template <typename T> class Dijkstra {
 	/** all allocated nodes for the user-data inputs */
 	std::unordered_map<const T*, DijkstraNode<T>*> nodes;
 
-	/** all already processed edges */
-	std::unordered_set<DijkstraEdge<T>> usedEdges;
-
-	/** to-be-processed nodes (NOTE: using std::list here was SLOWER!) */
-	std::vector<DijkstraNode<T>*> toBeProcessedNodes;
-
 public:
 
 	/** get the dijkstra-pendant for the given user-node */
@@ -37,16 +33,18 @@ public:
 		// NOTE: end is currently ignored!
 		// runs until all nodes were evaluated
 
-		// compare two nodes by their distance from the start
-		static auto comp = [] (const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) {return n1->cumWeight < n2->cumWeight;};
+		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start + " to ALL OTHER nodes", false);
+		Log::tick();
 
-		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start + " to ALL OTHER nodes");
-
-		// cleanup
-		toBeProcessedNodes.clear();
-		usedEdges.clear();
+		// cleanup previous runs
 		nodes.clear();
 
+		// sorted list of all to-be-processed nodes
+		ToProcess toBeProcessedNodes;
+
+		// all already processed edges
+		std::unordered_set<decltype(getEdge(nullptr,nullptr))> usedEdges;
+
 		// run from start
 		const T* cur = &start;
 
@@ -55,36 +53,29 @@ public:
 		dnStart->cumWeight = 0;
 
 		// add this node to the processing list
-		toBeProcessedNodes.push_back(dnStart);
+		toBeProcessedNodes.add(dnStart);
 
 		// until we are done
-		while(!toBeProcessedNodes.empty()) {
+		while(unlikely(!toBeProcessedNodes.empty())) {
 
 			// get the next to-be-processed node
-			const auto min = std::min_element(toBeProcessedNodes.begin(), toBeProcessedNodes.end(), comp);
-			DijkstraNode<T>* dnSrc = *min;
+			DijkstraNode<T>* dnSrc = toBeProcessedNodes.pop();
 
 			// stop when end was reached??
 			//if (dnSrc->element == &end) {break;}
 
-			// and remove him from the list
-			toBeProcessedNodes.erase(min);
-
 			// process each neighbor of the current element
 			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
 
 				// get the neighbor itself
 				const T* dst = acc.getNeighbor(*dnSrc->element, i);
 
-				// get the distance-weight to the neighbor
-				const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
-				_assertTrue(weight >= 0, "edge-weight must not be negative!");
-
 				// get-or-create a node for the neighbor
 				DijkstraNode<T>* dnDst = getNode(dst);
 
 				// get-or-create the edge describing the connection
-				const DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
+				//const DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
+				const auto edge = getEdge(dnSrc, dnDst);
 
 				// was this edge already processed? -> skip it
 				if (usedEdges.find(edge) != usedEdges.end()) {continue;}
@@ -92,10 +83,13 @@ public:
 				// otherwise: remember it
 				usedEdges.insert(edge);
 
-
-
 				// and add the node for later processing
-				toBeProcessedNodes.push_back(dnDst);
+				//toBeProcessedNodes.push_back(dnDst);
+				toBeProcessedNodes.add(dnDst);
+
+				// get the distance-weight to the neighbor
+				const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
+				_assertTrue(weight >= 0, "edge-weight must not be negative!");
 
 				// update the weight to the destination?
 				const float potentialWeight = dnSrc->cumWeight + weight;
@@ -108,23 +102,56 @@ public:
 
 		}
 
-		// reclaim temporal memory
-		toBeProcessedNodes.clear();
-		usedEdges.clear();
-
+		Log::tock();
 		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
 
 	}
 
 private:
 
+	/** helper class to sort to-be-processed nodes by their distance from the start */
+	class ToProcess {
+
+		/** sort comparator */
+		struct setComp {
+			bool operator() (const DijkstraNode<T>* dn1, const DijkstraNode<T>* dn2) {
+				return dn1->cumWeight < dn2->cumWeight;
+			}
+		};
+
+		/** sorted list of to-be-processed nodes */
+		std::set<DijkstraNode<T>*, setComp> toBeProcessedNodes;
+
+	public:
+
+		/** add a new to-be-processed node */
+		void add(DijkstraNode<T>* node) {toBeProcessedNodes.insert(node);}
+
+		/** get the next to-be-processed node (smallest distance) */
+		DijkstraNode<T>* pop() {
+			DijkstraNode<T>* next = *toBeProcessedNodes.begin();
+			toBeProcessedNodes.erase(toBeProcessedNodes.begin());
+			return next;
+		}
+
+		/** set empty? */
+		bool empty() const {return toBeProcessedNodes.empty();}
+
+	};
+
+
+
+
 	/** get (or create) a new node for the given user-node */
 	inline DijkstraNode<T>* getNode(const T* userNode) {
-		if (nodes.find(userNode) == nodes.end()) {
+		auto it = nodes.find(userNode);
+		if (unlikely(it == nodes.end())) {
 			DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
 			nodes[userNode] = dn;
+			return dn;
+		} else {
+			return it->second;
 		}
-		return nodes[userNode];
 	}
 
 	/** get the edge (bi-directional) between the two given nodes */

nav/dijkstra/DijkstraStructs.h

@@ -52,13 +52,23 @@ template <typename T> struct DijkstraEdge {
 				((src == other.dst) && (dst == other.src));
 	}
 
+	// std::set was slower than std::unordered_set
+//	bool operator < (const DijkstraEdge& other) const {
+//		return ((size_t)src * (size_t)dst) < ((size_t)other.src * (size_t)other.dst);
+//	}
+
 };
 
 /** allows adding DijkstraEdge<T> to hash-maps */
 namespace std {
 	template <typename T> struct hash<DijkstraEdge<T>>{
 		size_t operator()(const DijkstraEdge<T>& e) const {
-			return hash<size_t>()( (size_t)e.src^(size_t)e.dst);
+
+			// dunno why but this one provided the fastet results even though
+			// this should lead to the most hash-collissions?!
+			return hash<size_t>()(  std::min((size_t)e.src, (size_t)e.dst)   );
+			//return hash<size_t>()(  (size_t)e.src * (size_t)e.dst   );
+
 		}
 	};
 }