Adapt regular pathfinding

markov/main.cc

@@ -0,0 +1,214 @@
+// Example of world building, display, and successor computation for the artificial 
+// intelligence path-finding lab
+//
+// Author: Didier LIME
+// Adapted by : Jovian HERSEMEULE
+// Date: 2018-10-03
+
+#include <iostream>
+#include <unistd.h>
+
+using namespace std;
+
+// Default parameters
+#define DEFAULT_LENGTH 6
+#define DEFAULT_HEIGHT 5
+#define DEFAULT_ANIMATION true
+#define DEFAULT_ANIMATION_DELAY 50000 // us
+#define DEFAULT_COLOR_ENABLE true
+
+// Tile codes
+#define FLOOR_TILE 0
+#define WALL_TILE 1
+#define TARGET_TILE 2
+#define HOLE_TILE 3
+
+// Typical cost
+#define FLOOR_COST 0.04f
+#define HOLE_COST -1.0f
+#define TARGET_COST 1.0f
+
+unsigned int identifyTile(unsigned int y, unsigned int x, unsigned int l) {
+	return y * l + x;
+}
+
+void moveCursorUp(const unsigned int& h) {
+	cout << "\033[" << h << 'A';
+}
+
+class World
+{
+	private:
+		// Number of columns
+		unsigned int l;
+
+		// Number of lines
+		unsigned int h;
+
+		// Size of the array
+		const unsigned int size;
+
+		// Unidimensional array for tiles
+		int* board;
+
+		// Unidimensional array of costs
+		float* cost;
+
+		// Number of empty tiles
+		unsigned int tileQuantity;
+
+	public:
+		// Constructor
+		World(unsigned int l_, unsigned int h_)
+			:l(l_), h(h_), size(l_ * h_), tileQuantity(l_ * h_)
+		{
+			board = new int[size]();
+			cost = new float[size]();
+
+			// Clean
+			for (unsigned int k(0); k < size; k++) {
+				board[k] = FLOOR_TILE;
+				cost[k] = FLOOR_COST;
+			}
+
+			// Add walls to the first and last columns
+			for (unsigned int i = 0; i < h; i++)
+			{
+				board[i * l] = WALL_TILE;
+				board[i * l + l - 1] = WALL_TILE;
+				tileQuantity -= 2;
+			}
+
+			// Add walls to the first and last lines
+			for (unsigned int j = 0; j < l; j++)
+			{
+				board[j] = WALL_TILE;
+				board[(h - 1) * l + j] = WALL_TILE;
+				tileQuantity -= 2;
+			}
+
+			// Add hardcoded elements
+			board[identifyTile(2, 2, l)] = WALL_TILE;
+
+			board[identifyTile(1, 4, l)] = TARGET_TILE;
+			cost[identifyTile(1, 4, l)] = TARGET_COST;
+
+			board[identifyTile(2, 4, l)] = HOLE_TILE;
+			cost[identifyTile(2, 4, l)] = HOLE_COST;
+		}
+
+		// Copy constructor
+		World(const World& other)
+			:l(other.l), h(other.h), size(other.size), tileQuantity(other.tileQuantity)
+		{
+			board = new int[size]();
+			cost = new float[size]();
+
+			for (int k(0); k < size; k++) {
+				board[k] = other.board[k];
+			}
+		}
+
+		// Display the world
+		void display()
+		{
+			for (unsigned int i = 0; i < h; i++)
+			{
+				for (unsigned int j = 0; j < l; j++)
+				{
+					switch (board[identifyTile(i, j, l)])
+					{
+						case FLOOR_TILE:
+							cout << " ";
+							break;
+						case WALL_TILE:
+							if (DEFAULT_COLOR_ENABLE) cout << "\033[0;43m";
+							cout << "#";
+							break;
+						case HOLE_TILE:
+							if (DEFAULT_COLOR_ENABLE) cout << "\033[1;35m";
+							cout << "X";
+							break;
+						case TARGET_TILE:
+							if (DEFAULT_COLOR_ENABLE) cout << "\033[1;36m";
+							cout << "O";
+							break;
+					}
+
+					if (DEFAULT_COLOR_ENABLE) cout << "\033[0;30m";
+				}
+
+				cout << endl;
+			}
+		}
+
+		// compute the successors of tile number i in world w
+		// we return the number n of valid successors
+		// the actual list is in array r where only the first n
+		// elements are significant
+		unsigned int successors(unsigned int i, unsigned int r[4])
+		{
+			unsigned int n = 0;
+
+			if (i >= 0 && i < size && board[i] != WALL_TILE)
+			{
+				// if i is a correct tile number (inside the array and not on a wall)
+				// look in the four adjacent tiles and keep only those with no wall
+				const unsigned int moves[] = { i - 1, i + 1, i - l, i + l};
+
+				for (unsigned int k = 0; k < 4; k++)
+				{
+					if (board[moves[k]] != WALL_TILE)
+					{
+						r[n] = moves[k];
+						n++;
+					}
+				}
+			}
+
+			return n;
+		}
+
+		// Mark a point in the world
+		void markOne(unsigned int tile, int value = HOLE_TILE) {
+			board[tile] = value;
+		}
+
+		void showProperties() {
+			cout << "Length : " << l << endl;
+			cout << "Height : " << h << endl;
+			cout << "Number of floor tiles : " << tileQuantity << endl;
+		}
+};
+
+int main()
+{
+	// Create a world
+	const unsigned int l(DEFAULT_LENGTH), h(DEFAULT_HEIGHT);
+
+	World w(l, h);
+
+	// Display it
+	cout << endl << "Default world" << endl;
+	w.showProperties();
+	w.display();
+
+	const bool animation(DEFAULT_ANIMATION);
+
+	// 1
+	cout << endl << "Value iteration" << endl;
+
+	World w1(w);
+	w1.display();
+
+	// 2
+	cout << endl << "Policy iteration" << endl;
+
+	World w2(w);
+	w2.display();
+
+	// End
+	return 0;
+}
+
+