Astar C++로 만들기

// Astar.cpp : 이 파일에는 'main' 함수가 포함됩니다. 거기서 프로그램 실행이 시작되고 종료됩니다.
//

#include <iostream>
#include "olcConsoleGameEngine.h"
#include <list>
#include <vector>
#include<iomanip>
#include <map>
#include <cstdio>
#include <cstring>
#include <iterator>
#include<random>
#include <algorithm>
#include<queue>
#define HASH_TABLE_COUNT 11
#define DIR_COUNT 8
using namespace std;

enum NodeDirection : UINT8
{
	N,
	E,
	S,
	W,
	NE,
	NW,
	SE,
	SW
};


struct Vector2
{
	int x;
	int y;

	Vector2(int _x, int _y) : x(_x), y(_y) {}
};

Vector2 nodeDirOffset[] = { Vector2(0,1),Vector2(1,0) ,Vector2(0,-1) ,Vector2(-1,0) ,Vector2(1,1) ,Vector2(-1,1) ,Vector2(1,-1) ,Vector2(-1,-1) };

class Node
{
public:
	//X1, Y1, X2, Y2
	int drawLocation[4] = { 0, 0, 0, 0 };
	int x, y;
	int movedDirection;
	int ID;
	Node* parent;
	Node* neighborNodes[8];
	float costFromStart;
	float costToGoal;
	float totalCost;
	bool isInOpenList;
	bool isInCloseList;
	bool isWall;
	Node()
	{
		ZeroMemory(neighborNodes, sizeof(neighborNodes));
		parent = NULL;
		costFromStart = 0, costToGoal = 0, x = 0, y = 0, ID = 0, totalCost = 0, movedDirection = 0;
		isInOpenList = false;
		isInCloseList = false;
		isWall = false;
	}

	void SetDrawLocation(int x1, int y1, int x2, int y2)
	{
		drawLocation[0] = x1; drawLocation[1] = y1; drawLocation[2] = x2; drawLocation[3] = y2;
	}

	void SetLocation(int X, int Y)
	{
		x = X;
		y = Y;
	}
};

class HashTable
{
public:
	struct stData
	{
		int ID;
		Node* value;
		stData* next;
		stData()
		{
			ID = 0;
			value = 0;
			next = NULL;
		}
	};
	stData* hashList[HASH_TABLE_COUNT];
	int GetHashKey(int ID)
	{
		return ID % HASH_TABLE_COUNT;
	}

	void AddValue(int ID, Node* value)
	{
		stData* data = new stData;
		data->ID = ID;
		data->value = value;
		int index = GetHashKey(ID);
		stData* listData = hashList[index];
		if (hashList[index] != NULL)
		{
			data->next = listData;
			hashList[index] = data;
		}
		else
		{
			hashList[index] = data;
		}
	}

	void RemoveValue(int ID)
	{
		int index = GetHashKey(ID);
		stData* data = hashList[index];
		stData* tempData = data;
		while (data)
		{
			if (data->ID == ID)
			{
				if (hashList[index] == data)
				{
					hashList[index] = data->next;
				}
				else
				{
					tempData->next = data->next;
				}
				break;
			}
			tempData = data;
			data = data->next;
		}
	}

	void DisplayHash()
	{
		printf("\n");
		int i = 0;
		for (int i = 0; i < HASH_TABLE_COUNT; ++i)
		{
			stData* data = hashList[i];
			int size = 0;
			printf("값: ");
			while (data)
			{
				if (data->next != NULL)
				{
					//printf("%d, ", data->value);
				}
				else
				{
					//printf("%d      ", data->value);
				}
				data = data->next;
				++size;
			}
			//printf("\n[%d] 개수 : %d\n\n", i, size);
		}
		printf("\n");
	}
};

struct NodeCostCompare
{
	bool operator()(Node* nod1, Node* nod2)
	{
		if (nod1->totalCost == nod2->totalCost)
		{
			return nod1->costToGoal > nod2->costToGoal;
		}
		return nod1->totalCost > nod2->totalCost;
	}
};

class DrawAstar : public olcConsoleGameEngine
{
private:
	const static int mapWidth = 16;
	const static int mapHeight = 16;
	const static int wallCount = 12;
	int nodeBorder = 3;
	int nodeSizeWidth = 6;
	int nodeSizeHeight = 6;
	HashTable* hashTable;
	vector<Node*> nodeVector;
	Node* map[mapWidth][mapHeight];
	Node* startNode;
	Node* endNode;
	Node* wallNode[wallCount];
	//std::map<int[], NodeDirection> map;

	priority_queue<Node*, vector<Node*>, NodeCostCompare> openList;
	priority_queue<Node*> closeList;

public:
	DrawAstar()
	{
		hashTable = NULL;
		startNode = NULL;
		endNode = NULL;

	}

	Node* GetNodeByDir(NodeDirection dir, Node* node)
	{
		int nodeX = node->x + nodeDirOffset[dir].x;
		int nodeY = node->y + nodeDirOffset[dir].y;
		Node* neighborNode = NULL;

		nodeX = std::clamp(nodeX, 0, mapWidth - 1);
		nodeY = std::clamp(nodeY, 0, mapHeight - 1);

		neighborNode = map[nodeX][nodeY];
		if (neighborNode != node)
		{
			return neighborNode;
		}
		else
		{
			return NULL;
		}
	}

	NodeDirection GetDirByNode(Node* current, Node* goal)
	{
		NodeDirection dir;
		int X = 0;
		if (current->x != goal->x)
		{
			X = current->x > goal->x ? 1 : -1;
		}

		int Y = 0;
		if (current->y != goal->y)
		{
			Y = current->y > goal->y ? 1 : -1;
		}

		int offsetLen = sizeof(nodeDirOffset) / sizeof(Vector2);
			
		for (int i = 0; i < offsetLen; ++i)
		{
			if (nodeDirOffset[i].x == X && nodeDirOffset[i].y == Y)
			{
				return (NodeDirection)i;
			}
		}
	}


	int GetHeuristics(Node* current, Node* goal, Node* parentNode)
	{
		float width = current->x > goal->x ? current->x - goal->x : goal->x - current->x;
		float height = current->y > goal->y ? current->y - goal->y : goal->y - current->y;
		int H = sqrt((width * width) + (height * height)) * 10;
		return H;
	}


	virtual bool OnUserCreate()
	{
		hashTable = new HashTable;
		memset(hashTable->hashList, 0, sizeof(hashTable->hashList));
		int totalSize = mapWidth * mapHeight;
		nodeVector.reserve(totalSize);
		int id = 0;
		for (int i = 0; i < mapWidth; i++)
		{
			for (int k = 0; k < mapHeight; k++)
			{
				Node* node = new Node;
				int X1 = (i + 1) * nodeSizeWidth;
				int X2 = (i + 2) * nodeSizeWidth - nodeBorder;
				int Y1 = (k + 1) * nodeSizeHeight;
				int Y2 = (k + 2) * nodeSizeHeight - nodeBorder;
				node->SetDrawLocation(X1, Y1, X2, Y2);
				node->SetLocation(i, k);
				node->ID = i + k;
				nodeVector.insert(nodeVector.begin() + id, node);
				id++;
				map[i][k] = node;
			}
		}

		NodeDirection dirs[8] = { N, E, S, W, NE, NW, SE, SW };
		for (Node* node : nodeVector)
		{
			for (NodeDirection dir : dirs)
			{
				node->neighborNodes[dir] = GetNodeByDir(dir, node);
			}
		}

		srand(time(NULL));
		std::uniform_int_distribution<int>rand(0, totalSize-1);
		std::random_device device;
		int num1 = rand(device);
		int num2 = rand(device);
		while (num1 == num2)
		{
			num2 = rand(device);
		}



		startNode = nodeVector[num2];
		endNode = nodeVector[num1];
		num2 = rand(device);
		for (int i = 0; i < wallCount; ++i)
		{
			wallNode[i] = map[i][mapHeight/2];
			wallNode[i]->isWall = true;
		}
		//startNode = map[4][3];
		//endNode = map[6][13];
		NodeDirection dir;
		openList.push(startNode);

		Node* parentNode = startNode;

		while (!openList.empty())
		{
			Node* node = openList.top();
			openList.pop();
			if (node->parent != NULL)
			{
				if (node->parent != parentNode)
				{
					while (!openList.empty())
					{
						openList.pop();
					}

					while (!closeList.empty())
					{
						Node* popNode = closeList.top();
						popNode->isInCloseList = false;
						popNode->isInOpenList = true;
						openList.push(popNode);
						closeList.pop();
					}
					Node* n = node;
					while (n->parent)
					{
						closeList.push(n);
						n->isInCloseList = true;
						n->isInOpenList = false;
						n = n->parent;
					}
				}
			}
			node->isInOpenList = false;
			node->isInCloseList = true;
			closeList.push(node);
			if (node == endNode)
			{
				break;
			}
			else
			{
				int index = 0;
				parentNode = node;
				for (Node* neighborNode : node->neighborNodes)
				{
					index++;
					if (neighborNode == NULL)
					{
						continue;
					}

					if (neighborNode->isInCloseList || neighborNode->isWall)
					{
						continue;
					}
					//F = G(StartN->N(진짜)) + H(N->EndNode(추정))
					//Node부터 EndNode까지 추정
					neighborNode->parent = node;
					neighborNode->movedDirection = index;
					int H = GetHeuristics(neighborNode, endNode, node);

					int cost = 10;
					if (index > 3)
					{
						cost = 14;
					}
					neighborNode->costToGoal = H;
					Node* parent = node;
					int G = cost;
					G += parent->costFromStart;
					neighborNode->costFromStart = G;
					int F = G + H;
					neighborNode->totalCost = F;
					if (!neighborNode->isInOpenList)
					{
						neighborNode->isInOpenList = true;
						openList.push(neighborNode);
					}

				}
			}
		}
		return true;
	}

	virtual bool OnUserUpdate(float d)
	{

		Fill(0, 0, ScreenWidth(), ScreenHeight(), L' ');
		vector<Node*> ::iterator n = nodeVector.begin();
		while (n != nodeVector.end())
		{
			Node* node = *n;
			if (node == NULL)
			{
				continue;
			}
			short color = FG_WHITE;
			if (node == startNode)
			{
				color = FG_BLUE;
			}
			else if (node == endNode)
			{
				color = FG_RED;
			}
			if (color == FG_WHITE)
			{
				Fill(node->drawLocation[0], node->drawLocation[1], node->drawLocation[2], node->drawLocation[3], PIXEL_QUARTER, color);
			}
			++n;
		}

		for (int i = 0; i < wallCount; ++i)
		{
			Node* n = wallNode[i];
			if (n == NULL)
			{
				break;
			}
			Fill(n->drawLocation[0], n->drawLocation[1], n->drawLocation[2], n->drawLocation[3], PIXEL_SOLID, FG_CYAN);

		}

		priority_queue<Node*> queue = closeList;
		Node* node = queue.top();
		while (!queue.empty())
		{
			DrawLine(node->drawLocation[0], node->drawLocation[1], node->drawLocation[2], node->drawLocation[3], PIXEL_SOLID, FG_MAGENTA);
			queue.pop();
			if (!queue.empty())
			{
				node = queue.top();
			}
		}
		Fill(startNode->drawLocation[0], startNode->drawLocation[1], startNode->drawLocation[2], startNode->drawLocation[3], PIXEL_SOLID, FG_BLUE);
		Fill(endNode->drawLocation[0], endNode->drawLocation[1], endNode->drawLocation[2], endNode->drawLocation[3], PIXEL_SOLID, FG_RED);

		return true;
	}
};

int main()
{
	DrawAstar* draw = new DrawAstar;
	draw->ConstructConsole(160, 100, 8, 8);
	draw->Start();
}