-
Notifications
You must be signed in to change notification settings - Fork 0
/
AStarPathFinder.kt
163 lines (156 loc) · 6.71 KB
/
AStarPathFinder.kt
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
package cn.asone.endless.utils.pathfinding
import cn.asone.endless.utils.BlockUtils
import cn.asone.endless.utils.extensions.getBlock
import net.minecraft.block.BlockLiquid
import net.minecraft.block.BlockSlab
import net.minecraft.util.BlockPos
import net.minecraft.util.Vec3
import net.minecraft.util.Vec3i
import java.util.*
import kotlin.math.ceil
import kotlin.math.floor
import kotlin.math.sqrt
class AStarPathFinder(val startPos: BlockPos, val endPos: BlockPos, val maxDistance: Double) {
constructor(startPos: Vec3, endPos: Vec3, maxDistance: Double) : this(
BlockPos(
floor(startPos.xCoord),
ceil(startPos.yCoord) /* 此处向上取整的原因是我们假设玩家脚下的方块永远是完整的, 以方便之后的处理 */,
floor(startPos.zCoord)
),
BlockPos(floor(endPos.xCoord), ceil(endPos.yCoord), floor(endPos.zCoord)),
maxDistance
)
private val comparator: Comparator<Node> = Comparator { o1: Node, o2: Node ->
val d1 = o1.getHeuristicCost(endPos)
val d2 = o2.getHeuristicCost(endPos)
if (d1 == d2)
return@Comparator 0
if (d1 > d2)
return@Comparator 1
return@Comparator -1
}
private val queue = PriorityQueue(comparator)
private val visited = hashSetOf<BlockPos>()
val nodes = arrayListOf<Node>()
private val directions = arrayOf(
Vec3i(1, 0, 0),
Vec3i(1, 0, 1),
Vec3i(0, 0, 1),
Vec3i(-1, 0, 1),
Vec3i(-1, 0, 0),
Vec3i(-1, 0, -0),
Vec3i(0, 0, -1),
Vec3i(1, 0, -1)
)
var avoidLiquid = true
var maxFallDistance = 1
fun calculatePath(): ArrayList<BlockPos> {
nodes.clear()
queue.add(Node(startPos, 0.0, null))
visited.add(startPos)
while (queue.isNotEmpty()) {
val currentNode = queue.poll()
nodes.add(currentNode)
if (currentNode.pos == endPos) {
break
}
if (currentNode.pos.distanceSq(startPos) > maxDistance * maxDistance) {
continue
}
// 往周围八个方向探索
for ((index, it) in directions.withIndex()) {
var targetPos = currentNode.pos.add(it)
if (index and 1 == 1) { // 斜着走
// 只有两边没有阻挡才有斜着走的必要
if (!checkPositionValidity(currentNode.pos.add(it.x, 0, 0), false)
|| !checkPositionValidity(currentNode.pos.add(0, 0, it.z), false)
) {
continue
}
}
if (!checkPositionValidity(targetPos, true)) {
// 能否高一格
targetPos = targetPos.add(0, 1, 0)
if (!checkPositionValidity(targetPos, true)) {
targetPos = targetPos.add(0, -1, 0)
var flag = true
// 能否安全坠落
label@ for (distance in 1..maxFallDistance) {
targetPos = targetPos.add(0, -1, 0)
if (checkPositionValidity(targetPos, true)) {
// 坠落地点到上面是不是空的
for (i in 1 until distance) {
if (BlockUtils.isBlockSolid(targetPos.x, targetPos.y + i, targetPos.z))
break@label
}
flag = false
break
}
}
// 不能安全坠落
if (flag) {
continue
}
}
}
// 探索过了
if (visited.contains(targetPos)) {
continue
}
// 目标位置脚下的方块
val targetBlock = mc.theWorld.getBlock(targetPos.add(0, -1, 0))
// 当前位置脚下的方块
val currentBlock = mc.theWorld.getBlock(currentNode.pos.add(0, -1, 0))
// 计算高度差
val diffY =
targetBlock.blockBoundsMaxY - currentBlock.blockBoundsMaxY
// 能不能跳上去
if (diffY >= 1.1) {
continue
}
val targetNode =
Node(targetPos, sqrt(currentNode.pos.distanceSq(targetPos)) + currentNode.currentCost, currentNode)
queue.add(targetNode)
visited.add(targetPos)
}
}
val results = arrayListOf<BlockPos>()
// 从终点到起点生成路径
var currentNode: Node? = nodes.last()
while (currentNode != null) {
results.add(currentNode.pos)
currentNode = currentNode.cameFrom
}
results.reverse()
queue.clear()
visited.clear()
return results
}
private fun checkPositionValidity(pos: BlockPos, checkGround: Boolean): Boolean {
return checkPositionValidity(pos.x, pos.y, pos.z, checkGround)
}
private fun checkPositionValidity(x: Int, y: Int, z: Int, checkGround: Boolean): Boolean {
val block1 = mc.theWorld.getBlock(x, y, z)
val block2 = mc.theWorld.getBlock(x, y + 1, z)
val block3 = mc.theWorld.getBlock(x, y - 1, z)
if (avoidLiquid) {
if ((block1 is BlockLiquid || block2 is BlockLiquid))
return false
}
if (block2 is BlockSlab && block3 is BlockSlab && !block2.isDouble && !block3.isDouble) {
if (mc.theWorld.getBlockState(BlockPos(x, y + 1, z)).getValue(BlockSlab.HALF) == BlockSlab.EnumBlockHalf.TOP
&& mc.theWorld.getBlockState(BlockPos(x, y - 1, z))
.getValue(BlockSlab.HALF) == BlockSlab.EnumBlockHalf.BOTTOM
)
return true
}
return !BlockUtils.isBlockSolid(block1)
&& (!BlockUtils.isBlockSolid(block2)
|| (block2.blockBoundsMinY //上部的方块
+ 1 //下部的方块
+ 1 - block3.blockBoundsMaxY)/* 脚里的方块 */ >= mc.thePlayer.height //头顶的方块和脚下的方块之间的距离够玩家通过
) //上面两格能走过去
&& (BlockUtils.isBlockSolid(block3) || !checkGround)
&& BlockUtils.isSafeToWalkOn(block3)
}
}