PriorityQueue A*

Pathfinding.cs

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

public class Node : IComparable<Node>
{

    public bool walkable;
    public Vector3 worldPosition;
    public int gridX;
    public int gridY;

    public int gCost;
    public int hCost;
    public Node parent;

    public Node(bool _walkable, Vector3 _worldPos, int _gridX, int _gridY)
    {
        this.walkable = _walkable;
        this.worldPosition = _worldPos;
        gridX = _gridX;
        gridY = _gridY;
    }

    public int CompareTo(Node other)
    {
        if (this.fCost < other.fCost)
        {
            return -1;
        }
        else if (this.fCost > other.fCost)
        {
            return 1;
        }
        else
        {
            if (this.gCost < other.gCost)
            {
                return -1;
            }
            else if (this.gCost > other.gCost)
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }
    }

    public int fCost
    {
        get { return gCost + hCost; }
    }
}


public class PriorityQueue<T> where T : IComparable<T>
{
    private T[] data;
    private int itemCount;

    public PriorityQueue(int maxSize)
    {
        this.data = new T[maxSize];
        itemCount = 0;
    }

    public bool Contains(T item)
    {
        for (int i = 0; i < itemCount - 1; i++)
        {
            if (data[i].Equals(item))
                return true;
        }

        return false;
    }

    public void Enqueue(T item)
    {
        data[itemCount] = item;
        int ci = itemCount - 1;
        while (ci > 0)
        {
            int pi = (ci - 1) / 2;
            if (data[ci].CompareTo(data[pi]) >= 0)
                break;
            T tmp = data[ci]; data[ci] = data[pi]; data[pi] = tmp;
            ci = pi;
        }

        itemCount++;
    }

    public T Dequeue()
    {
        int li = itemCount - 1;
        T frontItem = data[0];
        data[0] = data[li];

        --li;
        int pi = 0;
        while (true)
        {
            int ci = pi * 2 + 1;
            if (ci > li) break;
            int rc = ci + 1;
            if (rc <= li && data[rc].CompareTo(data[ci]) < 0)
                ci = rc;
            if (data[pi].CompareTo(data[ci]) <= 0) break;
            T tmp = data[pi]; data[pi] = data[ci]; data[ci] = tmp;
            pi = ci;
        }

        itemCount--;
        return frontItem;
    }

    public int Count()
    {
        return itemCount;
    }

    public T Peek()
    {
        T frontItem = data[0];
        return frontItem;
    }

}


public class Pathfinding : MonoBehaviour
{

    public Transform seeker, target;
    Grid grid;

    void Start()
    {
        grid = GetComponent<Grid>();
    }

    void Update()
    {
        if (Input.GetMouseButtonDown(0))
        {
            FindPath(seeker.position, target.position);
            FindPathWithPriorityQueue(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)
        {

            Node currentNode = openSet[0];

            for (int i = 1; i < openSet.Count; i++)
            {
                if (openSet[i].fCost < currentNode.fCost || (openSet[i].fCost == currentNode.fCost && openSet[i].gCost > currentNode.gCost))
                {
                    currentNode = openSet[i];
                }
            }

            if (currentNode == targetNode)
            {
                RetracePath(startNode, targetNode);
                sw.Stop();
                print("List Elapsed Time : " + sw.ElapsedTicks + "ticks"); ;
                return;
            }

            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            foreach (Node n in grid.GetNeighbours(currentNode))
            {
                if (!n.walkable || closedSet.Contains(n))
                    continue;

                int g = currentNode.gCost + GetDistance(currentNode, n);
                int h = GetDistance(n, targetNode);
                int f = g + h;
                if (!openSet.Contains(n))
                {
                    n.gCost = g;
                    n.hCost = h;
                    n.parent = currentNode;
                    openSet.Add(n);
                }
                else
                {
                    if (n.fCost > f || (n.fCost == f && n.gCost > g))
                    {
                        n.gCost = g;
                        n.parent = currentNode;
                    }
                }
            }
        }

    }

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

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

        PriorityQueue<Node> openSet = new PriorityQueue<Node>(1000);
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Enqueue(startNode);

        while (openSet.Count() > 0)
        {
            Node currentNode = openSet.Dequeue();

            if (currentNode == targetNode)
            {
                RetracePath(startNode, targetNode);
                sw.Stop();
                print("Priority Queue Elapsed Time : " + sw.ElapsedTicks + "ticks"); ;
                return;
            }

            closedSet.Add(currentNode);

            foreach (Node n in grid.GetNeighbours(currentNode))
            {
                if (!n.walkable || closedSet.Contains(n))
                    continue;

                int g = currentNode.gCost + GetDistance(currentNode, n);
                int h = GetDistance(n, targetNode);
                int f = g + h;

                if (!openSet.Contains(n))
                {
                    n.gCost = g;
                    n.hCost = h;
                    n.parent = currentNode;
                    openSet.Enqueue(n);
                }
                else
                {
                    if (n.fCost > f || (n.fCost == f && n.gCost > g))
                    {
                        n.gCost = g;
                        n.parent = currentNode;
                    }
                }
            }
        }

    }

    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.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);
    }
}