added stuff

nav/a-star/AStar.h

@@ -43,9 +43,15 @@ public:
 		distance[source] = 0.0f;
 		Q.push(std::make_pair(source,distance[source]));
 
+                int iter = 0;
+                //std::cout << (std::string)*source << std::endl;
+                //std::cout << (std::string)*destination << std::endl;
+
 		// proceed until there are now new nodes to follow
 		while(!Q.empty()) {
 
+                    ++iter;
+
 			// fetch the next-nearest node from the queue
 			const T* u = Q.top().first;
 
@@ -66,12 +72,14 @@ public:
 				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)));
+                    Q.push(std::make_pair(&v, distance[&v] + acc.getHeuristic(v, *destination)));
 				}
 
 			}
 		}
 
+                //std::cout << iter << std::endl;
+
 
 		// construct the path
 		std::vector<const T*> path;