Merge branch 'a-star'

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;
@@ -35,6 +38,18 @@ unsigned int identifyTile(unsigned int y, unsigned int x, unsigned int l) {
 	return y * l + x;
 }
 
+void locateTile(unsigned int id, unsigned int l, unsigned int &x, unsigned int &y) {
+	y = id / l;
+	x = id - l * y;
+}
+
+unsigned int delta(unsigned int c1, unsigned int c2) {
+	if (c1 < c2)
+		return c2 - c1;
+	else
+		return c1 - c2;
+}
+
 void moveCursorUp(const unsigned int& h) {
 	cout << "\033[" << h << 'A';
 }
@@ -349,6 +364,101 @@ 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) {
+			// Manhattan
+			unsigned int x1, y1, x2, y2, dx, dy;
+
+			locateTile(from, l, x1, y1);
+			locateTile(to, l, x2, y2);
+
+			dx = delta(x1, x2);
+			dy = delta(y1, y2);
+
+			return dx + dy;
+		}
+
 		void animate(bool exitFound, const list<unsigned int>& discovered, const list<unsigned int>& path) {
 			for (unsigned int tile : discovered) {
 				markOne(tile, DISCOVERED);
@@ -444,6 +554,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;
 }