Алгоритм поиска пути A* в воксельной 3d игре на Unity

Введение

При разработке своей игры, я дошёл до момента создания первых NPC. И появился вопрос как заставить NPC обойти стену а не "идти в неё".

Полазив по интернету я нашёл такие алгоритмы:

• Поиск в ширину (BFS, Breadth-First Search)
• Алгоритм Дейкстры (Dijkstra)
• А Star "A со звёздочкой"
• Поиск по первому наилучшему совпадению (Best-First Search)
• IDA (A с итеративным углублением)
• Jump Point Search

И решил попробовать реализовать свой A* на воксельной 3д сетке.

Пример карты моей игры

Описание алгоритма

A* пошагово просматривает все пути, ведущие от начальной вершины в конечную, пока не найдёт минимальный. Как и все информированные алгоритмы поиска, он просматривает сначала те маршруты, которые «кажутся» ведущими к цели. От жадного алгоритма, который тоже является алгоритмом поиска по первому лучшему совпадению, его отличает то, что при выборе вершины он учитывает, помимо прочего, весь пройденный до неё путь.

Визуализация работы A* с википедии

Реализация

Так как алгоритму нужны "ноды" — точки для определения пути напишем класс структуру нода:

Код нода:

    public enum EMoveAction { walk, jump, fall, swim };

    public class PathPoint
    {
        // текущая точка
        public Vector3 point { get; set; }
        // расстояние от старта
        public float pathLenghtFromStart { get; set; }
        // примерное расстояние до цели
        public float heuristicEstimatePathLenght { get; set; }
        // еврестическое расстояние до цели
        public float estimateFullPathLenght
        {
            get
            {
               return this.heuristicEstimatePathLenght + this.pathLenghtFromStart;
            }
        }
        // способ движения
        public EMoveAction moveAction = EMoveAction.walk;
        // точка из которой пришли сюда
        public PathPoint cameFrom;
    }

Небольшие конструкторы класса:

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

Далее нам пригодится структура настроек поиска пути:

Код настроек поиска пути:

    public struct SPathFinderType
    {
        // Возможность персонажа ходить, прыгать, падать, и плавать
        public bool walk, jump, fall, swim;
        // Максимальная высота падения, прыжка
        public int maxFallDistance, jumpHeight, jumpDistance;
        // Высота персонажа
        public int characterHeight;

        // Создаём стандартные настройки
        public static 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;
        }
    }

Далее "World" — класс своебразная БД для хранения информации о блоках карты. У вас может быть реализован по другому.

Итог поиска пути функция получения маршрута:

    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

    // находит индекс точки ближайшей к точке назначения
    private int GetMinEstimate(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

    // Рисует линии из точек пути
    public void DrawPath(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

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

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

Функция ClosePoint зависит сугубо от реализации класса World, она добавляет в список открытых точек все возможные пути из неё и удаляёт из этого списка текущую точку (закрывает её). Приведу пример своего "закрытия точки" в первых четырёх направлениях.

Внимание большое нагромождение кода

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

Оптимизация

Простым делением пути от старта до текущей точки, мы сокращаем количество нодов во много раз и делаем его более "жадным".

return this.heuristicEstimatePathLenght + this.pathLenghtFromStart /2;

Итоги

Плюсы:

• Быстрый поиск на открытых пространствах.
• Универсальность алгоритма

Минусы:

• Требует много памяти для вычисления маршрута.

Зелёным показан открытый список нодов, красным путь до цели, синим закрытые ноды.

Полученные маршруты до оптимизации:

Полученные маршруты после оптимизации:

Литература

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