[알고리즘] 길찾기 알고리즘(A* Algorithm) 구현(C++/UE4)

2020-08-28 첫 작성

2021-02-03 1차 수정

2021-04-08 2차 수정

 

언리얼 엔진을 이용하여 구현해보았고, 위젯 버튼만 블루프린트 사용. 우선순위 큐 기반(TArray 사용)으로 작성.

 

마우스 왼쪽 버튼으로 시작 지점을 선택 후 도착 블록을 선택하게 되면 로직이 동작하고,

 

쉬프트+마우스 왼쪽 버튼으로 검은색 벽을 설정. 벽을 피해 경로가 설정된다.

 

경로 추출 시 타이머를 이용해 0.1초 간격으로 찾은 경로의 블록 매터리얼의 색상을 변경.

 

https://www.youtube.com/watch?v=0J2aWNcIU9o&feature=youtu.be

 

작동원리:

- 각각의 블록들에는 3개의 값이 들어 있다.

 G : 시작블록부터 해당 블록까지의 이동한 거리값
 H : 해당 블록에서부터 목표 블록까지의 남은 이동값
 F : G+H 값

- 각각의 블록들은 해당 블록을 선택한 부모 블록의 정보도 있다.

 

-  두 개의 리스트 정보가 존재하는데, 각각 열린 리스트/닫힌리스트라고 불리는 목록정보가 있으며,

    시작 블록에서부터 목표 블록까지의 도착하기까지, 현재 블록의 다음 블록이 될 수 있는 후보들을 열린 리스트에 넣고, 열린 리스트에 있는 목록들 중 F값과,  H값이 더 작은 값 블록들을 열린 리스트에서 제거 후 닫힌리스트에 넣는다.

닫힌리스트에 넣으면서 해당 블록의 부모 노드(이전 노드)도 지정해준다. 나중에 왔던 길을 다시 탐색하기 위함.

 

- 그리고 도착 노드에 도착하게 되면 저장된 닫힌리스트에서 도착 노드의 부모 노드를 역으로 추적해간다. 그렇게 시작 노드까지 역추적이 끝나면 그 길이 최종 찾게 된 최단거리 길이 된다.

 

 

코드 살펴보기.

1. 시작 블록 선택 시 

void AA_Star_AlgorithmBlockGrid::SelectStartBlock(class AA_Star_AlgorithmBlock * p_StartingBlock)
{
	//Check StartBlock
	if (curStartBlock != nullptr)
	{
		return;
	}

	curStartBlock = p_StartingBlock;

}//End of SelectStartBlock

2. 도착 블록 선택 시

void AA_Star_AlgorithmBlockGrid::SelectTargetBlock(class AA_Star_AlgorithmBlock * targetingBlock)
{
	if (curTargetBlock != nullptr)
	{
		return;
	}

	curTargetBlock = targetingBlock;

	if (curStartBlock == nullptr || curTargetBlock == nullptr)
	{
		return;
	}
    
    //경로 탐색 후 결과Path 반환 받음
	mCharacterPath = GetPath_While(curStartBlock->GetBlockNumber(), curTargetBlock->GetBlockNumber());

	//Not Found Path
	if (mCharacterPath.Num() <= 0)
	{
		UE_LOG(LogTemp, Warning, TEXT("Not Found Path"));
		return;
	}

    //mCharacterPath로 받은 경로를 색변경을 위해 타이머로 추적함
	mCurPathCountNum = 1; 
	GetWorldTimerManager().SetTimer(mCountdownTimerHandle, this, &AA_Star_AlgorithmBlockGrid::MovingTimer, mMovingTime, true);

}//end of SelectTargetBlock

현재 도착 블록이 없다면 해당 변수로 설정되고, GetPath_While(...); 경로 탐색 함수 실행 후 결과 mCharacterPath 경로목록 반환 받음. 만약 size가 0 이하면 탐색 실패, 성공이라면 타이머로 경로 추적 

 

 

3. 경로 찾기 함수(GetPath_While(...))

TArray<FVector2D>& AA_Star_AlgorithmBlockGrid::GetPath_While(const FVector2D& startPosition, const FVector2D& targetPosition)
{
	this->SetRelease();
	//StartPosition Push
	Node_Info* startNode = &mNodeArr[static_cast<int>(startPosition.X)][static_cast<int>(startPosition.Y)];
	
	mOpenList.Push(startNode);
	while (mOpenList.Num() > 0)
	{
		mCurrentNode = mOpenList[0];
		int curNodeSize = mOpenList.Num();

		for (int i = 0; i < curNodeSize; i++)
		{
			const int curNodeF = mOpenList[i]->GetCostF(); //F == TotalValue
			const int curNodeH = mOpenList[i]->GetCostH(); //H == RemainValue
			//Find More Little Value
			if ((curNodeF <= mCurrentNode->GetCostF()) && (curNodeF < mCurrentNode->GetCostH()))
			{
				mCurrentNode = mOpenList[i];
				mCurrentNode->SetCurBlock(mOpenList[i]->GetCurBlock());
			}
		}

		mOpenList.Remove(mCurrentNode); //Delete OpenList
		mClosedList.Push(mCurrentNode); //Add ClosedList

		const int curX = mCurrentNode->GetPoistion().X;
		const int curY = mCurrentNode->GetPoistion().Y;

		if (GetArriveTarget(mCurrentNode->GetPoistion(), targetPosition))
		{
			Node_Info* targetNode = &mNodeArr[curX][curY];

			while (targetNode->GetPoistion() != startPosition)
			{
				mFinalPathList.Push(targetNode->GetPoistion());
				targetNode = targetNode->GetParent();
			}

			mFinalPathList.Push(startPosition);

			ReverseArray();
			return mFinalPathList;
		}

		//Diagonal
		if (bAllowDiagonal == true)
		{
			for (int i = 0; i < 4; i++) 
			{
				const int nextX = curX + mDirDiagnolInfo[i].X;
				const int nextY = curY + mDirDiagnolInfo[i].Y;
				OpenListAdd(nextX, nextY);
			}
		}
		//Up Down Left Right
		for (int i = 0; i < 4; i++)
		{
			const int nextX = curX + mDirInfo[i].X;
			const int nextY = curY + mDirInfo[i].Y;
			OpenListAdd(nextX, nextY);
		}
	}//end of while

	return mFinalPathList;
}//end of GetPath_While

 

열린 리스트 / 닫힌 리스트 / 최종 경로 리스트 초기화 함수(SetRelasase()) 실행 후 시작

 

그리고 오픈리스트가 1개 이상이라면 계속해서 while문 반복한다.

 

시작 지점으로 클릭된 블록의 정보를 열린 리스트에 넣었으므로 시작된다.

 

		//..
		int curNodeSize = mOpenList.Num();
		for (int i = 0; i < curNodeSize; i++)
		{
			const int curNodeF = mOpenList[i]->GetCostF(); //F == TotalValue
			const int curNodeH = mOpenList[i]->GetCostH(); //H == RemainValue
			//Find More Little Value
			if ((curNodeF <= mCurrentNode->GetCostF()) && (curNodeF < mCurrentNode->GetCostH()))
			{
				mCurrentNode = mOpenList[i];
				mCurrentNode->SetCurBlock(mOpenList[i]->GetCurBlock());
			}
		}
              //..

 

현재 오픈리스트에 들어와 있는 크기만큼 for반복문이 돈다.

 

그리고 오픈리스트끼리 F값과 H값을 비교하는데

 

F값은 G+H 값이다.

 

1. G값은 시작블록에서부터 현재 블록까지의 소요비용이며, 

2. H값은 휴리스틱(heuristics) 추정 값으로 현재블록이 목표 블록까지 도달하는데 소요될 것 같은 추정비용이다.

그래서 현재 노드는 현재~목표 블록의 휴리스틱 값(H)과 시작~현재 블록의 값(G)이 가장 작은 값의 노드는

열린리스트에서 제거되고 닫힌리스트에 추가된다.

		//...
        mOpenList.Remove(mCurrentNode);
        mClosedList.Push(mCurrentNode);
        //...

 

 

그다음 선택된 현재 노드가 도착지점인지 확인한다.

3-1. 경로 찾기 함수(GetPath_While(...)) 중 도착지점 확인..

        //...
		const int curX = mCurrentNode->GetPoistion().X;
		const int curY = mCurrentNode->GetPoistion().Y;

		if (GetArriveTarget(mCurrentNode->GetPoistion(), targetPosition))
		{
			Node_Info* targetNode = &mNodeArr[curX][curY];

			while (targetNode->GetPoistion() != startPosition)
			{
				mFinalPathList.Push(targetNode->GetPoistion());
				targetNode = targetNode->GetParent();
			}

			mFinalPathList.Push(startPosition);

			ReverseArray();
			return mFinalPathList;
		}
        //...

 

현재 노드의 GetPosition과 p_TargetPosition이 같다면 도착이다. 아니라면 if 탈출

도착이라면 마지막 현재 노드의 X, Y 좌표를 가져와서 시작점이랑 같아질 때까지, 현재 노드의 부모 노드를 가져오며, mFinalPathList 리스트에 저장한다. (밑에서 확인하게 되는데, 왔던 길은 부모의 노드로 저장된다.). 

시작 좌표랑 같아지게 되면 while을 탈출하고 시작 지점까지 넣어준 뒤(좌표 확인 후 탈출해서) 역으로 뒤집어 준다. (ReverseArray 함수) 그리고 return 해주며 GetPath_While함수는 끝이다.

 

하지만! 도착지점이 아닐 경우를 봐야 한다!

3-2. 경로 찾기 함수(GetPath_While(...)) 중  열린 리스트에 추가 과정..

		//...
		//Diagonal
		if (bAllowDiagonal)
		{
			for (int i = 0; i < 4; i++) {
				const int x = curX + mDirDiagnolInfo[i].X;
				const int y = curY + mDirDiagnolInfo[i].Y;
				OpenListAdd(x, y);
			}
		}


		//Up Down Left Right
		for (int i = 0; i < 4; i++)
		{
			const int x = curX + mDirInfo[i].X;
			const int y = curY + mDirInfo[i].Y;
			OpenListAdd(x, y);
		}
        
        //...

 

우선 bAllowDiagonal 변수에 따라 대각선으로 갈 수 있는지 없는지를 변경해주는데,

해당 mDirDiagnolInfo 변수에 대각선 방향의 정보를 넣어주고 더해주었다.

	//...
	mDirInfo[0] = FVector2D(1, 0);	//up
	mDirInfo[1] = FVector2D(0, 1); //right
	mDirInfo[2] = FVector2D(-1, 0); //down 
	mDirInfo[3] = FVector2D(0, -1); //left

	mDirDiagnolInfo[0] = FVector2D(1, 1);
	mDirDiagnolInfo[1] = FVector2D(-1, 1); 
	mDirDiagnolInfo[2] = FVector2D(-1, -1); 
	mDirDiagnolInfo[3] = FVector2D(1, -1);
    //..

(생성자 부분)

 

그리고 OpenListAdd(x, y) 함수로 넣어주는데

void AA_Star_AlgorithmBlockGrid::OpenListAdd(const int& currentX, const int& currentY)
{
	//Check Out Of Range
	if (!CheckException(FVector2D(currentX, currentY)))
	{
		return;
	}

	if (bAllowDiagonal == true)
	{
		if (mNodeArr[mCurrentNode->GetX()][currentY].GetIsWall() && mNodeArr[currentX][mCurrentNode->GetY()].GetIsWall())
		{
			return;
		}
	}

	if (bDontCrossCorner == true)
	{
		if (mNodeArr[mCurrentNode->GetX()][currentY].GetIsWall() || mNodeArr[currentX][mCurrentNode->GetY()].GetIsWall())
		{
			return;
		}
	}

	Node_Info* neighborNode = &mNodeArr[currentX][currentY];

	//10 or 14
	int moveAddtive = (mCurrentNode->GetX() - currentX) == 0 || (mCurrentNode->GetY() - currentY) == 0 ? 10 : 14;

	//Calc CostG
	int curNodeG = mCurrentNode->GetCostG() + moveAddtive;

	if (curNodeG < neighborNode->GetCostG() || !mOpenList.Contains(neighborNode))
	{
		neighborNode->SetCostG(curNodeG);
		neighborNode->SetCostH(curTargetBlock->GetBlockNumber());
		neighborNode->SetParentNode(mCurrentNode);
		mOpenList.Push(neighborNode);
	}
}//end of OpenListAdd

위에서부터 확인해보면 입력받은 위치 좌표 노드가 문제없는지 확인하고, 문제가 된다면 return으로 탈출한다.

	if (!CheckException(FVector2D(p_CheckX, p_CheckY)))
	{
		return;
	}

	if (bAllowDiagonal)
	{
		if (mNodeArr[mCurrentNode->GetX()][p_CheckY].GetIsWall() && mNodeArr[p_CheckX][mCurrentNode->GetY()].GetIsWall())
		{
			return;
		}
	}

	if (bDontCrossCorner)
	{
		if (mNodeArr[mCurrentNode->GetX()][p_CheckY].GetIsWall() || mNodeArr[p_CheckX][mCurrentNode->GetY()].GetIsWall())
		{
			return;
		}
	}

CheckException() 함수로 파라미터로 받은 블록의 위치 값이 범위를 벗어나지 않는지 확인한다. (0 미만 or 최대 크기 초과 등)

이상이 없고, 대각선 허용 시 벽 사이로 통과가 안됨. 코너를 가로질러 갈 시에 수직 수평에 벽이 있으면 안 된다.

 

무사히 내려온다면

	//10 or 14
	int moveAddtive = (mCurrentNode->GetX() - currentX) == 0 || (mCurrentNode->GetY() - currentY) == 0 ? 10 : 14;

	//Calc CostG
	int newCostG = mCurrentNode->GetCostG() + moveAddtive;

	Node_Info* neighborNode = &mNodeArr[currentX][currentY];

	if (newCostG < neighborNode->GetCostG() || !mOpenList.Contains(neighborNode))
	{
		neighborNode->SetCostG(newCostG);
		neighborNode->SetCostH(curTargetBlock->GetBlockNumber());
		neighborNode->SetParentNode(mCurrentNode);
		mOpenList.Push(neighborNode);
	}

 

범위도 벗어나지 않고, 벽으로 설정된 노드도 아니라면 우선 이웃 노드가 현재 노드의 G값이 10인지 14인지 확인하고(대각선 여부에 따라 다름).

 

이웃 노드의 G값 지정을 위해 시작 노드부터 현재 노드까지의 G값에 추가로  G값(moveAddtive)을 더해준다(newCostG).

 

그리고 그렇게 계산한 이웃 노드의 이동비용이 현재 이웃 노드의 G값보다 작거나 혹은 오픈리스트에 들어가 있는 노드가 아니라면 이웃노드의 G값과 H값을 설정해준다. 그리고 이웃노드의 부모 노드(이전 노드)는 현재 노드가 된다다.

 

이렇게 현재 노드의 4방향 혹은 8방향에 따라 각각의 노드들의 G값을 계산하고 G값이 작거나, 열린 리스트에 없는 노드라면 열린리스트에 추가가 된다. 

 

그리고 다시 GetPath_While함수로 돌아가서 while 상단의 열린 리스트의 가장 앞에 있는 노드를 꺼내고 이후에 열린 리스트에 있는 노드들이랑 F값고 H값을 비교하여 가장 작은 값을 가진 노드를 현재 노드로 지정하고, 열린 리스트에서 제거 후 닫힌 리스트에 넣고 반복한다....

		//..
		mCurrentNode = mOpenList[0];
		int curNodeSize = mOpenList.Num();
		for (int i = 0; i < curNodeSize; i++)
		{
			const int curNodeF = mOpenList[i]->GetCostF(); //F == TotalValue
			const int curNodeH = mOpenList[i]->GetCostH(); //H == RemainValue
			//Find More Little Value
			if ((curNodeF <= mCurrentNode->GetCostF()) && (curNodeF < mCurrentNode->GetCostH()))
			{
				mCurrentNode = mOpenList[i];
				mCurrentNode->SetCurBlock(mOpenList[i]->GetCurBlock());
			}
		}
              //..

 

그렇게 반복하다 보면, 목표 블록에 도착하여 GetPath_While 함수가 끝나거나, 혹은 열린 리스트가 비어지게 된다면(더 이상 갈 노드가 없을 때, 경로를 찾지 못함) GetPath_While함수가 끝나게 된다....

 

정리해보자면 

 

1. 시작 노드 선택
2. 도착 노드 선택
3. 열린 리스트에 시작 노드 추가
4. 열린 리스트 중 가장 비용이 적게 드는(F 값, H값) 노드를 열린 리스트 제거 후, 닫힌 리스트에 저장
5. 현재 노드가 도착인지 확인
5-1. 도착이라면 현재 노드의 부모 노드들을 역추적하며 경로 완성
6. 도착이 아니라면 현재 노드 주변(4방향 혹은 8방향)노드 들의 각각의 노드들을 G 값 계산 후,

   기존 G값보다 새로운 G값이 더 작거나, 열린 리스트에 없는 노드라면, G값,H값, 부모노드 갱신 후 열린 리스트에 추가 후,  다시 4번으로 간 뒤 반복

 

* 열린 리스트란? 목적지까지의 길이 되는 후보들을 담고 있다가 비교해보며 닫힌리스트로 옮겨줌. 

* 닫힌 리스트란? 최종적으로 나오게 되는 길의 순서(노드)

 G : 시작블록부터 현재 블록까지의 이동한 거리값
 H : 현재 블록에서부터 목표 블록까지의 남은 이동값
 F : G+H 값

 

 

 

정리 잘 된 글:

www.gisdeveloper.co.kr/?p=3897

최단 경로 탐색 – A* 알고리즘 – GIS Developer