astar.cpp

#include <cstdio>
#include <queue>
#include <unordered_map>
#include <vector>
#include <algorithm>

using namespace std;

#ifndef _DEBUG
#define _DEBUG
#endif

// Exception
// Because yes
class InvalidPosition : public exception
{
	virtual const char* what() const throw()
	{
		return "No position matches given coordinates";
	}
};


// Types and Classes
/// Grid map definition
struct pos_t
{
	int x;
	int y;
	int w;
	int priority;

	friend bool operator==(const pos_t& a, const pos_t& b)
	{
		return (a.x == b.x) && (a.y == b.y);
	}

	friend bool operator!=(const pos_t& a, const pos_t& b)
	{
		return !(a == b);
	}

	friend bool operator<(const pos_t& a, const pos_t& b)
	{
		return !(a.priority < b.priority);
	}
};

class Grid
{
	vector<pos_t> positions;
	bool IsNeighbor(pos_t&, pos_t&) const;
public:
	void AddPosition(int, int, int);
	vector<pos_t> Neighbors(pos_t&);
	pos_t Position(int, int) const;
};

bool Grid::IsNeighbor(pos_t& a, pos_t& b) const
{
	return ((a.x == b.x && abs(a.y - b.y) == 1)
		|| (abs(a.x - b.x) == 1 && a.y == b.y));
}

void Grid::AddPosition(int x, int y, int w)
{
	pos_t position;
	position.x = x;
	position.y = y;
	position.w = w;
	position.priority = 0x7FFFFFFF;
	positions.push_back(position);
}

vector<pos_t> Grid::Neighbors(pos_t& current)
{
	vector<pos_t> _neighbors;
	for (auto& pos : positions)
	{
		if (IsNeighbor(current, pos))
		{
			_neighbors.push_back(pos);
#ifdef _DEBUG
			printf("-- Got neighbor (%02d, %02d)\n",
				pos.x, pos.y);
#endif
		}
		if (_neighbors.size() == 4)
			break;
	}

	return _neighbors;
}

pos_t Grid::Position(int x, int y) const
{
	try
	{
		for (const auto& pos : positions)
		{
			if (pos.x == x && pos.y == y)
				return pos;
		}

		throw InvalidPosition();
	}
	catch (InvalidPosition& e)
	{
		printf("[Grid::Position %02d, %02d] ", x, y);
	}
}

/// UMap Hash
namespace std
{
	template <>
	struct hash<pos_t>
	{
		size_t operator()(const pos_t& _Pos) const
		{
			return ((hash<int>()(_Pos.x)
				^ (hash<int>()(_Pos.y) << 1)) >> 1)
				^ (hash<int>()(_Pos.w) << 1);
		}
	};
}

/// AStar-wise
typedef unordered_map<pos_t, pos_t> SourceMap;

class PriorityMap
{
	unordered_map<pos_t, int> _map;
public:
	int& operator[](const pos_t& k)
	{
		if (_map.find(k) == _map.end())
			_map.emplace(k, 0x7FFFFFFF); // 32bit 'infinite'

		return _map[k];
	}
};

// Global shit
SourceMap smCameFrom;
PriorityMap pmScore;

// Function definitions
int heuristics(pos_t origin, pos_t destination)
{
	return abs(origin.x - destination.x)
		+ abs(origin.y - destination.y);
}

int AStarPathFinder(Grid& g, pos_t start, pos_t goal)
{
	priority_queue<pos_t> frontier;
	start.priority = heuristics(start, goal);
	frontier.push(start);

	pmScore[start] = 0;

	while (frontier.size() > 0)
	{
		pos_t current = frontier.top();
		frontier.pop();
#ifdef _DEBUG
		printf("Position: (%02d, %02d). Frontier size: %d\n",
			current.x, current.y, frontier.size());
#endif
		if (current == goal)
			break;

		for (auto& neighbor : g.Neighbors(current))
		{
			int score = pmScore[current] + neighbor.w;
			if (score < pmScore[neighbor])
			{
#ifdef _DEBUG
				printf("-- Neighbor (%02d, %02d). Score = %d + %d\n",
					neighbor.x, neighbor.y, score,
					heuristics(neighbor, goal));
#endif
				pmScore[neighbor] = score;
				smCameFrom[neighbor] = current;
				neighbor.priority = score + heuristics(neighbor, goal);
				frontier.push(neighbor);
			}
		}
	}

	return pmScore[goal];
}

vector<pos_t> ReconstructPath(pos_t goal)
{
	vector<pos_t> path;
	path.push_back(goal);
	pos_t current = goal;

	while (pmScore[current] != 0)
	{
		current = smCameFrom[current];
		path.push_back(current);
	}

	reverse(path.begin(), path.end());
	return path;
}

// Test 
int grid_example[10][10] =
{
	{ 5, 4, 5,  6,  7,  8,  9, 10, 11, 12 },
	{ 4, 3, 4,  5, 10, 13, 10, 11, 12, 13 },
	{ 3, 2, 3,  4,  9, 14, 15, 12, 13, 14 },
	{ 2, 1, 2,  3,  8, 13, 18, 17, 14,  0 },
	{ 1, 1, 6, 11, 16,  0,  0,  0,  0,  0 },
	{ 2, 1, 2,  7, 12, 17,  0,  0,  0,  0 },
	{ 3, 2, 3,  4,  9, 14, 19,  0,  0,  0 },
	{ 4, 0, 0,  0, 14, 19, 18,  0,  0,  0 },
	{ 5, 0, 0,  0, 15, 16, 13,  3,  0,  0 },
	{ 6, 7, 8,  9, 10, 11, 12, 13, 14,  0 }
};

int main()
{
	Grid g;

	for (int i = 0; i < 10; ++i)
		for (int j = 0; j < 10; ++j)
			if (grid_example[i][j] != 0)
				g.AddPosition(j, i, grid_example[i][j]);

	int cost = AStarPathFinder(g, g.Position(1, 4), g.Position(7, 8));

	for (auto position : ReconstructPath(g.Position(7, 8)))
	{
		printf("(%d, %d): %02d\n", position.x, position.y, position.w);
	}

	printf("Total cost: %d\n", cost);
	return 0;
}