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

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

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

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

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

Предполагается, что читатель уже знаком с AStar. Если нет, то вот ссылка: https://habr.com/ru/articles/331192/
Базовое понимание пространственного хэширования (Spatial Partitioning): https://habr.com/ru/articles/182998/
Понимание синтаксиса языка 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(...) - функция, выполняющая основную работу по поиску пути.

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

На каждом шаге AStar мы вычисляем смещение исходного квадранта (в центре которого находится агент, а размеры равны радиусу агента) на значение, равное stepSize (длине шага агента) во всех направлениях.
И производим проверку смещенных квадрантов на наличие препятствий.
Если препятствие найдено, узел помечается как unwalkable (непроходимый). Если нет, то walkable (проходимый).
Здесь нам как раз и пригодится условие, что размер ячейки нашей сетки должен быть меньше или равен длине шага. Если это не так и размер ячейки слишком большой, то мы рискуем никогда не выбраться из нее.
Опять же, слишком маленьким его тоже делать не стоит, поскольку это увеличит общее количество узлов сетки и время расчетов.
Визуально процесс инициализации соседних узлов можно изобразить так:

Далее, считается AStar с несколькими отличиями от классической реализации.
Во-первых, узлы выделяются динамически. Для этого нам и нужен PathNodeIndex.
Для каждого расчета пути calculatePath, мы запоминаем найденные узлы в кэш по индексам сетки NavGrid. После вычислений кэш обнуляется.
Во вторых, конечный узел мы ищем в некоторой окрестности, которая равна stepSize, а не всегда по точному совпадению. Это нужно, поскольку мы проецируем в общем случае не дискретный мир на дискретную сетку.
Возвращаем искомый путь, если он существует.

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

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

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

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

То же самое изображение, с наложенным navmesh. Красным подсвечены непроходимые участки, зеленым - проходимые.

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

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

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

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

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

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

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

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

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