A*算法实现

樂雾1

已于 2024-08-24 17:09:37 修改

阅读量163

点赞数 2

文章标签：算法

于 2024-08-24 09:12:08 首次发布

本文链接：https://blog.csdn.net/weixin_43625124/article/details/141489052

版权

A*算法（kotlin）

原文地址：https://blog.csdn.net/Zhouzi_heng/article/details/115035298

基于kotlin代码简单实现：

// An highlighted block
data class Mode(
    val x: Int,
    val y: Int,
    var g: Int = Int.MAX_VALUE,
    var h: Int = 0,
    var parent: Mode? = null
) {
    val f: Int get() = g + h
}

fun aStar(grid: Array<Array<Int>>, start: Mode, end: Mode): List<Mode>? {
    val openList = PriorityQueue<Mode>(compareBy<Mode?> { it!!.f }.thenComparingInt { it!!.g })
    val closeList = mutableSetOf<Mode>()

    start.g = 0
    start.h = heuristic(start, end)
    openList.add(start)

    while (openList.isNotEmpty()) {
        val currentNode = openList.poll()

        if (currentNode.x == end.x && currentNode.y == end.y) {
            return buildPath(currentNode)
        }

        val neighbors = getNeighbors(currentNode, end, grid)
        closeList.add(currentNode)

        for (neighbor in neighbors) {
            if (grid[neighbor.x][neighbor.y] == 1 || closeList.contains(neighbor)) {          //不可抵达点
                continue
            }

            val tentativeG =
                currentNode.g + (if (neighbor.x != currentNode.x && neighbor.y != currentNode.y) 14 else 10)

            if (!openList.contains(neighbor)) {
                neighbor.parent = currentNode
                neighbor.g = tentativeG
                neighbor.h = heuristic(neighbor, end)
                openList.add(neighbor)
            } else if (tentativeG < neighbor.g) {
                // 通过当前节点到达这个邻居节点的路径更短
                neighbor.parent = currentNode
                neighbor.g = tentativeG
                // openList已经包含该节点，无需重复添加
            }
        }
    }
    return null
}

fun heuristic(node: Mode, end: Mode): Int {
    // 使用曼哈顿距离作为启发式函数
    return Math.abs(node.x - end.x) + Math.abs(node.y - end.y)
}

fun getNeighbors(node: Mode, end: Mode, grid: Array<Array<Int>>): List<Mode> {
    val neighbors = mutableListOf<Mode>()
    val directions = listOf(
        Pair(0, 1),   // 上
        Pair(1, 0),   // 右
        Pair(0, -1),  // 下
        Pair(-1, 0),  // 左
        Pair(-1, -1), // 左上
        Pair(-1, 1),  // 右上
        Pair(1, -1),  // 左下
        Pair(1, 1)    // 右下
    )

    for ((first, second) in directions) {
        val newX = node.x + first
        val newY = node.y + second

        if (newX in grid.indices && newY in grid[0].indices && grid[newX][newY] == 0) {
            val neighbor = Mode(newX, newY)
            neighbor.h = heuristic(neighbor, end)
            neighbors.add(neighbor)
        }
    }

    return neighbors
}

fun buildPath(endNode: Mode): List<Mode> {
    val path = mutableListOf<Mode>()
    var currentNode: Mode? = endNode

    while (currentNode != null) {
        path.add(currentNode)
        currentNode = currentNode.parent
    }

    return path.reversed()
}

fun main() {
    
    val grid = arrayOf(
        arrayOf(0, 0, 0, 0, 0, 0, 0, 0),
        arrayOf(0, 0, 0, 0, 1, 0, 0, 0),
        arrayOf(0, 0, 0, 0, 1, 0, 0, 0),
        arrayOf(0, 0, 0, 0, 1, 0, 0, 0),
        arrayOf(0, 0, 0, 0, 0, 0, 0, 0),
        arrayOf(0, 0, 0, 0, 0, 0, 0, 0)
    )

    val start = Mode(2, 2)
    val end = Mode(2, 6)

    val t = aStar(grid, start, end)

    if (t!!.isNotEmpty()) {
        println("路径找到：")
        for (node in t) {
            println("(${node.x}, ${node.y})")
        }
    } else {
        println("未找到路径")
    }
}