[알고리즘] A* - 노드 기반

시작 지점과 종료 지점을 설정하여 최단 거리를 구한다

 

과정은 다익스트라와 비슷하지만 

거리 계산시 H(노드에서 종료 노드까지의 거리)를 추가로 계산에 사용하고, 경로를 추적할 노드 정보가 추가된다.

 

 

1. 각 노드정보를 초기화

- 각 노드에 다른 노드까지의 거리를 설정하고 시작지점에서 최소 거리는 처음 무한대로 설정

- 각 노드에서 종료 노드까지의 거리를 설정

 

2. 시작 노드 설정

- 시작 노드의 최소 거리는 0으로 설정

- 시작 노드를 열린 노드 셋에 등록

 

3. 대상 노드를 찾는다

- 현재 등록된 열린 노드셋에서 F(G + H)가 가장 작은 노드를 찾는다

- 만약 대상이 없거나 대상 노드가 종료 노드인경우 종료

 

4. 대상 노드는 닫힌 노드 셋에 등록하고 열린 노드셋에서 제거  

 

5. 대상 노드와 연결된 노드 중 다음 조건에 맞는 연결 노드를 찾는다

- 연결되어있는 노드(거리가 설정된 노드)

- 아직 처리되지 않은 노드(닫힌 노드 셋에 등록되지 않은 노드)

 

6. 대상 노드에 연결된 각 노드의 최소 거리를 조건에 따라 갱신

- 대상 노드의 G와 대상 노드와 연결된 노드까지의 거리를 더해 대상 노드를 거쳐 연결노드로 갈 때의 거리값(G)을 구한다

- 구한 G값을 연결노드의 기존 G와 비교하여 더 작다면 연결 노드의 G와 F를 갱신하고 부모 노드도 대상 노드로 변경

- 연결노드들은 열린 노드 셋에 등록

 

3 ~ 6번을 더이상 대상 노드가 없을 때까지 루프

 

#include <stdio.h>
#include <vector>
#include <set>
#include <list>


// 연결되지 않은 노드의 거리 값
#define INFINITY_DISTANCE (9999)

// 노드 리스트 카운트
#define NODE_LIST_COUNT (6)


//----------------------------------------------------------
// 노드 정보
//----------------------------------------------------------
struct Node
{
public:
	// 노드 인덱스
	int Index = -1;

	//-----------------------------------------------------------
	// 거리 정보
	//-----------------------------------------------------------
	// 시작 노드에서 해당 노드까지의 최소 거리
	int Distance_G = INFINITY_DISTANCE;

	// 해당 노드에서 끝노드까지의 거리
	int Distance_H = 0;
	
	// G + H
	int Distance_F = 0;

	// 해당 노드에서 연결된 노드의 거리정보를 설정
	// 연결되어있지 않는경우 INFINITY_DISTANCE
	std::vector<int> LinkNodesDistance;

	// 부모 노드
	Node* ParentNode = nullptr;

public:
	// 거리 정보가 갱신 되면 Distance_F를 갱신
	void UpdateDistanceF()
	{
		Distance_F = Distance_G + Distance_H;
	}
};


// 노드 정보 리스트
std::vector<Node> NodeList;

// 열린 노드 인덱스 리스트
std::set<int> OpenNodeIndexSet;

// 닫힌 노드 인덱스 리스트
std::set<int> CloseNodeIndexSet;



//---------------------------------------------------------------
// 기능
//---------------------------------------------------------------
// 노드 정보 리스트 초기화
// 여기서 도착노드는 5번 노드로 정하고 각 노드에서 5번노드까지의 거리를 설정
void InitNodeList()
{
	// 노드 정보 설정
	NodeList.resize(NODE_LIST_COUNT);

	// 대상노드
	Node* targetNode = nullptr;

	// 0번 노드 설정
	targetNode = &(NodeList[0]);
	targetNode->Index = 0;
	targetNode->Distance_H = 35;
	targetNode->LinkNodesDistance.resize(NODE_LIST_COUNT);
	targetNode->LinkNodesDistance[0] = 0;
	targetNode->LinkNodesDistance[1] = 10;
	targetNode->LinkNodesDistance[2] = 30;
	targetNode->LinkNodesDistance[3] = 20;
	targetNode->LinkNodesDistance[4] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[5] = INFINITY_DISTANCE;

	// 1번 노드 설정
	targetNode = &(NodeList[1]);
	targetNode->Index = 1;
	targetNode->Distance_H = 23;
	targetNode->LinkNodesDistance.resize(NODE_LIST_COUNT);
	targetNode->LinkNodesDistance[0] = 10;
	targetNode->LinkNodesDistance[1] = 0;
	targetNode->LinkNodesDistance[2] = 15;
	targetNode->LinkNodesDistance[3] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[4] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[5] = INFINITY_DISTANCE;

	// 2번 노드 설정
	targetNode = &(NodeList[2]);
	targetNode->Index = 2;
	targetNode->Distance_H = 10;
	targetNode->LinkNodesDistance.resize(NODE_LIST_COUNT);
	targetNode->LinkNodesDistance[0] = 30;
	targetNode->LinkNodesDistance[1] = 15;
	targetNode->LinkNodesDistance[2] = 0;
	targetNode->LinkNodesDistance[3] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[4] = 10;
	targetNode->LinkNodesDistance[5] = INFINITY_DISTANCE;

	// 3번 노드 설정
	targetNode = &(NodeList[3]);
	targetNode->Index = 3;
	targetNode->Distance_H = 19;
	targetNode->LinkNodesDistance.resize(NODE_LIST_COUNT);
	targetNode->LinkNodesDistance[0] = 20;
	targetNode->LinkNodesDistance[1] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[2] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[3] = 0;
	targetNode->LinkNodesDistance[4] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[5] = 15;

	// 4번 노드 설정
	targetNode = &(NodeList[4]);
	targetNode->Index = 4;
	targetNode->Distance_H = 10;
	targetNode->LinkNodesDistance.resize(NODE_LIST_COUNT);
	targetNode->LinkNodesDistance[0] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[1] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[2] = 10;
	targetNode->LinkNodesDistance[3] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[4] = 0;
	targetNode->LinkNodesDistance[5] = 10;

	// 5번 노드 설정
	targetNode = &(NodeList[5]);
	targetNode->Index = 5;
	targetNode->Distance_H = 0;
	targetNode->LinkNodesDistance.resize(NODE_LIST_COUNT);
	targetNode->LinkNodesDistance[0] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[1] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[2] = INFINITY_DISTANCE;
	targetNode->LinkNodesDistance[3] = 15;
	targetNode->LinkNodesDistance[4] = 10;
	targetNode->LinkNodesDistance[5] = 0;
}


// 다음 노드를 찾는다
Node* FindNextCheckNode()
{
	int minValue = INFINITY_DISTANCE;
	Node* minNode = nullptr;

	// 열린 노드중에 최소값이 가장 작은노드를 찾는다
	for (auto iter : OpenNodeIndexSet)
	{
		const int nodeIndex = iter;
		Node* node = &(NodeList[nodeIndex]);

		// 해당노드의 최소값이 더작다면 갱신
		if (node->Distance_F < minValue)
		{
			minNode = node;
			minValue = node->Distance_F;
		}
	}

	return minNode;
}


// 연결노드의 최소값 갱신
void UpdateLinkNode(Node* inNode)
{
	// 넘어온 노드의 링크 노드 정보 루프
	for (int nodeIndex = 0; nodeIndex < NODE_LIST_COUNT; ++nodeIndex)
	{
		// 대상 노드와 연결되어있지 않다면 처리하지 않는다
		const int linkNodeDistance = inNode->LinkNodesDistance[nodeIndex];
		if (INFINITY_DISTANCE == linkNodeDistance) {
			continue;
		}

		// 이미 처리된 노드인경우 처리하지 않는다
		if (CloseNodeIndexSet.end() != CloseNodeIndexSet.find(nodeIndex)) {
			continue;
		}

		// 현재노드에서 링크노드의 거리를 얻는다
		const int checkDistance = inNode->Distance_G + linkNodeDistance;

		// 링크 노드
		Node* linkNode = &(NodeList[nodeIndex]);

		// 체크 거리가 링크노드에 설정된 거리보다 작다면 해당 거리로 재설정
		if (checkDistance < linkNode->Distance_G) 
		{
			linkNode->Distance_G = checkDistance;
			linkNode->UpdateDistanceF();
			linkNode->ParentNode = inNode;
		}

		// 열린 노드에 추가
		OpenNodeIndexSet.insert(linkNode->Index);
	}
}




int main(void)
{
	// 노드 정보를 초기화
	InitNodeList();

	// 사용할 시작 노드 인덱스
	const int startNodeIndex = 0;

	// 이건 고정(각노드에서 종료노드까지의 거리가 미리 정해저야 해서)
	const int endNodeIndex = 5;

	// 시작노드는 시작지점에서 해당 노드까지의 최소 비용을 0으로 설정
	{
		Node* startNode = &NodeList[startNodeIndex];
		startNode->Distance_G = 0;
		startNode->UpdateDistanceF();
	}

	// 열린 노드에 등록
	OpenNodeIndexSet.insert(startNodeIndex);

	// 처리
	while (true)
	{
		// 다음 체크할 노드가 없거나 끝 노드와 동일하다면 종료
		Node* checkNode = FindNextCheckNode();
		if (nullptr == checkNode || checkNode == &NodeList[endNodeIndex]) {
			break;
		}

		// 체크한 노드는 닫힌 노드셋에 추가
		CloseNodeIndexSet.insert(checkNode->Index);

		// 열린 노드 정보에서는 제거
		OpenNodeIndexSet.erase(checkNode->Index);

		// 해당 노드와 연결된 노드의 최소값을 갱신
		UpdateLinkNode(checkNode);
	}

	// 결과 노드에서 부터 연결된 노드를 찾는다
	std::list<Node*> pathList;
	{
		Node* targetNode = &NodeList[endNodeIndex];
		while (nullptr != targetNode)
		{
			pathList.push_front(targetNode);
			targetNode = targetNode->ParentNode;
		}
	}
	
	for (auto node : pathList)
	{
		printf("-> node(%d) ", node->Index);
	}

	return 0;
}