Implement A-star with null euristic

main.cc

@@ -11,6 +11,9 @@
 #include <ctime>
 #include <stack>
 #include <queue>
+#include <utility> // pair
+#include <functional> // greater
+#include <limits>
 #include <unistd.h>
 
 using namespace std;
@@ -349,6 +352,93 @@ class World
 			return targetIsReached;
 		} 
 
+		const bool aStar(unsigned int origin, unsigned int target, list<unsigned int>& path, list<unsigned int>& discovered)
+		{
+			bool targetIsReached = false;
+
+			bool explored[size];
+			unsigned int previous[size];
+			unsigned int realCost[size]; // distance from origin
+
+			for (unsigned int k(0); k < size; k ++) {
+				explored[k] = false;
+				previous[k] = k;
+				realCost[k] = numeric_limits<unsigned int>::max();
+			}
+
+			explored[origin] = true;
+			realCost[origin] = 0;
+
+			priority_queue
+			< 
+				pair<unsigned int, unsigned int>,
+				vector<pair<unsigned int, unsigned int>>,
+				greater<pair<unsigned int, unsigned int>>
+			> open;
+
+			open.push(make_pair(euristic(origin, target), origin));
+
+			int current;
+			int currentCost;
+			int neighbour;
+			unsigned int succs[4];
+			unsigned int nbSuccs;
+
+			do {
+				current = open.top().second;
+				currentCost = realCost[current];
+				open.pop();
+
+				nbSuccs = successors(current, succs);
+
+				for (unsigned int i(0); i < nbSuccs; i++) {
+					neighbour = succs[i];
+
+					// Update path if better
+					if (currentCost + 1 < realCost[neighbour]) {
+						previous[neighbour] = current;
+						realCost[neighbour] = currentCost + 1;
+					}
+
+					// Explore if unknown
+					if (!explored[neighbour]) {
+						explored[neighbour] = true;
+						open.push(make_pair(realCost[neighbour] + euristic(neighbour, target), neighbour));
+					}
+				}
+
+				// Current tile is now processed
+				discovered.push_back(current);
+
+				// Stop if target found
+				targetIsReached = (current == target);
+
+			} while (!targetIsReached && !open.empty());
+
+			// Remove origin and target
+			if (!discovered.empty()) {
+				discovered.remove(origin);
+				discovered.remove(target);
+			}
+
+			// Build path
+			if (targetIsReached) {
+				do {
+					path.push_back(current);
+					current = previous[current];
+				} while (current != origin);
+
+				path.pop_front();
+			}
+
+			return targetIsReached;
+		} 
+
+		unsigned int euristic(unsigned int from, unsigned int to) {
+			// Todo : improve euristic
+			return 0;
+		}
+
 		void animate(bool exitFound, const list<unsigned int>& discovered, const list<unsigned int>& path) {
 			for (unsigned int tile : discovered) {
 				markOne(tile, DISCOVERED);
@@ -444,6 +534,19 @@ int main()
 
 	bfsWorld.showResults(bfsExitFound, bfsDiscovered, bfsPath);
 
+	// 3
+	cout << endl << "A star" << endl;
+
+	World aStarWorld(w);
+	list<unsigned int> aStarPath;
+	list<unsigned int> aStarDiscovered;
+	bool aStarExitFound = aStarWorld.aStar(start, end, aStarPath, aStarDiscovered);
+
+	if (animation)
+		aStarWorld.animate(aStarExitFound, aStarDiscovered, aStarPath);
+
+	aStarWorld.showResults(aStarExitFound, aStarDiscovered, aStarPath);
+
 	// End
 	return 0;
 }