Pathfinding.cs

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

public class Pathfinding : MonoBehaviour {

	public Transform seeker, target;
	Grid grid;
    
	void Start () 
	{
		grid = GetComponent<Grid> ();	
	}

	void Update()
	{
        //FindPath(seeker.position, target.position);
        if (Input.GetMouseButtonDown(0))
        {
            FindPath(seeker.position, target.position);
        }
    }

	void FindPath(Vector3 startPos, Vector3 targetPos)
	{
        Stopwatch sw = new Stopwatch();
        sw.Start();

		Node startNode = grid.NodeFromWorldPosition(startPos);
		Node targetNode = grid.NodeFromWorldPosition(targetPos);

		List<Node> openSet = new List<Node> ();
		HashSet<Node> closedSet = new HashSet<Node>();
		openSet.Add (startNode);

        while (openSet.Count > 0) {

            // OpenSet에서 가장 낮은 fCost를 가지는 노드를 가져온다. 
            // 만일 fCost가 동일할 경우 gCost가 적은 쪽을 택함. 
            Node currentNode = openSet[0];

            //2개 이상일 경우 비교가 필요할 때
            if (openSet.Count >= 2)
            {
                for(int i=0;i<openSet.Count;i++)
                {
                    //비교 대상 오픈셋이 현재로써의 노드보다 F코스트가 적을 경우
                    if(openSet[i].fCost<currentNode.fCost)
                    {
                        currentNode = openSet[i];
                    }
                    //비교 대상 오픈셋이 현재로써의 노드보다 F코스트가 적을 경우
                    else if (openSet[i].fCost==currentNode.fCost)
                    {
                        if (openSet[i].gCost < currentNode.gCost)
                        {
                            currentNode = openSet[i];
                        }
                    }
                }
            }

            // 찾은 노드가 최종 노드면 루프 종료
            if (currentNode == targetNode)
            {
                RetracePath(startNode, targetNode);
                sw.Stop();
                print("Elapsed Time : " + sw.ElapsedTicks + "Ticks");
                return;
            }

            // 해당 노드를 ClosedSet으로 이동
            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            // 현재 노드의 이웃들을 모두 가져와 비교
            foreach (Node n in grid.GetNeighbours(currentNode))
            {
                // 이웃 노드가 이미 ClosedSet에 있거나 걸어다니지 못하는 곳이면 제외한다.
                if (closedSet.Contains(n) || !n.walkable)
                    continue;

                // 현재 노드를 기점에서 파생된 이웃 노드의 fCost를 계산한다.
                n.gCost = GetDistance(currentNode, n);
                n.hCost = GetDistance(n, targetNode);
                n.parent = currentNode;
                //n.fCost는 자동 계산

                // 오픈셋에 현재 노드가 없으면 노드에 점수를 설정한 후 추가한다.
                if (!openSet.Contains(n))
                {
                    openSet.Add(n);
                }
                else
                {
                    // 오픈셋에 현재 노드가 이미 있으면 수치를 비교한 후 경로를 교체한다. 동일하면 g수치가 큰 쪽으로 교체한다.
                    if (n.fCost < currentNode.fCost)
                    {
                        n.parent = currentNode.parent;
                    }
                    else if(n.fCost==currentNode.fCost)
                    {
                        if(n.gCost < currentNode.gCost)
                        {
                            n.parent = currentNode.parent;
                        }
                    }
                }
            }
		}

	}

    // 경로를 보여준다.
	void RetracePath(Node startNode, Node endNode)
	{
		List<Node> path = new List<Node> ();
		Node currentNode = endNode;

		while (currentNode != startNode) {
			path.Add (currentNode);
			currentNode = currentNode.parent;
		}

		grid.path = path;
	}

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

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

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