Algorithm for finding the path A * in a voxel 3d game on Unity

Introduction


When developing my game, I got to the point of creation of the first NPCs. And the question was how to make the NPC go around the wall and not “go into it”.


Climbing on the Internet, I found these algorithms:


  • Search Wide (BFS, Breadth-First Search)
  • Dijkstra algorithm (Dijkstra)
  • And Star "A with an asterisk"
  • Search for the first best match (Best-First Search)
  • IDA (A with iterative deepening)
  • Jump Point Search

And I decided to try to implement my A * on the voxel 3d grid.



Sample card of my game

image


Algorithm Description


A * step through all the paths leading from the initial vertex to the final, until it finds the minimum. Like all informed search algorithms, he first looks at those routes that “seem” to lead to the goal. From the greedy algorithm, which is also the search algorithm for the first best match, it is distinguished by the fact that when choosing a vertex it takes into account, among other things, the entire path traveled to it.


Visualization of the work of A * from Wikipedia



Implementation


Since the algorithm needs "nodes" - points to determine the path, we write the class structure of the node:


Node code:
publicenum EMoveAction { walk, jump, fall, swim };
    publicclassPathPoint
    {// текущая точкаpublic Vector3 point { get; set; }
        // расстояние от стартаpublicfloat pathLenghtFromStart { get; set; }
        // примерное расстояние до целиpublicfloat heuristicEstimatePathLenght { get; set; }
        // еврестическое расстояние до целиpublicfloat estimateFullPathLenght
        {
            get
            {
               returnthis.heuristicEstimatePathLenght + this.pathLenghtFromStart;
            }
        }
        // способ движенияpublic EMoveAction moveAction = EMoveAction.walk;
        // точка из которой пришли сюдаpublic PathPoint cameFrom;
    }

Small class constructors:
private PathPoint NewPathPoint(Vector3 point, float pathLenghtFromStart, float heuristicEstimatePathLenght, EMoveAction moveAction){
        PathPoint a = new PathPoint();
        a.point = point;
        a.pathLenghtFromStart = pathLenghtFromStart;
        a.heuristicEstimatePathLenght = heuristicEstimatePathLenght;
        a.moveAction = moveAction;
        return a;
    }
    private PathPoint NewPathPoint(Vector3 point, float pathLenghtFromStart, float heuristicEstimatePathLenght, EMoveAction moveAction, PathPoint ppoint){
        PathPoint a = new PathPoint();
        a.point = point;
        a.pathLenghtFromStart = pathLenghtFromStart;
        a.heuristicEstimatePathLenght = heuristicEstimatePathLenght;
        a.moveAction = moveAction;
        a.cameFrom = ppoint;
        return a;
    }

Next, we need the structure of the path search settings:


Path Search Settings Code:
publicstructSPathFinderType
    {// Возможность персонажа ходить, прыгать, падать, и плаватьpublicbool walk, jump, fall, swim;
        // Максимальная высота падения, прыжкаpublicint maxFallDistance, jumpHeight, jumpDistance;
        // Высота персонажаpublicint characterHeight;
        // Создаём стандартные настройкиpublicstatic SPathFinderType normal(){
            SPathFinderType n = new SPathFinderType();
            n.walk = true;
            n.jump = true;
            n.fall = true;
            n.swim = false;
            n.maxFallDistance = 1;
            n.jumpHeight = 1;
            n.jumpDistance = 0;
            n.characterHeight = 1;
            return n;
        }
    }

Further, "World" is a class of a unique database for storing information about map blocks. You may have implemented differently.


The result of the search path is the function of obtaining the route:
public List<PathPoint> GetPathToTarget(Vector3 beginPoint, Vector3 targetPoint, World worldData, SPathFinderType pfType)
    {
        List<PathPoint> path = new List<PathPoint>();
        // Список не проверенных нодов
        List<PathPoint> openPoints = new List<PathPoint>();
        // Список проверенных нодов
        List<PathPoint> closedPoints = new List<PathPoint>();
        // Добавляем к открытым начальную точку
        openPoints.Add(NewPathPoint(beginPoint, 0, GameLogic.Distance(beginPoint, targetPoint), EMoveAction.walk));
        // закрываем точку
        closedPoints.Add(openPoints[0]);
        // открываем новые точки и удаляем закрытую
        openPoints = ClosePoint(0, openPoints, closedPoints, worldData, pfType, targetPoint);
        // "Стоп сигнал" для нашего поискаbool stopFlag = true;
        // Устанавливаем ограничители чтобы избежать проверки всей картыfloat maxEstimatePath = 1500;
        int maxNodes = 6000;
        while (stopFlag)
        {
            // Находим индекс точки с минимальным евристическим расстояниемint minIndex = GetMinEstimate(openPoints);
            if (openPoints.Count > 0)
            if (openPoints[minIndex].estimateFullPathLenght < maxEstimatePath)
            {
                // закрываем точку
                closedPoints.Add(openPoints[minIndex]);
                // добавляем новые если есть и удаляем minIndex
                openPoints = ClosePoint(minIndex, openPoints, closedPoints, worldData, pfType, targetPoint);
            }
            else
            {
                // если расстояние достигло пределов поиска// просто закрываем точку без добавления новых
                closedPoints.Add(openPoints[minIndex]);
                openPoints.RemoveAt(minIndex);
            }
            // Функция проверяет найден ли финишif (FinishFounded(closedPoints))
            {
                Debug.Log("Финиш найден!");
                path = GetPathToTarget(closedPoints);
                stopFlag = false; // остановка цикла если найдена цель
            }
            if (openPoints.Count <= 0)
                stopFlag = false; // остановка цикла если открытых точек нетif ((openPoints.Count>= maxNodes) ||(closedPoints.Count>= maxNodes))
                stopFlag = false; // остановка цикла слишком много нодов
        }
        Debug.Log("Nodes created "+ closedPoints.Count.ToString());
        // Рисуем полученные пути
        DrawPath(openPoints, Color.green, 6f);
        DrawPath(closedPoints, Color.blue, 6f);
        DrawPath(path, Color.red, 6f);
        return path;
    }

GetMinEstimate
// находит индекс точки ближайшей к точке назначенияprivateintGetMinEstimate(List<PathPoint> points){
        int min = 0;
        for (int i = 0; i < points.Count; i++)
        {
            if (points[i].estimateFullPathLenght < points[min].estimateFullPathLenght)
                min = i;
        }
        return min;
    }

Drawpath
// Рисует линии из точек путиpublicvoidDrawPath(List<PathPoint> points, Color c, float time){
        for (int i = 0; i < points.Count; i++)
        {
            if (points[i].cameFrom != null)
                Debug.DrawLine(points[i].point, points[i].cameFrom.point, c, time);
        }
    }

FinishFounded
// проверяет найдена ли цельprivateboolFinishFounded(List<PathPoint> points){
        for (int i = 0; i < points.Count; i++)
        {
            if (points[i].heuristicEstimatePathLenght <= 0)
                returntrue;
        }
        returnfalse;
    }

GetPathToTarget
// Возвращает список точек от старта до финишаprivate List<PathPoint> GetPathToTarget(List<PathPoint> points)
    {
        List<PathPoint> path = new List<PathPoint>();
        int targetIndex = 0;
        for (int i = 0; i < points.Count; i++)
        {
            if (points[i].heuristicEstimatePathLenght <= 0)
                targetIndex = i;
        }
        PathPoint ppoint = new PathPoint();
        ppoint = points[targetIndex];
        while (ppoint.pathLenghtFromStart > 0)
        {
            path.Add(ppoint);
            ppoint = ppoint.cameFrom;
        }
        path.Reverse();
        return path;
    }

Closepoint


The ClosePoint function depends entirely on the implementation of the World class, it adds all possible paths from it to the list of open points and removes the current point from this list (closes it). I will give an example of my "closing point" in the first four directions.


Attention big code jumble
private List<PathPoint> ClosePoint(int index, List<PathPoint> openPoints, List<PathPoint> closedPoints, World worldData, SPathFinderType pfType, Vector3 targetPoint)
    {
        List<PathPoint> newOpenPoints = openPoints;
        PathPoint lastPoint = openPoints[index];
        // если персонаж может идтиif (pfType.walk)
            // если персонаж может стоять на текущем блокеif (CanStand(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z), pfType.characterHeight, worldData))
        {
            // ---------------------------------------------------------------//north//   /|\//    |// если не в списке закрытыхif (!InList(closedPoints, new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z)))
                // и уже не добавленаif (!InList(newOpenPoints, new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z)))
                    // если может там стоять                    if (CanStand(new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z), pfType.characterHeight, worldData))
                    {
                        newOpenPoints.Add(NewPathPoint(new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z)
                        , lastPoint.pathLenghtFromStart + GetTravelCost(new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z), worldData, pfType.characterHeight)
                        , GameLogic.Distance(new Vector3(lastPoint.point.x + 1, lastPoint.point.y, lastPoint.point.z), targetPoint)
                        , EMoveAction.walk
                        , lastPoint));
                    }
            // south//    |//   \|/// если не в списке закрытыхif (!InList(closedPoints, new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z)))
                // и уже не добавленаif (!InList(newOpenPoints, new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z)))
                    // если может там стоять                    if (CanStand(new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z), pfType.characterHeight, worldData))
                    {
                        newOpenPoints.Add(NewPathPoint(new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z)
                        , lastPoint.pathLenghtFromStart + GetTravelCost(new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z), worldData, pfType.characterHeight)
                        , GameLogic.Distance(new Vector3(lastPoint.point.x - 1, lastPoint.point.y, lastPoint.point.z), targetPoint)
                        , EMoveAction.walk
                        , lastPoint));
                    }
            // east//    ---->//   // если не в списке закрытыхif (!InList(closedPoints, new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1)))
                // и уже не добавленаif (!InList(newOpenPoints, new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1)))
                    // если может там стоять                    if (CanStand(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1), pfType.characterHeight, worldData))
                    {
                        newOpenPoints.Add(NewPathPoint(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1)
                        , lastPoint.pathLenghtFromStart + GetTravelCost(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1), worldData, pfType.characterHeight)
                        , GameLogic.Distance(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z + 1), targetPoint)
                        , EMoveAction.walk
                        , lastPoint));
                    }
            // west//    <----//   // если не в списке закрытыхif (!InList(closedPoints, new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1)))
                // и уже не добавленаif (!InList(newOpenPoints, new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1)))
                    //если может стоять тамif (CanStand(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1), pfType.characterHeight, worldData))
                    {
                        newOpenPoints.Add(NewPathPoint(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1)
                        , lastPoint.pathLenghtFromStart + GetTravelCost(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1), worldData, pfType.characterHeight)
                        , GameLogic.Distance(new Vector3(lastPoint.point.x, lastPoint.point.y, lastPoint.point.z - 1), targetPoint)
                        , EMoveAction.walk
                        , lastPoint));
                    }
        }
        newOpenPoints.RemoveAt(index);
        return newOpenPoints;
}

Optimization


By simply dividing the path from the start to the current point, we reduce the number of nodes many times and make it more "greedy."


returnthis.heuristicEstimatePathLenght + this.pathLenghtFromStart /2;

Results


Pros:


  • Quick search in open spaces.
  • Universality of the algorithm

Minuses:


  • It requires a lot of memory to calculate the route.

Green is the open list of nodes, red is the way to the target, blue is closed nodes.


Received routes to optimization:



Received routes after optimization:



Literature


https://tproger.ru/articles/pathfindings/
https://ru.wikipedia.org/wiki/A*

Tags:

Also popular now: