AStar Pathfinding для агентов различного размера с использованием пространственного хэширования

Наверное, большинству людей, связанных с программированием игр, известен алгоритм AStar.

В интернете можно найти много примеров объяснения того, как он работает, и реализации для различных языков, когда размер (далее радиус) агента, которого необходимо перемещать по импровизированной карте, известен заранее и не меняется.

Но когда речь заходит о поддержке агентов, обладающих разным радиусом, увы, информации не так много.

Данный пробел я постараюсь восполнить в рамках этой статьи.

Что нам для этого понадобится ?

1. Предполагается, что читатель уже знаком с AStar. Если нет, то вот ссылка: https://habr.com/ru/articles/331192/

2. Базовое понимание пространственного хэширования (Spatial Partitioning): https://habr.com/ru/articles/182998/

3. Понимание синтаксиса языка Kotlin, поскольку примеры кода будут на нем. В действительности же это может быть реализовано на любом языке/технологии, например на Unity и С#.

Для тестов будет использоваться homemade движок, написанный на Kotlin и OpenGL: https://github.com/shirokovnv/serenity

Запускаемый файл для Windows можно скачать с Google Drive: https://drive.google.com/file/d/10bZyg9zdBMwnV_DfUdUnbCBh06gZMnjM/view?usp=drive_link

Определение агента:

interface NavMeshAgent {     var radius: Float     var stepSize: Float      fun getHeuristic(): PathHeuristicInterface }

radius: это радиус ограничивающей сферы для нашего агента

stepSize — размер «шага» для агента.

getHeuristic() — функция, возвращающая эвристику для AStar.

interface PathHeuristicInterface {     fun calculateDistanceCost(a: PathNode, b: PathNode): Float }

class PathNode (val point: Vector3): Comparable<PathNode> {     var gCost: Float = Float.MAX_VALUE     var hCost: Float = 0f     var fCost: Float = 0f     var isWalkable: Boolean = true     var prevNode: PathNode? = null      fun calculateFCost() {         fCost = gCost + hCost     }      override fun compareTo(other: PathNode): Int {         var compare = fCost.compareTo(other.fCost)         if (compare == 0) {             compare = hCost.compareTo(other.hCost)         }          return compare     } }

PathNode содержит данные, необходимые для расчетов AStar, и функцию сравнения с таким же узлом пути по fCost и hCost.

Далее, нам необходимо отобразить объекты сцены, по которой агент будет перемещаться, на пространственную структуру данных.

Интерфейс и реализация этой структуры представлены ниже.

interface SpatialPartitioningInterface {     fun insert(obj: Object): Boolean     fun remove(obj: Object): Boolean     fun countObjects(): Int     fun buildSearchResults(searchVolume: BoxAABB): List<Object> }

class NavGrid(private val bounds: Rect2d, private val dimensions: Vector2) : SpatialPartitioningInterface {     private val cells: HashMap<String, HashSet<Object>> = HashMap()     private val objectIndexCache: HashMap<Object, Array<Vector2>> = HashMap()      override fun insert(obj: Object): Boolean {         val objRect = obj.bounds()          val i1 = getCellIndex(objRect.min)         val i2 = getCellIndex(objRect.max)          var isAdded = true         for (xi in i1.x.toInt()..i2.x.toInt()) {             for (yi in i1.y.toInt()..i2.y.toInt()) {                 val k = getKey(xi, yi)                 cells.computeIfAbsent(k) { HashSet() }                 isAdded = isAdded and (cells[k]?.add(obj) ?: false)             }         }          objectIndexCache[obj] = arrayOf(i1, i2)          return isAdded     }      override fun remove(obj: Object): Boolean {         val objectIndices = objectIndexCache.remove(obj) ?: return false         val (i1, i2) = objectIndices          var isRemoved = true         for (xi in i1.x.toInt()..i2.x.toInt()) {             for (yi in i1.y.toInt()..i2.y.toInt()) {                 val k = getKey(xi, yi)                 cells.computeIfAbsent(k) { HashSet() }                 isRemoved = isRemoved and (cells[k]?.remove(obj) ?: false)             }         }          return isRemoved     }      override fun countObjects(): Int {         return objectIndexCache.keys.size     }      override fun buildSearchResults(searchVolume: BoxAABB): List<Object> {         val searchRect = searchVolume.toRect2d()          val i1 = getCellIndex(searchRect.min)         val i2 = getCellIndex(searchRect.max)          val objects = HashSet<Object>()          for (xi in i1.x.toInt()..i2.x.toInt()) {             for (yi in i1.y.toInt()..i2.y.toInt()) {                 val k = getKey(xi, yi)                 cells.computeIfAbsent(k) { HashSet() }                 objects.addAll(cells[k]?.filter { obj ->                     IntersectionDetector.intersects(obj.bounds(), searchRect)                 } ?: emptySet())             }         }          return objects.toList()     }      fun getCellIndices(objRect: Rect2d): IntArray {         val i1 = getCellIndex(objRect.min)         val i2 = getCellIndex(objRect.max)          return arrayOf(             i1.x.toInt(),             i1.y.toInt(),             i2.x.toInt(),             i2.y.toInt()         ).toIntArray()     }      private fun getCellIndex(position: Vector2): Vector2 {         val unionX = ((position.x - bounds.min.x) / (bounds.max.x - bounds.min.x)).saturate()         val unionY = ((position.y - bounds.min.y) / (bounds.max.y - bounds.min.y)).saturate()          val xIndex = floor(unionX * (dimensions.x - 1f))         val yIndex = floor(unionY * (dimensions.y - 1f))          return Vector2(xIndex, yIndex)     }      private fun getKey(xIndex: Int, yIndex: Int): String {         return "$xIndex.$yIndex"     } }

Структура принимает в качестве входных параметров bounds — ограничивающий объем всех объектов сцены (мира), dimensions — размерность сетки.

Пример:

val bounds = Rect2d(Vector2(0f, 0f), Vector2(100f, 100f)) val dimensions = Vector2(25f, 25f) // размерность задается в целых числах val navGrid = NavGrid(bounds, dimensions) // Создана сетка, покрывающая мировой квадрант (0,0) -> (100, 100)  // с размером ячейки, равным 100/25 = 4  // Получение списка всех препятствий и добавление в NavGrid val obstacles = getObstacles() obstacles.forEach { obj -> navGrid.insert(obj) }

Следует также отметить, что размерность сетки нужно задавать таким образом, чтобы размер (длина и ширина) одной ячейки был меньше или равен stepSize агента.

Для чего — будет раскрыто чуть позже.

Итак, каждому элементу игрового мира, который мы хотим использовать как препятствие, ставится в соответствие 1 или несколько ячеек нашей сетки.

Следующий шаг, использовать пространственную структуру в навигации с помощью AStar.

Начнем также с определения интерфейса:

interface NavigatorInterface {     fun calculatePath(start: Vector3, finish: Vector3, agent: NavMeshAgent): Path     fun evaluatePoint(point: Vector3, agent: NavMeshAgent): Boolean     fun outOfBounds(point: Vector3, agent: NavMeshAgent): Boolean }

И реализации:

class Navigator(     private val worldBounds: Rect2d,     private val grid: NavGrid ) : NavigatorInterface {      companion object {         private const val MAX_PATH_LENGTH = 4000         private const val MAX_CLOSED_NODES_LENGTH = 20000     }      override fun calculatePath(start: Vector3, finish: Vector3, agent: NavMeshAgent): Path {         if (outOfBounds(start, agent)) {             return Path(emptyList(), PathStatus.NOT_EXISTS)         }          if (outOfBounds(finish, agent)) {             return Path(emptyList(), PathStatus.NOT_EXISTS)         }          val nodes = mutableMapOf<PathNodeIndex, PathNode>()          val startNode = evaluateNode(Vector3(start.x, 0f, start.z), agent, nodes)         val endNode = evaluateNode(Vector3(finish.x, 0f, finish.z), agent, nodes)          if (startNode == null || endNode == null) {             return Path(emptyList(), PathStatus.NOT_EXISTS)         }          if (!startNode.isWalkable || !endNode.isWalkable) {             return Path(emptyList(), PathStatus.NOT_EXISTS)         }          val openedQueue = PriorityQueue<PathNode>()         openedQueue.add(startNode)         val closedSet = mutableSetOf<PathNode>()          startNode.gCost = 0f         startNode.hCost = agent.getHeuristic().calculateDistanceCost(startNode, endNode)         startNode.calculateFCost()          while (openedQueue.isNotEmpty()) {             val currentNode = openedQueue.remove()             closedSet.add(currentNode)              if (currentNode == endNode) {                 return buildPath(endNode)             }              if (isNodesNear(currentNode, endNode, agent.stepSize)) {                 endNode.prevNode = currentNode                 return buildPath(endNode)             }              if (closedSet.size >= MAX_CLOSED_NODES_LENGTH) {                 return Path(emptyList(), PathStatus.TOO_LONG)             }              for (neighbor in getNeighbours(currentNode, agent, nodes)) {                 if (closedSet.contains(neighbor)) continue                  if (!neighbor.isWalkable) {                     closedSet.add(neighbor)                     continue                 }                  val tentativeGCost =                     currentNode.gCost + agent.getHeuristic().calculateDistanceCost(currentNode, neighbor)                  if (tentativeGCost < neighbor.gCost) {                     neighbor.prevNode = currentNode                     neighbor.gCost = tentativeGCost                     neighbor.hCost = agent.getHeuristic().calculateDistanceCost(neighbor, endNode)                     neighbor.calculateFCost()                      if (!openedQueue.contains(neighbor)) {                         openedQueue.add(neighbor)                     } else {                         openedQueue.remove(neighbor)                         openedQueue.add(neighbor)                     }                 }             }         }          return Path(emptyList(), PathStatus.NOT_EXISTS)     }      override fun evaluatePoint(point: Vector3, agent: NavMeshAgent): Boolean {         if (outOfBounds(point, agent)) {             return false         }          val bounds = BoxAABB(Rect3d(point - agent.radius, point + agent.radius))         val searchResults = grid.buildSearchResults(bounds)          return ensureSearchResultsContainAtMostSelf(agent, searchResults)     }      private fun evaluateNode(         point: Vector3,         agent: NavMeshAgent,         existingNodes: MutableMap<PathNodeIndex, PathNode>     ): PathNode? {         val bounds = BoxAABB(Rect3d(point - agent.radius, point + agent.radius))          if (outOfBounds(point, agent)) {             return null         }          // Check node in cache         val indices = grid.getCellIndices(bounds.toRect2d())         val pathNodeIndex = PathNodeIndex(indices)         if (existingNodes.containsKey(pathNodeIndex)) {             return existingNodes[pathNodeIndex]         }          // Create a new one         val pathNode = PathNode(point)         pathNode.isWalkable = evaluatePoint(point, agent)         existingNodes[pathNodeIndex] = pathNode          return pathNode     }      override fun outOfBounds(point: Vector3, agent: NavMeshAgent): Boolean {         val minPoint = point - agent.radius         val maxPoint = point + agent.radius         val bounds = Rect2d(             Vector2(minPoint.x, minPoint.z),             Vector2(maxPoint.x, maxPoint.z)         )          return !OverlapDetector.contains(worldBounds, bounds)     }      private fun isNodesNear(nodeA: PathNode, nodeB: PathNode, stepSize: Float): Boolean {         return (distanceSquared(nodeA.point, nodeB.point) <= stepSize * stepSize)     }      private fun ensureSearchResultsContainAtMostSelf(         agent: NavMeshAgent,         searchResults: List<Object>     ): Boolean {         return searchResults.isEmpty() || (searchResults.size == 1 && searchResults.first() == agent.objectRef)     }      private fun getNeighbours(         currentNode: PathNode,         agent: NavMeshAgent,         existingNodes: MutableMap<PathNodeIndex, PathNode>     ): List<PathNode> {         val neighbours = mutableListOf<PathNode?>()         val p = currentNode.point         val stepSize = agent.stepSize          val p0 = Vector3(p.x - stepSize, 0f, p.z - stepSize)         val p1 = Vector3(p.x - stepSize, 0f, p.z)         val p2 = Vector3(p.x, 0f, p.z - stepSize)         val p3 = Vector3(p.x + stepSize, 0f, p.z)         val p4 = Vector3(p.x, 0f, p.z + stepSize)         val p5 = Vector3(p.x - stepSize, 0f, p.z + stepSize)         val p6 = Vector3(p.x + stepSize, 0f, p.z - stepSize)         val p7 = Vector3(p.x + stepSize, 0f, p.z + stepSize)          val n0 = evaluateNode(p0, agent, existingNodes)         val n1 = evaluateNode(p1, agent, existingNodes)         val n2 = evaluateNode(p2, agent, existingNodes)         val n3 = evaluateNode(p3, agent, existingNodes)         val n4 = evaluateNode(p4, agent, existingNodes)         val n5 = evaluateNode(p5, agent, existingNodes)         val n6 = evaluateNode(p6, agent, existingNodes)         val n7 = evaluateNode(p7, agent, existingNodes)          neighbours.addAll(listOf(n0, n1, n2, n3, n4, n5, n6, n7))          return neighbours.filterNotNull().filter { it.isWalkable }     }      private fun buildPath(endNode: PathNode): Path {         val path = mutableListOf(endNode)         var currentNode: PathNode? = endNode          while (currentNode?.prevNode != null) {             path.add(currentNode.prevNode!!)              if (path.size > MAX_PATH_LENGTH) {                 return Path(reversePath(path), PathStatus.TOO_LONG)             }              currentNode = currentNode.prevNode         }          return Path(reversePath(path), PathStatus.OK)     }      private fun reversePath(path: MutableList<PathNode>): List<PathNode> {         path.reverse()         return path     } }

Класс для пути выглядит так:

data class Path(val nodes: List<PathNode>, val status: PathStatus)  enum class PathStatus {     OK,     NOT_EXISTS,     TOO_LONG, }

Еще нам понадобится индекс пути:

data class PathNodeIndex(val indices: IntArray) {     override fun equals(other: Any?): Boolean {         if (this === other) return true         if (javaClass != other?.javaClass) return false          return indices.contentEquals((other as PathNodeIndex).indices)     }      override fun hashCode(): Int {         return indices.contentHashCode()     } }

Индекс — это min, max индекс квадранта, спроецированного на сетку NavGrid.

Давайте подробнее остановимся на нескольких моментах:

Первое — это ограничения:

MAX_PATH_LENGTH — максимальная длина пути в узлах. Поскольку наш мир может быть сколь угодно больших размеров, итерироваться по мелкой сетке до бесконечности мы не хотим.

MAX_CLOSED_NODES_LENGTH — максимальное количество узлов в структуре ClosedSet (список посещенных узлов AStar). Идея состоит в том, что иногда наш мир поделен на участки, которые либо не связаны друг с другом напрямую, либо расположены слишком далеко. В этом случае AStar будет перебирать узлы, пока там не окажутся все узлы из данной компоненты связности (локального участка мира), что тоже не всегда хорошо, если участок очень большой.

Если в ходе работы алгоритма данные лимиты будут превышены, то вернется пустой путь со статусом TOO_LONG (слишком длинный).

Далее — функциональность самого класса Navigator:

outOfBounds(…), как следует из названия, определяет, находится ли текущая точка (а точнее квадрант) за ограничивающим мировым прямоугольником или нет.

evaluatePoint(…) — проверяет, находится ли квадрант в пределах мира и есть ли внутри него какие-либо объекты, которые могут препятствовать движению.

Ну и calculatePath(…) — функция, выполняющая основную работу по поиску пути.

Процесс протекает следующим образом:

1. На каждом шаге AStar мы вычисляем смещение исходного квадранта (в центре которого находится агент, а размеры равны радиусу агента) на значение, равное stepSize (длине шага агента) во всех направлениях.

   И производим проверку смещенных квадрантов на наличие препятствий.

   Если препятствие найдено, узел помечается как unwalkable (непроходимый). Если нет, то walkable (проходимый).

   Здесь нам как раз и пригодится условие, что размер ячейки нашей сетки должен быть меньше или равен длине шага. Если это не так и размер ячейки слишком большой, то мы рискуем никогда не выбраться из нее.

   Опять же, слишком маленьким его тоже делать не стоит, поскольку это увеличит общее количество узлов сетки и время расчетов.

   Визуально процесс инициализации соседних узлов можно изобразить так:

2. Далее, считается AStar с несколькими отличиями от классической реализации.

   Во-первых, узлы выделяются динамически. Для этого нам и нужен PathNodeIndex.

   Для каждого расчета пути calculatePath, мы запоминаем найденные узлы в кэш по индексам сетки NavGrid. После вычислений кэш обнуляется.

   Во вторых, конечный узел мы ищем в некоторой окрестности, которая равна stepSize, а не всегда по точному совпадению. Это нужно, поскольку мы проецируем в общем случае не дискретный мир на дискретную сетку.

3. Возвращаем искомый путь, если он существует.

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

Для расчетов используется следующая эвристика:

class DiagonalDistanceHeuristic: PathHeuristicInterface {     companion object {         private const val MOVE_STRAIGHT_COST = 10f         private const val MOVE_DIAGONAL_COST = 14f     }      override fun calculateDistanceCost(a: PathNode, b: PathNode): Float {         val xDistance = abs(a.point.x - b.point.x)         val zDistance = abs(a.point.z - b.point.z)         val remaining = abs(xDistance - zDistance)         return MOVE_DIAGONAL_COST * min(xDistance, zDistance) + MOVE_STRAIGHT_COST * remaining     } }

Время подвести некоторые итоги:

Данный алгоритм позволяет рассчитывать пути для агентов с различным радиусом с помощью AStar.

Используется дискретная сетка, на которую проецируются объекты реального мира.

Размер ячейки сетки необходимо выбирать <= stepSize агента.

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

Движок, в котором все тестировалось, является экспериментальным и не используется в коммерческих или некоммерческих игровых проектах на момент написания статьи.

Ну а на этом все, надеюсь, что изложенный материал был полезен.

Спасибо за внимание.