PathFinding/main.cc

// 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 <list>
#include <cstdlib>
#include <ctime>
#include <stack>
#include <queue>
#include <utility> // pair
#include <functional> // greater
#include <limits>
#include <unistd.h>

using namespace std;

// Default parameters
#define DEFAULT_LENGTH 50
#define DEFAULT_HEIGHT 15
#define DEFAULT_PROBABILITY 0.2
#define DEFAULT_ANIMATION true
#define DEFAULT_ANIMATION_DELAY 50000 // us
#define DEFAULT_COLOR_ENABLE true

// Tile codes
#define FREE 0
#define WALL 1
#define ORIGIN 2
#define TARGET 3
#define DISCOVERED 4
#define TRACE 5

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';
}

class World
{
	private:
		// Number of columns
		unsigned int l;

		// Number of lines
		unsigned int h;

		// Size of the array
		unsigned int size;

		// Unidimensional array for tiles
		int* board;

		// Number of empty tiles
		unsigned int tileQuantity;

	public:
		// Default constructor
		World()
			:l(0), h(0), size(0), board(nullptr), tileQuantity(0)
		{}

		// Copy constructor
		World(const World& other)
			:l(other.l), h(other.h), size(other.size), tileQuantity(other.tileQuantity)
		{
			board = new int[size]();

			for (int k(0); k < size; k++) {
				board[k] = other.board[k];
			}
		}

		// Random generation
		void generate(unsigned int l_, unsigned int h_, double p)
		{
			// Error if board exists
			if (board != nullptr)
				cerr << "Can't generate world, board already exists" << endl;

			// Initialize attributes
			l = l_;
			h = h_;
			size = l * h;
			tileQuantity = size;

			board = new int[size]();

			// Add floor everywhere
			for (unsigned int k(0); k < size; k++)
				board[k] = FREE;

			// Add walls to the first and last columns
			for (unsigned int i(0); i < h; i++) {
				markOne(i * l, WALL);
				markOne(i * l + l - 1, WALL);
			}

			// Add walls to the first and last lines
			for (unsigned int j = 0; j < l; j++) {
				markOne(j, WALL);
				markOne((h - 1) * l + j, WALL);
			}

			// Randomly add walls
			for (unsigned int i = 0; i < h; i++)
			{
				for (unsigned int j = 0; j < l; j++)
				{
					// add a wall in this tile with probability p and provided that it is neither
					// the starting tile nor the goal tile 
					if ((double) rand() / RAND_MAX < p && !(i == 1 && j == 1) && !(i == h - 2 && j == l - 2))
						markOne(i * l + j, WALL);
				}
			}
		}

		// 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 FREE:
							cout << " ";
							break;
						case WALL:
							if (DEFAULT_COLOR_ENABLE) cout << "\033[0;43m";
							cout << "#";
							break;
						case ORIGIN:
							if (DEFAULT_COLOR_ENABLE) cout << "\033[1;33m";
							cout << "o";
							break;
						case TARGET:
							if (DEFAULT_COLOR_ENABLE) cout << "\033[1;33m";
							cout << "T";
							break;
						case DISCOVERED:
							if (DEFAULT_COLOR_ENABLE) cout << "\033[0;34m";
							cout << ":";
							break;
						case TRACE:
							if (DEFAULT_COLOR_ENABLE) {
								cout << "\033[0;32m";
								cout << "\033[1;42m";
								cout << ":";
							}
							else
								cout << "+";
							break;
					}

					if (DEFAULT_COLOR_ENABLE) cout << "\033[0m";
				}

				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)
			{
				// 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)
					{
						r[n] = moves[k];
						n++;
					}
				}
			}

			return n;
		}

		// Mark a list of points in the world
		void markAll(const list<unsigned int>& path, int value = TRACE) {
			for (auto tile : path) {
				markOne(tile, value);
			}
		}

		// Mark a point in the world
		void markOne(unsigned int tile, int value = ORIGIN) {
			if (value == WALL && board[tile] != WALL)
				tileQuantity --;

			board[tile] = value;
		}




		// Depth-first search
		// starting from tile number origin, find a path to tile number t
		// return true if such a path exists, false otherwise
		// if it exists the path is given in variable path (hence the reference &)
		const bool dfs(unsigned int origin, unsigned int target, list<unsigned int>& path, list<unsigned int>& discovered)
		{
			bool targetIsReached = false;

			bool explored[size];
			unsigned int previous[size];

			for (unsigned int k(0); k < size; k ++) {
				explored[k] = false;
				previous[k] = k;
			}

			explored[origin] = true;


			stack<unsigned int> open;
			open.push(origin);

			int current;
			int neighbour;
			unsigned int succs[4];
			unsigned int nbSuccs;

			do {
				current = open.top();
				open.pop();

				nbSuccs = successors(current, succs);

				for (unsigned int i(0); i < nbSuccs; i++) {
					neighbour = succs[i];

					if (!explored[neighbour]) {
						open.push(neighbour);
						explored[neighbour] = true;
						previous[neighbour] = current;
					}
				}

				// 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;
		} 

		// Breadth-first search
		// starting from tile number origin, find a path to tile number t
		// return true if such a path exists, false otherwise
		// if it exists the path is given in variable path (hence the reference &)
		const bool bfs(unsigned int origin, unsigned int target, list<unsigned int>& path, list<unsigned int>& discovered)
		{
			bool targetIsReached = false;

			bool explored[size];
			unsigned int previous[size];

			for (unsigned int k(0); k < size; k ++) {
				explored[k] = false;
				previous[k] = k;
			}

			explored[origin] = true;


			queue<unsigned int> open;
			open.push(origin);

			int current;
			int neighbour;
			unsigned int succs[4];
			unsigned int nbSuccs;

			do {
				current = open.front();
				open.pop();

				nbSuccs = successors(current, succs);

				for (unsigned int i(0); i < nbSuccs; i++) {
					neighbour = succs[i];

					if (!explored[neighbour]) {
						open.push(neighbour);
						explored[neighbour] = true;
						previous[neighbour] = current;
					}
				}

				// 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;
		} 

		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);
				display();
				usleep(DEFAULT_ANIMATION_DELAY);
				moveCursorUp(h);
			}

			if (exitFound)
				for (unsigned int tile : path) {
					markOne(tile, TRACE);
					display();
					usleep(DEFAULT_ANIMATION_DELAY);
					moveCursorUp(h);
				}
		}

		void showResults(bool exitFound, const list<unsigned int>& discovered, const list<unsigned int>& path) {

			markAll(discovered, DISCOVERED);

			if (!path.empty()) {
				markAll(path, TRACE);
			}

			display();

			// Display DFS results
			if (exitFound)
				cout << "SUCCESS !" << endl;
			else
				cout << "FAILURE ..." << endl;

			const unsigned int discoveryRate(100 * discovered.size() / tileQuantity);
			cout << discovered.size() << " tiles discovered (" << discoveryRate << "%);" << endl;
			cout << path.size() << " path length." << endl;
		}

		void showProperties() {
			cout << "Length : " << l << endl;
			cout << "Height : " << h << endl;
			cout << "Number of floor tiles : " << tileQuantity << endl;
		}
};

int main()
{
	// Initialise the random number generator
	srand(time(0));

	// Create a world
	const unsigned int l(DEFAULT_LENGTH), h(DEFAULT_HEIGHT);
	const double wallProbability(DEFAULT_PROBABILITY);

	World w;
	w.generate(l, h, wallProbability);

	unsigned int start(identifyTile(1, 1, l));
	unsigned int end(identifyTile(h - 2, l - 2, l));

	// Display it
	cout << endl << "Generated world" << endl;
	w.showProperties();
	w.markOne(start, ORIGIN);
	w.markOne(end, TARGET);
	w.display();

	const bool animation(DEFAULT_ANIMATION);

	// 1
	cout << endl << "Depth-First Search" << endl;

	World dfsWorld(w);
	list<unsigned int> dfsPath;
	list<unsigned int> dfsDiscovered;
	bool dfsExitFound = dfsWorld.dfs(start, end, dfsPath, dfsDiscovered);

	if (animation)
		dfsWorld.animate(dfsExitFound, dfsDiscovered, dfsPath);

	dfsWorld.showResults(dfsExitFound, dfsDiscovered, dfsPath);

	// 2
	cout << endl << "Breadth-First Search" << endl;

	World bfsWorld(w);
	list<unsigned int> bfsPath;
	list<unsigned int> bfsDiscovered;
	bool bfsExitFound = bfsWorld.bfs(start, end, bfsPath, bfsDiscovered);

	if (animation)
		bfsWorld.animate(bfsExitFound, bfsDiscovered, bfsPath);

	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;
}