worked on disjkstra and a*

grid/Grid.h

@@ -152,6 +152,11 @@ public:
 		return (hashes.find(uid) != hashes.end());
 	}
 
+	/** get a list of all nodes within the graph */
+	const std::vector<T>& getNodes() const {
+		return nodes;
+	}
+
 	/** get the center-node the given Point belongs to */
 	const T& getNodeFor(const GridPoint& p) {
 		const UID uid = getUID(p);

nav/a-star/AStar.h

@@ -0,0 +1,119 @@
+#ifndef ASTAR_H
+#define ASTAR_H
+
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include <limits>
+#include <queue>
+#include <unordered_map>
+#include <cassert>
+
+#include "../../grid/Grid.h"
+
+
+template <typename T> class AStar {
+
+public:
+
+
+
+	//dijkstra with priority queue O(E log V)
+	template <typename Access> static std::vector<const T*> get(const T* source, const T* destination, Access acc) {
+
+		// track distances from the source to each other node
+		std::unordered_map<const T*, float> distance;
+
+		// track the previous node for each node along the path
+		std::unordered_map<const T*, const T*> parent;
+
+		// all nodes
+		const std::vector<T>& nodes = acc.getAllNodes();
+
+		// priority queue to check which node is to-be-processed next
+		std::priority_queue<std::pair<T*,float>, std::vector<std::pair<const T*,float>>, Comparator2> Q;
+
+		// start with infinite distance
+		for(const auto& node : nodes){
+			distance[&node] = std::numeric_limits<float>::max();
+		}
+
+
+		// start at the source
+		distance[source] = 0.0f;
+		Q.push(std::make_pair(source,distance[source]));
+
+		// proceed until there are now new nodes to follow
+		while(!Q.empty()) {
+
+			// fetch the next-nearest node from the queue
+			const T* u = Q.top().first;
+
+			// and check whether we reached the destination
+			if (u == destination) {break;}
+
+			// remove from the Queue
+			Q.pop();
+
+			// process all neighbors for the current element
+			for( const T& v : acc.getNeighbors(*u)) {
+
+				// weight (distance) between the current node and its neighbor
+				//const float w = ((Point3)v - (Point3)*u).length();
+				const float w = acc.getWeightBetween(v, *u);
+
+				// found a better route?
+				if (distance[&v] > distance[u] + w) {
+					distance[&v] = distance[u] + w;
+					parent[&v] = u;
+					Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *source)));
+				}
+
+			}
+		}
+
+
+		// construct the path
+		std::vector<const T*> path;
+		const T* p = destination;
+		path.push_back(destination);
+
+		// until we reached the source-node
+		while (p!=source) {
+			if (p) {
+				p = parent[p];
+				path.push_back(p);
+			} else {
+				return std::vector<const T*>(); //if no path could be found, just return an empty vector.
+			}
+		}
+
+		// done
+		return path;
+
+	}
+
+
+//	template <typename Access> static std::vector<const T*> getShortestPathAStar(const T* src, const T* dst, Access acc){
+
+//		std::vector<const T*> shortestPath;
+
+//		//here we could do some preprocessing. e.g. area of interest of nodes
+
+
+//		// call aStar
+//		shortestPath = aStar(src, dst, acc);
+
+//		return shortestPath;
+//	}
+
+	class Comparator2 {
+	public:
+		int operator() ( const std::pair<const T*,float>& p1, const std::pair<const T*,float>& p2){
+			return p1.second > p2.second;
+		}
+	};
+
+};
+
+#endif // ASTAR_H

nav/dijkstra/Dijkstra.h

@@ -49,7 +49,7 @@ public:
 		// runs until all nodes were evaluated
 		(void) end;
 
-		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)*start + " to ALL OTHER nodes", false);
+		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)*start + " to " + ((end)?((std::string)*end):"ALL OTHER nodes"), true);
 		Log::tick();
 
 		// cleanup previous runs
@@ -75,10 +75,10 @@ public:
 			DijkstraNode<T>* dnSrc = toBeProcessedNodes.pop();
 
 			// when an end is given, stop when end was reached
-			if (end != nullptr && dnSrc->element == end) {break;}
+			if (end != nullptr && dnSrc->element == end) {Log::add("Dijkstra", "reached target node"); break;}
 
 			// when a maximum weight is given, stop when current cum-dist > maxWeight
-			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {break;}
+			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached distance limit"); break;}
 
 			// visit (and maybe update) each neighbor of the current element
 			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {
@@ -113,8 +113,8 @@ public:
 
 		}
 
+		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes", false);
 		Log::tock();
-		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");
 
 	}
 

tests/nav/a-star/TestAStar.cpp

@@ -0,0 +1,48 @@
+#ifdef WITH_TESTS
+
+#include "../../Tests.h"
+
+#include "../../../grid/Grid.h"
+#include "../../../nav/a-star/AStar.h"
+#include "../../grid/Plot.h"
+
+TEST(AStar, build) {
+
+	Grid<GP> grid(1);
+
+	int idx1 = grid.add(GP( 0, 0, 0));
+	int idx2 = grid.add(GP( 0, 1, 0));
+	int idx3 = grid.add(GP( 0,-1, 0));
+	int idx4 = grid.add(GP( 1, 0, 0));
+	int idx5 = grid.add(GP(-1, 0, 0));
+
+	grid.connectBiDir(idx1, idx2);
+	grid.connectBiDir(idx1, idx3);
+	grid.connectBiDir(idx1, idx4);
+	grid.connectBiDir(idx1, idx5);
+
+	class TMP {
+		Grid<GP>& grid;
+	public:
+		TMP(Grid<GP>& grid) : grid(grid) {;}
+//		int getNumNeighbors(const GP& node) const {return node.getNumNeighbors();}
+//		const GP* getNeighbor(const GP& node, const int idx) const {return &grid.getNeighbor(node, idx);}
+		const std::vector<GP>& getAllNodes() const {return grid.getNodes();}
+		decltype(grid.neighbors(GP())) getNeighbors(const GP& node) const {return grid.neighbors(node);}
+		float getWeightBetween(const GP& n1, const GP& n2) const {return ((Point3)n1 - (Point3)n2).length();}
+		float getHeuristic(const GP& n1, const GP& n2) const {return std::abs(n1.x_cm - n2.x_cm) + std::abs(n1.y_cm - n2.y_cm);}
+	} tmp(grid);
+
+	AStar<GP> nav;
+	std::vector<const GP*> vec = nav.get(&grid[0], &grid[4], tmp);
+
+	for (const GP* g : vec) {
+		std::cout << g << std::endl;
+	}
+
+
+}
+
+
+
+#endif