I am working on a game where the player and enemies move between discreet tiles, with only orthagonal movement (like pokemon). The game also runs on a turn system, so the player makes a move input, then the enemies calculate their paths and begin moving simultaneously with the player to the first node in their path. My enemies calculate their paths one after another and set move points at the position they intend to travel to. This lets enemies check if there is already a move point where they are trying to pathfind to, in which case they'll choose an alternate adjacent point to move to. After all the paths are calculated the enemies move simultaneously. I also implemented enemy aggro, so only enemies within the bounds of the camera path to the player. The rest path to a random adjacent node to keep the paths short. My problem is I've been playtesting with 20 enemies on a reasonably small grid (56x28) and it takes 130 ms or so to calculate the paths of all the enemies. This results in some pretty undesirable movement, since the player's control is locked until all enemies have finished moving. The player noticeably pauses for the enemies to finish moving in between taking steps.

I've posted the relevant sections of my code below, does anyone see a glaring issue with how I'm approaching this? I've adapted a pathfinding tutorial series by Sebastian Lague on YouTube to fit my game. I also looked at the A* Pathfinding Project by Aron Granburg, but stayed away from it because it looked much more complicated to adapt to my specific project. If anyone is familiar with the A* pathfinding project, is it highly optimized to the point where it might be worth it to scrap what I have and try to use that instead?

Also relevant: I changed my fixed timestep in my unity project settings to 0.001 from 0.02. Since I had to include a WaitForFixedUpdate to make sure enemies were accurately checking for collision this sped up the enemy turns quite a bit, and it doesn't seem to have created significant overhead.

Also also relevant (maybe): Performance differs when I have the scene view open. In this case enemy pathfinding takes ~ 255 ms rather than the ~130 ms when the game view is maximized.

From TurnSystem.cs:

        public IEnumerator EnemyTurn(bool playerTookMoveAction)
        {
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

            playerController.LockControl(true);

            sw.Start();
            WaitForFixedUpdate wait = new WaitForFixedUpdate();  
            for (int i = 0; i < enemies.Count; i++)
            {
                if (enemies[i].GetTookFightAction()) continue;
                yield return StartCoroutine(enemies[i].SetMovePoint());
                yield return wait;
            }

            sw.Stop();
            print("Elapsed Time: " + sw.ElapsedMilliseconds/1000f);

            for (int i = 0; i < enemies.Count; i++)
            {
                if (enemies[i].GetTookFightAction()) continue;
                StartCoroutine(enemies[i].TakeStep());
            }
            
            while(enemiesFinishedAction < enemies.Count)
            {
                yield return null;
            }
            enemiesFinishedAction = 0;

            for (int i = 0; i < enemies.Count; i++)
            {
                enemies[i].SetAggro();
            }

            StartCoroutine(playerController.UnlockControlAfterMovement());
            OnReadyToSpawn();
        }

        public void AddEnemy(EnemyAIController enemy)
        {
            enemies.Add(enemy);
        }

        public void RemoveEnemy(EnemyAIController enemy)
        {
            enemies.Remove(enemy);
        }

        public void EnemyFinishedAction()
        {
            enemiesFinishedAction++;
        }

From EnemyAIController.cs:

        public void OnPathFound(Vector2[] newPath, bool pathSuccessful)
        {
            if (pathSuccessful && newPath.Length > 0)
            {
                path = newPath;
            }
        }

        public IEnumerator SetMovePoint()
        {
            if (!isLocked)
            {
                if (isAggroed) 
                {
                    target = playerMovePoint.position;
                }
                else
                {
                    target = GetRandomAdjacentTarget();
                }
                yield return PathRequestManager.RequestPath(transform.position, target, OnPathFound);
                if (path != null && !Physics2D.OverlapCircle(path[0], 0.3f, collisionLayer))
                {
                    movePoint.position = path[0];
                }
                else
                {
                    movePoint.position = GetAlternateWaypoint();
                }
            }
            else yield return null; 
        }

        protected Vector2 GetRandomAdjacentTarget()
        {
            float random = UnityEngine.Random.Range(0,4);
            if (random == 0) return new Vector2 (transform.position.x, transform.position.y - 2);
            if (random == 1) return new Vector2 (transform.position.x - 2, transform.position.y);
            if (random == 2) return new Vector2 (transform.position.x, transform.position.y + 2);
            if (random == 3) return new Vector2 (transform.position.x + 2, transform.position.y);
            return transform.position;
        }

        protected Vector2 GetAlternateWaypoint()
        {
            if (path != null && path[0] != null && path[0] != (Vector2)movePoint.position)
            {
                if(path[0].x != movePoint.position.x)
                {
                    if (target.y > movePoint.position.y && !Physics2D.OverlapCircle(new Vector2(movePoint.position.x, movePoint.position.y + 1), 0.3f, collisionLayer))
                    {
                        return new Vector2(movePoint.position.x, movePoint.position.y + 1);
                    }
                    else if (target.y <= movePoint.position.y && !Physics2D.OverlapCircle(new Vector2(movePoint.position.x, movePoint.position.y - 1), 0.3f, collisionLayer))
                    {
                        return new Vector2(movePoint.position.x, movePoint.position.y - 1);
                    }
                }
                else if (path[0].y != movePoint.position.y)
                {
                    if (target.x > movePoint.position.x && !Physics2D.OverlapCircle(new Vector2(movePoint.position.x + 1, movePoint.position.y), 0.3f, collisionLayer))
                    {
                        return new Vector2(movePoint.position.x + 1, movePoint.position.y);
                    }
                    else if (target.x <= movePoint.position.x && !Physics2D.OverlapCircle(new Vector2(movePoint.position.x - 1, movePoint.position.y), 0.3f, collisionLayer))
                    {
                        return new Vector2(movePoint.position.x - 1, movePoint.position.y);
                    }
                }
            }
            return movePoint.position;
        }

From PathRequestManager.cs:

public class PathRequestManager : MonoBehaviour
    {
        Queue<PathRequest> pathRequestQueue = new Queue<PathRequest>();
        PathRequest currentPathRequest;

        static PathRequestManager instance;
        Pathfinding pathfinding;

        bool isProcessingPath;

        private void Awake() 
        {
            instance = this;
            pathfinding = GetComponent<Pathfinding>();
        }

        public static IEnumerator RequestPath(Vector2 pathStart, Vector2 pathEnd, Action<Vector2[], bool> callback)  //does this need to be static?
        {
            PathRequest newRequest = new PathRequest(pathStart, pathEnd, callback);
            instance.pathRequestQueue.Enqueue(newRequest);
            instance.TryProcessNext();
            yield return null;
        }
        private void TryProcessNext()
        {
            if (!isProcessingPath && pathRequestQueue.Count > 0)
            {
                currentPathRequest = pathRequestQueue.Dequeue();
                isProcessingPath = true;
                pathfinding.StartFindPath(currentPathRequest.pathStart, currentPathRequest.pathEnd);
            }
        }

        public void FinishedProcessingPath(Vector2[] path, bool success)
        {
            currentPathRequest.callback(path, success);
            isProcessingPath = false;
            TryProcessNext();
        }

        struct PathRequest
        {
            public Vector2 pathStart;
            public Vector2 pathEnd;
            public Action<Vector2[], bool> callback;

            public PathRequest(Vector2 _start, Vector2 _end, Action<Vector2[], bool> _callback)
            {
                pathStart = _start;
                pathEnd = _end;
                callback = _callback;
            }
        }
    }

From Pathfinding.cs:

    public class Pathfinding : MonoBehaviour
    {
        PathRequestManager requestManager;
        AStarGrid grid;

        private void Awake() 
        {
            requestManager = GetComponent<PathRequestManager>();
            grid = GetComponent<AStarGrid>();
        }

        public void StartFindPath(Vector2 startPos, Vector2 targetPos)
        {
            StartCoroutine(FindPath(startPos, targetPos));
        }

        IEnumerator FindPath(Vector2 startPos, Vector2 targetPos)
        {
            Vector2[] waypoints = new Vector2[0];
            bool pathSuccess = false;

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

            if(startNode.walkable && targetNode.walkable)
            {
                Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
                HashSet<Node> closedSet = new HashSet<Node>();
                openSet.Add(startNode);

                while (openSet.Count > 0)
                {
                    Node currentNode = openSet.RemoveFirst();
                    closedSet.Add(currentNode);

                    if (currentNode == targetNode)//found the path
                    {
                        pathSuccess = true;
                        break;
                    }

                    foreach(Node neighbor in grid.GetNeighbors(currentNode))
                    {
                        if(!neighbor.walkable || closedSet.Contains(neighbor)) continue;
                        int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
                        if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                        {
                            neighbor.gCost = newMovementCostToNeighbor;
                            neighbor.hCost = GetDistance(neighbor, targetNode);
                            neighbor.parent = currentNode;

                            if (!openSet.Contains(neighbor))
                            {
                                openSet.Add(neighbor);
                            }
                            else
                            {
                                openSet.UpdateItem(neighbor);
                            }
                        }
                    }
                }
            }
            yield return null;
            if (pathSuccess)
            {
                waypoints = RetracePath(startNode, targetNode);
            }
            requestManager.FinishedProcessingPath(waypoints, pathSuccess);
        }

        Vector2[] RetracePath(Node startNode, Node endNode)
        {
            List<Node> path = new List<Node>();
            Node currentNode = endNode;

            while(currentNode != startNode)
            {
                path.Add(currentNode);
                currentNode = currentNode.parent;
            }
            Vector2[] waypoints = SimplifyPath(path);
            // Array.Reverse(waypoints);
            return waypoints;
        }

        Vector2[] SimplifyPath(List<Node> path)//I simplified this section of code since I just needed the first waypoint of the path returned, this did help performance
        {
            List<Vector2> waypoints = new List<Vector2>();
            // Vector2 directionOld = Vector2.zero;

            for(int i = Mathf.Max(path.Count - 1, 0); i < path.Count; i ++)
            // for(int i = 1; i < path.Count; i ++)
            {
                // Vector2 directionNew = new Vector2(path[i-1].gridX - path[i].gridX, path[i-1].gridY - path[i].gridY);
                // if(directionNew != directionOld)
                // {

                    waypoints.Add(path[i].worldPosition);
                // }
                // directionOld = directionNew;
            }
            return waypoints.ToArray();
        }

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

            return distanceX + distanceY;
        }
    }

From node.cs:

    public class Node : IHeapItem<Node>
    {
        public bool walkable;
        public Vector2 worldPosition;
        public int gridX;
        public int gridY;

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

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

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

        public int HeapIndex
        {
            get
            {
                return heapIndex;
            }
            set
            {
                heapIndex = value;
            }
        }

        public int CompareTo(Node nodeToCompare)
        {
            int compare = fCost.CompareTo(nodeToCompare.fCost);
            if (compare == 0)
            {
                compare = hCost.CompareTo(nodeToCompare.hCost);
            }
            return -compare;
        }
    }

From Heap.cs

    public class Heap<T> where T : IHeapItem<T> {
        
        T[] items;
        int currentItemCount;
        
        public Heap(int maxHeapSize) {
            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 childIndexLeft = item.HeapIndex * 2 + 1;
                int childIndexRight = item.HeapIndex * 2 + 2;
                int swapIndex = 0;

                if (childIndexLeft < currentItemCount) {
                    swapIndex = childIndexLeft;

                    if (childIndexRight < currentItemCount) {
                        if (items[childIndexLeft].CompareTo(items[childIndexRight]) < 0) {
                            swapIndex = childIndexRight;
                        }
                    }

                    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];
                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;
        }
    }

From AStarGrid.cs

    public class AStarGrid : MonoBehaviour
    {
        public bool displayGridGizmos;
        public LayerMask unwalkableMask;
        public Vector2 gridWorldSize;
        public float nodeRadius;
        Node[,] grid;

        float nodeDiameter;
        int gridSizeX, gridSizeY;

        private void Awake() 
        {
            nodeDiameter = nodeRadius *2;
            gridSizeX = Mathf.RoundToInt(gridWorldSize.x/nodeDiameter);
            gridSizeY = Mathf.RoundToInt(gridWorldSize.y/nodeDiameter);
            CreateGrid();
        }

        public int MaxSize
        {
            get
            {
                return gridSizeX * gridSizeY;
            }
        }

        private void CreateGrid()
        {
            grid = new Node[gridSizeX, gridSizeY];
            Vector2 worldBottomLeft = (Vector2)transform.position - Vector2.right * gridWorldSize.x/2 - Vector2.up * gridWorldSize.y/2;

            for (int x = 0; x < gridSizeX; x++)
            {
                for (int y = 0; y < gridSizeY; y++)
                {
                    Vector2 worldPoint = worldBottomLeft + Vector2.right * (x * nodeDiameter + nodeRadius) + Vector2.up * (y * nodeDiameter + nodeRadius);
                    bool walkable = (Physics2D.OverlapCircle(worldPoint,nodeRadius,unwalkableMask) == null);
                    grid[x,y] = new Node (walkable, worldPoint, x, y);
                }
            }
        }

        public List<Node> GetNeighbors(Node node)
        {
            List<Node> neighbors = new List<Node>();

            for (int x = -1; x <= 1; x++)
            {
                for (int y = -1; y <= 1; y++)
                {
                    if (x == 0 && y == 0 || Mathf.Abs(x) + Mathf.Abs(y) == 2) continue;

                    int checkX = node.gridX + x;
                    int checkY = node.gridY + y;

                    if(checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)// if checkX and checkY are on the grid
                    {
                        neighbors.Add(grid[checkX, checkY]);
                    }
                }
            }

            return neighbors;
        }

        public Node NodeFromWorldPoint(Vector2 worldPosition)
        {
            float percentX = worldPosition.x / gridWorldSize.x + 0.5f;
            float percentY = worldPosition.y / gridWorldSize.y + 0.5f;

            int x = Mathf.FloorToInt(Mathf.Clamp((gridSizeX) * percentX, 0, gridSizeX - 1));
            int y = Mathf.FloorToInt(Mathf.Clamp((gridSizeY) * percentY, 0, gridSizeY - 1));
            
            return grid[x,y];
        }

        public List<Node> path;
        void OnDrawGizmos() 
        {
        Gizmos.DrawWireCube(transform.position,new Vector2(gridWorldSize.x,gridWorldSize.y));
        if (grid != null && displayGridGizmos) {
            foreach (Node n in grid) {
                Gizmos.color = Color.red;
                if (n.walkable)
                    Gizmos.color = Color.white;

                Gizmos.DrawCube(n.worldPosition, Vector3.one * (nodeDiameter-.8f));
            }
        }
        }
    }