Skip to content

Commit 86d7874

Browse files
AvanatikerAvanatiker
committed
Theta*
1 parent 71bbfec commit 86d7874

File tree

8 files changed

+176
-71
lines changed

8 files changed

+176
-71
lines changed

‎common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt‎

Lines changed: 66 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,9 @@ import com.lambda.event.events.RenderEvent
2323
import com.lambda.event.events.RotationEvent
2424
import com.lambda.event.events.TickEvent
2525
import com.lambda.event.listener.SafeListener.Companion.listen
26+
import com.lambda.graphics.renderer.esp.builders.buildFilled
2627
import com.lambda.graphics.renderer.esp.builders.buildLine
28+
import com.lambda.graphics.renderer.esp.global.StaticESP
2729
import com.lambda.interaction.request.rotation.Rotation
2830
import com.lambda.interaction.request.rotation.Rotation.Companion.rotationTo
2931
import com.lambda.interaction.request.rotation.RotationManager.onRotate
@@ -32,16 +34,19 @@ import com.lambda.module.Module
3234
import com.lambda.module.tag.ModuleTag
3335
import com.lambda.pathing.Path
3436
import com.lambda.pathing.Pathing.findPathAStar
37+
import com.lambda.pathing.Pathing.thetaStarClearance
3538
import com.lambda.pathing.PathingSettings
3639
import com.lambda.pathing.goal.SimpleGoal
3740
import com.lambda.threading.runConcurrent
3841
import com.lambda.util.Communication.info
42+
import com.lambda.util.Formatting.string
3943
import com.lambda.util.math.setAlpha
4044
import com.lambda.util.player.MovementUtils.buildMovementInput
4145
import com.lambda.util.player.MovementUtils.mergeFrom
4246
import com.lambda.util.world.fastVectorOf
4347
import com.lambda.util.world.toBlockPos
4448
import com.lambda.util.world.toFastVec
49+
import net.minecraft.util.math.Box
4550
import net.minecraft.util.math.Vec3d
4651
import java.awt.Color
4752
import kotlin.math.cos
@@ -56,7 +61,9 @@ object Pathfinder : Module(
5661
private val pathing = PathingSettings(this)
5762
private val rotation = RotationSettings(this)
5863

59-
var path: Path? = null
64+
private val target = fastVectorOf(0, 78, 0)
65+
private var longPath = Path()
66+
private var shortPath = Path()
6067
private var currentTarget: Vec3d? = null
6168
private var integralError = Vec3d.ZERO
6269
private var lastError = Vec3d.ZERO
@@ -66,73 +73,89 @@ object Pathfinder : Module(
6673
onEnable {
6774
integralError = Vec3d.ZERO
6875
lastError = Vec3d.ZERO
76+
calculating = false
6977
}
7078

7179
listen<TickEvent.Pre> {
80+
updateTargetNode()
81+
7282
if (calculating) return@listen
7383
calculating = true
7484

7585
runConcurrent {
76-
val took = measureTimeMillis {
77-
path = findPathAStar(
86+
val long: Path
87+
val aStar = measureTimeMillis {
88+
long = findPathAStar(
7889
player.blockPos.toFastVec(),
79-
// SimpleGoal(fastVectorOf(0, 78, 0)),
80-
SimpleGoal(fastVectorOf(0, 120, 0)),
81-
pathing.cutoffTimeout
90+
SimpleGoal(target),
91+
pathing
8292
)
8393
}
84-
info("Found path of length ${path?.moves?.size} in $took ms")
85-
println("Path: ${path?.toString()}")
86-
calculating = false
94+
val short: Path
95+
val thetaStar = measureTimeMillis {
96+
short = thetaStarClearance(long, pathing)
97+
}
98+
info("A* (Length: ${long.length.string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length.string} Nodes: ${short.moves.size} T: $thetaStar ms)")
99+
println("Long: $long | Short: $short")
100+
longPath = long
101+
shortPath = short
102+
// calculating = false
87103
}
88104
}
89105

90-
// listen<RotationEvent.StrafeInput> { event ->
91-
// updateTargetNode()
92-
// currentTarget?.let { target ->
93-
// event.strafeYaw = player.eyePos.rotationTo(target).yaw
94-
// val adjustment = calculatePID(target)
95-
// val yawRad = Math.toRadians(event.strafeYaw)
96-
// val forward = -sin(yawRad)
97-
// val strafe = cos(yawRad)
98-
//
99-
// val forwardComponent = adjustment.x * forward + adjustment.z * strafe
106+
listen<RotationEvent.StrafeInput> { event ->
107+
currentTarget?.let { target ->
108+
event.strafeYaw = player.eyePos.rotationTo(target).yaw
109+
val adjustment = calculatePID(target)
110+
val yawRad = Math.toRadians(event.strafeYaw)
111+
val forward = -sin(yawRad)
112+
val strafe = cos(yawRad)
113+
114+
val forwardComponent = adjustment.x * forward + adjustment.z * strafe
100115
// val strafeComponent = adjustment.x * strafe - adjustment.z * forward
101-
//
102-
// val moveInput = buildMovementInput(
103-
// forward = forwardComponent,
104-
// strafe = strafeComponent,
105-
// jump = player.isOnGround && adjustment.y > 0.5
106-
// )
107-
// event.input.mergeFrom(moveInput)
108-
// }
109-
// }
110-
//
111-
// onRotate {
112-
// val nextTarget = path?.moves?.getOrNull(2)?.pos?.toBlockPos() ?: return@onRotate
116+
117+
val moveInput = buildMovementInput(
118+
forward = forwardComponent,
119+
strafe = 0.0/*strafeComponent*/,
120+
jump = player.isOnGround && adjustment.y > 0.5
121+
)
122+
event.input.mergeFrom(moveInput)
123+
}
124+
}
125+
126+
onRotate {
127+
// val nextTarget = shortPath.moves.getOrNull(2)?.pos?.toBlockPos() ?: return@onRotate
113128
// val part = player.eyePos.rotationTo(Vec3d.ofBottomCenter(nextTarget))
114-
// val targetRotation = Rotation(part.yaw, player.pitch.toDouble())
115-
//
116-
// lookAt(targetRotation).requestBy(rotation)
117-
// }
129+
val currentTarget = currentTarget ?: return@onRotate
130+
val part = player.eyePos.rotationTo(currentTarget)
131+
val targetRotation = Rotation(part.yaw, player.pitch.toDouble())
132+
133+
lookAt(targetRotation).requestBy(rotation)
134+
}
118135

119136
listen<RenderEvent.StaticESP> { event ->
120-
path?.moves?.zipWithNext { current, next ->
121-
val currentPos = current.pos.toBlockPos().toCenterPos()
122-
val nextPos = next.pos.toBlockPos().toCenterPos()
123-
event.renderer.buildLine(currentPos, nextPos, Color.GREEN)
124-
}
137+
longPath.render(event.renderer, Color.YELLOW)
138+
shortPath.render(event.renderer, Color.GREEN)
139+
event.renderer.buildFilled(Box(target.toBlockPos()), Color.PINK.setAlpha(0.25))
140+
}
141+
}
142+
143+
private fun Path.render(renderer: StaticESP, color: Color) {
144+
moves.zipWithNext { current, next ->
145+
val currentPos = current.pos.toBlockPos().toCenterPos()
146+
val nextPos = next.pos.toBlockPos().toCenterPos()
147+
renderer.buildLine(currentPos, nextPos, color)
125148
}
126149
}
127150

128151
private fun SafeContext.updateTargetNode() {
129-
path?.moves?.firstOrNull()?.let { firstNode ->
152+
shortPath.moves.firstOrNull()?.let { firstNode ->
130153
val nodeVec = Vec3d.ofBottomCenter(firstNode.pos.toBlockPos())
131154
if (player.pos.distanceTo(nodeVec) < pathing.tolerance) {
132-
path?.moves?.removeFirst()
155+
shortPath.moves.removeFirst()
133156
integralError = Vec3d.ZERO
134157
}
135-
val next = path?.moves?.firstOrNull()?.pos?.toBlockPos() ?: return
158+
val next = shortPath.moves.firstOrNull()?.pos?.toBlockPos() ?: return
136159
currentTarget = Vec3d.ofBottomCenter(next)
137160
} ?: run {
138161
currentTarget = null

‎common/src/main/kotlin/com/lambda/pathing/Path.kt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,15 +18,22 @@
1818
package com.lambda.pathing
1919

2020
import com.lambda.pathing.move.Move
21+
import com.lambda.util.world.dist
2122
import com.lambda.util.world.toBlockPos
2223

2324
data class Path(
2425
val moves: ArrayDeque<Move> = ArrayDeque(),
2526
) {
27+
fun append(move: Move) {
28+
moves.addLast(move)
29+
}
30+
2631
fun prepend(move: Move) {
2732
moves.addFirst(move)
2833
}
2934

35+
val length get() = moves.zipWithNext { a, b -> a.pos dist b.pos }.sum()
36+
3037
override fun toString() =
3138
moves.joinToString(" -> ") { "(${it.pos.toBlockPos().toShortString()})" }
3239
}

‎common/src/main/kotlin/com/lambda/pathing/Pathing.kt‎

Lines changed: 48 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -27,11 +27,13 @@ import com.lambda.pathing.move.MoveFinder.moveOptions
2727
import com.lambda.pathing.move.TraverseMove
2828
import com.lambda.util.Communication.warn
2929
import com.lambda.util.world.FastVector
30+
import com.lambda.util.world.WorldUtils.isPathClear
3031
import com.lambda.util.world.toBlockPos
32+
import com.lambda.util.world.y
3133
import java.util.PriorityQueue
3234

3335
object Pathing {
34-
fun SafeContext.findPathAStar(start: FastVector, goal: Goal, timeout: Long = 50L): Path {
36+
fun SafeContext.findPathAStar(start: FastVector, goal: Goal, config: PathingConfig): Path {
3537
MoveFinder.clean()
3638
val startedAt = System.currentTimeMillis()
3739
val openSet = PriorityQueue<Move>()
@@ -43,9 +45,9 @@ object Pathing {
4345

4446
println("Starting pathfinding at ${start.toBlockPos().toShortString()} to $goal")
4547

46-
while (openSet.isNotEmpty() && startedAt + timeout > System.currentTimeMillis()) {
48+
while (openSet.isNotEmpty() && startedAt + config.cutoffTimeout > System.currentTimeMillis()) {
4749
val current = openSet.remove()
48-
println("Considering node: ${current.pos.toBlockPos()}")
50+
// println("Considering node: ${current.pos.toBlockPos()}")
4951
if (goal.inGoal(current.pos)) {
5052
println("Not yet considered nodes: ${openSet.size}")
5153
println("Closed nodes: ${closedSet.size}")
@@ -54,19 +56,59 @@ object Pathing {
5456

5557
closedSet.add(current.pos)
5658

57-
moveOptions(current, goal).forEach { move ->
58-
println("Considering move: $move")
59+
moveOptions(current, goal, config).forEach { move ->
60+
// println("Considering move: $move")
5961
if (closedSet.contains(move.pos)) return@forEach
6062
val tentativeGCost = current.gCost + move.cost
6163
if (tentativeGCost >= move.gCost) return@forEach
6264
move.predecessor = current
6365
move.gCost = tentativeGCost
6466
openSet.add(move)
65-
println("Using move: $move")
67+
// println("Using move: $move")
6668
}
6769
}
6870

6971
warn("Only partial path found!")
7072
return if (openSet.isNotEmpty()) openSet.remove().createPathToSource() else Path()
7173
}
74+
75+
fun SafeContext.thetaStarClearance(path: Path, config: PathingConfig): Path {
76+
if (path.moves.isEmpty()) return Path()
77+
78+
val cleanedPath = Path()
79+
var currentIndex = 0
80+
81+
while (currentIndex < path.moves.size) {
82+
// Always add the current node to the cleaned path
83+
val startMove = path.moves[currentIndex]
84+
cleanedPath.append(startMove)
85+
86+
// Attempt to skip over as many nodes as possible
87+
// by checking if they share the same Y and have a clear path
88+
var nextIndex = currentIndex + 1
89+
while (nextIndex < path.moves.size) {
90+
val candidateMove = path.moves[nextIndex]
91+
92+
// Only try to skip if both moves are on the same Y level
93+
if (startMove.pos.y != candidateMove.pos.y) break
94+
95+
// Verify there's a clear path from the start move to the candidate
96+
if (
97+
isPathClear(
98+
startMove.pos.toBlockPos(),
99+
candidateMove.pos.toBlockPos(),
100+
config.pathClearanceCheckDistance
101+
)
102+
) nextIndex++ else break
103+
}
104+
105+
// Move to the last node that was confirmed reachable
106+
// (subtract 1 because 'nextIndex' might have gone one too far)
107+
currentIndex = if (nextIndex > currentIndex + 1) nextIndex - 1 else nextIndex
108+
}
109+
110+
return cleanedPath
111+
}
112+
113+
72114
}

‎common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,8 @@ interface PathingConfig {
2323
val kD: Double
2424
val tolerance: Double
2525
val cutoffTimeout: Long
26+
val shortcutLength: Int
27+
val pathClearanceCheckDistance: Double
2628

2729
val assumeJesus: Boolean
2830
}

‎common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,8 @@ class PathingSettings(
3434
override val kD by c.setting("D Gain", 0.2, 0.0..1.0, 0.01) { vis() && page == Page.Execution }
3535
override val tolerance by c.setting("Node Tolerance", 0.1, 0.01..1.0, 0.01) { vis() && page == Page.Execution }
3636
override val cutoffTimeout by c.setting("Cutoff Timeout", 50L, 1L..2000L, 10L) { vis() && page == Page.Execution }
37+
override val shortcutLength by c.setting("Shortcut Length", 10, 1..100, 1) { vis() && page == Page.Execution }
38+
override val pathClearanceCheckDistance by c.setting("Path Clearance Check Distance", 0.3, 0.0..1.0, 0.01) { vis() && page == Page.Execution }
3739

3840
override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc }
3941
}

‎common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt‎

Lines changed: 24 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -18,17 +18,21 @@
1818
package com.lambda.pathing.move
1919

2020
import com.lambda.context.SafeContext
21+
import com.lambda.pathing.PathingConfig
2122
import com.lambda.pathing.goal.Goal
2223
import com.lambda.util.BlockUtils.blockState
2324
import com.lambda.util.BlockUtils.fluidState
2425
import com.lambda.util.world.FastVector
2526
import com.lambda.util.world.WorldUtils.isPathClear
27+
import com.lambda.util.world.WorldUtils.playerFitsIn
2628
import com.lambda.util.world.WorldUtils.traversable
2729
import com.lambda.util.world.add
2830
import com.lambda.util.world.fastVectorOf
2931
import com.lambda.util.world.length
3032
import com.lambda.util.world.manhattanLength
33+
import com.lambda.util.world.offset
3134
import com.lambda.util.world.toBlockPos
35+
import com.lambda.util.world.y
3236
import net.minecraft.block.BlockState
3337
import net.minecraft.block.Blocks
3438
import net.minecraft.block.CampfireBlock
@@ -51,10 +55,10 @@ import net.minecraft.util.math.EightWayDirection
5155
object MoveFinder {
5256
private val nodeTypeCache = HashMap<FastVector, NodeType>()
5357

54-
fun SafeContext.moveOptions(origin: Move, goal: Goal) =
58+
fun SafeContext.moveOptions(origin: Move, goal: Goal, config: PathingConfig) =
5559
EightWayDirection.entries.flatMap { direction ->
5660
(-1..1).mapNotNull { y ->
57-
getPathNode(goal, origin, direction, y)
61+
getPathNode(goal, origin, direction, y, config)
5862
}
5963
}
6064

@@ -63,21 +67,33 @@ object MoveFinder {
6367
origin: Move,
6468
direction: EightWayDirection,
6569
height: Int,
70+
config: PathingConfig
6671
): Move? {
6772
val offset = fastVectorOf(direction.offsetX, height, direction.offsetZ)
6873
val checkingPos = origin.pos.add(offset)
6974
val checkingBlockPos = checkingPos.toBlockPos()
75+
val originBlockPos = origin.pos.toBlockPos()
7076
if (!world.worldBorder.contains(checkingBlockPos)) return null
7177

7278
val nodeType = findPathType(checkingPos)
73-
val hCost = goal.heuristic(checkingPos)/* * nodeType.penalty*/
74-
val cost = offset.length()
75-
val currentFeetY = getFeetY(checkingBlockPos)
76-
7779
if (nodeType == NodeType.BLOCKED) return null
7880

79-
// // ToDo: Different for jumping etc
80-
if (!isPathClear(origin.pos.toBlockPos(), checkingBlockPos)) return null
81+
val clear = when {
82+
height == 0 -> isPathClear(originBlockPos, checkingBlockPos)
83+
height > 0 -> {
84+
val between = origin.pos.offset(0, height, 0)
85+
isPathClear(origin.pos, between, supportCheck = false) && isPathClear(between, checkingPos, supportCheck = false)
86+
}
87+
else -> {
88+
val between = origin.pos.offset(direction.offsetX, 0, direction.offsetZ)
89+
isPathClear(origin.pos, between, supportCheck = false) && isPathClear(between, checkingPos, supportCheck = false)
90+
}
91+
}
92+
if (!clear) return null
93+
94+
val hCost = goal.heuristic(checkingPos) /** nodeType.penalty*/
95+
val cost = offset.length()
96+
val currentFeetY = getFeetY(checkingBlockPos)
8197

8298
return when {
8399
// (currentFeetY - origin.feetY) > player.stepHeight -> ParkourMove(checkingPos, hCost, nodeType, currentFeetY, cost)

‎common/src/main/kotlin/com/lambda/util/world/Position.kt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -131,7 +131,7 @@ fun FastVector.offset(x: Int, y: Int, z: Int): FastVector = fastVectorOf(this.x
131131

132132
fun FastVector.manhattanLength() = abs(x) + abs(y) + abs(z)
133133

134-
fun FastVector.length() = sqrt((x * x + y * y + z * z).toDouble())
134+
fun FastVector.length() = sqrt((abs(x * x) + abs(y * y) + abs(z * z)).toDouble())
135135

136136
/**
137137
* Adds the given vector to the position.

0 commit comments

Comments
 (0)