Unity에서 A* 알고리즘 구현해보기

개념

A* 알고리즘은 출발 지점에서 목표 지점까지의 최단 경로를 찾는 그래프 탐색 알고리즘이다.
다익스트라 알고리즘을 현실 세계에 적용하기에는 실제 경로를 모두 노드화시켜야 하고, 출근길 정체 같은 다양한 변수를 반영하기 힘들어 다익스트라를 확장해 만들었다.

 

 

 

A* 알고리즘은
$g(x)$ : 현재 위치까지의 비용과
$h(x)$ : 현재 위치에서 도착 위치까지의 예상 비용에 대해
$f(x) = g(x) + h(x)$에 대해 $f(x)$가 최소가 되는 지점을 먼저 탐색한다.

다익스트라 알고리즘에 $h(x)$라는 가중치를 두어 탐색을 더 빠르고 효율적으로 하는 알고리즘이다.
$h(x) = 0$이면 A* 알고리즘은 다익스트라 알고리즘이 된다.
여기서 예상 비용 $h(x)$은 정확히 구할 수 없는 값이다. 우리는 여기서 Heuristic 함수를 사용한다.
즉, 우리가 생각하기에 현재 위치에서 도착 위치까지의 예상 비용으로 적당하고 합리적으로 보이는 함수를 사용한다.

알고리즘의 수도코드는 다음과 같다.

1. $f(x)$를 오름차순 우선순위 큐에 넣는다.
2. 우선순위 큐에서 노드를 Pop 한다.
3. 해당 노드에서 이동할 수 있는 노드를 찾는다.
4. 이동할 수 있는 노드의 $f(x)$를 찾아 우선순위 큐에 넣는다.
5. 목표 지점에 도달할 때까지 반복한다.

---

구현

A*

Grid에서 길을 찾는 A*를 구현해 보자.
우선 Node를 정의해보자. Node는 index, $g(x)$ G, $h(x)$ H, $f(x)$ F,와 Path 추적을 위한 parent를 가지고 있어야 한다.

private class AStar
{
    public class Node
    {
        public Vector2Int index;
        public float G;
        public float H;
        public float F;
        public Node parent;

        public Node(Vector2Int index, float G, float H, Node parent)
        {
            this.index = index;
            this.G = G;
            this.H = H;
            this.F = G + H;
            this.parent = parent;
        }
    }
}

수도코드에 따라 FindPath를 구현해보자.

private class AStar
{
    ...
    // Neighbor Index 순환을 위한 dx dy
    private static int[] dx = { -1, 0, 0, 1, -1, -1, 1, 1 };
    private static int[] dy = { 0, 1, -1, 0, 1, -1, 1, -1 };

    public static List<Vector3> FindPath(Vector3 start, Vector3 finalDest)
    {
        // 좌표의 index 찾기
        Vector2Int startIndex = GameMode.Instance.plane.GetIndex(start);
        Vector2Int finalIndex = GameMode.Instance.plane.GetIndex(finalDest);

        // 우선순위 큐
        List<Node> queue = new List<Node>();
        // 방문한 노드
        HashSet<Vector2Int> visited = new HashSet<Vector2Int>();

        // 시작 노드를 queue에 넣는다.
        queue.Add(new Node(startIndex, 0, Heuristic.GetH(startIndex, finalIndex), null));

        // queue가 비어질 때까지
        while (queue.Count > 0)
        {
            // 가장 작은 F를 가진 노드 찾기 (우선순위 큐를 쓰면 더 빠르다)
            Node current = queue[0];
            for (int i = 1; i < queue.Count; i++)
            {
                current = (queue[i].F < current.F) ? queue[i] : current;
            }

            // 찾은 값이 도착 지점이라면 
            // ReconstructionPath로 경로를 추적해 반환
            if (current.index == finalIndex)
            {
                // 방문 노드 출력
                string arrayAsString = string.Join(", ", visited);
                Debug.Log("Visited : " + arrayAsString);
                // 방문 횟수 출력
                GameMode.Instance.SetSearched(visited.Count);
                return ReconstructionPath(current, start, finalDest);
            }

            // 가장 작은 F를 가진 노드를 Pop
            queue.Remove(current);
            visited.Add(current.index);

            // Pop한 노드에서 이동할 수 있는 노드에 대해
            for (int i = 0; i < 8; i++)
            {
                Vector2Int index = new Vector2Int(current.index.x + dx[i], current.index.y + dy[i]);
                // 유효한 index이고, visited가 아니라면
                if (isValidIndex(index) && !visited.Contains(index))
                {
                    // 상하좌우
                    if (i < 4)
                    {
                        float tempG = current.G + 1; // 상하좌우 이동은 1
                        //Heuristic.GetH(index, finalIndex);
                        float tempH = Heuristic.GetH(index, finalIndex);
                        queue.Add(new Node(index, tempG, tempH, current));
                    }
                    // 대각선
                    else
                    {
                        float tempG = current.G + 1.4f; // 대각선 이동은 루트 2
                        float tempH =  Heuristic.GetH(index, finalIndex);
                        queue.Add(new Node(index, tempG, tempH, current));
                    }
                }
            }
        }
        // 못 찾았으면
        return null;
    }
}

Heuristic 함수 $h(x)$는 다양하게 구현할 수 있지만 가장 단순한 방법으로는

• 맨해튼 거리 : $|px - qx| + |py - qy|$
• 유클리드 거리 : $\sqrt{(px - qx)^2 + (py - qy)^2}$
  를 쓸 수 있다.

private class AStar
{
    ...
    ...
    public interface IHeuristic
    {
        float Calc(Vector2Int start, Vector2Int dest);
    }

    // 맨해튼 거리
    public class ManhattanHeuristic : IHeuristic
    {
        public float Calc(Vector2Int start, Vector2Int dest)
        {
            return Mathf.Abs(start.x - dest.x) + Mathf.Abs(start.y - dest.y);
        }
    }

    // 유클리드 거리
    public class EuclidHeuristic : IHeuristic
    {
        public float Calc(Vector2Int start, Vector2Int dest)
        {
            return Vector2.Distance(start, dest);
        }
    }

    // h(x) = 0
    public class DijkstraHeuristic : IHeuristic
    {
        public float Calc(Vector2Int start, Vector2Int dest)
        {
            return 0f;
        }
    }

    public class Heuristic
    {
        private static IHeuristic heuristic;
        public static float GetH(Vector2Int start, Vector2Int dest)
        {
            switch (GameMode.Instance.heuristicIndex)
            {
                case 0:
                    heuristic = new ManhattanHeuristic();
                    break;
                case 1:
                    heuristic = new EuclidHeuristic();
                    break;
                case 2:
                    heuristic = new DijkstraHeuristic();
                    break;
                default:
                    Debug.LogError("Unknown heuristic selected");
                    heuristic = new ManhattanHeuristic();
                    break;
            }
            return heuristic.Calc(start, dest);
        }
    }
}

isValidIndex는 Grid를 탐색할 때 좌표가 범위를 벗어나거나, Blocked 되어있는 좌표에 접근하는 것을 막는다.

private class AStar
{
    ...
    ...
    ...
    private static bool isValidIndex(Vector2Int index)
    {
        // Grid 크기 변수
        int gridLength = GameMode.Instance.plane.gridLength;
        if (index.x < 0 || index.y < 0 || gridLength <= index.x || gridLength <= index.y)
        {
            return false;
        }
        // Grid의 좌표가 막혀있으면
        if (GameMode.Instance.plane.Grid[index.y, index.x] == false)
        {
            return false;
        }
        return true;
    }
}

FindPath에서 도착 지점에 도달했으면, 경로를 찾아 반환해야 한다.
저장해둔 Parent를 타고 올라가 경로 값을 찾는다.

private class AStar
{
    ...
    ...
    ...
    ...
    public static List<Vector3> ReconstructionPath(Node node, Vector3 start, Vector3 dest)
    {
        List<Vector3> path = new List<Vector3>();
        while (node != null)
        {
            // GetCoord는 Index로 좌표값을 찾는 함수이다.
            path.Add(GameMode.Instance.plane.GetCoord(node.index));
            node = node.parent;
        }
        path.Reverse();

        // 시작 Index의 위치가 아닌 현재 위치
        path[0] = start;
        // 끝 Index가 아닌 도착 위치
        path[path.Count - 1] = dest;
        return path;
    }
}

보완할 것

1. 우선순위 큐를 List로 구현하는 것은 비효율적으로 보인다.
2. visited도 배열을 사용한 더 나은 방법이 있을 수 있다.
3. Reconstruct가 Stack을 사용한다면 Reverse 하지 않고 뒤에 넣은 값을 먼저 뺄 것이다.

나머지

구현한 A* 알고리즘을 PlayerController에 추가해 보자.

using System.Collections.Generic;
using UnityEngine;

public class PlayerController : MonoBehaviour
{
    [SerializeField] public float moveSpeed = 1f;

    private bool isMoving = false;
    // 경로
    private List<Vector3> path;
    // 현재 경로의 목표
    private Vector3 dest;

    private class AStar
    {
        ...
    }

    private void Update()
    {
        Vector3 mousePos = Input.mousePosition;
        mousePos.z = 100f;
        mousePos = Camera.main.ScreenToWorldPoint(mousePos);

        // 마우스 클릭을 받았을 때
        if (Input.GetMouseButtonDown(0))
        {
            Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
            RaycastHit hit;
            if (Physics.Raycast(ray, out hit))
            {
                if (hit.collider.gameObject.CompareTag("Plane"))
                {
                    Vector2Int hitIndex = GameMode.Instance.plane.GetIndex(hit.point);
                    // 유효한 좌표를 클릭했을 때 
                    if (GameMode.Instance.plane.Grid[hitIndex.y, hitIndex.x])
                    {
                        // 현재 위치부터 마우스로 클릭한 지점까지의 경로
                        path = AStar.FindPath(gameObject.transform.position, hit.point);

                        // 경로를 찾았으면
                        if (path != null)
                        {
                            isMoving = true;
                            // 첫 경로 Pop
                            dest = path[0];
                            path.RemoveAt(0);
                        }
                    }
                }
            }
        }

        if (isMoving && path != null)
        {
            // Rotation
            Vector3 direction = (dest - transform.position).normalized;
            if (direction != Vector3.zero)
            {
                Quaternion toRotation = Quaternion.LookRotation(direction, Vector3.up);
                transform.rotation = toRotation;
            }

            // Path에서 뽑은 위치로 이동
            transform.position = Vector3.MoveTowards(transform.position, dest, moveSpeed * Time.deltaTime);

            // Path 지점에 도착했으면
            if (Vector3.Distance(transform.position, dest) < 0.01f)
            {
                //모든 Path를 이동했으면 이동 종료
                if (path.Count == 0)
                {
                    isMoving = false;
                }
                else
                {
                    // 새 지점 Pop
                    dest = path[0];
                    path.RemoveAt(0);
                }
            }
        }
    }
}

나머지 내용은 Github에서 볼 수 있다.

결과

AStarAlgorithm

링크에서 직접 플레이해볼수도 있다.

그리드의 크기가 커지면 다익스트라 > 유클리드 >= 맨해튼 순으로 점점 시간이 오래 걸린다.
C# 문법은 더 공부해봐야할 것 같다.