Help with optimizing A* path finding algorithm

I have been using the A* path finding algorithm for my enemies and I am having issues with frame rates dropping and enemies not choosing paths efficiently. The game is using 2D tilemaps built in to Unity. Here is the code:

public class Pathfinding : MonoBehaviour
{
    public Tilemap TileMap;
        
    public CalculatedPath FindPath(AStarNode start, AStarNode goal)
    {
        Debug.Log("Start pathfinding");

        return Run(start, goal);        
    }

    private CalculatedPath Run(AStarNode startNode, AStarNode goalNode)
    {
        // create two lists. the open list is the one to be evaluated
        var open = new List<AStarNode>();
        // the closed list has already been evaluated
        var closedList = new HashSet<AStarNode>();
        //adding start node to the open list
        open.Add(startNode);
        while (open.Count > 0)
        {
            // get the current node in the open list with the lowest F cost
            var currentNode = open[0];

            for (int i = 1; i < open.Count; i++)
            {
                if (open<em>.FCost < currentNode.FCost || open_.FCost == currentNode.FCost && open*.HCost < currentNode.HCost)*_</em>

{
currentNode = open*;*
}
}

open.Remove(currentNode);
// add the current node to the closed list
closedList.Add(currentNode);

// if the current node is the goal position, generate a path and see if we can leave the loop
if (currentNode == goalNode)
{
return GenerateFinalPath(startNode, goalNode);
}

List neighbours = FindNeighbourNodes(currentNode);

foreach (var neighbour in neighbours)
{
// if its not able to walk on or the closed list contains this neighbour, skip
if (neighbour.IsBlocked || closedList.Contains(neighbour))
{
continue;
}

int newMoveCostToNeighbour = currentNode.GCost + GetDistance(currentNode, neighbour);

if (newMoveCostToNeighbour < neighbour.GCost || !open.Contains(neighbour))
{
neighbour.GCost = newMoveCostToNeighbour;
neighbour.HCost = GetDistance(neighbour, goalNode);
neighbour.Parent = currentNode;

if (!open.Contains(neighbour))
{
open.Add(neighbour);
}
}
}
}

return null;
}

private int GetDistance(AStarNode nodeA, AStarNode 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);
}

private List FindNeighbourNodes(AStarNode node)
{
List neighbours = new List();

for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0)
{
continue;
}

int checkX = node.GridX + x;
int checkY = node.GridY + y;

AStarNode neighbour = GameManager.Instance.AStarGrid[checkX, checkY];
neighbours.Add(neighbour);
}
}

return neighbours;
}

private CalculatedPath GenerateFinalPath(AStarNode startNode, AStarNode goalNode)
{
var currentNode = goalNode;

while (currentNode != null && currentNode != startNode)
{
currentNode = currentNode.Parent;
}

var calculatedPath = new CalculatedPath(startNode, goalNode);
if (!GameManager.Instance.CalculatedPaths.Contains(calculatedPath))
{
GameManager.Instance.CalculatedPaths.Add(calculatedPath);
}

Debug.Log("Generated path from " + startNode.GridPosition + " to " + goalNode.GridPosition);

return calculatedPath;
}
}

public class AStarNode
{
public Vector3Int GridPosition
{
get
{
return new Vector3Int(GridX, GridY, 0);
}
}

public int GCost { get; set; }
public int HCost { get; set; }
public int FCost { get { return GCost + HCost; } }

public int GridX;
public int GridY;

public bool IsBlocked { get; set; }

public AStarNode Parent { get; set; }

public AStarNode(int gridX, int gridY)
{
GridX = gridX;
GridY = gridY;
}

public override bool Equals(object obj)
{
return GridPosition == ((AStarNode)obj).GridPosition;
}

public int CompareTo(AStarNode other)
{
int compare = FCost.CompareTo(other.FCost);

if (compare == 0)
{
compare = HCost.CompareTo(other.HCost);
}

return -compare;
}
}

public class CalculatedPath
{
public AStarNode StartNode { get; set; }
public AStarNode GoalNode { get; set; }

public CalculatedPath(AStarNode startNode, AStarNode goalNode)
{
StartNode = startNode;
GoalNode = goalNode;
}
}

public class GameManager : MonoBehaviour
{

private AStarNode[,] aStarGrid;
public AStarNode[,] AStarGrid { get => aStarGrid; set => aStarGrid = value; }

public List CalculatedPaths = new List();
}

I think at some point I may have messed up the algorithm when I was attempting to optimize it. I hope this is enough information to answer the question. Here is the enemy path code:

public class EnemyPathState : IEnemyState
{
private Stack pathStack;
private Vector3 goal;

private Vector3Int moveTowardsTile;
private Enemy parent;
private AStarNode currentNode;
private AStarNode nextNode;

public void Enter(Enemy parent)
{
this.parent = parent;
RestartPath();
}

private void RestartPath()
{
pathStack = new Stack();
var startNode = GameManager.Instance.AStarGrid[(int)parent.transform.position.x, (int)parent.transform.position.y];
var goalNode = GameManager.Instance.AStarGrid[(int)parent.CurrentEnemyTarget.transform.position.x, (int)parent.CurrentEnemyTarget.transform.position.y];

goal = parent.CurrentEnemyTarget.transform.position;
moveTowardsTile = parent.CurrentEnemyTarget.CurrentTileStandingOn;

var path = GameManager.Instance.CalculatedPaths.FirstOrDefault(m => m.StartNode == startNode && m.GoalNode == goalNode);

if (path == null)
{
path = parent.AStar.FindPath(startNode, goalNode);
}
var current = path.GoalNode;
while (current != null && current != path.StartNode)
{
pathStack.Push(current);
current = current.Parent;
}
Debug.Log("Path stack count " + pathStack.Count);

if (pathStack.Count > 0)
{
currentNode = pathStack.Pop();
nextNode = pathStack.Pop();
}
}

public void Exit()
{
}

public void Update()
{
CheckIfEnterAttackState();

if (parent.CurrentEnemyTarget.CurrentTileStandingOn != moveTowardsTile)
{
RestartPath();
}

if (pathStack != null)
{
MoveOnPath();
}
}

private void CheckIfEnterAttackState()
{
var distanceFromPlayer = Vector2.Distance(parent.CurrentEnemyTarget.transform.position, parent.transform.position);
if (distanceFromPlayer <= parent.AttackRange)
{
EnterAttackState();
return;
}
}

private void MoveOnPath()
{
Vector3 currentPosition = currentNode.GridPosition;
Vector3 destinationPosition = nextNode.GridPosition;
parent.transform.position = Vector2.MoveTowards(parent.transform.position, destinationPosition, parent.MoveSpeed * Time.deltaTime);

parent.CurrentTileStandingOn = GameManager.Instance.Tilemaps[0].WorldToCell(parent.transform.position);
float distance = Vector2.Distance(destinationPosition, parent.transform.position);

if (distance < 1.0f)
{
if (pathStack != null && pathStack.Count > 0)
{
currentNode = nextNode;
nextNode = pathStack.Pop();
}
else
{
pathStack = new Stack();
}
}

if (currentPosition.y > destinationPosition.y)
{
parent.Direction = Vector2.down;
}
else if (currentPosition.y < destinationPosition.y)
{
parent.Direction = Vector2.up;
}
if (currentPosition.y == destinationPosition.y)
{
if (currentPosition.x > destinationPosition.x)
{
parent.Direction = Vector2.left;
}
else if (currentPosition.x < destinationPosition.x)
{
parent.Direction = Vector2.right;
}
}
}

private void EnterAttackState()
{
Debug.Log(“Enemy enter attack state”);
parent.Direction = Vector2.zero;
parent.ChangeState(new EnemyAttackState());
}
}

The issue I am having is that when I have like 10 enemies on the screen trying to find a path to me at once, the game’s fps just goes down to like 10. Also the more complex the maze I put the enemy in, the harder it is to find a path to me, also causing performance issues.
Can anyone help identify problems in my code to optimize and fix up this algorithm? Or can someone suggest a better way to achieve the goal?
Any help is appreciated, thanks!

There are many things that should be changed:

  • First of all you should implement the open list as a binary heap. A* is the prime example for the use of a min binary heap. It speeds up insertion and deletion of the lowest element drastically. At the moment you iterate through the whole open list each time you want to remove the smallest element. In a min heap the smallest element is always the first one.
  • Next if you have a really large node graph and the open list can get huge, you may want to use an additional hashset for the openlist to do the contains checks. Contains of a List (or binary heap) is an O(n) operation as it has to iterate through all elements. Alternatively you could implement a boolean flag inside your Node to indicate that the node is in the openlist.
  • Since you seem to use this A* implementation only for a grid based node graph, using a List<Node> as return type for your FindNeighbourNodes method is not very efficient as you allocate a lot garbage each time you call this method. You have two alternatives: Either get rid of the method and just inline it at the place where you use it. Since you only use it in one place that shouldn’t by an issue. Alternatively you could use one of my StructList implementations. They are essentially fixed sized structures but provide similar functionality as the generic List class. However since it’s a struct no heap memory will be allocated.

Apart from that your implementation seems a bit strange. First of all you don’t seem to check against the grid bounds. You just blindly going ±1 in each direction when looking for neighbors. If you call this method on a border tile you will get an index out of bounds exception.

Next thing is your neighbour.IsBlocked check should probably be done right inside your FindNeighbourNodes and you shouldn’t return this neighour if it can’t be walked on. Also at the moment all your nodes have equal cost. This is fine if you have a binary gird (walkable or non walkable). Usually you would include a “tile cost” depending on the type of tile. Of course this is not necessary but would only be a tiny change in your existing code.

Finally there are several other things strange which are hard to follow. For example you “GenerateFinalPath” does a lot useless stuff. All if actually does is creating a “CalculatedPath” instance and then save it to that “CalculatedPaths” list in your manager class. This also seems very strange. You don’t actually save your generated path. You only save the start and end node. This would work with only one enemy / agent using those nodes. However if you have multiple enemies using the same nodes this won’t work. If two enemies calculating a path they will overwrite all the node parameters (Gcost, HCost as well as the parent reference). Once you completed generating a path you should trace back that path immediately and save all the steps into a list / array. Just saving the start and end node won’t do.