diff --git a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt
new file mode 100644
index 000000000..3637fd5fa
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt
@@ -0,0 +1,171 @@
+/*
+ * Copyright 2024 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.command.commands
+
+import com.lambda.brigadier.argument.boolean
+import com.lambda.brigadier.argument.double
+import com.lambda.brigadier.argument.integer
+import com.lambda.brigadier.argument.literal
+import com.lambda.brigadier.argument.value
+import com.lambda.brigadier.execute
+import com.lambda.brigadier.optional
+import com.lambda.brigadier.required
+import com.lambda.command.LambdaCommand
+import com.lambda.module.modules.movement.Pathfinder
+import com.lambda.pathing.move.MoveFinder
+import com.lambda.util.Communication.info
+import com.lambda.util.extension.CommandBuilder
+import com.lambda.util.world.fastVectorOf
+import com.lambda.util.world.string
+
+object PathCommand : LambdaCommand(
+ name = "pathfinder",
+ usage = "path ",
+ description = "Finds a quick path through the world",
+ aliases = setOf("path")
+) {
+ override fun CommandBuilder.create() {
+ required(literal("target")) {
+ required(integer("X", -30000000, 30000000)) { x ->
+ required(integer("Y", -64, 255)) { y ->
+ required(integer("Z", -30000000, 30000000)) { z ->
+ execute {
+ val v = fastVectorOf(x().value(), y().value(), z().value())
+ Pathfinder.target = v
+ this@PathCommand.info("Set new target at ${v.string}")
+ }
+ }
+ }
+ }
+ }
+
+ required(literal("invalidate")) {
+ required(integer("X", -30000000, 30000000)) { x ->
+ required(integer("Y", -64, 255)) { y ->
+ required(integer("Z", -30000000, 30000000)) { z ->
+ optional(boolean("prune")) { prune ->
+ execute {
+ val v = fastVectorOf(x().value(), y().value(), z().value())
+ val pruneGraph = if (prune != null) {
+ prune().value()
+ } else false
+ Pathfinder.dStar.invalidate(v, pruneGraph)
+ MoveFinder.clear(v)
+ Pathfinder.needsUpdate = true
+ this@PathCommand.info("Invalidated ${v.string}")
+ }
+ }
+ }
+ }
+ }
+ }
+
+ required(literal("remove")) {
+ required(integer("X", -30000000, 30000000)) { x ->
+ required(integer("Y", -64, 255)) { y ->
+ required(integer("Z", -30000000, 30000000)) { z ->
+ execute {
+ val v = fastVectorOf(x().value(), y().value(), z().value())
+ Pathfinder.graph.removeNode(v)
+ this@PathCommand.info("Removed ${v.string}")
+ }
+ }
+ }
+ }
+ }
+
+ required(literal("update")) {
+ required(integer("X", -30000000, 30000000)) { x ->
+ required(integer("Y", -64, 255)) { y ->
+ required(integer("Z", -30000000, 30000000)) { z ->
+ execute {
+ val u = fastVectorOf(x().value(), y().value(), z().value())
+ Pathfinder.dStar.updateVertex(u)
+ this@PathCommand.info("Updated ${u.string}")
+ }
+ }
+ }
+ }
+ }
+
+ required(literal("successor")) {
+ required(integer("X", -30000000, 30000000)) { x ->
+ required(integer("Y", -64, 255)) { y ->
+ required(integer("Z", -30000000, 30000000)) { z ->
+ execute {
+ val v = fastVectorOf(x().value(), y().value(), z().value())
+ this@PathCommand.info("Successors: ${Pathfinder.graph.successors[v]?.entries?.joinToString { "${it.key.string}: ${it.value}" }}")
+ }
+ }
+ }
+ }
+ }
+
+ required(literal("predecessors")) {
+ required(integer("X", -30000000, 30000000)) { x ->
+ required(integer("Y", -64, 255)) { y ->
+ required(integer("Z", -30000000, 30000000)) { z ->
+ execute {
+ val v = fastVectorOf(x().value(), y().value(), z().value())
+ this@PathCommand.info("Predecessors: ${Pathfinder.graph.predecessors[v]?.entries?.joinToString { "${it.key.string}: ${it.value}" }}")
+ }
+ }
+ }
+ }
+ }
+
+ required(literal("setEdge")) {
+ required(integer("X1", -30000000, 30000000)) { x1 ->
+ required(integer("Y1", -64, 255)) { y1 ->
+ required(integer("Z1", -30000000, 30000000)) { z1 ->
+ required(integer("X2", -30000000, 30000000)) { x2 ->
+ required(integer("Y2", -64, 255)) { y2 ->
+ required(integer("Z2", -30000000, 30000000)) { z2 ->
+ required(double("cost")) { cost ->
+ execute {
+ val v1 = fastVectorOf(x1().value(), y1().value(), z1().value())
+ val v2 = fastVectorOf(x2().value(), y2().value(), z2().value())
+ val c = cost().value()
+ Pathfinder.dStar.updateEdge(v1, v2, c)
+ Pathfinder.needsUpdate = true
+ this@PathCommand.info("Updated edge ${v1.string} -> ${v2.string} to cost of $c")
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+
+ required(literal("clear")) {
+ execute {
+ Pathfinder.graph.clear()
+ this@PathCommand.info("Cleared graph")
+ }
+ }
+
+ required(literal("refresh")) {
+ execute {
+ Pathfinder.needsUpdate = true
+ this@PathCommand.info("Marked pathfinder for refresh")
+ }
+ }
+ }
+}
diff --git a/common/src/main/kotlin/com/lambda/graphics/renderer/esp/builders/StaticESPBuilders.kt b/common/src/main/kotlin/com/lambda/graphics/renderer/esp/builders/StaticESPBuilders.kt
index e82818916..986d4f37c 100644
--- a/common/src/main/kotlin/com/lambda/graphics/renderer/esp/builders/StaticESPBuilders.kt
+++ b/common/src/main/kotlin/com/lambda/graphics/renderer/esp/builders/StaticESPBuilders.kt
@@ -27,6 +27,7 @@ import com.lambda.util.extension.min
import net.minecraft.block.BlockState
import net.minecraft.util.math.BlockPos
import net.minecraft.util.math.Box
+import net.minecraft.util.math.Vec3d
import net.minecraft.util.shape.VoxelShape
import java.awt.Color
@@ -222,3 +223,13 @@ fun StaticESPRenderer.buildOutline(
if (outlineMode.check(hasEast, hasSouth)) buildLine(trf, brf)
if (outlineMode.check(hasSouth, hasWest)) buildLine(tlf, blf)
}
+
+fun StaticESPRenderer.buildLine(
+ start: Vec3d,
+ end: Vec3d,
+ color: Color,
+) = outlineBuilder.use {
+ val v1 by lazy { vertex { vec3(start.x, start.y, start.z).color(color) } }
+ val v2 by lazy { vertex { vec3(end.x, end.y, end.z).color(color) } }
+ buildLine(v1, v2)
+}
diff --git a/common/src/main/kotlin/com/lambda/interaction/construction/simulation/Simulation.kt b/common/src/main/kotlin/com/lambda/interaction/construction/simulation/Simulation.kt
index d00c50568..4d1b0ab29 100644
--- a/common/src/main/kotlin/com/lambda/interaction/construction/simulation/Simulation.kt
+++ b/common/src/main/kotlin/com/lambda/interaction/construction/simulation/Simulation.kt
@@ -28,14 +28,13 @@ import com.lambda.interaction.construction.simulation.BuildSimulator.simulate
import com.lambda.interaction.request.rotation.RotationConfig
import com.lambda.module.modules.client.TaskFlowModule
import com.lambda.threading.runSafe
-import com.lambda.util.BlockUtils.blockState
import com.lambda.util.world.FastVector
+import com.lambda.util.world.WorldUtils.playerBox
+import com.lambda.util.world.WorldUtils.traversable
import com.lambda.util.world.toBlockPos
import com.lambda.util.world.toVec3d
import net.minecraft.client.network.ClientPlayerEntity
import net.minecraft.util.math.BlockPos
-import net.minecraft.util.math.Box
-import net.minecraft.util.math.Direction
import net.minecraft.util.math.Vec3d
import java.awt.Color
@@ -57,10 +56,7 @@ data class Simulation(
val isTooFar = blueprint.getClosestPointTo(view).distanceTo(view) > 10.0
runSafe {
if (isOutOfBounds && isTooFar) return@getOrPut emptySet()
- val blockPos = pos.toBlockPos()
- val isWalkable = blockState(blockPos.down()).isSideSolidFullSquare(world, blockPos, Direction.UP)
- if (!isWalkable) return@getOrPut emptySet()
- if (!playerFitsIn(blockPos)) return@getOrPut emptySet()
+ if (!traversable(pos.toBlockPos())) return@getOrPut emptySet()
}
blueprint.simulate(view, interact, rotation, inventory, build)
@@ -74,13 +70,7 @@ data class Simulation(
}
}
- private fun SafeContext.playerFitsIn(pos: BlockPos): Boolean {
- return world.isSpaceEmpty(Vec3d.ofBottomCenter(pos).playerBox())
- }
-
companion object {
- fun Vec3d.playerBox(): Box = Box(x - 0.3, y, z - 0.3, x + 0.3, y + 1.8, z + 0.3).contract(1.0E-6)
-
fun Blueprint.simulation(
interact: InteractionConfig = TaskFlowModule.interact,
rotation: RotationConfig = TaskFlowModule.rotation,
diff --git a/common/src/main/kotlin/com/lambda/module/hud/PathfinderHUD.kt b/common/src/main/kotlin/com/lambda/module/hud/PathfinderHUD.kt
new file mode 100644
index 000000000..402bbb931
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/module/hud/PathfinderHUD.kt
@@ -0,0 +1,29 @@
+/*
+ * Copyright 2024 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.module.hud
+
+import com.lambda.module.HudModule
+import com.lambda.module.modules.movement.Pathfinder
+import com.lambda.module.tag.ModuleTag
+
+object PathfinderHUD : HudModule.Text(
+ name = "PathfinderHUD",
+ defaultTags = setOf(ModuleTag.CLIENT),
+) {
+ override fun getText() = Pathfinder.debugInfo()
+}
diff --git a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt
new file mode 100644
index 000000000..0deb6377b
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt
@@ -0,0 +1,288 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.module.modules.movement
+
+import com.lambda.context.SafeContext
+import com.lambda.event.events.MovementEvent
+import com.lambda.event.events.RenderEvent
+import com.lambda.event.events.RotationEvent
+import com.lambda.event.events.TickEvent
+import com.lambda.event.events.WorldEvent
+import com.lambda.event.listener.SafeListener.Companion.listen
+import com.lambda.graphics.gl.Matrices
+import com.lambda.graphics.renderer.esp.builders.buildFilled
+import com.lambda.interaction.request.rotation.Rotation
+import com.lambda.interaction.request.rotation.Rotation.Companion.rotationTo
+import com.lambda.interaction.request.rotation.RotationManager.onRotate
+import com.lambda.interaction.request.rotation.visibilty.lookAt
+import com.lambda.module.Module
+import com.lambda.module.tag.ModuleTag
+import com.lambda.pathing.Path
+import com.lambda.pathing.Pathing.findPathAStar
+import com.lambda.pathing.Pathing.thetaStarClearance
+import com.lambda.pathing.PathingConfig
+import com.lambda.pathing.PathingSettings
+import com.lambda.pathing.incremental.DStarLite
+import com.lambda.pathing.incremental.LazyGraph
+import com.lambda.pathing.goal.SimpleGoal
+import com.lambda.pathing.move.MoveFinder
+import com.lambda.pathing.move.MoveFinder.moveOptions
+import com.lambda.pathing.move.NodeType
+import com.lambda.pathing.move.TraverseMove
+import com.lambda.threading.runSafe
+import com.lambda.threading.runSafeConcurrent
+import com.lambda.util.Communication.info
+import com.lambda.util.Formatting.asString
+import com.lambda.util.Formatting.string
+import com.lambda.util.math.setAlpha
+import com.lambda.util.player.MovementUtils.buildMovementInput
+import com.lambda.util.player.MovementUtils.mergeFrom
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.WorldUtils.hasSupport
+import com.lambda.util.world.dist
+import com.lambda.util.world.fastVectorOf
+import com.lambda.util.world.string
+import com.lambda.util.world.toBlockPos
+import com.lambda.util.world.toFastVec
+import com.lambda.util.world.x
+import com.lambda.util.world.y
+import com.lambda.util.world.z
+import kotlinx.coroutines.delay
+import net.minecraft.util.math.BlockPos
+import net.minecraft.util.math.Box
+import net.minecraft.util.math.Vec3d
+import java.awt.Color
+import kotlin.math.abs
+import kotlin.math.cos
+import kotlin.math.sin
+import kotlin.system.measureTimeMillis
+
+object Pathfinder : Module(
+ name = "Pathfinder",
+ description = "Get from A to B",
+ defaultTags = setOf(ModuleTag.MOVEMENT)
+) {
+ private val pathing = PathingSettings(this)
+
+ var target = fastVectorOf(0, 78, 0)
+ val graph = LazyGraph { origin ->
+ runSafe {
+ moveOptions(origin, ::heuristic, pathing).associate { it.pos to it.cost }
+ } ?: emptyMap()
+ }
+ val dStar = DStarLite(graph, fastVectorOf(0, 0, 0), target, ::heuristic)
+ private var coarsePath = Path()
+ private var refinedPath = Path()
+ private var currentTarget: Vec3d? = null
+ private var integralError = Vec3d.ZERO
+ private var lastError = Vec3d.ZERO
+ var needsUpdate = false
+ private var currentStart = BlockPos.ORIGIN.toFastVec()
+
+ private fun heuristic(u: FastVector): Double =
+ (abs(u.x) + abs(u.y) + abs(u.z)).toDouble()
+
+ private fun heuristic(u: FastVector, v: FastVector): Double =
+ (abs(u.x - v.x) + abs(u.y - v.y) + abs(u.z - v.z)).toDouble()
+
+ init {
+ onEnable {
+ integralError = Vec3d.ZERO
+ lastError = Vec3d.ZERO
+ coarsePath = Path()
+ refinedPath = Path()
+ currentTarget = null
+ graph.clear()
+ dStar.initialize()
+ needsUpdate = true
+ currentStart = player.blockPos.toFastVec()
+ startBackgroundThread()
+ }
+
+ onDisable {
+ MoveFinder.clean()
+ graph.clear()
+ }
+
+ listen {
+ val playerPos = player.blockPos
+ val currentPos = playerPos.toFastVec()
+ val positionOutdated = currentPos dist currentStart > pathing.tolerance
+ if (player.isOnGround && hasSupport(playerPos) && positionOutdated) {
+ currentStart = currentPos
+ needsUpdate = true
+ }
+ if (pathing.moveAlongPath) updateTargetNode()
+// info("${isPathClear(playerPos, targetPos)}")
+ }
+
+ listen {
+ if (it.newState == it.oldState) return@listen
+ val pos = it.pos.toFastVec()
+ MoveFinder.clear(pos)
+ dStar.invalidate(pos, pathing.pruneGraph)
+ needsUpdate = true
+ info("Updated block at ${it.pos.asString()} from ${it.oldState.block.name.string} to ${it.newState.block.name.string} rescheduled D*Lite.")
+ }
+
+ listen { event ->
+ if (!pathing.moveAlongPath) return@listen
+
+ currentTarget?.let { target ->
+ event.strafeYaw = player.eyePos.rotationTo(target).yaw
+ val adjustment = calculatePID(target)
+ val yawRad = Math.toRadians(event.strafeYaw)
+ val forward = -sin(yawRad)
+ val strafe = cos(yawRad)
+
+ val forwardComponent = adjustment.x * forward + adjustment.z * strafe
+// val strafeComponent = adjustment.x * strafe - adjustment.z * forward
+
+ val moveInput = buildMovementInput(
+ forward = forwardComponent,
+ strafe = 0.0/*strafeComponent*/,
+ jump = player.isOnGround && adjustment.y > 0.5
+ )
+ event.input.mergeFrom(moveInput)
+ }
+ }
+
+ onRotate {
+ if (!pathing.moveAlongPath) return@onRotate
+
+ val currentTarget = currentTarget ?: return@onRotate
+ val part = player.eyePos.rotationTo(currentTarget)
+ val targetRotation = Rotation(part.yaw, player.pitch.toDouble())
+
+ lookAt(targetRotation).requestBy(pathing.rotation)
+ }
+
+ listen {
+ if (!pathing.moveAlongPath) return@listen
+ if (refinedPath.moves.isEmpty()) return@listen
+
+ player.isSprinting = pathing.allowSprint
+ it.sprint = pathing.allowSprint
+ }
+
+ listen { event ->
+ if (pathing.renderCoarsePath) coarsePath.render(event.renderer, Color.YELLOW)
+ if (pathing.renderRefinedPath) refinedPath.render(event.renderer, Color.GREEN)
+ if (pathing.renderGoal) event.renderer.buildFilled(Box(target.toBlockPos()), Color.PINK.setAlpha(0.25))
+ graph.render(event.renderer, pathing)
+ }
+
+ listen {
+ if (!pathing.renderGraph) return@listen
+
+ Matrices.push {
+ val c = mc.gameRenderer.camera.pos.negate()
+ translate(c.x, c.y, c.z)
+ dStar.buildDebugInfoRenderer(pathing)
+ }
+ }
+ }
+
+ private fun startBackgroundThread() {
+ runSafeConcurrent {
+ while (isEnabled) {
+ if (!needsUpdate) {
+ delay(50L)
+ continue
+ }
+ needsUpdate = false
+ updatePaths()
+ }
+ }
+ }
+
+ private fun SafeContext.updateTargetNode() {
+ currentTarget = refinedPath.moves.firstOrNull()?.let { current ->
+ if (player.pos.distanceTo(current.bottomPos) < pathing.tolerance) {
+ refinedPath.moves.removeFirst()
+ integralError = Vec3d.ZERO
+ }
+ refinedPath.moves.firstOrNull()?.bottomPos
+ }
+ }
+
+ private fun SafeContext.updatePaths() {
+ val goal = SimpleGoal(target)
+ when (pathing.algorithm) {
+ PathingConfig.PathingAlgorithm.A_STAR -> updateAStar(currentStart, goal)
+ PathingConfig.PathingAlgorithm.D_STAR_LITE -> updateDStar(currentStart, goal)
+ }
+ }
+
+ private fun SafeContext.updateAStar(start: FastVector, goal: SimpleGoal) {
+ val long: Path
+ val aStar = measureTimeMillis {
+ long = findPathAStar(start, goal, pathing)
+ }
+ val short: Path
+ val thetaStar = measureTimeMillis {
+ short = if (pathing.refinePath) {
+ thetaStarClearance(long, pathing)
+ } else long
+ }
+ info("A* (Length: ${long.length().string} Nodes: ${long.size} T: $aStar ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)")
+// println("Long: $long | Short: $short")
+ coarsePath = long
+ refinedPath = short
+ }
+
+ private fun SafeContext.updateDStar(start: FastVector, goal: SimpleGoal) {
+ val long: Path
+ val dStarTime = measureTimeMillis {
+ dStar.updateStart(start)
+ dStar.computeShortestPath(pathing.cutoffTimeout)
+ val nodes = dStar.path(pathing.maxPathLength).map { TraverseMove(it, 0.0, NodeType.OPEN, 0.0, 0.0) }
+ long = Path(ArrayDeque(nodes))
+ }
+ val short: Path
+ val thetaStar = measureTimeMillis {
+ short = if (pathing.refinePath) {
+ thetaStarClearance(long, pathing)
+ } else long
+ }
+ info("Lazy D* Lite (Length: ${long.length().string} Nodes: ${long.size} Graph Size: ${graph.size} T: $dStarTime ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)")
+// println("Long: $long | Short: $short")
+ coarsePath = long
+ refinedPath = short
+ println(dStar.toString())
+ }
+
+ private fun SafeContext.calculatePID(target: Vec3d): Vec3d {
+ val error = target.subtract(player.pos)
+ integralError = integralError.add(error)
+ val derivativeError = error.subtract(lastError)
+ lastError = error
+
+ return error.multiply(pathing.kP)
+ .add(integralError.multiply(pathing.kI))
+ .add(derivativeError.multiply(pathing.kD))
+ }
+
+ fun debugInfo() = buildString {
+ appendLine("Current Start: ${currentStart.string}")
+ appendLine("Current Target: ${currentTarget?.string}")
+ appendLine("Path Length: ${coarsePath.length().string} Nodes: ${coarsePath.size}")
+ if (pathing.refinePath) appendLine("Refined Path: ${refinedPath.length().string} Nodes: ${refinedPath.size}")
+ if (pathing.algorithm == PathingConfig.PathingAlgorithm.D_STAR_LITE) append(dStar.toString())
+ }
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/module/modules/player/Freecam.kt b/common/src/main/kotlin/com/lambda/module/modules/player/Freecam.kt
index 53bbd0c5b..055c7a378 100644
--- a/common/src/main/kotlin/com/lambda/module/modules/player/Freecam.kt
+++ b/common/src/main/kotlin/com/lambda/module/modules/player/Freecam.kt
@@ -143,5 +143,9 @@ object Freecam : Module(
listen {
disable()
}
+
+ listen {
+ disable()
+ }
}
}
diff --git a/common/src/main/kotlin/com/lambda/pathing/Path.kt b/common/src/main/kotlin/com/lambda/pathing/Path.kt
new file mode 100644
index 000000000..9578f3e54
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/Path.kt
@@ -0,0 +1,80 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing
+
+import com.lambda.graphics.renderer.esp.builders.ofBox
+import com.lambda.graphics.renderer.esp.global.StaticESP
+import com.lambda.pathing.move.Move
+import com.lambda.util.collections.updatableLazy
+import com.lambda.util.math.component1
+import com.lambda.util.math.component2
+import com.lambda.util.math.component3
+import com.lambda.util.math.setAlpha
+import com.lambda.util.world.dist
+import com.lambda.util.world.toBlockPos
+import net.minecraft.util.math.Box
+import java.awt.Color
+
+data class Path(
+ val moves: ArrayDeque = ArrayDeque(),
+) {
+ fun append(move: Move) {
+ moves.addLast(move)
+ length.clear()
+ }
+
+ fun prepend(move: Move) {
+ moves.addFirst(move)
+ length.clear()
+ }
+
+ private val length = updatableLazy {
+ moves.zipWithNext { a, b -> a.pos dist b.pos }.sum()
+ }
+
+ fun render(renderer: StaticESP, color: Color) {
+ moves.zipWithNext { current, next ->
+ val start = current.pos.toBlockPos().toCenterPos()
+ val end = next.pos.toBlockPos().toCenterPos()
+ val direction = end.subtract(start)
+ val distance = direction.length()
+ if (distance <= 0) return@zipWithNext
+
+ val stepSize = 0.2
+ val steps = (distance / stepSize).toInt()
+ val stepDirection = direction.normalize().multiply(stepSize)
+
+ var currentPos = start
+
+ (0 until steps).forEach { _ ->
+ val (x, y, z) = currentPos
+ val d = 0.03
+ val box = Box(x - d, y - d, z - d, x + d, y + d, z + d)
+ renderer.ofBox(box, color.brighter().setAlpha(0.25), color.darker())
+ currentPos = currentPos.add(stepDirection)
+ }
+ }
+ }
+
+ fun length() = length.value
+
+ val size get() = moves.size
+
+ override fun toString() =
+ moves.joinToString(" -> ") { "(${it.pos.toBlockPos().toShortString()})" }
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt
new file mode 100644
index 000000000..7c732b5da
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt
@@ -0,0 +1,108 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing
+
+import com.lambda.context.SafeContext
+import com.lambda.pathing.goal.Goal
+import com.lambda.pathing.move.Move
+import com.lambda.pathing.move.MoveFinder
+import com.lambda.pathing.move.MoveFinder.findPathType
+import com.lambda.pathing.move.MoveFinder.getFeetY
+import com.lambda.pathing.move.MoveFinder.moveOptions
+import com.lambda.pathing.move.TraverseMove
+import com.lambda.util.Communication.warn
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.WorldUtils.isPathClear
+import com.lambda.util.world.toBlockPos
+import com.lambda.util.world.y
+import java.util.PriorityQueue
+
+object Pathing {
+ fun SafeContext.findPathAStar(start: FastVector, goal: Goal, config: PathingConfig): Path {
+ MoveFinder.clean()
+ val startedAt = System.currentTimeMillis()
+ val openSet = PriorityQueue()
+ val closedSet = mutableSetOf()
+ val startFeetY = getFeetY(start.toBlockPos())
+ val startNode = TraverseMove(start, goal.heuristic(start), findPathType(start), startFeetY, 0.0)
+ startNode.gCost = 0.0
+ openSet.add(startNode)
+
+ println("Starting pathfinding at ${start.toBlockPos().toShortString()} to $goal")
+
+ while (openSet.isNotEmpty() && startedAt + config.cutoffTimeout > System.currentTimeMillis()) {
+ val current = openSet.remove()
+// println("Considering node: ${current.pos.toBlockPos()}")
+ if (goal.inGoal(current.pos)) {
+ println("Not yet considered nodes: ${openSet.size}")
+ println("Closed nodes: ${closedSet.size}")
+ return current.createPathToSource()
+ }
+
+ closedSet.add(current.pos)
+
+ moveOptions(current.pos, goal::heuristic, config).forEach { move ->
+// println("Considering move: $move")
+ if (closedSet.contains(move.pos)) return@forEach
+ val tentativeGCost = current.gCost + move.cost
+ if (tentativeGCost >= move.gCost) return@forEach
+ move.predecessor = current
+ move.gCost = tentativeGCost
+ openSet.add(move)
+// println("Using move: $move")
+ }
+ }
+
+ warn("Only partial path found!")
+ return if (openSet.isNotEmpty()) openSet.remove().createPathToSource() else Path()
+ }
+
+ fun SafeContext.thetaStarClearance(path: Path, config: PathingConfig): Path {
+ if (path.moves.isEmpty()) return path
+
+ val cleanedPath = Path()
+ var currentIndex = 0
+
+ while (currentIndex < path.moves.size) {
+ // Always add the current node to the cleaned path
+ val startMove = path.moves[currentIndex]
+ cleanedPath.append(startMove)
+
+ // Attempt to skip over as many nodes as possible
+ var nextIndex = currentIndex + 1
+ while (nextIndex < path.moves.size) {
+ val candidateMove = path.moves[nextIndex]
+ val startPos = startMove.pos.toBlockPos()
+ val candidatePos = candidateMove.pos.toBlockPos()
+
+ // Only try to skip if both moves are on the same Y level
+ if (startPos.y != candidatePos.y) break
+
+ // Verify there's a clear path from the start move to the candidate
+ val isClear = isPathClear(startPos, candidatePos, config.clearancePrecision)
+ if (isClear) nextIndex++ else break
+ }
+
+ // Move to the last node that was confirmed reachable
+ // (subtract 1 because 'nextIndex' might have gone one too far)
+ currentIndex = if (nextIndex > currentIndex + 1) nextIndex - 1 else nextIndex
+ }
+
+ return cleanedPath
+ }
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt
new file mode 100644
index 000000000..23a1b7c34
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt
@@ -0,0 +1,79 @@
+/*
+ * Copyright 2024 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing
+
+import com.lambda.interaction.request.rotation.RotationConfig
+import com.lambda.util.NamedEnum
+
+interface PathingConfig {
+ val algorithm: PathingAlgorithm
+ val pruneGraph: Boolean
+ val cutoffTimeout: Long
+ val maxFallHeight: Double
+ val mlg: Boolean
+ val useWaterBucket: Boolean
+ val useLavaBucket: Boolean
+ val useBoat: Boolean
+ val maxPathLength: Int
+
+ val refinePath: Boolean
+ val useThetaStar: Boolean
+ val shortcutLength: Int
+ val clearancePrecision: Double
+ val findShortcutJumps: Boolean
+ val maxJumpDistance: Double
+ val spline: Spline
+ val epsilon: Double
+
+ val moveAlongPath: Boolean
+ val kP: Double
+ val kI: Double
+ val kD: Double
+ val tolerance: Double
+ val allowSprint: Boolean
+
+ val rotation: RotationConfig
+
+ val renderCoarsePath: Boolean
+ val renderRefinedPath: Boolean
+ val renderGoal: Boolean
+ val renderGraph: Boolean
+ val renderSuccessors: Boolean
+ val renderPredecessors: Boolean
+ val renderInvalidated: Boolean
+ val renderPositions: Boolean
+ val renderCost: Boolean
+ val renderG: Boolean
+ val renderRHS: Boolean
+ val renderKey: Boolean
+ val renderQueue: Boolean
+ val maxRenderObjects: Int
+ val fontScale: Double
+ val assumeJesus: Boolean
+
+ enum class PathingAlgorithm(override val displayName: String) : NamedEnum {
+ A_STAR("A*"),
+ D_STAR_LITE("Lazy D* Lite"),
+ }
+
+ enum class Spline {
+ None,
+ CatmullRom,
+ CubicBezier,
+ }
+}
diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt
new file mode 100644
index 000000000..fcfc221ea
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt
@@ -0,0 +1,78 @@
+/*
+ * Copyright 2024 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing
+
+import com.lambda.config.Configurable
+import com.lambda.config.groups.RotationSettings
+
+class PathingSettings(
+ c: Configurable,
+ vis: () -> Boolean = { true }
+) : PathingConfig {
+ enum class Page {
+ Pathfinding, Refinement, Movement, Rotation, Debug, Misc
+ }
+
+ private val page by c.setting("Pathing Page", Page.Pathfinding, "Current page", vis)
+
+ override val algorithm by c.setting("Algorithm", PathingConfig.PathingAlgorithm.A_STAR) { vis() && page == Page.Pathfinding }
+ override val pruneGraph by c.setting("Prune Graph", true) { vis() && page == Page.Pathfinding && algorithm == PathingConfig.PathingAlgorithm.D_STAR_LITE }
+ override val cutoffTimeout by c.setting("Cutoff Timeout", 500L, 1L..2000L, 10L, "Timeout of path calculation", " ms") { vis() && page == Page.Pathfinding }
+ override val maxFallHeight by c.setting("Max Fall Height", 3.0, 0.0..30.0, 0.5) { vis() && page == Page.Pathfinding }
+ override val mlg by c.setting("Do MLG", false) { vis() && page == Page.Pathfinding }
+ override val useWaterBucket by c.setting("Use Water Bucket", true) { vis() && page == Page.Pathfinding && mlg }
+ override val useLavaBucket by c.setting("Use Lava Bucket", true) { vis() && page == Page.Pathfinding && mlg }
+ override val useBoat by c.setting("Use Boat", true) { vis() && page == Page.Pathfinding && mlg }
+ override val maxPathLength by c.setting("Max Path Length", 10_000, 1..100_000, 100) { vis() && page == Page.Pathfinding }
+
+ override val refinePath by c.setting("Refine Path", true) { vis() && page == Page.Refinement }
+ override val useThetaStar by c.setting("Use θ* (Any Angle)", true) { vis() && refinePath && page == Page.Refinement }
+ override val shortcutLength by c.setting("Shortcut Length", 15, 1..100, 1) { vis() && refinePath && useThetaStar && page == Page.Refinement }
+ override val clearancePrecision by c.setting("Clearance Precision", 0.1, 0.0..1.0, 0.01) { vis() && refinePath && useThetaStar && page == Page.Refinement }
+ override val findShortcutJumps by c.setting("Find Shortcut Jumps", true) { vis() && refinePath && page == Page.Refinement }
+ override val maxJumpDistance by c.setting("Max Jump Distance", 4.0, 1.0..5.0, 0.1) { vis() && refinePath && findShortcutJumps && page == Page.Refinement }
+ override val spline by c.setting("Use Splines", PathingConfig.Spline.CatmullRom) { vis() && refinePath && page == Page.Refinement }
+ override val epsilon by c.setting("ε", 0.3, 0.0..1.0, 0.01) { vis() && refinePath && spline != PathingConfig.Spline.None && page == Page.Refinement }
+
+ override val moveAlongPath by c.setting("Move Along Path", true) { vis() && page == Page.Movement }
+ override val kP by c.setting("P Gain", 0.5, 0.0..2.0, 0.01) { vis() && moveAlongPath && page == Page.Movement }
+ override val kI by c.setting("I Gain", 0.0, 0.0..1.0, 0.01) { vis() && moveAlongPath && page == Page.Movement }
+ override val kD by c.setting("D Gain", 0.2, 0.0..1.0, 0.01) { vis() && moveAlongPath && page == Page.Movement }
+ override val tolerance by c.setting("Node Tolerance", 0.6, 0.01..2.0, 0.05) { vis() && moveAlongPath && page == Page.Movement }
+ override val allowSprint by c.setting("Allow Sprint", true) { vis() && moveAlongPath && page == Page.Movement }
+
+ override val rotation = RotationSettings(c) { page == Page.Rotation }
+
+ override val renderCoarsePath by c.setting("Render Coarse Path", false) { vis() && page == Page.Debug }
+ override val renderRefinedPath by c.setting("Render Refined Path", true) { vis() && page == Page.Debug }
+ override val renderGoal by c.setting("Render Goal", true) { vis() && page == Page.Debug }
+ override val renderGraph by c.setting("Render Graph", false) { vis() && page == Page.Debug }
+ override val renderSuccessors by c.setting("Render Successors", false) { vis() && page == Page.Debug && renderGraph }
+ override val renderPredecessors by c.setting("Render Predecessors", false) { vis() && page == Page.Debug && renderGraph }
+ override val renderInvalidated by c.setting("Render Invalidated", false) { vis() && page == Page.Debug && renderGraph }
+ override val renderPositions by c.setting("Render Positions", false) { vis() && page == Page.Debug && renderGraph }
+ override val renderCost by c.setting("Render Cost", false) { vis() && page == Page.Debug && renderGraph }
+ override val renderG by c.setting("Render G", false) { vis() && page == Page.Debug && renderGraph }
+ override val renderRHS by c.setting("Render RHS", false) { vis() && page == Page.Debug && renderGraph }
+ override val renderKey by c.setting("Render Key", false) { vis() && page == Page.Debug && renderGraph }
+ override val renderQueue by c.setting("Render Queue", false) { vis() && page == Page.Debug && renderGraph }
+ override val maxRenderObjects by c.setting("Max Render Objects", 1000, 0..10_000, 100) { vis() && page == Page.Debug && renderGraph }
+ override val fontScale by c.setting("Font Scale", 0.4, 0.0..2.0, 0.01) { vis() && renderGraph && page == Page.Debug }
+
+ override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc }
+}
diff --git a/common/src/main/kotlin/com/lambda/pathing/goal/Goal.kt b/common/src/main/kotlin/com/lambda/pathing/goal/Goal.kt
new file mode 100644
index 000000000..afd3d9337
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/goal/Goal.kt
@@ -0,0 +1,26 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.goal
+
+import com.lambda.util.world.FastVector
+
+interface Goal {
+ fun inGoal(pos: FastVector): Boolean
+
+ fun heuristic(pos: FastVector): Double
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt b/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt
new file mode 100644
index 000000000..41e9512a9
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt
@@ -0,0 +1,33 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.goal
+
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.distManhattan
+import com.lambda.util.world.distSq
+import com.lambda.util.world.toBlockPos
+
+class SimpleGoal(
+ val pos: FastVector,
+) : Goal {
+ override fun inGoal(pos: FastVector) = pos == this.pos
+
+ override fun heuristic(pos: FastVector) = pos distManhattan this.pos
+
+ override fun toString() = "Goal at (${pos.toBlockPos().toShortString()})"
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt
new file mode 100644
index 000000000..3dfa60c4b
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt
@@ -0,0 +1,367 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.incremental
+
+import com.lambda.graphics.gl.Matrices
+import com.lambda.graphics.gl.Matrices.buildWorldProjection
+import com.lambda.graphics.gl.Matrices.withVertexTransform
+import com.lambda.graphics.renderer.gui.FontRenderer
+import com.lambda.graphics.renderer.gui.FontRenderer.drawString
+import com.lambda.pathing.PathingSettings
+import com.lambda.util.GraphUtil
+import com.lambda.util.math.Vec2d
+import com.lambda.util.math.minus
+import com.lambda.util.math.plus
+import com.lambda.util.math.times
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.string
+import com.lambda.util.world.toCenterVec3d
+import kotlin.math.abs
+import kotlin.math.min
+
+/**
+ * Lazy D* Lite Implementation.
+ *
+ * @param graph The LazyGraph on which we plan.
+ * @param start The agent's initial position.
+ * @param goal The fixed goal vertex.
+ * @param heuristic A consistent (or at least nonnegative) heuristic function h(a,b).
+ */
+class DStarLite(
+ private val graph: LazyGraph,
+ var start: FastVector,
+ val goal: FastVector,
+ val heuristic: (FastVector, FastVector) -> Double,
+ private val connectivity: GraphUtil.Connectivity = GraphUtil.Connectivity.N26,
+) {
+ // gMap[u], rhsMap[u] store g(u) and rhs(u) or default to ∞ if not present
+ private val gMap = mutableMapOf()
+ private val rhsMap = mutableMapOf()
+
+ // Priority queue holding inconsistent vertices.
+ val U = UpdatablePriorityQueue()
+
+ // km accumulates heuristic differences as the start changes.
+ var km = 0.0
+
+ init {
+ initialize()
+ }
+
+ /** Re-initialize the algorithm. */
+ fun initialize() {
+ U.clear()
+ km = 0.0
+ gMap.clear()
+ rhsMap.clear()
+ graph.clear()
+
+ setRHS(goal, 0.0)
+ U.insert(goal, Key(heuristic(start, goal), 0.0))
+ }
+
+ /**
+ * Computes the shortest path from the start node to the goal node using the D* Lite algorithm.
+ * Updates the priority queue and node values iteratively until consistency is achieved or the operation times out.
+ *
+ * @param cutoffTimeout The maximum amount of time (in milliseconds) allowed for the computation to run before timing out. Defaults to 500ms.
+ */
+ fun computeShortestPath(cutoffTimeout: Long = 500L) {
+ val startTime = System.currentTimeMillis()
+
+ fun timedOut() = (System.currentTimeMillis() - startTime) > cutoffTimeout
+
+ fun checkCondition() = U.topKey(Key.INFINITY) < calculateKey(start) || rhs(start) > g(start)
+
+ while (checkCondition() && !timedOut()) {
+ val u = U.top() // Get node with smallest key
+ val kOld = U.topKey(Key.INFINITY) // Key before potential update
+ val kNew = calculateKey(u) // Recalculate key
+
+ when {
+ // Case 1: Key increased (inconsistency detected or km changed priority)
+ kOld < kNew -> {
+ U.update(u, kNew)
+ }
+ // Case 2: Overconsistent state (g > rhs) -> Make consistent
+ g(u) > rhs(u) -> {
+ setG(u, rhs(u)) // Set g = rhs
+ U.remove(u) // Remove from queue, now consistent (g=rhs)
+ // Propagate change to predecessors s
+ graph.predecessors(u).forEach { (s, c) ->
+ if (s != goal) setRHS(s, min(rhs(s), graph.cost(s, u) + g(u)))
+ updateVertex(s)
+ }
+ }
+ // Case 3: Underconsistent state (g <= rhs but needs update, implies g < ∞)
+ // Typically g < rhs, but equality handled by removal in updateVertex.
+ // Here g is likely outdatedly low. Set g = ∞ and update neighbors.
+ else -> {
+ val gOld = g(u)
+ setG(u, INF)
+
+ (graph.predecessors(u).keys + u).forEach { s ->
+ // If rhs(s) was based on the old g(u) path cost
+ if (rhs(s) == graph.cost(s, u) + gOld && s != goal) {
+ // Recalculate rhs(s) based on its *current* successors' g-values
+ setRHS(s, minSuccessorCost(s))
+ }
+ updateVertex(s) // Check consistency of s
+ }
+ }
+ }
+ }
+ }
+
+ /**
+ * Updates the starting point of the pathfinding algorithm to a new position.
+ * If the new starting point is different from the current one, the heuristic cost (`km`) is updated
+ * to account for the change in path distance.
+ *
+ * @param newStart The new starting position represented by a `FastVector`.
+ */
+ fun updateStart(newStart: FastVector) {
+ if (newStart == start) return
+ val lastStart = start
+ start = newStart
+ km += heuristic(lastStart, start)
+ }
+
+ /**
+ * Invalidates a node and updates affected neighbors.
+ * Also updates the neighbors of neighbors to ensure diagonal paths are correctly recalculated.
+ * Optionally prunes the graph after invalidation to remove unnecessary nodes and edges.
+ *
+ * @param u The node to invalidate
+ */
+ fun invalidate(u: FastVector, prune: Boolean = false) {
+ val modified = mutableSetOf(u)
+ (GraphUtil.neighborhood(u, connectivity).keys + u).forEach { v ->
+ val current = graph.neighbors(v)
+ val updated = graph.nodeInitializer(v)
+ val removed = current.filter { w -> w !in updated }
+ removed.forEach { w ->
+ updateEdge(v, w, INF)
+ updateEdge(w, v, INF)
+ }
+ updated.forEach { (w, c) ->
+ updateEdge(v, w, c)
+ updateEdge(w, v, c)
+ }
+ modified.addAll(removed + updated.keys + v)
+ }
+ if (prune) prune(modified)
+ }
+
+ private fun prune(modifiedNodes: Set = emptySet()) {
+ graph.prune(modifiedNodes).forEach {
+ gMap.remove(it)
+ rhsMap.remove(it)
+ }
+ }
+
+ /**
+ * Updates the cost of an edge between two nodes in the graph and adjusts the algorithm's state accordingly.
+ *
+ * @param u The starting node of the edge to update.
+ * @param v The ending node of the edge to update.
+ * @param c The new cost value to set for the edge.
+ */
+ fun updateEdge(u: FastVector, v: FastVector, c: Double) {
+ val cOld = graph.cost(u, v)
+ graph.setCost(u, v, c)
+ when {
+ cOld > c -> if (u != goal) setRHS(u, min(rhs(u), c + g(v)))
+ rhs(u) == cOld + g(v) -> if (u != goal) setRHS(u, minSuccessorCost(u))
+ }
+ updateVertex(u)
+ }
+
+ /**
+ * Retrieves a path from start to goal by always choosing the successor
+ * with the lowest `g(successor) + cost(current, successor)` value.
+ * If no path is found (INF cost), the path stops early.
+ *
+ * @param maxLength The maximum number of nodes to include in the path
+ * @return A list of nodes representing the path from start to goal
+ */
+ fun path(maxLength: Int = 10_000): List {
+ val path = mutableListOf()
+ if (start !in graph) return emptyList()
+ if (rhs(start) == INF) return emptyList()
+
+ var current = start
+ path.add(current)
+
+ var iterations = 0
+ while (current != goal && iterations < maxLength) {
+ iterations++
+ val cheapest = graph.successors(current)
+ .minByOrNull { (succ, cost) -> cost + g(succ) } ?: break
+ current = cheapest.key
+ if (current !in path) path.add(current) else break
+ }
+ return path
+ }
+
+ /** Provides the calculated g-value for a node (cost from start). INF if unknown/unreachable. */
+ fun g(u: FastVector): Double = gMap[u] ?: INF
+
+ /** Provides the calculated rhs-value for a node. INF if unknown/unreachable. */
+ fun rhs(u: FastVector): Double = rhsMap[u] ?: INF
+
+ private fun setG(u: FastVector, gVal: Double) {
+ if (gVal == INF) gMap.remove(u) else gMap[u] = gVal
+ }
+
+ private fun setRHS(u: FastVector, rhsVal: Double) {
+ if (rhsVal == INF) rhsMap.remove(u) else rhsMap[u] = rhsVal
+ }
+
+ /** Internal key calculation using current start and km. */
+ private fun calculateKey(s: FastVector): Key {
+ val minGRHS = min(g(s), rhs(s))
+ return Key(minGRHS + heuristic(start, s) + km, minGRHS)
+ }
+
+ /** Updates a vertex's state in the priority queue based on its consistency (g vs rhs). */
+ fun updateVertex(u: FastVector) {
+ val uInQueue = u in U
+ when {
+ // Inconsistent and in Queue: Update priority
+ g(u) != rhs(u) && uInQueue -> {
+ U.update(u, calculateKey(u))
+ }
+ // Inconsistent and not in Queue: Insert
+ g(u) != rhs(u) && !uInQueue -> {
+ U.insert(u, calculateKey(u))
+ }
+ // Consistent and in Queue: Remove
+ g(u) == rhs(u) && uInQueue -> {
+ U.remove(u)
+ }
+ // Consistent and not in Queue: Do nothing
+ }
+ }
+
+ /** Computes min_{s' in Succ(s)} (c(s, s') + g(s')). */
+ private fun minSuccessorCost(s: FastVector) =
+ graph.successors(s).minOfOrNull { (s1, cost) -> cost + g(s1) } ?: INF
+
+ fun buildDebugInfoRenderer(config: PathingSettings) {
+ if (!config.renderGraph) return
+ val mode = Matrices.ProjRotationMode.TO_CAMERA
+ val scale = config.fontScale
+ graph.nodes.take(config.maxRenderObjects).forEach { origin ->
+ val label = mutableListOf()
+ if (config.renderPositions) label.add(origin.string)
+ if (config.renderG) label.add("g: %.3f".format(g(origin)))
+ if (config.renderRHS) label.add("rhs: %.3f".format(rhs(origin)))
+ if (config.renderKey) label.add("k: ${calculateKey(origin)}")
+ if (config.renderQueue && origin in U) label.add("QUEUED")
+
+ if (label.isNotEmpty()) {
+ val pos = origin.toCenterVec3d()
+ val projection = buildWorldProjection(pos, scale, mode)
+ withVertexTransform(projection) {
+ var height = -0.5 * label.size * (FontRenderer.getHeight() + 2)
+
+ label.forEach {
+ drawString(it, Vec2d(-FontRenderer.getWidth(it) * 0.5, height))
+ height += FontRenderer.getHeight() + 2
+ }
+ }
+ }
+
+ if (config.renderCost) {
+ graph.successors[origin]?.forEach { (neighbor, cost) ->
+ val centerO = origin.toCenterVec3d()
+ val centerN = neighbor.toCenterVec3d()
+ val center = centerO + (centerN - centerO) * (1.0 / 3.0)
+ val projection = buildWorldProjection(center, scale, mode)
+ withVertexTransform(projection) {
+ val msg = "sc: %.3f".format(cost)
+ drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0))
+ }
+ }
+ }
+ }
+ }
+
+ /**
+ * Verifies that the current graph is consistent with a freshly generated graph.
+ * This is useful for ensuring that incremental updates maintain correctness.
+ */
+ fun compareWith(
+ other: DStarLite
+ ): Pair> {
+ // Compare edge consistency between the two graphs
+ val graphDifferences = graph.compareWith(other.graph)
+
+ // Compare g and rhs values for common nodes
+ val commonNodes = graph.nodes.intersect(other.graph.nodes)
+ val wrong = mutableSetOf()
+
+ commonNodes.forEach { node ->
+ val g1 = g(node)
+ val g2 = other.g(node)
+
+ if (abs(g1 - g2) > 1e-6) {
+ wrong.add(ValueDifference(ValueDifference.Value.G, g1, g2))
+ }
+
+ val rhs1 = rhs(node)
+ val rhs2 = other.rhs(node)
+
+ if (abs(rhs1 - rhs2) > 1e-6) {
+ wrong.add(ValueDifference(ValueDifference.Value.RHS, rhs1, rhs2))
+ }
+ }
+
+ return Pair(graphDifferences, wrong)
+ }
+
+ data class ValueDifference(
+ val type: Value,
+ val v1: Double,
+ val v2: Double,
+ ) {
+ enum class Value { G, RHS }
+ override fun toString() = "${type.name} is $v1 but should be $v2"
+ }
+
+ override fun toString() = buildString {
+ appendLine("D* Lite State:")
+ appendLine("Start: ${start.string}, Goal: ${goal.string}, k_m: $km")
+ appendLine("Queue Size: ${U.size()}")
+ if (!U.isEmpty()) {
+ appendLine("Top Key: ${U.topKey(Key.INFINITY)}, Top Node: ${U.top().string}")
+ }
+ appendLine("Graph Size: ${graph.size}")
+ appendLine("Known Nodes (${graph.nodes.size}):")
+ val show = 30
+ graph.nodes.take(show).forEach {
+ appendLine(" ${it.string} g: ${"%.2f".format(g(it))}, rhs: ${"%.2f".format(rhs(it))}, key: ${calculateKey(it)}")
+ }
+ if (graph.nodes.size > show) appendLine(" ... (${graph.nodes.size - show} more nodes)")
+ }
+
+ companion object {
+ private const val INF = Double.POSITIVE_INFINITY
+ }
+}
diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/Key.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/Key.kt
new file mode 100644
index 000000000..eb477e2d8
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/incremental/Key.kt
@@ -0,0 +1,34 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.incremental
+
+/**
+ * Represents the Key used in the D* Lite algorithm.
+ * It's a pair of comparable values, typically Doubles or Ints.
+ * Comparison is done lexicographically as described in Field D*.
+ */
+data class Key(val first: Double, val second: Double) : Comparable {
+ override fun compareTo(other: Key) =
+ compareValuesBy(this, other, { it.first }, { it.second })
+
+ override fun toString() = "(%.3f, %.3f)".format(first, second)
+
+ companion object {
+ val INFINITY = Key(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY)
+ }
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt
new file mode 100644
index 000000000..da80bf2c5
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt
@@ -0,0 +1,274 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.incremental
+
+import com.lambda.graphics.renderer.esp.builders.buildLine
+import com.lambda.graphics.renderer.esp.global.StaticESP
+import com.lambda.pathing.PathingSettings
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.string
+import com.lambda.util.world.toCenterVec3d
+import java.awt.Color
+import java.util.concurrent.ConcurrentHashMap
+import kotlin.math.abs
+
+/**
+ * A 3D graph that uses FastVector (a Long) to represent 3D nodes.
+ *
+ * Runtime Complexity:
+ * - successor(u): O(N), where N is the number of neighbors of node u, due to node initialization.
+ * - predecessors(u): O(N), similar reasoning to successors(u).
+ * - cost(u, v): O(1), constant time lookup after initialization.
+ * - contains(u): O(1), hash map lookup.
+ *
+ * Space Complexity:
+ * - O(V + E), where V = number of nodes (vertices) initialized and E = number of edges stored.
+ * - Additional memory overhead is based on the dynamically expanding hash maps.
+ */
+class LazyGraph(
+ val nodeInitializer: (FastVector) -> Map
+) {
+ val successors = ConcurrentHashMap>()
+ val predecessors = ConcurrentHashMap>()
+
+ val nodes get() = successors.keys + predecessors.keys
+ val size get() = nodes.size
+
+ /** Initializes a node if not already initialized, then returns successors. */
+ fun successors(u: FastVector): ConcurrentHashMap =
+ successors.getOrPut(u) {
+ ConcurrentHashMap(nodeInitializer(u).onEach { (neighbor, cost) ->
+ predecessors.getOrPut(neighbor) { ConcurrentHashMap() }[u] = cost
+ })
+ }
+
+ /** Initializes predecessors by ensuring successors of neighboring nodes. */
+ fun predecessors(u: FastVector): ConcurrentHashMap =
+ predecessors.getOrPut(u) {
+ ConcurrentHashMap(nodeInitializer(u).onEach { (neighbor, cost) ->
+ successors.getOrPut(neighbor) { ConcurrentHashMap() }[u] = cost
+ })
+ }
+
+ fun removeNode(u: FastVector) {
+ successors.remove(u)
+ successors.values.forEach { it.remove(u) }
+ predecessors.remove(u)
+ predecessors.values.forEach { it.remove(u) }
+ }
+
+ private fun removeEdge(u: FastVector, v: FastVector) {
+ successors[u]?.remove(v)
+ predecessors[v]?.remove(u)
+ if (successors[u]?.isEmpty() == true) {
+ successors.remove(u)
+ }
+ if (predecessors[v]?.isEmpty() == true) {
+ predecessors.remove(v)
+ }
+ }
+
+ fun setCost(u: FastVector, v: FastVector, c: Double) {
+ successors.getOrPut(u) { ConcurrentHashMap() }[v] = c
+ predecessors.getOrPut(v) { ConcurrentHashMap() }[u] = c
+ }
+
+ fun clear() {
+ successors.clear()
+ predecessors.clear()
+ }
+
+ /**
+ * Prunes the graph by removing unnecessary edges and nodes.
+ * This helps keep the graph clean and efficient.
+ *
+ * @param modifiedNodes A set of nodes that have been modified and need to be checked for pruning
+ */
+ fun prune(modifiedNodes: Set = emptySet()): Set {
+ val nodesToCheck = if (modifiedNodes.isEmpty()) {
+ // If no modified nodes specified, check all nodes
+ nodes.toSet()
+ } else {
+ // Only check modified nodes and their neighbors
+ val nodesToProcess = mutableSetOf()
+ nodesToProcess.addAll(modifiedNodes)
+
+ // Add neighbors of modified nodes
+ modifiedNodes.forEach { node ->
+ if (node in this) {
+ nodesToProcess.addAll(neighbors(node))
+ }
+ }
+
+ nodesToProcess
+ }
+
+ // First, remove all edges with infinite cost
+ nodesToCheck.forEach { u ->
+ val successorsToRemove = mutableListOf()
+
+ // Find successors with infinite cost
+ successors[u]?.forEach { (v, cost) ->
+ if (cost.isInfinite()) {
+ successorsToRemove.add(v)
+ }
+ }
+
+ // Remove the identified edges
+ successorsToRemove.forEach { v ->
+ removeEdge(u, v)
+ }
+ }
+
+ // Then, remove nodes that only have infinite connections or no connections
+ val nodesToRemove = mutableSetOf()
+
+ nodesToCheck.forEach { node ->
+ // Check if this node has any finite outgoing edges
+ val hasFiniteOutgoing = successors[node]?.any { (_, cost) -> cost.isFinite() } ?: false
+
+ // Check if this node has any finite incoming edges
+ val hasFiniteIncoming = predecessors[node]?.any { (_, cost) -> cost.isFinite() } ?: false
+
+ // If the node has no finite connections, mark it for removal
+ if (!hasFiniteOutgoing && !hasFiniteIncoming) {
+ nodesToRemove.add(node)
+ }
+ }
+
+ // Remove nodes with only infinite connections
+ nodesToRemove.forEach { removeNode(it) }
+ return nodesToRemove
+ }
+
+ /**
+ * Returns the successors of a node without initializing it if it doesn't exist.
+ * This is useful for debugging and testing.
+ */
+ fun getSuccessorsWithoutInitializing(u: FastVector): Map {
+ return successors[u]?.filter { it.value.isFinite() } ?: emptyMap()
+ }
+
+ /**
+ * Returns the predecessors of a node without initializing it if it doesn't exist.
+ * This is useful for debugging and testing.
+ */
+ fun getPredecessorsWithoutInitializing(u: FastVector): Map {
+ return predecessors[u]?.filter { it.value.isFinite() } ?: emptyMap()
+ }
+
+ fun edges(u: FastVector) = successors(u).entries + predecessors(u).entries
+ fun neighbors(u: FastVector): Set = getSuccessorsWithoutInitializing(u).keys + getPredecessorsWithoutInitializing(u).keys
+
+ /** Returns the cost of the edge from u to v (or ∞ if none exists) */
+ fun cost(u: FastVector, v: FastVector): Double = successors(u)[v] ?: Double.POSITIVE_INFINITY
+
+ operator fun contains(u: FastVector): Boolean = nodes.contains(u)
+
+ /**
+ * Result of a graph comparison containing categorized edge differences
+ */
+ data class GraphDifferences(
+ val missingEdges: Set, // Edges that should exist but don't
+ val wrongEdges: Set, // Edges that exist but have incorrect costs
+ val excessEdges: Set // Edges that shouldn't exist but do
+ ) {
+ /**
+ * Represents an edge difference between two graphs
+ */
+ data class Edge(
+ val source: FastVector,
+ val target: FastVector,
+ val thisGraphCost: Double?, // null if edge doesn't exist in this graph
+ val otherGraphCost: Double? // null if edge doesn't exist in other graph
+ ) {
+ override fun toString(): String = when {
+ thisGraphCost == null -> "Edge from ${source.string} to ${target.string} with cost $otherGraphCost"
+ otherGraphCost == null -> "Edge from ${source.string} to ${target.string} with cost $thisGraphCost"
+ else -> "Edge from ${source.string} to ${target.string} (cost: $thisGraphCost vs $otherGraphCost)"
+ }
+ }
+
+ val hasAnyDifferences: Boolean
+ get() = missingEdges.isNotEmpty() || wrongEdges.isNotEmpty() /*|| excessEdges.isNotEmpty()*/
+
+ override fun toString(): String {
+ val parts = mutableListOf()
+ if (missingEdges.isNotEmpty()) {
+ parts.add("Missing edges: ${missingEdges.joinToString("\n ", prefix = "\n ")}")
+ }
+ if (wrongEdges.isNotEmpty()) {
+ parts.add("Wrong edges: ${wrongEdges.joinToString("\n ", prefix = "\n ")}")
+ }
+ if (excessEdges.isNotEmpty()) {
+ parts.add("Excess edges: ${excessEdges.joinToString("\n ", prefix = "\n ")}")
+ }
+ return if (parts.isEmpty()) "No differences" else parts.joinToString("\n")
+ }
+ }
+
+ /**
+ * Compares this graph with another graph for edge consistency.
+ *
+ * @param other The other graph to compare with
+ * @return Categorized edge differences between the two graphs
+ */
+ fun compareWith(other: LazyGraph): GraphDifferences {
+ val missing = mutableSetOf()
+ val wrong = mutableSetOf()
+ val excess = mutableSetOf()
+
+ nodes.union(other.nodes).forEach { node ->
+ val thisSuccessors = getSuccessorsWithoutInitializing(node)
+ val otherSuccessors = other.getSuccessorsWithoutInitializing(node)
+
+ // Check for missing and wrong edges
+ otherSuccessors.forEach { (neighbor, otherCost) ->
+ val thisCost = thisSuccessors[neighbor]
+ if (thisCost == null) {
+ missing.add(GraphDifferences.Edge(node, neighbor, null, otherCost))
+ } else if (abs(thisCost - otherCost) > 1e-9) {
+ wrong.add(GraphDifferences.Edge(node, neighbor, thisCost, otherCost))
+ }
+ }
+
+ // Check for excess edges
+ thisSuccessors.forEach { (neighbor, thisCost) ->
+ if (!otherSuccessors.containsKey(neighbor)) {
+ excess.add(GraphDifferences.Edge(node, neighbor, thisCost, null))
+ }
+ }
+ }
+
+ return GraphDifferences(missing, wrong, excess)
+ }
+
+ fun render(renderer: StaticESP, config: PathingSettings) {
+ if (!config.renderGraph) return
+ if (config.renderSuccessors) successors.entries.take(config.maxRenderObjects).forEach { (origin, neighbors) ->
+ neighbors.forEach { (neighbor, _) ->
+ renderer.buildLine(origin.toCenterVec3d(), neighbor.toCenterVec3d(), Color.PINK)
+ }
+ }
+ if (config.renderPredecessors) predecessors.entries.take(config.maxRenderObjects).forEach { (origin, neighbors) ->
+ neighbors.forEach { (neighbor, _) ->
+ renderer.buildLine(origin.toCenterVec3d(), neighbor.toCenterVec3d(), Color.PINK)
+ }
+ }
+ }
+}
diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/UpdatablePriorityQueue.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/UpdatablePriorityQueue.kt
new file mode 100644
index 000000000..4be4f47c8
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/incremental/UpdatablePriorityQueue.kt
@@ -0,0 +1,172 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.incremental
+
+import java.util.*
+import kotlin.NoSuchElementException
+import kotlin.collections.HashMap
+
+/**
+ * A Priority Queue implementation supporting efficient updates and removals,
+ * suitable for algorithms like D* Lite.
+ *
+ * @param V The type of the values (vertices/states) stored in the queue.
+ * @param K The type of the keys (priorities) used for ordering, must be Comparable.
+ */
+class UpdatablePriorityQueue> {
+
+ // Internal data class to hold value-key pairs within the Java PriorityQueue
+ private data class Entry>(val value: V, var key: K) : Comparable> {
+ override fun compareTo(other: Entry): Int = this.key.compareTo(other.key)
+ }
+
+ // The core priority queue storing Entry objects, ordered by key
+ private val queue = PriorityQueue>()
+ // HashMap to map values to their corresponding Entry objects for quick access
+ private val entryMap = HashMap>()
+
+ /**
+ * Inserts a vertex/value 's' into the priority queue 'U' with priority 'k'.
+ * Does nothing if the value already exists with the same key.
+ * Updates the key if the value exists with a different key.
+ * Corresponds to U.Insert(s, k) and parts of U.Update(s, k).
+ *
+ * @param value The value (vertex) to insert.
+ * @param key The priority key associated with the value.
+ */
+ fun insert(value: V, key: K) {
+ if (entryMap.containsKey(value)) {
+ update(value, key) // Handle as an update if it already exists
+ } else {
+ val entry = Entry(value, key)
+ entryMap[value] = entry
+ queue.add(entry)
+ }
+ }
+
+ /**
+ * Changes the priority of vertex 's' in priority queue 'U' to 'k'.
+ * Corresponds to U.Update(s, k).
+ * It does nothing if the current priority of vertex s already equals k.
+ *
+ * @param value The value (vertex) whose key needs updating.
+ * @param newKey The new priority key.
+ * @throws NoSuchElementException if the value is not found in the queue.
+ */
+ fun update(value: V, newKey: K) {
+ val entry = entryMap[value] ?: throw NoSuchElementException("Value not found in priority queue for update.")
+
+ if (entry.key == newKey) {
+ return // Key is the same, do nothing as per description
+ }
+
+ // Standard PriorityQueue doesn't support direct update.
+ // We remove the old entry and add a new one with the updated key.
+ queue.remove(entry)
+ entry.key = newKey // Update the key in the existing entry object
+ queue.add(entry) // Re-add the updated entry
+ }
+
+ /**
+ * Removes vertex 's' from priority queue 'U'.
+ * Corresponds to U.Remove(s).
+ *
+ * @param value The value (vertex) to remove.
+ * @return True if the value was removed, false otherwise.
+ */
+ fun remove(value: V): Boolean {
+ val entry = entryMap.remove(value)
+ return if (entry != null) {
+ queue.remove(entry)
+ } else {
+ false
+ }
+ }
+
+ /**
+ * Deletes the vertex with the smallest priority in priority queue 'U' and returns the vertex.
+ * Corresponds to U.Pop().
+ *
+ * @return The value (vertex) with the smallest key.
+ * @throws NoSuchElementException if the queue is empty.
+ */
+ fun pop(): V {
+ if (isEmpty()) throw NoSuchElementException("Priority queue is empty.")
+ val entry = queue.poll()
+ entryMap.remove(entry.value)
+ return entry.value
+ }
+
+ /**
+ * Returns a vertex with the smallest priority of all vertices in priority queue 'U'.
+ * Corresponds to U.Top().
+ *
+ * @return The value (vertex) with the smallest key.
+ * @throws NoSuchElementException if the queue is empty.
+ */
+ fun top(): V {
+ if (isEmpty()) throw NoSuchElementException("Priority queue is empty.")
+ return queue.peek().value
+ }
+
+ /**
+ * Returns the smallest priority of all vertices in priority queue 'U'.
+ * Corresponds to U.TopKey().
+ * Returns a representation of infinity if the queue is empty (specific to D* Lite context).
+ *
+ * @param infinityKey The key value representing infinity (e.g., DStarLiteKey.INFINITY).
+ * @return The smallest key, or infinityKey if the queue is empty.
+ */
+ fun topKey(infinityKey: K) =
+ if (isEmpty()) {
+ infinityKey
+ } else {
+ queue.peek().key
+ }
+
+ /**
+ * Checks if the priority queue contains the specified value (vertex).
+ *
+ * @param value The value to check for.
+ * @return True if the value is present, false otherwise.
+ */
+ operator fun contains(value: V) = entryMap.containsKey(value)
+
+ /**
+ * Checks if the priority queue is empty.
+ *
+ * @return True if the queue contains no elements, false otherwise.
+ */
+ fun isEmpty() = queue.isEmpty()
+
+ /**
+ * Returns the number of elements in the priority queue.
+ *
+ * @return The size of the queue.
+ */
+ fun size() = queue.size
+
+ /**
+ * Removes all elements from the priority queue.
+ * Corresponds to U <- empty set.
+ */
+ fun clear() {
+ queue.clear()
+ entryMap.clear()
+ }
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/move/BreakMove.kt b/common/src/main/kotlin/com/lambda/pathing/move/BreakMove.kt
new file mode 100644
index 000000000..b64bb98fd
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/move/BreakMove.kt
@@ -0,0 +1,33 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.move
+
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.toBlockPos
+
+class BreakMove(
+ override val pos: FastVector,
+ override val hCost: Double,
+ override val nodeType: NodeType,
+ override val feetY: Double,
+ override val cost: Double
+) : Move() {
+ override val name: String = "Break"
+
+ override fun toString() = "BreakMove(pos=(${pos.toBlockPos().toShortString()}), hCost=$hCost, nodeType=$nodeType, feetY=$feetY, cost=$cost)"
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/move/Move.kt b/common/src/main/kotlin/com/lambda/pathing/move/Move.kt
new file mode 100644
index 000000000..7779ce729
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/move/Move.kt
@@ -0,0 +1,53 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.move
+
+import com.lambda.pathing.Path
+import com.lambda.task.Task
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.toBlockPos
+import net.minecraft.util.math.Vec3d
+
+abstract class Move : Comparable, Task() {
+ abstract val pos: FastVector
+ abstract val hCost: Double
+ abstract val nodeType: NodeType
+ abstract val feetY: Double
+ abstract val cost: Double
+
+ var predecessor: Move? = null
+ var gCost: Double = Double.POSITIVE_INFINITY
+
+ // use updateable lazy and recompute on gCost change
+ private val fCost get() = gCost + hCost
+
+ val bottomPos: Vec3d get() = Vec3d.ofBottomCenter(pos.toBlockPos())
+
+ override fun compareTo(other: Move) =
+ fCost.compareTo(other.fCost)
+
+ fun createPathToSource(): Path {
+ val path = Path()
+ var current: Move? = this
+ while (current != null) {
+ path.prepend(current)
+ current = current.predecessor
+ }
+ return path
+ }
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt
new file mode 100644
index 000000000..cc4e600fb
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt
@@ -0,0 +1,165 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.move
+
+import com.lambda.context.SafeContext
+import com.lambda.pathing.PathingConfig
+import com.lambda.pathing.goal.Goal
+import com.lambda.util.BlockUtils.blockState
+import com.lambda.util.BlockUtils.fluidState
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.WorldUtils.traversable
+import com.lambda.util.world.add
+import com.lambda.util.world.fastVectorOf
+import com.lambda.util.world.length
+import com.lambda.util.world.toBlockPos
+import net.minecraft.block.BlockState
+import net.minecraft.block.Blocks
+import net.minecraft.block.CampfireBlock
+import net.minecraft.block.DoorBlock
+import net.minecraft.block.FenceGateBlock
+import net.minecraft.block.LeavesBlock
+import net.minecraft.block.TrapdoorBlock
+import net.minecraft.enchantment.EnchantmentHelper
+import net.minecraft.enchantment.Enchantments
+import net.minecraft.entity.EquipmentSlot
+import net.minecraft.entity.effect.StatusEffects
+import net.minecraft.item.Items
+import net.minecraft.registry.tag.BlockTags
+import net.minecraft.registry.tag.FluidTags
+import net.minecraft.util.math.BlockPos
+import net.minecraft.util.math.Box
+import net.minecraft.util.math.Direction
+import net.minecraft.util.math.EightWayDirection
+import kotlin.reflect.KFunction1
+
+object MoveFinder {
+ private val nodeTypeCache = HashMap()
+
+ fun SafeContext.moveOptions(origin: FastVector, heuristic: KFunction1, config: PathingConfig): Set {
+ if (!traversable(origin.toBlockPos())) return setOf()
+ return EightWayDirection.entries.flatMap { direction ->
+ (-1..1).mapNotNull { y ->
+ getPathNode(heuristic, origin, direction, y, config)
+ }
+ }.toSet()
+ }
+
+ private fun SafeContext.getPathNode(
+ heuristic: KFunction1,
+ origin: FastVector,
+ direction: EightWayDirection,
+ height: Int,
+ config: PathingConfig
+ ): Move? {
+ val offset = fastVectorOf(direction.offsetX, height, direction.offsetZ)
+ val diagonal = direction.ordinal.mod(2) == 1
+ val checkingPos = origin.add(offset)
+ val checkingBlockPos = checkingPos.toBlockPos()
+ val originBlockPos = origin.toBlockPos()
+ if (!world.worldBorder.contains(checkingBlockPos)) return null
+
+ val nodeType = findPathType(checkingPos)
+ if (nodeType == NodeType.BLOCKED) return null
+
+ val clear = if (diagonal) {
+ val enclose = when {
+ checkingBlockPos.y == originBlockPos.y -> Box.enclosing(originBlockPos.up(), checkingBlockPos)
+ checkingBlockPos.y < originBlockPos.y -> Box.enclosing(originBlockPos.up(), checkingBlockPos.up())
+ else -> Box.enclosing(originBlockPos.up(2), checkingBlockPos)
+ }
+ traversable(checkingBlockPos) && world.isSpaceEmpty(enclose.contract(0.01))
+ } else {
+ traversable(checkingBlockPos)
+ }
+ if (!clear) return null
+
+ val hCost = heuristic(checkingPos) /** nodeType.penalty*/
+ val cost = offset.length()
+ val currentFeetY = getFeetY(checkingBlockPos)
+
+ return when {
+// cost == Double.POSITIVE_INFINITY -> BreakMove(checkingPos, hCost, nodeType, currentFeetY, cost)
+// (currentFeetY - origin.feetY) > player.stepHeight -> ParkourMove(checkingPos, hCost, nodeType, currentFeetY, cost)
+ else -> TraverseMove(checkingPos, hCost, nodeType, currentFeetY, cost)
+ }
+ }
+
+ fun SafeContext.findPathType(pos: FastVector) = nodeTypeCache.getOrPut(pos) {
+ val blockPos = pos.toBlockPos()
+ val state = blockState(blockPos)
+ val fluidState = fluidState(blockPos)
+
+ when {
+ state.isAir -> NodeType.OPEN
+ fluidState.isIn(FluidTags.WATER) -> NodeType.WATER
+ state.isFullCube(world, blockPos) -> NodeType.BLOCKED
+ fluidState.isIn(FluidTags.LAVA) -> NodeType.LAVA
+ state.isIn(BlockTags.LEAVES) && !state.getOrEmpty(LeavesBlock.PERSISTENT).orElse(false) -> NodeType.LEAVES
+ state.isOf(Blocks.LADDER) -> NodeType.LADDER
+ state.isOf(Blocks.SCAFFOLDING) -> NodeType.SCAFFOLDING
+ state.isOf(Blocks.POWDER_SNOW) -> when {
+ player.getEquippedStack(EquipmentSlot.FEET).isOf(Items.LEATHER_BOOTS) -> NodeType.DANGER_POWDER_SNOW
+ else -> NodeType.POWDER_SNOW
+ }
+ state.isOf(Blocks.BIG_DRIPLEAF) -> NodeType.DRIP_LEAF
+ state.isOf(Blocks.CACTUS) || state.isOf(Blocks.SWEET_BERRY_BUSH) -> NodeType.DAMAGE_OTHER
+ state.isOf(Blocks.HONEY_BLOCK) -> NodeType.STICKY_HONEY
+ state.isOf(Blocks.SLIME_BLOCK) -> NodeType.SLIME
+ state.isOf(Blocks.SOUL_SAND) -> NodeType.SOUL_SAND
+ state.isOf(Blocks.SOUL_SOIL) && EnchantmentHelper.getEquipmentLevel(Enchantments.SOUL_SPEED, player) > 0 -> NodeType.SOUL_SOIL
+ state.isOf(Blocks.WITHER_ROSE) && state.isOf(Blocks.POINTED_DRIPSTONE) -> NodeType.DAMAGE_CAUTIOUS
+ state.inflictsFireDamage() -> when {
+ !player.hasStatusEffect(StatusEffects.FIRE_RESISTANCE) -> NodeType.DAMAGE_FIRE
+ else -> NodeType.DANGER_FIRE
+ }
+ state.isIn(BlockTags.DOORS) -> when {
+ state.getOrEmpty(DoorBlock.OPEN).orElse(false) -> NodeType.DOOR_OPEN
+ state.isIn(BlockTags.WOODEN_DOORS) -> NodeType.DOOR_WOOD_CLOSED
+ else -> NodeType.DOOR_IRON_CLOSED
+ }
+ state.isIn(BlockTags.TRAPDOORS) -> when {
+ state.getOrEmpty(TrapdoorBlock.OPEN).orElse(false) -> NodeType.TRAPDOOR_OPEN
+ else -> NodeType.TRAPDOOR_CLOSED
+ }
+ state.isIn(BlockTags.FENCE_GATES) -> when {
+ state.getOrEmpty(FenceGateBlock.OPEN).orElse(false) -> NodeType.FENCE_GATE_OPEN
+ else -> NodeType.FENCE_GATE_CLOSED
+ }
+ state.isIn(BlockTags.FENCES) || state.isIn(BlockTags.WALLS) -> NodeType.FENCE
+ else -> NodeType.OPEN
+ }
+
+ }
+
+ private fun BlockState.inflictsFireDamage() =
+ isIn(BlockTags.FIRE)
+ || isOf(Blocks.LAVA)
+ || isOf(Blocks.MAGMA_BLOCK)
+ || CampfireBlock.isLitCampfire(this)
+ || isOf(Blocks.LAVA_CAULDRON)
+
+ fun SafeContext.getFeetY(pos: BlockPos): Double {
+ val blockPos = pos.down()
+ val voxelShape = blockState(blockPos).getCollisionShape(world, blockPos)
+ return blockPos.y.toDouble() + (if (voxelShape.isEmpty) 0.0 else voxelShape.getMax(Direction.Axis.Y))
+ }
+
+ fun clear(u: FastVector) = nodeTypeCache.remove(u)
+ fun clean() = nodeTypeCache.clear()
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/move/NodeType.kt b/common/src/main/kotlin/com/lambda/pathing/move/NodeType.kt
new file mode 100644
index 000000000..a67452aed
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/move/NodeType.kt
@@ -0,0 +1,50 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.move
+
+enum class NodeType(val penalty: Float) {
+ BLOCKED(-1.0f),
+ OPEN(0.0f),
+ WALKABLE(0.0f),
+ WALKABLE_DOOR(0.0f),
+ TRAPDOOR_CLOSED(0.0f),
+ TRAPDOOR_OPEN(0.0f),
+ POWDER_SNOW(-1.0f),
+ DANGER_POWDER_SNOW(0.0f),
+ FENCE(-1.0f),
+ FENCE_GATE_OPEN(0.0f),
+ FENCE_GATE_CLOSED(-1.0f),
+ LAVA(-1.0f),
+ WATER(8.0f),
+ DANGER_FIRE(8.0f),
+ DAMAGE_FIRE(16.0f),
+ DANGER_OTHER(8.0f),
+ DAMAGE_OTHER(-1.0f),
+ DOOR_OPEN(0.0f),
+ DOOR_WOOD_CLOSED(-1.0f),
+ DOOR_IRON_CLOSED(-1.0f),
+ LEAVES(-1.0f),
+ STICKY_HONEY(8.0f),
+ SLIME(0.0f),
+ SOUL_SAND(0.0f),
+ SOUL_SOIL(0.0f),
+ DAMAGE_CAUTIOUS(0.0f),
+ LADDER(0.0f),
+ SCAFFOLDING(0.0f),
+ DRIP_LEAF(8.0f)
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/move/ParkourMove.kt b/common/src/main/kotlin/com/lambda/pathing/move/ParkourMove.kt
new file mode 100644
index 000000000..3c4b55e2e
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/move/ParkourMove.kt
@@ -0,0 +1,30 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.move
+
+import com.lambda.util.world.FastVector
+
+class ParkourMove(
+ override val pos: FastVector,
+ override val hCost: Double,
+ override val nodeType: NodeType,
+ override val feetY: Double,
+ override val cost: Double
+) : Move() {
+ override val name: String = "Parkour"
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/move/SwimMove.kt b/common/src/main/kotlin/com/lambda/pathing/move/SwimMove.kt
new file mode 100644
index 000000000..a21660a07
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/move/SwimMove.kt
@@ -0,0 +1,30 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.move
+
+import com.lambda.util.world.FastVector
+
+class SwimMove(
+ override val pos: FastVector,
+ override val hCost: Double,
+ override val nodeType: NodeType,
+ override val feetY: Double,
+ override val cost: Double
+) : Move() {
+ override val name: String = "Swim"
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/pathing/move/TraverseMove.kt b/common/src/main/kotlin/com/lambda/pathing/move/TraverseMove.kt
new file mode 100644
index 000000000..bd665dc46
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/pathing/move/TraverseMove.kt
@@ -0,0 +1,33 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.pathing.move
+
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.toBlockPos
+
+class TraverseMove(
+ override val pos: FastVector,
+ override val hCost: Double,
+ override val nodeType: NodeType,
+ override val feetY: Double,
+ override val cost: Double
+) : Move() {
+ override val name: String = "Traverse"
+
+ override fun toString() = "TraverseMove(pos=(${pos.toBlockPos().toShortString()}), hCost=$hCost, nodeType=$nodeType, feetY=$feetY, cost=$cost)"
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt
new file mode 100644
index 000000000..56f6050a0
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt
@@ -0,0 +1,104 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.util
+
+import com.lambda.pathing.incremental.LazyGraph
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.dist
+import com.lambda.util.world.fastVectorOf
+import com.lambda.util.world.string
+import com.lambda.util.world.x
+import com.lambda.util.world.y
+import com.lambda.util.world.z
+import kotlin.math.abs
+import kotlin.math.sqrt
+
+object GraphUtil {
+ // Simple Manhattan distance heuristic
+ fun manhattanHeuristic(a: FastVector, b: FastVector): Double {
+ return (abs(a.x - b.x) + abs(a.y - b.y) + abs(a.z - b.z)).toDouble()
+ }
+
+ // Euclidean distance heuristic (more accurate for diagonal movement)
+ fun euclideanHeuristic(a: FastVector, b: FastVector): Double {
+ val dx = (a.x - b.x).toDouble()
+ val dy = (a.y - b.y).toDouble()
+ val dz = (a.z - b.z).toDouble()
+ return sqrt(dx * dx + dy * dy + dz * dz)
+ }
+
+ // 6-connectivity (Axis-aligned moves only)
+ fun createGridGraph6Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph {
+ return LazyGraph { node ->
+ if (node in blockedNodes) emptyMap()
+ else n6(node).filterKeys { it !in blockedNodes }
+ }
+ }
+
+ // 18-connectivity (Axis-aligned + Face diagonal moves)
+ fun createGridGraph18Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph {
+ return LazyGraph { node ->
+ if (node in blockedNodes) emptyMap()
+ else n18(node).filterKeys { it !in blockedNodes }
+ }
+ }
+
+ // 26-connectivity (Axis-aligned + Face diagonal + Cube diagonal moves)
+ fun createGridGraph26Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph {
+ return LazyGraph { node ->
+ if (node in blockedNodes) emptyMap()
+ else n26(node).filterKeys { it !in blockedNodes }
+ }
+ }
+
+ fun neighborhood(o: FastVector, conn: Connectivity) = neighborhood(o, conn.minDistSq, conn.maxDistSq)
+ fun n6(o: FastVector) = neighborhood(o, minDistSq = 1, maxDistSq = 1)
+ fun n18(o: FastVector) = neighborhood(o, minDistSq = 1, maxDistSq = 2)
+ fun n26(o: FastVector) = neighborhood(o, minDistSq = 1, maxDistSq = 3)
+
+ fun neighborhood(origin: FastVector, minDistSq: Int = 1, maxDistSq: Int = 1): Map =
+ (-1..1).flatMap { dx ->
+ (-1..1).flatMap { dy ->
+ (-1..1).mapNotNull { dz ->
+ val distSq = dx*dx + dy*dy + dz*dz
+ if (distSq in minDistSq..maxDistSq) {
+ val neighbor = fastVectorOf(origin.x + dx, origin.y + dy, origin.z + dz)
+ val cost = when (distSq) {
+ 1 -> 1.0
+ 2 -> COST_SQRT_2
+ 3 -> COST_SQRT_3
+ else -> error("Unexpected squared distance: $distSq")
+ }
+ neighbor to cost
+ } else null
+ }
+ }
+ }.toMap()
+
+ fun List.string() = joinToString(" -> ") { it.string }
+ fun List.length() = zipWithNext { a, b -> a dist b }.sum()
+
+ private const val COST_SQRT_2 = 1.4142135623730951
+ private const val COST_SQRT_3 = 1.7320508075688772
+
+ enum class Connectivity(val minDistSq: Int, val maxDistSq: Int) {
+ N6(minDistSq = 1, maxDistSq = 1),
+ N18(minDistSq = 1, maxDistSq = 2),
+ N26(minDistSq = 1, maxDistSq = 3);
+ }
+}
\ No newline at end of file
diff --git a/common/src/main/kotlin/com/lambda/util/collections/UpdatableLazy.kt b/common/src/main/kotlin/com/lambda/util/collections/UpdatableLazy.kt
index f70260392..0e065c075 100644
--- a/common/src/main/kotlin/com/lambda/util/collections/UpdatableLazy.kt
+++ b/common/src/main/kotlin/com/lambda/util/collections/UpdatableLazy.kt
@@ -31,21 +31,28 @@ class UpdatableLazy(private val initializer: () -> T) {
private var _value: T? = null
/**
- * Lazily initializes and retrieves a value of type [T] using the provided initializer function.
- * If the value has not been initialized previously, the initializer function is called
- * to generate the value, which is then cached for subsequent accesses.
+ * Retrieves the lazily initialized value of type [T]. If the value has not been
+ * initialized yet, it is computed using the initializer function, stored, and then returned.
*
- * This property ensures that the value is only initialized when it is first accessed,
- * and maintains its state until explicitly updated or reset.
+ * This property supports lazy initialization where the value is generated only on first access.
+ * Once initialized, the value is cached for subsequent accesses, ensuring consistent behavior
+ * across invocations. If the value needs to be recomputed intentionally, it can be reset externally
+ * using the appropriate function in the containing class.
*
- * @return The lazily initialized value, or `null` if the initializer function
- * is designed to produce a `null` result or has not been called yet.
+ * @return The currently initialized or newly computed value of type [T].
*/
- val value: T?
- get() {
- if (_value == null) _value = initializer()
- return _value
- }
+ val value: T get() = _value ?: initializer().also { _value = it }
+
+ /**
+ * Clears the currently stored value, setting it to null.
+ *
+ * This function is used to explicitly reset the stored value, effectively marking
+ * it as uninitialized. It can subsequently be re-initialized through lazy evaluation
+ * when accessed again.
+ */
+ fun clear() {
+ _value = null
+ }
/**
* Resets the current value to a new value generated by the initializer function.
diff --git a/common/src/main/kotlin/com/lambda/util/world/Position.kt b/common/src/main/kotlin/com/lambda/util/world/Position.kt
index f8208639a..b18962744 100644
--- a/common/src/main/kotlin/com/lambda/util/world/Position.kt
+++ b/common/src/main/kotlin/com/lambda/util/world/Position.kt
@@ -21,6 +21,8 @@ import net.minecraft.util.math.BlockPos
import net.minecraft.util.math.Direction
import net.minecraft.util.math.Vec3d
import net.minecraft.util.math.Vec3i
+import kotlin.math.abs
+import kotlin.math.sqrt
/**
* Represents a position in the world encoded as a long.
@@ -113,20 +115,26 @@ infix fun FastVector.addY(value: Int): FastVector = setY(y + value)
*/
infix fun FastVector.addZ(value: Int): FastVector = setZ(z + value)
+fun FastVector.offset(x: Int, y: Int, z: Int): FastVector = fastVectorOf(this.x + x, this.y + y, this.z + z)
+
+fun FastVector.manhattanLength() = abs(x) + abs(y) + abs(z)
+
+fun FastVector.length() = sqrt((abs(x * x) + abs(y * y) + abs(z * z)).toDouble())
+
/**
* Adds the given vector to the position.
*/
-infix fun FastVector.plus(vec: FastVector): FastVector = fastVectorOf(x + vec.x, y + vec.y, z + vec.z)
+infix fun FastVector.add(vec: FastVector): FastVector = fastVectorOf(x + vec.x, y + vec.y, z + vec.z)
/**
* Adds the given vector to the position.
*/
-infix fun FastVector.plus(vec: Vec3i): FastVector = fastVectorOf(x + vec.x, y + vec.y, z + vec.z)
+operator fun FastVector.plus(vec: Vec3i): FastVector = fastVectorOf(x + vec.x, y + vec.y, z + vec.z)
/**
* Adds the given vector to the position.
*/
-infix fun FastVector.plus(vec: Vec3d): FastVector =
+operator fun FastVector.plus(vec: Vec3d): FastVector =
fastVectorOf(x + vec.x.toLong(), y + vec.y.toLong(), z + vec.z.toLong())
/**
@@ -137,12 +145,12 @@ infix fun FastVector.minus(vec: FastVector): FastVector = fastVectorOf(x - vec.x
/**
* Subtracts the given vector from the position.
*/
-infix fun FastVector.minus(vec: Vec3i): FastVector = fastVectorOf(x - vec.x, y - vec.y, z - vec.z)
+operator fun FastVector.minus(vec: Vec3i): FastVector = fastVectorOf(x - vec.x, y - vec.y, z - vec.z)
/**
* Subtracts the given vector from the position.
*/
-infix fun FastVector.minus(vec: Vec3d): FastVector =
+operator fun FastVector.minus(vec: Vec3d): FastVector =
fastVectorOf(x - vec.x.toLong(), y - vec.y.toLong(), z - vec.z.toLong())
/**
@@ -178,6 +186,8 @@ infix fun FastVector.remainder(scalar: Int): FastVector = fastVectorOf(x % scala
infix fun FastVector.remainder(scalar: Double): FastVector =
fastVectorOf((x % scalar).toLong(), (y % scalar).toLong(), (z % scalar).toLong())
+infix fun FastVector.dist(other: FastVector): Double = sqrt(distSq(other))
+
/**
* Returns the squared distance between this position and the other.
*/
@@ -188,6 +198,16 @@ infix fun FastVector.distSq(other: FastVector): Double {
return (dx * dx + dy * dy + dz * dz).toDouble()
}
+/**
+ * Returns the Manhattan distance between this position and the other.
+ */
+infix fun FastVector.distManhattan(other: FastVector): Double {
+ val dx = x - other.x
+ val dy = y - other.y
+ val dz = z - other.z
+ return (abs(dx) + abs(dy) + abs(dz)).toDouble()
+}
+
/**
* Returns the squared distance between this position and the Vec3i.
*/
@@ -229,6 +249,11 @@ fun Vec3d.toFastVec(): FastVector = fastVectorOf(x.toLong(), y.toLong(), z.toLon
*/
fun FastVector.toVec3d(): Vec3d = Vec3d(x.toDouble(), y.toDouble(), z.toDouble())
+/**
+ * [FastVector] to a centered [Vec3d]
+ */
+fun FastVector.toCenterVec3d(): Vec3d = Vec3d(x + 0.5, y + 0.5, z + 0.5)
+
/**
* Converts the [FastVector] into a [BlockPos].
*/
@@ -241,3 +266,6 @@ internal fun Long.bitSetTo(value: Long, position: Int, length: Int): Long {
val mask = (1L shl length) - 1L
return this and (mask shl position).inv() or (value and mask shl position)
}
+
+val FastVector.string: String
+ get() = "($x, $y, $z)"
diff --git a/common/src/main/kotlin/com/lambda/util/world/WorldDsl.kt b/common/src/main/kotlin/com/lambda/util/world/SearchDsl.kt
similarity index 96%
rename from common/src/main/kotlin/com/lambda/util/world/WorldDsl.kt
rename to common/src/main/kotlin/com/lambda/util/world/SearchDsl.kt
index ded36d784..ae61bfaa6 100644
--- a/common/src/main/kotlin/com/lambda/util/world/WorldDsl.kt
+++ b/common/src/main/kotlin/com/lambda/util/world/SearchDsl.kt
@@ -20,11 +20,11 @@ package com.lambda.util.world
import com.lambda.context.SafeContext
import com.lambda.core.annotations.InternalApi
import com.lambda.util.math.distSq
-import com.lambda.util.world.WorldUtils.internalGetBlockEntities
-import com.lambda.util.world.WorldUtils.internalGetEntities
-import com.lambda.util.world.WorldUtils.internalGetFastEntities
-import com.lambda.util.world.WorldUtils.internalSearchBlocks
-import com.lambda.util.world.WorldUtils.internalSearchFluids
+import com.lambda.util.world.SearchUtils.internalGetBlockEntities
+import com.lambda.util.world.SearchUtils.internalGetEntities
+import com.lambda.util.world.SearchUtils.internalGetFastEntities
+import com.lambda.util.world.SearchUtils.internalSearchBlocks
+import com.lambda.util.world.SearchUtils.internalSearchFluids
import net.minecraft.block.BlockState
import net.minecraft.block.entity.BlockEntity
import net.minecraft.entity.Entity
diff --git a/common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt b/common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt
new file mode 100644
index 000000000..a3107c3c1
--- /dev/null
+++ b/common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt
@@ -0,0 +1,278 @@
+/*
+ * Copyright 2024 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.util.world
+
+import com.lambda.context.SafeContext
+import com.lambda.core.annotations.InternalApi
+import com.lambda.util.extension.filterPointer
+import com.lambda.util.extension.getBlockState
+import com.lambda.util.extension.getFluidState
+import net.minecraft.block.BlockState
+import net.minecraft.block.entity.BlockEntity
+import net.minecraft.entity.Entity
+import net.minecraft.fluid.Fluid
+import net.minecraft.fluid.FluidState
+import net.minecraft.util.math.ChunkSectionPos
+import kotlin.math.ceil
+import kotlin.reflect.KClass
+
+/**
+ * Utility functions for working with the Minecraft world.
+ *
+ * This object employs a pass-by-reference model, allowing functions to modify
+ * data structures passed to them rather than creating new ones.
+ *
+ * This approach offers two main benefits, being performance and reduce GC overhead
+ *
+ * @see IBM - Pass By Reference
+ * @see Florida State University - Pass By Reference vs. Pass By Value
+ * @see IBM - Garbage Collection Impacts on Java Performance
+ * @see Medium - GC and Its Effect on Java Performance
+ */
+object SearchUtils {
+ /**
+ * A magic vector that can be used to represent a single block
+ * It is the same as `fastVectorOf(1, 1, 1)`
+ */
+ @InternalApi
+ const val MAGICVECTOR = 274945015809L
+
+ /**
+ * Gets all entities of type [T] within a specified distance from a position.
+ *
+ * This function retrieves entities of type [T] within a specified distance from a given position. It efficiently
+ * queries nearby chunks based on the distance and returns a list of matching entities, excluding the player entity.
+ *
+ * Examples:
+ * - Getting all hostile entities within a certain distance:
+ * ```
+ * val hostileEntities = mutableListOf()
+ * getFastEntities(player.pos, 30.0, hostileEntities)
+ * ```
+ *
+ * Please note that this implementation is optimized for performance at small distances
+ * For larger distances, it is recommended to use the [internalGetEntities] function instead
+ * With the time complexity, we can determine that the performance of this function will degrade after 64 blocks
+ *
+ * @param pos The position to search from
+ * @param distance The maximum distance to search for entities
+ * @param pointer The mutable list to store the entities in
+ * @param predicate Predicate to filter entities
+ */
+ @InternalApi
+ inline fun SafeContext.internalGetFastEntities(
+ pos: FastVector,
+ distance: Double,
+ pointer: MutableList = mutableListOf(),
+ predicate: (T) -> Boolean = { true },
+ ) = internalGetFastEntities(T::class, pos, distance, pointer, predicate)
+
+ @InternalApi
+ inline fun SafeContext.internalGetFastEntities(
+ kClass: KClass,
+ pos: FastVector,
+ distance: Double,
+ pointer: MutableList = mutableListOf(),
+ predicate: (T) -> Boolean = { true },
+ ): MutableList {
+ val chunks = ceil(distance / 16.0).toInt()
+ val sectionX = pos.x shr 4
+ val sectionY = pos.y shr 4
+ val sectionZ = pos.z shr 4
+
+ // Here we iterate over all sections within the specified distance and add all entities of type [T] to the list.
+ // We do not have to worry about performance here, as the number of sections is very limited.
+ // For example, if the player is on the edge of a section and the distance is 16, we only have to iterate over 9 sections.
+ for (x in sectionX - chunks..sectionX + chunks) {
+ for (y in sectionY - chunks..sectionY + chunks) {
+ for (z in sectionZ - chunks..sectionZ + chunks) {
+ val section = world
+ .entityManager
+ .cache
+ .findTrackingSection(ChunkSectionPos.asLong(x, y, z)) ?: continue
+
+ section.collection.filterPointer(kClass, pointer) { entity ->
+ entity != player &&
+ pos distSq entity.pos <= distance * distance &&
+ predicate(entity)
+ }
+ }
+ }
+ }
+
+ return pointer
+ }
+
+ /**
+ * Gets all entities of type [T] within a specified distance from a position.
+ *
+ * This function retrieves entities of type [T] within a specified distance from a given position. Unlike
+ * [internalGetFastEntities], it traverses all entities in the world to find matches, while also excluding the player entity.
+ *
+ * @param pos The block position to search from.
+ * @param distance The maximum distance to search for entities.
+ * @param pointer The mutable list to store the entities in.
+ * @param predicate Predicate to filter entities.
+ */
+ @InternalApi
+ inline fun SafeContext.internalGetEntities(
+ pos: FastVector,
+ distance: Double,
+ pointer: MutableList = mutableListOf(),
+ predicate: (T) -> Boolean = { true },
+ ) = internalGetEntities(T::class, pos, distance, pointer, predicate)
+
+ @InternalApi
+ inline fun SafeContext.internalGetEntities(
+ kClass: KClass,
+ pos: FastVector,
+ distance: Double,
+ pointer: MutableList = mutableListOf(),
+ predicate: (T) -> Boolean = { true },
+ ): MutableList {
+ world.entities.filterPointer(kClass, pointer) { entity ->
+ entity != player &&
+ pos distSq entity.pos <= distance * distance &&
+ predicate(entity)
+ }
+
+ return pointer
+ }
+
+ @InternalApi
+ inline fun SafeContext.internalGetBlockEntities(
+ pos: FastVector,
+ distance: Double,
+ pointer: MutableList = mutableListOf(),
+ predicate: (T) -> Boolean = { true },
+ ) = internalGetBlockEntities(T::class, pos, distance, pointer, predicate)
+
+ @InternalApi
+ inline fun SafeContext.internalGetBlockEntities(
+ kClass: KClass,
+ pos: FastVector,
+ distance: Double,
+ pointer: MutableList = mutableListOf(),
+ predicate: (T) -> Boolean = { true },
+ ): MutableList {
+ val chunks = ceil(distance / 16).toInt()
+ val chunkX = pos.x shr 4
+ val chunkZ = pos.z shr 4
+
+ for (x in chunkX - chunks..chunkX + chunks) {
+ for (z in chunkZ - chunks..chunkZ + chunks) {
+ val chunk = world.getChunk(x, z)
+
+ chunk.blockEntities
+ .values.filterPointer(kClass, pointer) { entity ->
+ pos distSq entity.pos <= distance * distance &&
+ predicate(entity)
+ }
+ }
+ }
+
+ return pointer
+ }
+
+ /**
+ * Returns all the blocks and positions within the range where the predicate is true.
+ *
+ * @param pos The position to search from.
+ * @param range The maximum distance to search for entities in each axis.
+ * @param pointer The mutable map to store the positions to blocks in.
+ * @param predicate Predicate to filter the blocks.
+ */
+ @InternalApi
+ inline fun SafeContext.internalSearchBlocks(
+ pos: FastVector,
+ range: FastVector = MAGICVECTOR times 7,
+ step: FastVector = MAGICVECTOR,
+ pointer: MutableMap = mutableMapOf(),
+ predicate: (FastVector, BlockState) -> Boolean = { _, _ -> true },
+ ): MutableMap {
+ internalIteratePositions(pos, range, step) { position ->
+ world.getBlockState(position).let { state ->
+ val fulfilled = predicate(position, state)
+ if (fulfilled) pointer[position] = state
+ }
+ }
+
+ return pointer
+ }
+
+ /**
+ * Returns all the position within the range where the predicate is true.
+ *
+ * @param pos The position to search from.
+ * @param range The maximum distance to search for fluids in each axis.
+ * @param pointer The mutable list to store the positions in.
+ * @param predicate Predicate to filter the fluids.
+ */
+ @InternalApi
+ inline fun SafeContext.internalSearchFluids(
+ pos: FastVector,
+ range: FastVector = MAGICVECTOR times 7,
+ step: FastVector = MAGICVECTOR,
+ pointer: MutableMap = mutableMapOf(),
+ predicate: (FastVector, FluidState) -> Boolean = { _, _ -> true },
+ ) = internalSearchFluids(T::class, pos, range, step, pointer, predicate)
+
+ @InternalApi
+ inline fun SafeContext.internalSearchFluids(
+ kClass: KClass,
+ pos: FastVector,
+ range: FastVector = MAGICVECTOR times 7,
+ step: FastVector = MAGICVECTOR,
+ pointer: MutableMap = mutableMapOf(),
+ predicate: (FastVector, FluidState) -> Boolean = { _, _ -> true },
+ ): MutableMap {
+ @Suppress("UNCHECKED_CAST")
+ internalIteratePositions(pos, range, step) { position ->
+ world.getFluidState(position.x, position.y, position.z).let { state ->
+ val fulfilled = kClass.isInstance(state.fluid) && predicate(position, state)
+ if (fulfilled) pointer[position] = state.fluid as T
+ }
+ }
+
+ return pointer
+ }
+
+ /**
+ * Iterates over all positions within the specified range.
+ * @param pos The position to start from.
+ * @param range The maximum distance to search for entities in each axis.
+ * @param step The step to increment the position by.
+ * @param iterator Iterator to perform operations on each position.
+ */
+ @InternalApi
+ inline fun internalIteratePositions(
+ pos: FastVector,
+ range: FastVector,
+ step: FastVector,
+ iterator: (FastVector) -> Unit = { _ -> },
+ ) {
+ for (x in -range.x..range.x step step.x) {
+ for (y in -range.y..range.y step step.y) {
+ for (z in -range.z..range.z step step.z) {
+ iterator(pos + fastVectorOf(x, y, z))
+ }
+ }
+ }
+ }
+}
+
diff --git a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt
index cb43b65c4..94500b8d0 100644
--- a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt
+++ b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt
@@ -1,5 +1,5 @@
/*
- * Copyright 2024 Lambda
+ * Copyright 2025 Lambda
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
@@ -18,263 +18,62 @@
package com.lambda.util.world
import com.lambda.context.SafeContext
-import com.lambda.core.annotations.InternalApi
-import com.lambda.util.extension.filterPointer
-import com.lambda.util.extension.getBlockState
-import com.lambda.util.extension.getFluidState
-import net.minecraft.block.BlockState
-import net.minecraft.block.entity.BlockEntity
-import net.minecraft.entity.Entity
-import net.minecraft.fluid.Fluid
-import net.minecraft.fluid.FluidState
-import net.minecraft.util.math.ChunkSectionPos
-import kotlin.math.ceil
-import kotlin.reflect.KClass
+import com.lambda.util.BlockUtils.blockState
+import net.minecraft.util.math.BlockPos
+import net.minecraft.util.math.Box
+import net.minecraft.util.math.Direction
+import net.minecraft.util.math.Vec3d
-/**
- * Utility functions for working with the Minecraft world.
- *
- * This object employs a pass-by-reference model, allowing functions to modify
- * data structures passed to them rather than creating new ones.
- *
- * This approach offers two main benefits, being performance and reduce GC overhead
- *
- * @see IBM - Pass By Reference
- * @see Florida State University - Pass By Reference vs. Pass By Value
- * @see IBM - Garbage Collection Impacts on Java Performance
- * @see Medium - GC and Its Effect on Java Performance
- */
object WorldUtils {
- /**
- * A magic vector that can be used to represent a single block
- * It is the same as `fastVectorOf(1, 1, 1)`
- */
- @InternalApi
- const val MAGICVECTOR = 274945015809L
-
- /**
- * Gets all entities of type [T] within a specified distance from a position.
- *
- * This function retrieves entities of type [T] within a specified distance from a given position. It efficiently
- * queries nearby chunks based on the distance and returns a list of matching entities, excluding the player entity.
- *
- * Examples:
- * - Getting all hostile entities within a certain distance:
- * ```
- * val hostileEntities = mutableListOf()
- * getFastEntities(player.pos, 30.0, hostileEntities)
- * ```
- *
- * Please note that this implementation is optimized for performance at small distances
- * For larger distances, it is recommended to use the [internalGetEntities] function instead
- * With the time complexity, we can determine that the performance of this function will degrade after 64 blocks
- *
- * @param pos The position to search from
- * @param distance The maximum distance to search for entities
- * @param pointer The mutable list to store the entities in
- * @param predicate Predicate to filter entities
- */
- @InternalApi
- inline fun SafeContext.internalGetFastEntities(
- pos: FastVector,
- distance: Double,
- pointer: MutableList = mutableListOf(),
- predicate: (T) -> Boolean = { true },
- ) = internalGetFastEntities(T::class, pos, distance, pointer, predicate)
-
- @InternalApi
- inline fun SafeContext.internalGetFastEntities(
- kClass: KClass,
- pos: FastVector,
- distance: Double,
- pointer: MutableList = mutableListOf(),
- predicate: (T) -> Boolean = { true },
- ): MutableList {
- val chunks = ceil(distance / 16.0).toInt()
- val sectionX = pos.x shr 4
- val sectionY = pos.y shr 4
- val sectionZ = pos.z shr 4
-
- // Here we iterate over all sections within the specified distance and add all entities of type [T] to the list.
- // We do not have to worry about performance here, as the number of sections is very limited.
- // For example, if the player is on the edge of a section and the distance is 16, we only have to iterate over 9 sections.
- for (x in sectionX - chunks..sectionX + chunks) {
- for (y in sectionY - chunks..sectionY + chunks) {
- for (z in sectionZ - chunks..sectionZ + chunks) {
- val section = world
- .entityManager
- .cache
- .findTrackingSection(ChunkSectionPos.asLong(x, y, z)) ?: continue
-
- section.collection.filterPointer(kClass, pointer) { entity ->
- entity != player &&
- pos distSq entity.pos <= distance * distance &&
- predicate(entity)
- }
- }
- }
- }
-
- return pointer
- }
-
- /**
- * Gets all entities of type [T] within a specified distance from a position.
- *
- * This function retrieves entities of type [T] within a specified distance from a given position. Unlike
- * [internalGetFastEntities], it traverses all entities in the world to find matches, while also excluding the player entity.
- *
- * @param pos The block position to search from.
- * @param distance The maximum distance to search for entities.
- * @param pointer The mutable list to store the entities in.
- * @param predicate Predicate to filter entities.
- */
- @InternalApi
- inline fun SafeContext.internalGetEntities(
- pos: FastVector,
- distance: Double,
- pointer: MutableList = mutableListOf(),
- predicate: (T) -> Boolean = { true },
- ) = internalGetEntities(T::class, pos, distance, pointer, predicate)
-
- @InternalApi
- inline fun SafeContext.internalGetEntities(
- kClass: KClass,
- pos: FastVector,
- distance: Double,
- pointer: MutableList = mutableListOf(),
- predicate: (T) -> Boolean = { true },
- ): MutableList {
- world.entities.filterPointer(kClass, pointer) { entity ->
- entity != player &&
- pos distSq entity.pos <= distance * distance &&
- predicate(entity)
- }
-
- return pointer
- }
-
- @InternalApi
- inline fun SafeContext.internalGetBlockEntities(
- pos: FastVector,
- distance: Double,
- pointer: MutableList = mutableListOf(),
- predicate: (T) -> Boolean = { true },
- ) = internalGetBlockEntities(T::class, pos, distance, pointer, predicate)
-
- @InternalApi
- inline fun SafeContext.internalGetBlockEntities(
- kClass: KClass,
- pos: FastVector,
- distance: Double,
- pointer: MutableList = mutableListOf(),
- predicate: (T) -> Boolean = { true },
- ): MutableList {
- val chunks = ceil(distance / 16).toInt()
- val chunkX = pos.x shr 4
- val chunkZ = pos.z shr 4
-
- for (x in chunkX - chunks..chunkX + chunks) {
- for (z in chunkZ - chunks..chunkZ + chunks) {
- val chunk = world.getChunk(x, z)
-
- chunk.blockEntities
- .values.filterPointer(kClass, pointer) { entity ->
- pos distSq entity.pos <= distance * distance &&
- predicate(entity)
- }
- }
- }
-
- return pointer
- }
-
- /**
- * Returns all the blocks and positions within the range where the predicate is true.
- *
- * @param pos The position to search from.
- * @param range The maximum distance to search for entities in each axis.
- * @param pointer The mutable map to store the positions to blocks in.
- * @param predicate Predicate to filter the blocks.
- */
- @InternalApi
- inline fun SafeContext.internalSearchBlocks(
- pos: FastVector,
- range: FastVector = MAGICVECTOR times 7,
- step: FastVector = MAGICVECTOR,
- pointer: MutableMap = mutableMapOf(),
- predicate: (FastVector, BlockState) -> Boolean = { _, _ -> true },
- ): MutableMap {
- internalIteratePositions(pos, range, step) { position ->
- world.getBlockState(position).let { state ->
- val fulfilled = predicate(position, state)
- if (fulfilled) pointer[position] = state
+ fun SafeContext.traversable(pos: BlockPos) =
+ hasSupport(pos) && hasClearance(pos)
+
+ fun SafeContext.isPathClear(
+ start: BlockPos,
+ end: BlockPos,
+ stepSize: Double = 0.3,
+ supportCheck: Boolean = true,
+ ) = isPathClear(Vec3d.ofBottomCenter(start), Vec3d.ofBottomCenter(end), stepSize, supportCheck)
+
+ fun SafeContext.isPathClear(
+ start: Vec3d,
+ end: Vec3d,
+ stepSize: Double = 0.3,
+ supportCheck: Boolean = true,
+ ): Boolean {
+ val direction = end.subtract(start)
+ val distance = direction.length()
+ if (distance <= 0) return true
+
+ val steps = (distance / stepSize).toInt()
+ val stepDirection = direction.normalize().multiply(stepSize)
+
+ var currentPos = start
+
+ (0 until steps).forEach { _ ->
+ val playerNotFitting = !hasClearance(currentPos)
+ val hasNoSupport = !hasSupport(currentPos)
+ if (playerNotFitting || (supportCheck && hasNoSupport)) {
+ return false
}
+ currentPos = currentPos.add(stepDirection)
}
- return pointer
+ return hasClearance(end)
}
- /**
- * Returns all the position within the range where the predicate is true.
- *
- * @param pos The position to search from.
- * @param range The maximum distance to search for fluids in each axis.
- * @param pointer The mutable list to store the positions in.
- * @param predicate Predicate to filter the fluids.
- */
- @InternalApi
- inline fun SafeContext.internalSearchFluids(
- pos: FastVector,
- range: FastVector = MAGICVECTOR times 7,
- step: FastVector = MAGICVECTOR,
- pointer: MutableMap = mutableMapOf(),
- predicate: (FastVector, FluidState) -> Boolean = { _, _ -> true },
- ) = internalSearchFluids(T::class, pos, range, step, pointer, predicate)
+ private fun SafeContext.hasClearance(pos: Vec3d) =
+ world.isSpaceEmpty(player, pos.playerBox().contract(1.0E-6))
- @InternalApi
- inline fun SafeContext.internalSearchFluids(
- kClass: KClass,
- pos: FastVector,
- range: FastVector = MAGICVECTOR times 7,
- step: FastVector = MAGICVECTOR,
- pointer: MutableMap = mutableMapOf(),
- predicate: (FastVector, FluidState) -> Boolean = { _, _ -> true },
- ): MutableMap {
- @Suppress("UNCHECKED_CAST")
- internalIteratePositions(pos, range, step) { position ->
- world.getFluidState(position.x, position.y, position.z).let { state ->
- val fulfilled = kClass.isInstance(state.fluid) && predicate(position, state)
- if (fulfilled) pointer[position] = state.fluid as T
- }
- }
+ fun SafeContext.hasSupport(pos: Vec3d) =
+ !world.isSpaceEmpty(player, pos.playerBox().expand(1.0E-6).contract(0.05, 0.0, 0.05))
- return pointer
- }
+ private fun SafeContext.hasClearance(pos: BlockPos) =
+ blockState(pos).isAir && blockState(pos.up()).isAir
- /**
- * Iterates over all positions within the specified range.
- * @param pos The position to start from.
- * @param range The maximum distance to search for entities in each axis.
- * @param step The step to increment the position by.
- * @param iterator Iterator to perform operations on each position.
- */
- @InternalApi
- inline fun internalIteratePositions(
- pos: FastVector,
- range: FastVector,
- step: FastVector,
- iterator: (FastVector) -> Unit = { _ -> },
- ) {
- for (x in -range.x..range.x step step.x) {
- for (y in -range.y..range.y step step.y) {
- for (z in -range.z..range.z step step.z) {
- iterator(
- pos plus fastVectorOf(x, y, z),
- )
- }
- }
- }
- }
-}
+ fun SafeContext.hasSupport(pos: BlockPos) =
+ blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP)
+ fun Vec3d.playerBox(): Box =
+ Box(x - 0.3, y, z - 0.3, x + 0.3, y + 1.8, z + 0.3)
+}
\ No newline at end of file
diff --git a/common/src/test/kotlin/com/lambda/util/GraphUtilTest.kt b/common/src/test/kotlin/com/lambda/util/GraphUtilTest.kt
new file mode 100644
index 000000000..2cd66e73d
--- /dev/null
+++ b/common/src/test/kotlin/com/lambda/util/GraphUtilTest.kt
@@ -0,0 +1,242 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package com.lambda.util
+
+import com.lambda.util.GraphUtil.n6
+import com.lambda.util.GraphUtil.n18
+import com.lambda.util.GraphUtil.n26
+import com.lambda.util.GraphUtil.neighborhood
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.fastVectorOf
+import kotlin.math.sqrt
+import kotlin.test.Test
+import kotlin.test.assertEquals
+import kotlin.test.assertTrue
+
+/**
+ * Unit tests for the neighbor functions in GraphUtil.
+ */
+class GraphUtilTest {
+
+ /**
+ * Test for n6 function which should return 6-connectivity neighbors
+ * (only axis-aligned moves).
+ */
+ @Test
+ fun `test n6 connectivity`() {
+ val origin = fastVectorOf(0, 0, 0)
+ val neighbors = n6(origin)
+
+ // Should have exactly 6 neighbors
+ assertEquals(6, neighbors.size, "n6 should return exactly 6 neighbors")
+
+ // Expected neighbors (axis-aligned)
+ val expectedNeighbors = setOf(
+ fastVectorOf(1, 0, 0),
+ fastVectorOf(-1, 0, 0),
+ fastVectorOf(0, 1, 0),
+ fastVectorOf(0, -1, 0),
+ fastVectorOf(0, 0, 1),
+ fastVectorOf(0, 0, -1)
+ )
+
+ // Check that all expected neighbors are present
+ for (neighbor in expectedNeighbors) {
+ assertTrue(neighbor in neighbors.keys, "Expected neighbor $neighbor not found")
+ assertEquals(1.0, neighbors[neighbor], "Cost for axis-aligned neighbor should be 1.0")
+ }
+ }
+
+ /**
+ * Test for n18 function which should return 18-connectivity neighbors
+ * (axis-aligned + face diagonal moves).
+ */
+ @Test
+ fun `test n18 connectivity`() {
+ val origin = fastVectorOf(0, 0, 0)
+ val neighbors = n18(origin)
+
+ // Should have exactly 18 neighbors
+ assertEquals(18, neighbors.size, "n18 should return exactly 18 neighbors")
+
+ // Expected axis-aligned neighbors (6)
+ val expectedAxisNeighbors = setOf(
+ fastVectorOf(1, 0, 0),
+ fastVectorOf(-1, 0, 0),
+ fastVectorOf(0, 1, 0),
+ fastVectorOf(0, -1, 0),
+ fastVectorOf(0, 0, 1),
+ fastVectorOf(0, 0, -1)
+ )
+
+ // Expected face diagonal neighbors (12)
+ val expectedFaceDiagonalNeighbors = setOf(
+ // XY plane diagonals
+ fastVectorOf(1, 1, 0),
+ fastVectorOf(1, -1, 0),
+ fastVectorOf(-1, 1, 0),
+ fastVectorOf(-1, -1, 0),
+ // XZ plane diagonals
+ fastVectorOf(1, 0, 1),
+ fastVectorOf(1, 0, -1),
+ fastVectorOf(-1, 0, 1),
+ fastVectorOf(-1, 0, -1),
+ // YZ plane diagonals
+ fastVectorOf(0, 1, 1),
+ fastVectorOf(0, 1, -1),
+ fastVectorOf(0, -1, 1),
+ fastVectorOf(0, -1, -1)
+ )
+
+ // Check that all expected axis-aligned neighbors are present
+ for (neighbor in expectedAxisNeighbors) {
+ assertTrue(neighbor in neighbors.keys, "Expected axis-aligned neighbor $neighbor not found")
+ assertEquals(1.0, neighbors[neighbor], "Cost for axis-aligned neighbor should be 1.0")
+ }
+
+ // Check that all expected face diagonal neighbors are present
+ for (neighbor in expectedFaceDiagonalNeighbors) {
+ assertTrue(neighbor in neighbors.keys, "Expected face diagonal neighbor $neighbor not found")
+ assertEquals(sqrt(2.0), neighbors[neighbor], "Cost for face diagonal neighbor should be sqrt(2.0)")
+ }
+ }
+
+ /**
+ * Test for n26 function which should return 26-connectivity neighbors
+ * (axis-aligned + face diagonal + cube diagonal moves).
+ */
+ @Test
+ fun `test n26 connectivity`() {
+ val origin = fastVectorOf(0, 0, 0)
+ val neighbors = n26(origin)
+
+ // Should have exactly 26 neighbors
+ assertEquals(26, neighbors.size, "n26 should return exactly 26 neighbors")
+
+ // Expected axis-aligned neighbors (6)
+ val expectedAxisNeighbors = setOf(
+ fastVectorOf(1, 0, 0),
+ fastVectorOf(-1, 0, 0),
+ fastVectorOf(0, 1, 0),
+ fastVectorOf(0, -1, 0),
+ fastVectorOf(0, 0, 1),
+ fastVectorOf(0, 0, -1)
+ )
+
+ // Expected face diagonal neighbors (12)
+ val expectedFaceDiagonalNeighbors = setOf(
+ // XY plane diagonals
+ fastVectorOf(1, 1, 0),
+ fastVectorOf(1, -1, 0),
+ fastVectorOf(-1, 1, 0),
+ fastVectorOf(-1, -1, 0),
+ // XZ plane diagonals
+ fastVectorOf(1, 0, 1),
+ fastVectorOf(1, 0, -1),
+ fastVectorOf(-1, 0, 1),
+ fastVectorOf(-1, 0, -1),
+ // YZ plane diagonals
+ fastVectorOf(0, 1, 1),
+ fastVectorOf(0, 1, -1),
+ fastVectorOf(0, -1, 1),
+ fastVectorOf(0, -1, -1)
+ )
+
+ // Expected cube diagonal neighbors (8)
+ val expectedCubeDiagonalNeighbors = setOf(
+ fastVectorOf(1, 1, 1),
+ fastVectorOf(1, 1, -1),
+ fastVectorOf(1, -1, 1),
+ fastVectorOf(1, -1, -1),
+ fastVectorOf(-1, 1, 1),
+ fastVectorOf(-1, 1, -1),
+ fastVectorOf(-1, -1, 1),
+ fastVectorOf(-1, -1, -1)
+ )
+
+ // Check that all expected axis-aligned neighbors are present
+ for (neighbor in expectedAxisNeighbors) {
+ assertTrue(neighbor in neighbors.keys, "Expected axis-aligned neighbor $neighbor not found")
+ assertEquals(1.0, neighbors[neighbor], "Cost for axis-aligned neighbor should be 1.0")
+ }
+
+ // Check that all expected face diagonal neighbors are present
+ for (neighbor in expectedFaceDiagonalNeighbors) {
+ assertTrue(neighbor in neighbors.keys, "Expected face diagonal neighbor $neighbor not found")
+ assertEquals(sqrt(2.0), neighbors[neighbor], "Cost for face diagonal neighbor should be sqrt(2.0)")
+ }
+
+ // Check that all expected cube diagonal neighbors are present
+ for (neighbor in expectedCubeDiagonalNeighbors) {
+ assertTrue(neighbor in neighbors.keys, "Expected cube diagonal neighbor $neighbor not found")
+ assertEquals(sqrt(3.0), neighbors[neighbor], "Cost for cube diagonal neighbor should be sqrt(3.0)")
+ }
+ }
+
+ /**
+ * Test for the neighborhood function with custom distance parameters.
+ */
+ @Test
+ fun `test neighborhood with custom parameters`() {
+ val origin = fastVectorOf(0, 0, 0)
+
+ // Test with minDistSq=2, maxDistSq=2 (should only return face diagonals)
+ val faceDiagonalNeighbors = neighborhood(origin, minDistSq = 2, maxDistSq = 2)
+ assertEquals(12, faceDiagonalNeighbors.size, "Should return exactly 12 face diagonal neighbors")
+
+ // Test with minDistSq=3, maxDistSq=3 (should only return cube diagonals)
+ val cubeDiagonalNeighbors = neighborhood(origin, minDistSq = 3, maxDistSq = 3)
+ assertEquals(8, cubeDiagonalNeighbors.size, "Should return exactly 8 cube diagonal neighbors")
+
+ // Test with minDistSq=1, maxDistSq=3 (should return all neighbors, same as n26)
+ val allNeighbors = neighborhood(origin, minDistSq = 1, maxDistSq = 3)
+ assertEquals(26, allNeighbors.size, "Should return exactly 26 neighbors (same as n26)")
+
+ // Test with invalid range (should return empty map)
+ val emptyNeighbors = neighborhood(origin, minDistSq = 4, maxDistSq = 5)
+ assertEquals(0, emptyNeighbors.size, "Should return empty map for invalid distance range")
+ }
+
+ /**
+ * Test for the neighborhood function with non-origin center point.
+ */
+ @Test
+ fun `test neighborhood with non-origin center`() {
+ val center = fastVectorOf(10, 20, 30)
+ val neighbors = n6(center)
+
+ // Should have exactly 6 neighbors
+ assertEquals(6, neighbors.size, "n6 should return exactly 6 neighbors")
+
+ // Expected neighbors (axis-aligned)
+ val expectedNeighbors = setOf(
+ fastVectorOf(11, 20, 30),
+ fastVectorOf(9, 20, 30),
+ fastVectorOf(10, 21, 30),
+ fastVectorOf(10, 19, 30),
+ fastVectorOf(10, 20, 31),
+ fastVectorOf(10, 20, 29)
+ )
+
+ // Check that all expected neighbors are present
+ for (neighbor in expectedNeighbors) {
+ assertTrue(neighbor in neighbors.keys, "Expected neighbor $neighbor not found")
+ assertEquals(1.0, neighbors[neighbor], "Cost for axis-aligned neighbor should be 1.0")
+ }
+ }
+}
\ No newline at end of file
diff --git a/common/src/test/kotlin/pathing/DStarLiteTest.kt b/common/src/test/kotlin/pathing/DStarLiteTest.kt
new file mode 100644
index 000000000..f34d81fa0
--- /dev/null
+++ b/common/src/test/kotlin/pathing/DStarLiteTest.kt
@@ -0,0 +1,390 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package pathing
+
+import com.lambda.pathing.incremental.DStarLite
+import com.lambda.pathing.incremental.Key
+import com.lambda.pathing.incremental.LazyGraph
+import com.lambda.util.GraphUtil.createGridGraph18Conn
+import com.lambda.util.GraphUtil.createGridGraph26Conn
+import com.lambda.util.GraphUtil.createGridGraph6Conn
+import com.lambda.util.GraphUtil.euclideanHeuristic
+import com.lambda.util.GraphUtil.length
+import com.lambda.util.GraphUtil.manhattanHeuristic
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.fastVectorOf
+import com.lambda.util.world.x
+import com.lambda.util.world.y
+import com.lambda.util.world.z
+import org.junit.jupiter.api.BeforeEach
+import kotlin.math.sqrt
+import kotlin.test.Test
+import kotlin.test.assertEquals
+import kotlin.test.assertFalse
+import kotlin.test.assertTrue
+
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+internal class DStarLiteTest {
+ private lateinit var graph6: LazyGraph
+ private lateinit var graph18: LazyGraph
+ private lateinit var graph26: LazyGraph
+
+ @BeforeEach
+ fun setup() {
+ graph6 = createGridGraph6Conn()
+ graph18 = createGridGraph18Conn()
+ graph26 = createGridGraph26Conn()
+ }
+
+ @Test
+ fun `initialize sets goal rhs to 0 and adds to queue (grid graph)`() {
+ val startNode = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(5, 0, 0)
+ val dStar = DStarLite(graph6, startNode, goalNode, ::manhattanHeuristic) // Use any graph type
+
+ assertEquals(0.0, dStar.rhs(goalNode))
+ assertEquals(Double.POSITIVE_INFINITY, dStar.g(goalNode))
+ assertEquals(1, dStar.U.size()) // Access internal U for test verification
+ assertEquals(goalNode, dStar.U.top())
+ // Initial key uses heuristic + km (0) + min(g=inf, rhs=0) = h(start, goal) + 0
+ assertEquals(Key(manhattanHeuristic(startNode, goalNode), 0.0), dStar.U.topKey(Key.INFINITY))
+ }
+
+ @Test
+ fun `computeShortestPath finds straight path on 6-conn graph`() {
+ val startNode = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(5, 0, 0) // Straight line along X
+ val dStar = DStarLite(graph6, startNode, goalNode, ::manhattanHeuristic)
+
+ dStar.computeShortestPath()
+
+ // Check g values (should be Manhattan distance)
+ assertEquals(5.0, dStar.g(fastVectorOf(0, 0, 0)), 0.001)
+ assertEquals(4.0, dStar.g(fastVectorOf(1, 0, 0)), 0.001)
+ assertEquals(1.0, dStar.g(fastVectorOf(4, 0, 0)), 0.001)
+ assertEquals(0.0, dStar.g(goalNode), 0.001)
+
+ // Check path
+ val path = dStar.path()
+ assertEquals(6, path.size) // 0, 1, 2, 3, 4, 5
+ assertEquals(startNode, path.first())
+ assertEquals(goalNode, path.last())
+ // Check intermediate node
+ assertEquals(fastVectorOf(1, 0, 0), path[1])
+ }
+
+ @Test
+ fun `computeShortestPath finds diagonal path on 26-conn graph`() {
+ val startNode = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(2, 2, 2) // Cube diagonal
+ // Use Euclidean heuristic for diagonal graphs
+ val dStar = DStarLite(graph26, startNode, goalNode, ::euclideanHeuristic)
+
+ dStar.computeShortestPath()
+
+ // Expected g value is Euclidean distance * cost multiplier (which is 1 here)
+ // Path: (0,0,0) -> (1,1,1) -> (2,2,2). Cost = 2 * sqrt(3)
+ val expectedG = 2.0 * sqrt(3.0)
+ assertEquals(expectedG, dStar.g(startNode), 0.001)
+ assertEquals(sqrt(3.0), dStar.g(fastVectorOf(1, 1, 1)), 0.001)
+ assertEquals(0.0, dStar.g(goalNode), 0.001)
+
+ // Check path
+ val path = dStar.path()
+ // Optimal path is direct diagonal steps
+ assertEquals(listOf(
+ fastVectorOf(0, 0, 0),
+ fastVectorOf(1, 1, 1),
+ fastVectorOf(2, 2, 2)
+ ), path)
+ }
+
+ @Test
+ fun `computeShortestPath finds mixed path on 18-conn graph`() {
+ val startNode = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(2, 1, 0) // Requires axis + diagonal
+ val dStar = DStarLite(graph18, startNode, goalNode, ::euclideanHeuristic)
+
+ dStar.computeShortestPath()
+
+ // Optimal path likely (0,0,0) -> (1,0,0) -> (2,1,0)
+ // Cost = sqrt(2) + 1.0
+ val expectedG = sqrt(2.0) + 1.0
+ assertEquals(expectedG, dStar.g(startNode), 0.001)
+ assertEquals(1.0, dStar.g(fastVectorOf(1, 1, 0)), 0.001)
+ assertEquals(0.0, dStar.g(goalNode), 0.001)
+
+ // Check path
+ val path = dStar.path()
+ assertEquals(listOf(
+ fastVectorOf(0, 0, 0),
+ fastVectorOf(1, 0, 0),
+ fastVectorOf(2, 1, 0) // Axis move
+ ), path)
+ }
+
+ @Test
+ fun `updateStart changes km and path calculation (grid graph)`() {
+ val startNode1 = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(5, 0, 0)
+ val dStar = DStarLite(graph6, startNode1, goalNode, ::manhattanHeuristic)
+ dStar.computeShortestPath()
+ assertEquals(5.0, dStar.g(startNode1), 0.001) // g(0) should be 5.0
+
+ val startNode2 = fastVectorOf(1, 0, 0)
+ dStar.updateStart(startNode2)
+ assertEquals(manhattanHeuristic(startNode1, startNode2), dStar.km, 0.001) // km = h(0,1) = 1.0
+
+ dStar.computeShortestPath()
+ assertEquals(4.0, dStar.g(startNode2), 0.001) // g(1) should be 4.0
+ val path = dStar.path()
+ assertEquals(5, path.size) // 1, 2, 3, 4, 5
+ assertEquals(startNode2, path.first())
+ assertEquals(goalNode, path.last())
+ }
+
+ @Test
+ fun `computeShortestPath handles start equals goal (grid graph)`() {
+ val startNode = fastVectorOf(3, 3, 3)
+ val dStar = DStarLite(graph26, startNode, startNode, ::euclideanHeuristic) // start == goal
+ dStar.computeShortestPath()
+
+ assertEquals(0.0, dStar.g(startNode), 0.001)
+ assertEquals(0.0, dStar.rhs(startNode), 0.001)
+ val path = dStar.path()
+ assertEquals(listOf(startNode), path) // Path is just the start/goal node
+ }
+
+ @Test
+ fun `invalidate node forces path recalculation on 6-conn graph`() {
+ // Create a graph with a straight path from (0,0,0) to (5,0,0)
+ val startNode = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(5, 0, 0)
+ val localBlockedNodes = mutableSetOf()
+ val graph = createGridGraph6Conn(localBlockedNodes)
+ val dStar = DStarLite(graph, startNode, goalNode, ::manhattanHeuristic)
+
+ // Compute initial path
+ dStar.computeShortestPath()
+ val initialPath = dStar.path()
+
+ // Verify initial path is straight
+ assertEquals(6, initialPath.size)
+ assertEquals(startNode, initialPath.first())
+ assertEquals(goalNode, initialPath.last())
+ assertEquals(fastVectorOf(1, 0, 0), initialPath[1])
+ assertEquals(fastVectorOf(2, 0, 0), initialPath[2])
+
+ // Invalidate a node in the middle of the path
+ val nodeToInvalidate = fastVectorOf(2, 0, 0)
+ localBlockedNodes.add(nodeToInvalidate)
+ dStar.invalidate(nodeToInvalidate)
+
+ // Recompute path
+ dStar.computeShortestPath()
+ val newPath = dStar.path()
+
+ // Verify new path avoids the invalidated node
+ assertTrue(nodeToInvalidate !in newPath, "Path should not contain the invalidated node")
+ assertEquals(startNode, newPath.first())
+ assertEquals(goalNode, newPath.last())
+
+ // The new path should be longer as it has to go around the blocked node
+ assertTrue(newPath.size > initialPath.size, "New path should be longer than the initial path")
+ }
+
+ @Test
+ fun `invalidate multiple nodes forces complex rerouting on 6-conn graph`() {
+ // Create a graph with a straight path from (0,0,0) to (5,0,0)
+ val startNode = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(5, 0, 0)
+
+ // Pre-block nodes in the blockedNodes set before creating the graph
+ val localBlockedNodes = mutableSetOf()
+ val nodesToBlock = listOf(
+ fastVectorOf(2, 0, 0), // Block straight path
+ fastVectorOf(2, 1, 0), // Block one alternative
+ fastVectorOf(2, -1, 0) // Block another alternative
+ )
+
+ // Add nodes to blocked set before creating the graph
+ localBlockedNodes.addAll(nodesToBlock)
+
+ // Create graph with pre-blocked nodes
+ val graph = createGridGraph6Conn(localBlockedNodes)
+ val dStar = DStarLite(graph, startNode, goalNode, ::manhattanHeuristic)
+
+ // Compute path with pre-blocked nodes
+ dStar.computeShortestPath()
+ val path = dStar.path()
+
+ // Print debug info about the path
+ println("[DEBUG_LOG] Path with pre-blocked nodes:")
+ path.forEach { node ->
+ println("[DEBUG_LOG] - Node: $node (x=${node.x}, y=${node.y}, z=${node.z})")
+ }
+
+ // Verify path avoids all blocked nodes
+ nodesToBlock.forEach { node ->
+ assertTrue(node !in path, "Path should not contain blocked node $node")
+ }
+
+ // Verify path starts and ends at the correct nodes
+ assertEquals(startNode, path.first())
+ assertEquals(goalNode, path.last())
+
+ // The path should be longer than a straight line (which would be 6 nodes)
+ assertTrue(path.size > 6, "Path should be longer than a straight line")
+ }
+
+ @Test
+ fun `nodeInitializer correctly omits blocked nodes from graph`() {
+ // Create a set of blocked nodes
+ val localBlockedNodes = mutableSetOf()
+ val nodeToBlock = fastVectorOf(2, 0, 0)
+ localBlockedNodes.add(nodeToBlock)
+
+ // Create graph with blocked nodes
+ val graph = createGridGraph6Conn(localBlockedNodes)
+
+ // Check that the nodeInitializer correctly omits the blocked node
+ val startNode = fastVectorOf(1, 0, 0) // Node adjacent to blocked node
+ val successors = graph.successors(startNode)
+
+ // The blocked node should not be in the successors
+ assertFalse(nodeToBlock in successors.keys, "Blocked node should not be in successors")
+
+ // Create a DStarLite instance and compute path
+ val goalNode = fastVectorOf(3, 0, 0) // Goal is on the other side of blocked node
+ val dStar = DStarLite(graph, startNode, goalNode, ::manhattanHeuristic)
+ dStar.computeShortestPath()
+ val path = dStar.path()
+
+ // Verify path avoids the blocked node
+ assertTrue(nodeToBlock !in path, "Path should not contain the blocked node")
+ assertEquals(startNode, path.first())
+ assertEquals(goalNode, path.last())
+ }
+
+ @Test
+ fun `invalidate node updates connectivity on diagonal graph`() {
+ // Create a graph with diagonal connectivity
+ val startNode = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(2, 2, 0)
+ val localBlockedNodes = mutableSetOf()
+ val graph = createGridGraph18Conn(localBlockedNodes)
+ val dStar = DStarLite(graph, startNode, goalNode, ::euclideanHeuristic)
+
+ // Compute initial path
+ dStar.computeShortestPath()
+ val initialPath = dStar.path()
+
+ // Initial path should be diagonal
+ assertEquals(3, initialPath.size)
+ assertEquals(startNode, initialPath.first())
+ assertEquals(fastVectorOf(1, 1, 0), initialPath[1])
+ assertEquals(goalNode, initialPath.last())
+
+ // Invalidate the diagonal node
+ val nodeToInvalidate = fastVectorOf(1, 1, 0)
+ localBlockedNodes.add(nodeToInvalidate)
+ dStar.invalidate(nodeToInvalidate)
+
+ // Recompute path
+ dStar.computeShortestPath()
+ val newPath = dStar.path()
+
+ // Verify new path avoids the invalidated node
+ assertTrue(nodeToInvalidate !in newPath, "Path should not contain the invalidated node")
+ assertEquals(startNode, newPath.first())
+ assertEquals(goalNode, newPath.last())
+
+ // The new path should go around the blocked diagonal
+ assertTrue(newPath.size > initialPath.size, "New path should be longer than the initial path")
+ }
+
+ @Test
+ fun `invalidate node correctly updates rhs values for new nodes`() {
+ // Create a straight line path
+ val startNode = fastVectorOf(0, 0, 0)
+ val goalNode = fastVectorOf(0, 0, 3)
+ val localBlockedNodes = mutableSetOf()
+ val graph = createGridGraph26Conn(localBlockedNodes)
+ val dStar = DStarLite(graph, startNode, goalNode, ::euclideanHeuristic)
+
+ // Compute initial path
+ dStar.computeShortestPath()
+ val initialPath = dStar.path()
+
+ // Initial path should be straight
+ assertEquals(4, initialPath.size)
+ assertEquals(startNode, initialPath.first())
+ assertEquals(fastVectorOf(0, 0, 1), initialPath[1])
+ assertEquals(fastVectorOf(0, 0, 2), initialPath[2])
+ assertEquals(goalNode, initialPath.last())
+
+ // Block a node in the middle of the path
+ val nodeToInvalidate = fastVectorOf(0, 0, 1)
+ localBlockedNodes.add(nodeToInvalidate)
+ dStar.invalidate(nodeToInvalidate)
+
+ // Recompute path
+ dStar.computeShortestPath()
+ val newPath = dStar.path()
+
+ // Verify new path avoids the invalidated node
+ assertTrue(nodeToInvalidate !in newPath, "Path should not contain the invalidated node")
+ assertEquals(startNode, newPath.first())
+ assertEquals(goalNode, newPath.last())
+
+ // Check if any new nodes were created (nodes that weren't in the initial path)
+ val newNodes = newPath.filter { it !in initialPath && it != startNode && it != goalNode }
+
+ // Verify that new nodes have correct rhs values
+ newNodes.forEach { node ->
+ val rhs = dStar.rhs(node)
+ val minSuccCost = graph.successors(node)
+ .mapNotNull { (succ, cost) ->
+ if (cost == Double.POSITIVE_INFINITY) null else cost + dStar.g(succ)
+ }
+ .minOrNull() ?: Double.POSITIVE_INFINITY
+
+ assertEquals(minSuccCost, rhs, 0.001,
+ "Node $node should have rhs value equal to minimum successor cost")
+ }
+
+ // The new path should go around the blocked node
+ assertTrue(newPath.length() >= initialPath.length(), "New path should be at least as long as the initial path")
+ }
+}
diff --git a/common/src/test/kotlin/pathing/GraphConsistencyTest.kt b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt
new file mode 100644
index 000000000..0f5402e1c
--- /dev/null
+++ b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt
@@ -0,0 +1,287 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package pathing
+
+import com.lambda.pathing.incremental.DStarLite
+import com.lambda.util.GraphUtil.createGridGraph18Conn
+import com.lambda.util.GraphUtil.createGridGraph26Conn
+import com.lambda.util.GraphUtil.createGridGraph6Conn
+import com.lambda.util.GraphUtil.euclideanHeuristic
+import com.lambda.util.GraphUtil.length
+import com.lambda.util.GraphUtil.string
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.fastVectorOf
+import kotlin.test.Test
+import kotlin.test.assertEquals
+import kotlin.test.assertFalse
+import kotlin.test.assertTrue
+
+/**
+ * Tests for graph maintenance consistency in the D* Lite algorithm.
+ * These tests verify that the graph state after invalidation and pruning
+ * is consistent with a fresh graph created with the same blocked nodes.
+ */
+class GraphConsistencyTest {
+ /**
+ * Simple test with a single blocked node in a 6-connectivity graph.
+ */
+ @Test
+ fun `graph consistency with single blocked node N6`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph6Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block a node and invalidate
+ val blockedNode = fastVectorOf(0, 0, 4)
+ blockedNodes.add(blockedNode)
+
+ dStar1.invalidate(blockedNode)
+ dStar1.computeShortestPath()
+ val blocked = dStar1.path()
+
+ // Verify that the path changed after blocking
+ val initialLength = initialPath.length()
+ val blockedLength = blocked.length()
+ assertTrue(initialLength < blockedLength,
+ "Initial path length ($initialLength) should be less than blocked path length ($blockedLength)")
+
+ // Create a fresh graph with the blocked node and compute path
+ val graph2 = createGridGraph6Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(blocked.length(), path2.length(),
+ "Paths should have identical length after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}")
+
+ // Verify the graph structure is consistent
+ val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2)
+ assertFalse(graphDifferences.hasAnyDifferences,
+ "Graph structures should be identical: $graphDifferences")
+ assertFalse(valueDifferences.isNotEmpty(),
+ "Node values should be identical: ${valueDifferences.joinToString("\n ")}")
+ }
+
+ /**
+ * Test with multiple blocked nodes in a 6-connectivity graph.
+ */
+ @Test
+ fun `graph consistency with multiple blocked nodes N6`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph6Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block multiple nodes and invalidate one by one
+ val blockedNode1 = fastVectorOf(0, 0, 4)
+ val blockedNode2 = fastVectorOf(1, 0, 4)
+ val blockedNode3 = fastVectorOf(-1, 0, 4)
+
+ blockedNodes.add(blockedNode1)
+ dStar1.invalidate(blockedNode1)
+
+ blockedNodes.add(blockedNode2)
+ dStar1.invalidate(blockedNode2)
+
+ blockedNodes.add(blockedNode3)
+ dStar1.invalidate(blockedNode3)
+
+ dStar1.computeShortestPath()
+ val blocked = dStar1.path()
+
+ // Verify that the path changed after blocking
+ val initialLength = initialPath.length()
+ val blockedLength = blocked.length()
+ assertTrue(initialLength < blockedLength,
+ "Initial path length ($initialLength) should be less than blocked path length ($blockedLength)")
+
+ // Create a fresh graph with all blocked nodes and compute path
+ val graph2 = createGridGraph6Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(blocked.length(), path2.length(),
+ "Paths should have identical length after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}")
+
+ // Verify the graph structure is consistent
+ val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2)
+ assertFalse(graphDifferences.hasAnyDifferences,
+ "Graph structures should be identical: $graphDifferences")
+ assertFalse(valueDifferences.isNotEmpty(),
+ "Node values should be identical: ${valueDifferences.joinToString("\n ")}")
+ }
+
+ /**
+ * Test with a more complex graph structure (18-connectivity).
+ */
+ @Test
+ fun `graph consistency with 18-connectivity`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph18Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block a node and invalidate
+ val blockedNode = fastVectorOf(0, 0, 4)
+ blockedNodes.add(blockedNode)
+
+ dStar1.invalidate(blockedNode)
+ dStar1.computeShortestPath()
+ val blocked = dStar1.path()
+
+ // Verify that the path changed after blocking
+ val initialLength = initialPath.length()
+ val blockedLength = blocked.length()
+ assertTrue(initialLength < blockedLength,
+ "Initial path length ($initialLength) should be less than blocked path length ($blockedLength)")
+
+ // Create a fresh graph with the blocked node and compute path
+ val graph2 = createGridGraph18Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(blocked.length(), path2.length(),
+ "Paths should have identical length after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}")
+
+ // Verify graph structure is consistent
+ val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2)
+ assertFalse(graphDifferences.hasAnyDifferences,
+ "Graph structures should be identical: $graphDifferences")
+ assertFalse(valueDifferences.isNotEmpty(),
+ "Node values should be identical: ${valueDifferences.joinToString("\n ")}")
+ }
+
+ /**
+ * Test with a more complex graph structure (26-connectivity).
+ */
+ @Test
+ fun `graph consistency with 26-connectivity`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph26Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block a node and invalidate
+ val blockedNode = fastVectorOf(0, 0, 4)
+ blockedNodes.add(blockedNode)
+
+ dStar1.invalidate(blockedNode)
+ dStar1.computeShortestPath()
+ val blocked = dStar1.path()
+
+ // Verify that the path changed after blocking
+ val initialLength = initialPath.length()
+ val blockedLength = blocked.length()
+ assertTrue(initialLength < blockedLength,
+ "Initial path length ($initialLength) should be less than blocked path length ($blockedLength)")
+
+ // Create a fresh graph with the blocked node and compute path
+ val graph2 = createGridGraph26Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(blocked.length(), path2.length(),
+ "Paths should have identical length after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}")
+
+ // Verify graph structure is consistent
+ val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2)
+ assertFalse(graphDifferences.hasAnyDifferences,
+ "Graph structures should be identical: $graphDifferences")
+ assertFalse(valueDifferences.isNotEmpty(),
+ "Node values should be identical: ${valueDifferences.joinToString("\n ")}")
+ }
+
+ /**
+ * Test with a node that becomes unblocked (simulating a world update where a block changes).
+ */
+ @Test
+ fun `graph consistency when unblocking a node`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Block a node initially
+ val nodeToToggle = fastVectorOf(0, 0, 4)
+ blockedNodes.add(nodeToToggle)
+
+ // Create initial graph with the blocked node
+ val graph1 = createGridGraph6Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Unblock the node and invalidate
+ blockedNodes.remove(nodeToToggle)
+
+ // The nodeInitializer should handle this correctly
+ dStar1.invalidate(nodeToToggle)
+ dStar1.computeShortestPath()
+ val path1 = dStar1.path()
+
+ // Verify that the path changed after blocking
+ val initialLength = initialPath.length()
+ val unblockedLength = path1.length()
+ assertTrue(initialLength > unblockedLength,
+ "Initial path length ($initialLength) should be longer than unblocked path length ($unblockedLength)")
+
+ // Create a fresh graph without the blocked node and compute path
+ val graph2 = createGridGraph6Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(path1.length(), path2.length(),
+ "Paths should have identical length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}")
+
+ // Verify graph structure is consistent
+ val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2)
+ assertFalse(graphDifferences.hasAnyDifferences,
+ "Graph structures should be identical: $graphDifferences")
+ assertFalse(valueDifferences.isNotEmpty(),
+ "Node values should be identical: ${valueDifferences.joinToString("\n ")}")
+ }
+}
diff --git a/common/src/test/kotlin/pathing/KeyTest.kt b/common/src/test/kotlin/pathing/KeyTest.kt
new file mode 100644
index 000000000..9db39d574
--- /dev/null
+++ b/common/src/test/kotlin/pathing/KeyTest.kt
@@ -0,0 +1,81 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package pathing
+
+import com.lambda.pathing.incremental.Key
+import org.junit.jupiter.api.Assertions.*
+import org.junit.jupiter.api.Test
+
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+internal class KeyTest {
+
+ @Test
+ fun `compareTo checks first element primarily`() {
+ assertTrue(Key(1.0, 10.0) < Key(2.0, 1.0))
+ assertTrue(Key(3.0, 1.0) > Key(2.0, 10.0))
+ }
+
+ @Test
+ fun `compareTo checks second element when first elements are equal`() {
+ assertTrue(Key(5.0, 1.0) < Key(5.0, 2.0))
+ assertTrue(Key(5.0, 3.0) > Key(5.0, 2.0))
+ }
+
+ @Test
+ fun `compareTo handles equal keys`() {
+ assertEquals(0, Key(5.0, 2.0).compareTo(Key(5.0, 2.0)))
+ assertTrue(Key(5.0, 2.0) <= Key(5.0, 2.0))
+ assertTrue(Key(5.0, 2.0) >= Key(5.0, 2.0))
+ }
+
+ @Test
+ fun `compareTo handles infinity`() {
+ assertTrue(Key(1000.0, 1000.0) < Key.INFINITY)
+ assertTrue(Key.INFINITY > Key(0.0, 0.0))
+ assertEquals(0, Key.INFINITY.compareTo(Key.INFINITY))
+ }
+
+ @Test
+ fun `equals checks both elements`() {
+ assertEquals(Key(1.0, 2.0), Key(1.0, 2.0))
+ assertNotEquals(Key(1.0, 2.0), Key(2.0, 2.0))
+ assertNotEquals(Key(1.0, 2.0), Key(1.0, 3.0))
+ }
+
+ @Test
+ fun `hashCode is consistent with equals`() {
+ assertEquals(Key(1.0, 2.0).hashCode(), Key(1.0, 2.0).hashCode())
+ assertNotEquals(Key(1.0, 2.0).hashCode(), Key(1.0, 3.0).hashCode())
+ }
+}
diff --git a/common/src/test/kotlin/pathing/PathConsistencyTest.kt b/common/src/test/kotlin/pathing/PathConsistencyTest.kt
new file mode 100644
index 000000000..85f7868a6
--- /dev/null
+++ b/common/src/test/kotlin/pathing/PathConsistencyTest.kt
@@ -0,0 +1,347 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package pathing
+
+import com.lambda.pathing.incremental.DStarLite
+import com.lambda.util.GraphUtil.createGridGraph6Conn
+import com.lambda.util.GraphUtil.createGridGraph18Conn
+import com.lambda.util.GraphUtil.createGridGraph26Conn
+import com.lambda.util.GraphUtil.euclideanHeuristic
+import com.lambda.util.GraphUtil.length
+import com.lambda.util.GraphUtil.string
+import com.lambda.util.world.FastVector
+import com.lambda.util.world.fastVectorOf
+import com.lambda.util.world.string
+import kotlin.test.Test
+import kotlin.test.assertEquals
+import kotlin.test.assertTrue
+
+/**
+ * Tests for path consistency in the D* Lite algorithm.
+ * These tests verify that the paths produced after invalidation
+ * match the paths produced by a freshly created graph with the same blocked nodes.
+ */
+class PathConsistencyTest {
+
+ /**
+ * Test path consistency with a single blocked node in a 6-connectivity graph.
+ */
+ @Test
+ fun `path consistency with single blocked node N6`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph6Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block a node and invalidate
+ val blockedNode = fastVectorOf(0, 0, 4)
+ blockedNodes.add(blockedNode)
+
+ dStar1.invalidate(blockedNode)
+ dStar1.computeShortestPath()
+ val path1 = dStar1.path()
+
+ assertTrue(!path1.contains(blockedNode), "Blocked node ${blockedNode.string} should not be in path ${path1.string()}")
+
+ // Verify that the path changed after blocking
+ assertTrue(initialPath.length() < path1.length(),
+ "Initial path length (${initialPath.length()}) should be less than blocked path length (${path1.length()})")
+
+ // Create a fresh graph with the blocked node and compute path
+ val graph2 = createGridGraph6Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths have the same length (there can be multiple valid paths)
+ assertEquals(path1.length(), path2.length(),
+ "Paths should have the same length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}")
+
+ // Verify both paths avoid blocked nodes
+ blockedNodes.forEach { blocked ->
+ assertTrue(!path1.contains(blocked), "Path1 should not contain blocked node ${blocked.string}")
+ assertTrue(!path2.contains(blocked), "Path2 should not contain blocked node ${blocked.string}")
+ }
+ }
+
+ /**
+ * Test path consistency with multiple blocked nodes in a 6-connectivity graph.
+ */
+ @Test
+ fun `path consistency with multiple blocked nodes N6`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph6Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block multiple nodes and invalidate one by one
+ val blockedNode1 = fastVectorOf(0, 0, 4)
+ val blockedNode2 = fastVectorOf(1, 0, 4)
+ val blockedNode3 = fastVectorOf(-1, 0, 4)
+
+ blockedNodes.add(blockedNode1)
+ dStar1.invalidate(blockedNode1)
+
+ blockedNodes.add(blockedNode2)
+ dStar1.invalidate(blockedNode2)
+
+ blockedNodes.add(blockedNode3)
+ dStar1.invalidate(blockedNode3)
+
+ dStar1.computeShortestPath()
+ val path1 = dStar1.path()
+
+ blockedNodes.forEach { blockedNode ->
+ assertTrue(!path1.contains(blockedNode), "Blocked node ${blockedNode.string} should not be in path ${path1.string()}")
+ }
+
+ val initialPathLength = initialPath.length()
+ val length1 = path1.length()
+ // Verify that the path changed after blocking
+ assertTrue(initialPathLength < length1,
+ "Initial path ${initialPath.string()} length ($initialPathLength) should be less than blocked path ${path1.string()} size ($length1)")
+
+ // Create a fresh graph with all blocked nodes and compute path
+ val graph2 = createGridGraph6Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths have the same length (there can be multiple valid paths)
+ assertEquals(path1.length(), path2.length(),
+ "Paths should have the same length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}")
+
+ // Verify both paths avoid blocked nodes
+ blockedNodes.forEach { blocked ->
+ assertTrue(!path1.contains(blocked), "Path1 should not contain blocked node ${blocked.string}")
+ assertTrue(!path2.contains(blocked), "Path2 should not contain blocked node ${blocked.string}")
+ }
+ }
+
+ /**
+ * Test path consistency with a more complex graph structure (18-connectivity).
+ */
+ @Test
+ fun `path consistency with 18-connectivity`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph18Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block a node and invalidate
+ val blockedNode = fastVectorOf(0, 0, 4)
+ blockedNodes.add(blockedNode)
+
+ dStar1.invalidate(blockedNode)
+ dStar1.computeShortestPath()
+ val path1 = dStar1.path()
+
+ assertTrue(!path1.contains(blockedNode), "Blocked node ${blockedNode.string} should not be in path ${path1.string()}")
+
+ // Create a fresh graph with the blocked node and compute path
+ val graph2 = createGridGraph18Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(path1, path2,
+ "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}")
+ }
+
+ /**
+ * Test path consistency with a more complex graph structure (26-connectivity).
+ */
+ @Test
+ fun `path consistency with 26-connectivity`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph26Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block a node and invalidate
+ val blockedNode = fastVectorOf(0, 0, 4)
+ blockedNodes.add(blockedNode)
+
+ dStar1.invalidate(blockedNode)
+ dStar1.computeShortestPath()
+ val path1 = dStar1.path()
+
+ assertTrue(!path1.contains(blockedNode), "Blocked node ${blockedNode.string} should not be in path ${path1.string()}")
+
+ // Create a fresh graph with the blocked node and compute path
+ val graph2 = createGridGraph26Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(path1.length(), path2.length(),
+ "Paths should be same length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}")
+ }
+
+ /**
+ * Test path consistency when unblocking a node.
+ */
+ @Test
+ fun `path consistency when unblocking a node`() {
+ val startNode = fastVectorOf(0, 0, 10)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create a 6x6 wall to force a detour
+ (-3..3).forEach { x ->
+ (-3..3).forEach { y ->
+ blockedNodes.add(fastVectorOf(x, y, 5))
+ }
+ }
+
+ // Create initial graph with blocked nodes
+ val graph1 = createGridGraph6Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ blockedNodes.forEach { blockedNode ->
+ assertTrue(!initialPath.contains(blockedNode), "Blocked node ${blockedNode.string} should not be in path ${initialPath.string()}")
+ }
+
+ // Unblock one node in the middle
+ val nodeToToggle = fastVectorOf(0, 0, 5)
+ if (blockedNodes.remove(nodeToToggle)) println("Unblocked node ${nodeToToggle.string}")
+
+ // Now it should path through the hole in the wall
+ dStar1.invalidate(nodeToToggle)
+ dStar1.computeShortestPath()
+ val path1 = dStar1.path()
+
+ // Create a fresh graph with the updated blocked nodes and compute path
+ val graph2 = createGridGraph6Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(path1.length(), path2.length(),
+ "Paths should be same length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}")
+ }
+
+ /**
+ * Test path consistency with a complex scenario involving multiple invalidations.
+ */
+ @Test
+ fun `path consistency with complex scenario`() {
+ val startNode = fastVectorOf(0, 0, 10)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph6Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+ val initialPath = dStar1.path()
+
+ // Block multiple nodes to create a complex scenario
+ (-3..3).forEach { x ->
+ (-3..3).forEach { y ->
+ val blockedNode = fastVectorOf(x, y, 5)
+ blockedNodes.add(blockedNode)
+ dStar1.invalidate(blockedNode)
+ }
+ }
+
+ dStar1.computeShortestPath()
+ val path1 = dStar1.path()
+
+ blockedNodes.forEach { blockedNode ->
+ assertTrue(!path1.contains(blockedNode), "Blocked node ${blockedNode.string} should not be in path ${path1.string()}")
+ }
+
+ // Verify that the path changed after blocking
+ assertTrue(initialPath.length() < path1.length(),
+ "Initial path length (${initialPath.length()}) should be less than blocked path length (${path1.length()})")
+
+ // Create a fresh graph with all blocked nodes and compute path
+ val graph2 = createGridGraph6Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical
+ assertEquals(path1.length(), path2.length(),
+ "Paths should be same length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}")
+ }
+
+ /**
+ * Test path consistency with a disconnected graph scenario.
+ */
+ @Test
+ fun `path consistency with disconnected graph`() {
+ val startNode = fastVectorOf(0, 0, 5)
+ val goalNode = fastVectorOf(0, 0, 0)
+ val blockedNodes = mutableSetOf()
+
+ // Create initial graph and compute path
+ val graph1 = createGridGraph6Conn(blockedNodes)
+ val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic)
+ dStar1.computeShortestPath()
+
+ // Block nodes to completely disconnect start from goal
+ // Block all nodes at z=3
+ for (x in -2..2) {
+ for (y in -2..2) {
+ val blockedNode = fastVectorOf(x, y, 3)
+ blockedNodes.add(blockedNode)
+ dStar1.invalidate(blockedNode)
+ }
+ }
+
+ dStar1.computeShortestPath()
+ val path1 = dStar1.path()
+
+ // Create a fresh graph with all blocked nodes and compute path
+ val graph2 = createGridGraph6Conn(blockedNodes)
+ val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic)
+ dStar2.computeShortestPath()
+ val path2 = dStar2.path()
+
+ // Verify paths are identical (both should be empty or contain only the start node)
+ assertEquals(path1.length(), path2.length(),
+ "Paths should have identical length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}")
+ }
+}
diff --git a/common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt b/common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt
new file mode 100644
index 000000000..bb1caa4e0
--- /dev/null
+++ b/common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt
@@ -0,0 +1,275 @@
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+package pathing
+
+import com.lambda.pathing.incremental.Key
+import com.lambda.pathing.incremental.UpdatablePriorityQueue
+import org.junit.jupiter.api.BeforeEach
+import org.junit.jupiter.api.Assertions.*
+import org.junit.jupiter.api.Test
+import org.junit.jupiter.api.assertThrows
+
+/*
+ * Copyright 2025 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+internal class UpdatablePriorityQueueTest {
+
+ private lateinit var queue: UpdatablePriorityQueue
+ private val infinityKey = Int.MAX_VALUE // Use Int.MAX_VALUE as infinity for Int keys
+
+ @BeforeEach
+ fun setUp() {
+ queue = UpdatablePriorityQueue()
+ }
+
+ @Test
+ fun `queue is initially empty`() {
+ assertTrue(queue.isEmpty())
+ assertEquals(0, queue.size())
+ assertEquals(infinityKey, queue.topKey(infinityKey))
+ assertThrows { queue.top() }
+ assertThrows { queue.pop() }
+ }
+
+ @Test
+ fun `insert adds element and updates size`() {
+ queue.insert("A", 10)
+ assertFalse(queue.isEmpty())
+ assertEquals(1, queue.size())
+ assertTrue(queue.contains("A"))
+ }
+
+ @Test
+ fun `insert multiple elements maintains order`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+ queue.insert("C", 15)
+
+ assertEquals(3, queue.size())
+ assertEquals(5, queue.topKey(infinityKey))
+ assertEquals("B", queue.top())
+ }
+
+ @Test
+ fun `pop removes and returns top element maintaining order`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+ queue.insert("C", 15)
+
+ assertEquals("B", queue.pop())
+ assertEquals(2, queue.size())
+ assertEquals(10, queue.topKey(infinityKey))
+ assertEquals("A", queue.top())
+ assertFalse(queue.contains("B"))
+
+ assertEquals("A", queue.pop())
+ assertEquals(1, queue.size())
+ assertEquals(15, queue.topKey(infinityKey))
+ assertEquals("C", queue.top())
+ assertFalse(queue.contains("A"))
+
+ assertEquals("C", queue.pop())
+ assertEquals(0, queue.size())
+ assertTrue(queue.isEmpty())
+ assertFalse(queue.contains("C"))
+ }
+
+ @Test
+ fun `pop on empty queue throws exception`() {
+ assertThrows { queue.pop() }
+ }
+
+ @Test
+ fun `top on empty queue throws exception`() {
+ assertThrows { queue.top() }
+ }
+
+ @Test
+ fun `topKey on empty queue returns infinityKey`() {
+ assertEquals(infinityKey, queue.topKey(infinityKey))
+ }
+
+ @Test
+ fun `contains checks for element presence`() {
+ queue.insert("A", 10)
+ assertTrue(queue.contains("A"))
+ assertFalse(queue.contains("B"))
+ }
+
+ @Test
+ fun `remove existing element works`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+ queue.insert("C", 15)
+
+ assertTrue(queue.remove("A"))
+ assertEquals(2, queue.size())
+ assertFalse(queue.contains("A"))
+ assertEquals(5, queue.topKey(infinityKey)) // B should be top
+ assertEquals("B", queue.top())
+
+ assertTrue(queue.remove("C"))
+ assertEquals(1, queue.size())
+ assertFalse(queue.contains("C"))
+ assertEquals(5, queue.topKey(infinityKey)) // B still top
+ assertEquals("B", queue.top())
+ }
+
+ @Test
+ fun `remove affects top element`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+
+ assertTrue(queue.remove("B")) // Remove the top element
+ assertEquals(1, queue.size())
+ assertEquals(10, queue.topKey(infinityKey))
+ assertEquals("A", queue.top())
+ }
+
+ @Test
+ fun `remove non-existing element returns false`() {
+ queue.insert("A", 10)
+ assertFalse(queue.remove("B"))
+ assertEquals(1, queue.size())
+ }
+
+ @Test
+ fun `update changes key and maintains order - smaller key`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+ queue.insert("C", 15)
+
+ queue.update("A", 2) // Update A's key to be the smallest
+ assertEquals(3, queue.size())
+ assertEquals(2, queue.topKey(infinityKey))
+ assertEquals("A", queue.top())
+ }
+
+ @Test
+ fun `update changes key and maintains order - larger key`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+ queue.insert("C", 15)
+
+ queue.update("B", 20) // Update B's key to be the largest
+ assertEquals(3, queue.size())
+ assertEquals(10, queue.topKey(infinityKey)) // A should be top now
+ assertEquals("A", queue.top())
+
+ // Pop A and check again
+ assertEquals("A", queue.pop())
+ assertEquals(15, queue.topKey(infinityKey)) // C should be top
+ assertEquals("C", queue.top())
+ }
+
+ @Test
+ fun `update with the same key does nothing`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+ queue.update("A", 10) // Update A with the same key
+
+ assertEquals(2, queue.size())
+ assertEquals(5, queue.topKey(infinityKey)) // B should still be top
+ assertEquals("B", queue.top())
+ }
+
+
+ @Test
+ fun `update non-existing element throws exception`() {
+ queue.insert("A", 10)
+ assertThrows { queue.update("B", 20) }
+ }
+
+ @Test
+ fun `insert existing element acts as update`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+ queue.insert("A", 2) // Re-insert A with a smaller key
+
+ assertEquals(2, queue.size()) // Size should not increase
+ assertEquals(2, queue.topKey(infinityKey)) // A should be top now
+ assertEquals("A", queue.top())
+
+ queue.insert("A", 20) // Re-insert A with a larger key
+ assertEquals(2, queue.size())
+ assertEquals(5, queue.topKey(infinityKey)) // B should be top now
+ assertEquals("B", queue.top())
+ }
+
+
+ @Test
+ fun `clear removes all elements`() {
+ queue.insert("A", 10)
+ queue.insert("B", 5)
+ queue.insert("C", 15)
+ assertFalse(queue.isEmpty())
+
+ queue.clear()
+ assertTrue(queue.isEmpty())
+ assertEquals(0, queue.size())
+ assertFalse(queue.contains("A"))
+ assertFalse(queue.contains("B"))
+ assertFalse(queue.contains("C"))
+ assertEquals(infinityKey, queue.topKey(infinityKey))
+ assertThrows { queue.top() }
+ }
+
+ @Test
+ fun `works with DStarLiteKey`() {
+ val dsQueue = UpdatablePriorityQueue()
+ val key1 = Key(10.0, 5.0)
+ val key2 = Key(5.0, 1.0)
+ val key3 = Key(5.0, 2.0)
+ val infinityDsKey = Key.INFINITY
+
+ dsQueue.insert("A", key1)
+ dsQueue.insert("B", key2)
+ dsQueue.insert("C", key3)
+
+ assertEquals(3, dsQueue.size())
+ assertEquals(key2, dsQueue.topKey(infinityDsKey))
+ assertEquals("B", dsQueue.top())
+
+ assertEquals("B", dsQueue.pop())
+ assertEquals(key3, dsQueue.topKey(infinityDsKey))
+ assertEquals("C", dsQueue.top())
+
+ dsQueue.update("A", Key(1.0, 1.0))
+ assertEquals(Key(1.0, 1.0), dsQueue.topKey(infinityDsKey))
+ assertEquals("A", dsQueue.top())
+
+ assertTrue(dsQueue.contains("C"))
+ dsQueue.remove("C")
+ assertFalse(dsQueue.contains("C"))
+ assertEquals("A", dsQueue.top())
+ }
+}
\ No newline at end of file