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

Введение


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


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


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

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



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

image


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


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*

Поделиться публикацией
Комментарии 17
    0
    на первой гифке не a*
      0
      Ну и как теперь определить какую гифку вы имели ввиду и не поменял ли ее с тех пор автор?
      Вот эта:
      image
      ?
      Почему не a*?
      Зависит от эвристики ©
      Но да, если бы эвристикой было бы простое эвклидово расстояние, он пошел бы вдоль стенки
        0
        Вот так как-то image
        Хотя зависит от того как подкрутить h (эвристику) и g (вес ребра)
          0
          Это не A star, путь явно не оптимальный. Либо подкручено не правильно.

          Для квадратов давно разработаны эвристики, для только вертикаль/горизонталь — Манхэттен, для плюс диагоналей — Чебышев.
          https://neerc.ifmo.ru/wiki/index.php?title=Алгоритм_A*
            0
            Где можно срезать?
            Вот здесь?image
            Так длина пути не изменится.
            1 клетка — 1 шаг. Как раз по Чебышеву.
              +1
              Если хочется минимизировать количество изломов — то нужно либо напрямую за них штраф добавлять, либо отказаться от чебышева в пользу эвклида. (чтобы шаг по диагонали был не 1, а sqrt(2))
                0
                Да не а-стар это, картинка не такая должна быть. :)

                qiao.github.io/PathFinding.js/visual — здесь эмулируете? Если нет — дайте ссылку где именно, мне прямо интересно посмотреть как A* дает такую картинку.

                A* будет давать другую, вот такую:


                То что на вашем скрине — судя по всему Trace.
                  0
                  С Weight 5 и более и Octile или Manhattan получилось несколько похожее получить — но это не правильно при диагональных переходах, Manhattan точно нельзя (Octile не знаю как работает эвристика).

                  Скажите уже параметры, не томите.
                    0
                    Что значит «неправильно»? Что вам визуально не нравится рисунок?
                    Путь кратчайший? Кратчайший.
                    Фронтир на момент достижения пути — минимизирован. (Меньше посетили клеток перед тем как нашли финиш)
                    Эвклида я взял:
                    image
                      0
                      Да, согласен, был не прав — по клеткам столько же, уже прочитал ваш коммент выше. Если брать длину желтой ломаной — то так больше получается.
                        0
                        Ну у меня была цель минимизировать количество «синего» — посещенных клеток.
                        А так, можно в эвристику затолкать много чего. Например направление на север сделать менее желанным, итп
                          0
                          Да, я знаю — у меня AI для платформера (вернее лабиринта) сча в разработке, своя реализация A*, все бегают/прыгают/лазают, просто у меня идет подсчет переходов между нодами (не клетки, так генерятся), там везде разные длины, я удивился что находит такой путь, но длина по клеткам в общем то это объясняет.
            0

            Ну она очень странно выглядит. На следующем шаге берутся смежные вершины. А тут после упора в стену анимация прыгает с одной стороны на другую. Понятно что можно извратиться и с эвристикой и с поиском соседей так что получиться нечто похожее. Но это так и к поиску в ширину можно свести

          0
          // закрываем точку
          closedPoints.Add(openPoints[0]);
          // открываем новые точки и удаляем закрытую
          openPoints = ClosePoint(0, openPoints, closedPoints, worldData, pfType, targetPoint);


          Это обычно делается в основном цикле, зачем вы вынесли это отдельно для первой точки?

          По оптимизации не очень понятно, слишком кратко написали. Можете попонятнее расписать, это самая интересная часть статьи.

          Кстате у вас много открытых пространств, Jump Point Search искал бы быстрее.
            0
            JPS будет сканировать почти всю карту если нужно обойти гору. А карта большая.
            0
            Если не учитывать вопрос особенностей реализации A* алгоритма и, соответственно, его производительность даже при поиске пути одного NPC, что во многих случаях может оказаться минусом алгоритма, то в минусы можно записать еще и игнорирование данным алгоритмом движущихся объектов и любых изменений воксельного мира после нахождения оптимального пути. Проще говоря, достаточно поставить единственный воксель на пути NPC и он застрянет и не узнает об этом, а постоянные перерасчеты A* будут стоить крайне дорого.
              0
              На открытых пространствах алгоритм считает быстро.

            Только полноправные пользователи могут оставлять комментарии. Войдите, пожалуйста.

            Самое читаемое