Наверное, большинству людей, связанных с программированием игр, известен алгоритм 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
}
}



Время подвести некоторые итоги:
Данный алгоритм позволяет рассчитывать пути для агентов с различным радиусом с помощью AStar.
Используется дискретная сетка, на которую проецируются объекты реального мира.
Размер ячейки сетки необходимо выбирать <= stepSize агента.
Алгоритм создает узлы, необходимые для анализа AStar на лету, что может потребовать дополнительной оптимизации при большом количестве запросов.
Движок, в котором все тестировалось, является экспериментальным и не используется в коммерческих или некоммерческих игровых проектах на момент написания статьи.
Ну а на этом все, надеюсь, что изложенный материал был полезен.
Спасибо за внимание.
Автор: durden_tailer