A* 알고리즘을 이용한 길찾기2 : Heap을 이용한 최적화

저번 포스팅에서 사용한 A* 알고리즘은 Cost_F가 가장 낮은 노드를 찾기 위해 매번 모든 열린 목록을 탐색해야 한다는 단점이 있었다. 이번 포스팅에서는 Heap(각 노드가 자식 노드를 최대 두 개씩 가지는 일종의 이진 트리 자료구조. 최대값과 최소값을 빠르게 찾아낼 수 있다)을 이용해 A* 알고리즘을 효율적으로 수정해 보았다. 코드는 다음과 같다.

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System;

public class Heap<T> where T : IHeapItem<T>
{
    //노드 배열을 지정하는 대신 T유형의 배열을 지정
    T[] Items;
    int CurrentItemCount;
    
    //힙은 최대 크기를 정해준다.
    public Heap(int maxHeapsize)
    {
        //gridSizeX, gridSizeY 를 곱하면 최대값을 알 수 있다.
        Items = new T[maxHeapsize];
    }
    //새 항목을 힙에 추가하기 위한 함수
    public void Add(T item)
    {
        item.HeapIndex = CurrentItemCount;//추가될 항목의 인덱스는 현재까지 등록된 항목의 숫자와 같다
        Items[CurrentItemCount] = item;
        SortUp(item);
        CurrentItemCount++;
    }

    //가장 비용이 낮은 항목부터 제거 == 루트 항목 제거
    public T RemoveFirst()
    {

        T FirstItem = Items[0];
        CurrentItemCount--;
        Items[0] = Items[CurrentItemCount];
        Items[0].HeapIndex = 0;

        SortDown(Items[0]);
        return FirstItem;
    }
    public void UpdateItem(T item)
    {
        SortUp(item);
    }
    public int Count
    {
        get { return CurrentItemCount; }
    }
    public bool Contains(T item)
    {
        return Equals(Items[item.HeapIndex], item);
    }
    void SortDown(T item)
    {
        while (true)
        {
            //자식 인덱스 구하기
            int childrenIndexLeft = item.HeapIndex * 2 + 1;
            int childrenIndexRight = item.HeapIndex * 2 + 2;
            int SwapIndex = 0;

            //childrenIndexLeft의 자식이 존재하는지 확인
            if (childrenIndexLeft < CurrentItemCount)
            {
                SwapIndex = childrenIndexLeft;
               //childrenIndexRight의 자식이 존재하는지 확인
                if (childrenIndexRight < CurrentItemCount)
                {
                    if(Items[childrenIndexLeft].CompareTo(Items[childrenIndexRight]) < 0)
                    {
                        SwapIndex = childrenIndexRight;
                    }
                }
                //현재 노드와 자식 노드를 비교해 자식 노드의 항목이 더 작으면 자식과 위치 변경
                if (item.CompareTo(Items[SwapIndex]) < 0)
                {
                    Swap(item, Items[SwapIndex]);
                }
                else
                {
                    return;
                }
            }
            //자식이 없는 경우
            else 
            {
                return;
            }
        }
    }
    void SortUp(T item)
    {
        int ParentIndex = (item.HeapIndex - 1) / 2;

        while (true)
        {
            T ParentItem = Items[ParentIndex];

            //Add 함수에서 추가된 항목이 부모 항목보다 크다면 추가된 항목과 부모 항목의 자리를 바꾼다 == 추가된 항목이 부모가 됨.
            if (item.CompareTo(ParentItem) > 0)
            {
                Swap(item, ParentItem);
            }
            else 
            {
                break;
            }
            ParentIndex = (item.HeapIndex - 1) / 2;  //스왑한 뒤 다시 그 위의 부모와도 비교하기 위해 부모의 인덱스를 새로 구함
        }
    }
    void Swap(T itemA, T itemB)
    {
        Items[itemA.HeapIndex] = itemB;
        Items[itemB.HeapIndex] = itemA;

        int itemAindex = itemA.HeapIndex;
        itemA.HeapIndex = itemB.HeapIndex;
        itemB.HeapIndex = itemAindex;
    }
}
public interface IHeapItem<T> :IComparable<T>
{
    int HeapIndex
    {
        get;
        set;
    }
}

  힙을 만들어준 뒤 기존의 Node 스크립트에 IHeapItem<Node>를 상속해준다. Node 스크립트는 다음과 같다.

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Node : IHeapItem<Node>
{
    public bool bWalkable;
    public Vector3 WorldPositon;

    public Node Parent;
    public int Cost_G;
    public int Cost_H;

    public int Grid_x;
    public int Grid_y;
    int heapIndex;
    
    public int Cost_F
    {
        get {
            return Cost_G + Cost_H;
        }
    }

    public Node(bool _Walkable, Vector3 _WorldPos, int _gridx, int _gridy)
    {
        bWalkable = _Walkable;
        WorldPositon = _WorldPos;

        Grid_x = _gridx;
        Grid_y = _gridy;
    }
    public int HeapIndex
    {
        get {
            return heapIndex;
        }
        set
        {
            heapIndex = value;
        }
    }
    public int CompareTo(Node nodeToCompare)
    {
        int compare = Cost_F.CompareTo(nodeToCompare.Cost_F);
        if (compare == 0)
        {
            compare = Cost_H.CompareTo(nodeToCompare.Cost_H);
        }
        return -compare;
    }
}

  마지막으로, PathFind도 수정해주면 끝.

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Diagnostics;

public class PathFind : MonoBehaviour
{
    Grid Grid;
    public Transform Seeker, Target;

    private void Awake()
    {
        Grid = GetComponent<Grid>();
    }
    private void Update()
    {
        if (Input.GetKeyDown(KeyCode.Space))
        { 
            FindPath(Seeker.position, Target.position); 
        }
    }
    void FindPath(Vector3 startPos, Vector3 TargetPos)
    {
        Node StartNode = Grid.NodeWorldPoint(startPos);
        Node TargetNode = Grid.NodeWorldPoint(TargetPos);
        
        Stopwatch sw = new Stopwatch();
        sw.Start();

        Heap<Node> openset = new Heap<Node>(Grid.MaxSize);
        HashSet<Node> closeset = new HashSet<Node>();

        openset.Add(StartNode);

        while (openset.Count > 0)
        {
            Node CurrentNode = openset.RemoveFirst();
            
            closeset.Add(CurrentNode);

            if (CurrentNode == TargetNode)
            {
                sw.Stop();
                print("PathFind time : " + sw.ElapsedMilliseconds + "ms");
                ReTracePath(StartNode, TargetNode);
                return;
            }

            foreach (Node neighbour in Grid.GetNeighbour(CurrentNode)) {
                if (!neighbour.bWalkable || closeset.Contains(neighbour))
                {
                    continue;
                }

                int newMovementCostToNeighbour = CurrentNode.Cost_G + GetDistance(CurrentNode, neighbour);
                if (newMovementCostToNeighbour < neighbour.Cost_G || !openset.Contains(neighbour))
                {
                    neighbour.Cost_G = newMovementCostToNeighbour;
                    neighbour.Cost_H = GetDistance(neighbour, TargetNode);
                    neighbour.Parent = CurrentNode;

                    if (!openset.Contains(neighbour))
                        openset.Add(neighbour);
                }

            }
        }
    }

    void ReTracePath(Node startNode, Node endNode)
    {
        List<Node> Path = new List<Node>();
        Node CurrentNode = endNode;

        while (CurrentNode != startNode)
        {
            Path.Add(CurrentNode);
            CurrentNode = CurrentNode.Parent;
        }
        Path.Reverse();

        Grid.path = Path;
    }

    int GetDistance(Node nodeA, Node nodeB)
    {
        int dstX = Mathf.Abs(nodeA.Grid_x - nodeB.Grid_x);
        int dstY = Mathf.Abs(nodeA.Grid_y - nodeB.Grid_y);

        if (dstX > dstY)
            return 14 * dstY + 10 * (dstX - dstY);
        return 14 * dstX + 10 * (dstY - dstX);
    }
}