added missing code changes

started working on refactoring of sensors
new test-cases

nav/dijkstra/Dijkstra.h

@@ -44,18 +44,15 @@ public:
 		// 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;
 
 		// create a node for the start element
-		DijkstraNode<T>* dnStart = getNode(cur);
+		DijkstraNode<T>* dnStart = getOrCreateNode(cur);
 		dnStart->cumWeight = 0;
 
-		// add this node to the processing list
-		toBeProcessedNodes.add(dnStart);
+		// add this node to the processing list (and mark it as "enqueued")
+		toBeProcessedNodes.addOnce(dnStart, dnStart->cumWeight);
 
 		// until we are done
 		while(unlikely(!toBeProcessedNodes.empty())) {
@@ -66,28 +63,14 @@ public:
 			// stop when end was reached??
 			//if (dnSrc->element == &end) {break;}
 
-			// process each neighbor of the current element
+			// visit (and maybe update) 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-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 auto edge = getEdge(dnSrc, dnDst);
-
-				// was this edge already processed? -> skip it
-				if (usedEdges.find(edge) != usedEdges.end()) {continue;}
-
-				// otherwise: remember it
-				usedEdges.insert(edge);
-
-				// and add the node for later processing
-				//toBeProcessedNodes.push_back(dnDst);
-				toBeProcessedNodes.add(dnDst);
+				// get-or-create a DijkstraNode for the neighbor
+				DijkstraNode<T>* dnDst = getOrCreateNode(dst);
 
 				// get the distance-weight to the neighbor
 				const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
@@ -96,8 +79,17 @@ public:
 				// update the weight to the destination?
 				const float potentialWeight = dnSrc->cumWeight + weight;
 				if (potentialWeight < dnDst->cumWeight) {
+
+					// re-sort (weight has changed) within the list of to-be-processed nodes
+					toBeProcessedNodes.addOrUpdate(dnDst, dnDst->cumWeight, potentialWeight);
 					dnDst->cumWeight = potentialWeight;
 					dnDst->previous = dnSrc;
+
+				} else {
+
+					// if this neighbor was encountered for the first time, add it (and mark it as "enqueued")
+					toBeProcessedNodes.addOnce(dnDst, dnDst->cumWeight);
+
 				}
 
 			}
@@ -114,30 +106,67 @@ 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;
+		/** assign a weight to a node and provide the corresponding comparator */
+		struct WeightedNode {
+
+			DijkstraNode<T>* dn;
+			const float weight;
+
+			/** ctor */
+			WeightedNode(DijkstraNode<T>* dn, const float weight) : dn(dn), weight(weight) {;}
+
+			/** compare by weight. same weight? : compare by pointer */
+			bool operator < (const WeightedNode& wn) const	{
+				return (weight != wn.weight) ? (weight < wn.weight) : (dn < wn.dn);
 			}
+
 		};
 
 		/** sorted list of to-be-processed nodes */
-		std::set<DijkstraNode<T>*, setComp> toBeProcessedNodes;
+		std::set<WeightedNode> nodes;
 
 	public:
 
 		/** add a new to-be-processed node */
-		void add(DijkstraNode<T>* node) {toBeProcessedNodes.insert(node);}
+		void addOnce(DijkstraNode<T>* node, const float weight) {
+
+			// skip nodes that were already enqueued
+			if (node->enqueued) {return;}
+
+			// add the combination (node+weight)
+			nodes.insert(WeightedNode(node, weight));
+
+			// mark the node as processed
+			node->enqueued = true;
+
+		}
+
+		/** add a new to-be-processed node or update its old weight to the new one */
+		void addOrUpdate(DijkstraNode<T>* node, const float oldWeight, const float newWeight) {
+
+			// find and remove the previous combination (node+weight) if any
+			const auto old = nodes.find(WeightedNode(node, oldWeight));
+			if (old != nodes.end()) {nodes.erase(old);}
+
+			// add the new combination (node+weight)
+			nodes.insert(WeightedNode(node, newWeight));
+
+			// mark the node as processed
+			node->enqueued = true;
+
+		}
 
 		/** get the next to-be-processed node (smallest distance) */
 		DijkstraNode<T>* pop() {
-			DijkstraNode<T>* next = *toBeProcessedNodes.begin();
-			toBeProcessedNodes.erase(toBeProcessedNodes.begin());
-			return next;
+			DijkstraNode<T>* dn = (*nodes.begin()).dn;
+			nodes.erase(nodes.begin());
+			return dn;
 		}
 
 		/** set empty? */
-		bool empty() const {return toBeProcessedNodes.empty();}
+		bool empty() const {
+			return nodes.empty();
+		}
 
 	};
 
@@ -145,7 +174,7 @@ private:
 
 
 	/** get (or create) a new node for the given user-node */
-	inline DijkstraNode<T>* getNode(const T* userNode) {
+	inline DijkstraNode<T>* getOrCreateNode(const T* userNode) {
 		auto it = nodes.find(userNode);
 		if (unlikely(it == nodes.end())) {
 			DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);

nav/dijkstra/DijkstraPath.h

@@ -48,6 +48,8 @@ public:
 	const DijkstraNode<T>& operator [] (const int idx) const	{return *(path[idx]);}
 	size_t size() const											{return path.size();}
 
+	/** get the idx-th element from the starting-node. if this is beyond the end, the last node is returned */
+	const DijkstraNode<T>& getFromStart(const int idx) const	{return (idx >= (int) path.size()) ? (*path.back()) : (*path[idx]);}
 
 	/** NANOFLANN: number of elements in the path */
 	inline int kdtree_get_point_count() const {

nav/dijkstra/DijkstraStructs.h

@@ -19,15 +19,12 @@ template <typename T> struct DijkstraNode {
 	/** the weight from the start up to this element */
 	float cumWeight;
 
+	/** whether we already enqueued this node into the processing list (do NOT examing nodes twice) */
+	bool enqueued;
+
 
 	/** ctor */
-	DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF) {;}
-
-
-//	/** equal? (bi-dir) */
-//	bool operator == (const DijkstraNode<T>& other) const {
-//		return element == other.element;
-//	}
+	DijkstraNode(const T* element) : element(element), previous(), cumWeight(INF), enqueued(false) {;}
 
 };