From e7a5f8eb8cf9d1b9779a4a6ca6e228ff5cdf7ed9 Mon Sep 17 00:00:00 2001
From: FrankE <support@frank-ebner.de>
Date: Wed, 13 Apr 2016 11:06:00 +0200
Subject: [PATCH] added a-star testing version

---
 nav/a-star/AStar.h | 32 +++++++++++++++++++++-----------
 1 file changed, 21 insertions(+), 11 deletions(-)

diff --git a/nav/a-star/AStar.h b/nav/a-star/AStar.h
index 131d18f..792cad1 100644
--- a/nav/a-star/AStar.h
+++ b/nav/a-star/AStar.h
@@ -16,16 +16,20 @@ template <typename T> class AStar {
 
 public:
 
-
+#define LE_MAX 500000
 
 	//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;
+		//std::unordered_map<const T*, float> distance;
+
+		float distance[LE_MAX];
+
 
 		// track the previous node for each node along the path
-		std::unordered_map<const T*, const T*> parent;
+		//std::unordered_map<const T*, const T*> parent;
+		const T* parent[LE_MAX];
 
 		// all nodes
 		const std::vector<T>& nodes = acc.getAllNodes();
@@ -35,13 +39,14 @@ public:
 
 		// start with infinite distance
 		for(const auto& node : nodes){
-			distance[&node] = std::numeric_limits<float>::max();
+			distance[node.getIdx()] = std::numeric_limits<float>::max();
 		}
 
 
 		// start at the source
-		distance[source] = 0.0f;
-		Q.push(std::make_pair(source,distance[source]));
+		distance[source->getIdx()] = 0.0f;
+		Q.push(std::make_pair(source,distance[source->getIdx()]));
+		int iter = 0;
 
 		// proceed until there are now new nodes to follow
 		while(!Q.empty()) {
@@ -63,15 +68,20 @@ public:
 				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)));
+				if (distance[v.getIdx()] > distance[u->getIdx()] + w) {
+					distance[v.getIdx()] = distance[u->getIdx()] + w;
+					parent[v.getIdx()] = u;
+					Q.push(std::make_pair(&v, distance[v.getIdx()] + acc.getHeuristic(v, *destination)));		// SOURCE OR DEST?!
 				}
 
 			}
+			++iter;
 		}
 
+		std::cout << (std::string) *source << std::endl;
+		std::cout << (std::string) *destination << std::endl;
+		std::cout << iter << std::endl;
+
 
 		// construct the path
 		std::vector<const T*> path;
@@ -81,7 +91,7 @@ public:
 		// until we reached the source-node
 		while (p!=source) {
 			if (p) {
-				p = parent[p];
+				p = parent[p->getIdx()];
 				path.push_back(p);
 			} else {
 				return std::vector<const T*>(); //if no path could be found, just return an empty vector.