How to speed up my pathfinding algorithm?

Question

How can I speed up my code? And what am I doing wrong? I have a two-dimensional array of cells that store data about what is in them. And I have a map of only 100x100 and with 10 colonists and this already causes freezes. Although my game is quite raw.

And when should I build a route for a colonist? Every step he takes? Because if an unexpectedly built wall appears on his way. Then he will have to immediately change the route.

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

public class PathFinding : MonoBehaviour
{
    public MainWorld MainWorld;

    public MainWorld.Cell CurrentWorldCell;
    public MainWorld.Cell NeighborWorldCell;

    public List<MainWorld.Cell> UnvisitedWorldCells;
    public List<MainWorld.Cell> VisitedWorldCells;

    public Dictionary<MainWorld.Cell, MainWorld.Cell> PathTraversed;
    public List<MainWorld.Cell> PathToObject;

    public List<MainWorld.Cell> FindPath(MainWorld.Cell StartWorldCell, string ObjectID)
    {
        CurrentWorldCell = new MainWorld.Cell();
        NeighborWorldCell = new MainWorld.Cell();

        UnvisitedWorldCells = new List<MainWorld.Cell>();
        VisitedWorldCells = new List<MainWorld.Cell>();

        PathTraversed = new Dictionary<MainWorld.Cell, MainWorld.Cell>();

        UnvisitedWorldCells.Add(StartWorldCell);

        while (UnvisitedWorldCells.Count > 0)
        {
            CurrentWorldCell = UnvisitedWorldCells[0];

            NeighborWorldCell = MainWorld.Data[CurrentWorldCell.Position.x, CurrentWorldCell.Position.y + 1];
            CheckWorldCell(CurrentWorldCell, NeighborWorldCell, ObjectID);

            NeighborWorldCell = MainWorld.Data[CurrentWorldCell.Position.x + 1, CurrentWorldCell.Position.y];
            CheckWorldCell(CurrentWorldCell, NeighborWorldCell, ObjectID);

            NeighborWorldCell = MainWorld.Data[CurrentWorldCell.Position.x, CurrentWorldCell.Position.y - 1];
            CheckWorldCell(CurrentWorldCell, NeighborWorldCell, ObjectID);

            NeighborWorldCell = MainWorld.Data[CurrentWorldCell.Position.x - 1, CurrentWorldCell.Position.y];
            CheckWorldCell(CurrentWorldCell, NeighborWorldCell, ObjectID);

            UnvisitedWorldCells.Remove(CurrentWorldCell);

            if (CurrentWorldCell.ObjectID == ObjectID)
            {
                return CreatePath(StartWorldCell, CurrentWorldCell);
            }
        }

        return null;
    }

    public void CheckWorldCell(MainWorld.Cell CurrentWorldCell, MainWorld.Cell NeighborWorldCell, string ObjectID)
    {
        if (VisitedWorldCells.Contains(NeighborWorldCell) == false)
        {
            if (NeighborWorldCell.IsPassable == true ||
                NeighborWorldCell.ObjectID == ObjectID)
            {
                UnvisitedWorldCells.Add(NeighborWorldCell);
                VisitedWorldCells.Add(NeighborWorldCell);
                PathTraversed.Add(NeighborWorldCell, CurrentWorldCell);
            }
            else
            {
                VisitedWorldCells.Add(NeighborWorldCell);
            }
        }
    }

    public List<MainWorld.Cell> CreatePath(MainWorld.Cell StartWorldCell, MainWorld.Cell EndWorldCell)
    {
        PathToObject = new List<MainWorld.Cell>();

        PathToObject.Add(EndWorldCell);

        while (PathToObject[PathToObject.Count - 1] != StartWorldCell)
        {
            PathToObject.Add(PathTraversed[PathToObject[PathToObject.Count - 1]]);
        }

        PathToObject.Reverse();

        return PathToObject;
    }
}

Answer 1

On each iteration of your pathfinding loop, there is a check for each of the four neighboring cells. Within each check, you are searching the VisitedWorldCells List. If there are many cells in that list, it could be your first issue. Next, you either add an element to one or three lists. Manipulating lists is slow. To create a path, you are making a list and adding a bunch of elements. Doing all of this 10 times per frame is unreasonable.

Use preallocated 2d boolean arrays. You know how large the map is.

bool[,] array = new bool[mapSizeX, mapSizeY];

Using this, you can check the cells directly with something like

if(!VistedWorldCells[14,16])

Instead of checking the entire VisitedWorldCells list (which also could be a slow comparison depending on what the MainWorld.Cell type is).

Preallocated arrays and directly checking/setting the values will speed this up by orders of magnitude.

Answer 2

As far as I can tell your algorithm is a simple breadth first search. The first step should be to replace the UnvisitedWorldCells list (often called the 'open set') with a queue, and the VisitedWorldCells (often called closed set) with a hashSet. Or just use a 2D array for a constant time lookup. You might also consider using a more compact representation of your node, like a simple pair of x/y coordinates.

The next improvement step would be Djikstras shortest path algorithm . And there are plenty of examples how this works if you search around a bit. This works in a similar way, but uses a priority queue (usually a min heap) for the unvisited cells, this allows for different 'cost' to be specified.

The next step should probably be A* , this is an 'optimal' algorithm for pathfinding, but if you only have 100x100 nodes I would not expect a huge difference. This uses a heuristic to guess what nodes to visit first, so the priority queue does not just use the cost to traverse to the node, but also the estimated remaining cost.

As always when it comes to performance you should measure and profile your code to find bottlenecks.

When to pathfind would be a more complicated issue. A fairly simple and probably effective solution would be to create a path once, and just follow it. If a node has been blocked you can redo the path finding then. You could also redo pathfinding every n seconds in case a better path has appeared. If you have multiple units you might introduce additional requirements like "pushing" blocking units out of the way, trying to keep groups of units in a compact group as they navigate tight passages etc. You can probably spend years taking every possible feature in consideration.