From 9602e3f63dec4e7663dd63da6bac22bdd27e0ce7 Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 26 Feb 2025 03:58:04 +0100 Subject: [PATCH 01/39] Added pathfinder --- .../esp/builders/StaticESPBuilders.kt | 13 + .../construction/simulation/Simulation.kt | 12 +- .../module/modules/movement/Pathfinder.kt | 52 ++++ .../main/kotlin/com/lambda/pathing/AStar.kt | 78 +++++ .../main/kotlin/com/lambda/pathing/Node.kt | 52 ++++ .../main/kotlin/com/lambda/pathing/Path.kt | 31 ++ .../kotlin/com/lambda/pathing/goal/Goal.kt | 26 ++ .../com/lambda/pathing/goal/SimpleGoal.kt | 29 ++ .../kotlin/com/lambda/pathing/move/Move.kt | 51 ++++ .../lambda/util/collections/UpdatableLazy.kt | 20 +- .../com/lambda/util/combat/CombatUtils.kt | 2 +- .../kotlin/com/lambda/util/world/Position.kt | 13 + .../com/lambda/util/world/SearchUtils.kt | 280 ++++++++++++++++++ .../kotlin/com/lambda/util/world/WorldDsl.kt | 10 +- .../com/lambda/util/world/WorldUtils.kt | 269 +---------------- 15 files changed, 653 insertions(+), 285 deletions(-) create mode 100644 common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/AStar.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/Node.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/Path.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/goal/Goal.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/move/Move.kt create mode 100644 common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt 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 8e70716f3..586a73cad 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 @@ -225,3 +226,15 @@ fun StaticESPRenderer.buildOutline( updateOutlines = true } + +fun StaticESPRenderer.buildLine( + start: Vec3d, + end: Vec3d, + color: Color, +) = outlines.use { + val vertex1 by vertex(outlineVertices, start.x, start.y, start.z, color) + val vertex2 by vertex(outlineVertices, end.x, end.y, end.z, color) + putLine(vertex1, vertex2) + + updateOutlines = true +} 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..5e775136c 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 @@ -30,12 +30,13 @@ 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.playerFitsIn +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 +58,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,10 +72,6 @@ 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) 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..844292c8a --- /dev/null +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -0,0 +1,52 @@ +/* + * 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.event.events.RenderEvent +import com.lambda.event.listener.SafeListener.Companion.listen +import com.lambda.graphics.renderer.esp.builders.buildLine +import com.lambda.module.Module +import com.lambda.module.modules.client.TaskFlowModule.drawables +import com.lambda.module.tag.ModuleTag +import com.lambda.pathing.AStar +import com.lambda.pathing.AStar.findPathAStar +import com.lambda.pathing.goal.SimpleGoal +import com.lambda.util.math.setAlpha +import com.lambda.util.world.fastVectorOf +import com.lambda.util.world.toBlockPos +import com.lambda.util.world.toFastVec +import com.lambda.util.world.toVec3d +import java.awt.Color + +object Pathfinder : Module( + name = "Pathfinder", + description = "Get from A to B", + defaultTags = setOf(ModuleTag.MOVEMENT) +) { + init { + listen { + val path = findPathAStar(player.blockPos.toFastVec(), SimpleGoal(fastVectorOf(0, 120, 0))) + + path.nodes.zipWithNext { current, next -> + val currentPos = current.pos.toBlockPos().toCenterPos() + val nextPos = next.pos.toBlockPos().toCenterPos() + it.renderer.buildLine(currentPos, nextPos, Color.BLUE.setAlpha(0.25)) + } + } + } +} \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/AStar.kt b/common/src/main/kotlin/com/lambda/pathing/AStar.kt new file mode 100644 index 000000000..d7b656c7d --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/AStar.kt @@ -0,0 +1,78 @@ +/* + * 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.Node.Companion.toNode +import com.lambda.pathing.goal.Goal +import com.lambda.pathing.move.Move +import com.lambda.util.world.FastVector +import com.lambda.util.world.WorldUtils.traversable +import com.lambda.util.world.toBlockPos +import java.util.PriorityQueue + +object AStar { + fun SafeContext.findPathAStar(start: FastVector, goal: Goal): Path { + val openSet = PriorityQueue() + val closedSet = mutableSetOf() + val startNode = start.toNode(goal) + startNode.gCost = 0.0 + openSet.add(startNode) + + while (openSet.isNotEmpty()) { + val current = openSet.remove() + if (goal.inGoal(current.pos)) { +// println("Not yet considered nodes: ${openSet.size}") +// println("Closed nodes: ${closedSet.size}") + return current.createPathToSource() + } + + closedSet.add(current) + + moveOptions(current.pos).forEach { move -> + val successor = move.node(current.pos, goal) + if (closedSet.contains(successor)) return@forEach + val tentativeGCost = current.gCost + move.cost() + if (tentativeGCost >= successor.gCost) return@forEach + successor.predecessor = current + successor.gCost = tentativeGCost + openSet.add(successor) + } + } + + println("No path found") + return Path() + } + + private fun SafeContext.moveOptions(origin: FastVector): List { + val originPos = origin.toBlockPos() + return Move.entries.filter { move -> + traversable(originPos.add(move.x, move.y, move.z)) + } + } + +// class Move( +// private val origin: FastVector, +// val offset: FastVector, +// ) { +// val cost: Double = origin.distManhattan(offset).toDouble() +// +// fun nextNode(goal: Goal) = +// origin.plus(offset).toNode(goal) +// } +} \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/Node.kt b/common/src/main/kotlin/com/lambda/pathing/Node.kt new file mode 100644 index 000000000..12305efbb --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/Node.kt @@ -0,0 +1,52 @@ +/* + * 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.pathing.goal.Goal +import com.lambda.util.world.FastVector +import com.lambda.util.world.toBlockPos + +data class Node( + val pos: FastVector, + var predecessor: Node? = null, + var gCost: Double = Double.POSITIVE_INFINITY, + val hCost: Double +) : Comparable { + // use updateable lazy and recompute on gCost change + private val fCost get() = gCost + hCost + + override fun compareTo(other: Node) = + fCost.compareTo(other.fCost) + + fun createPathToSource(): Path { + val path = Path() + var current: Node? = this + while (current != null) { + path.prepend(current) + current = current.predecessor + } + return path + } + + override fun toString() = "Node(pos=${pos.toBlockPos().toShortString()}, gCost=$gCost, hCost=$hCost)" + + companion object { + fun FastVector.toNode(goal: Goal) = + Node(this, hCost = goal.heuristic(this)) + } +} \ No newline at end of file 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..044f4bd7f --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/Path.kt @@ -0,0 +1,31 @@ +/* + * 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.util.world.toBlockPos + +data class Path( + val nodes: ArrayDeque = ArrayDeque(), +) { + fun prepend(node: Node) { + nodes.addFirst(node) + } + + override fun toString() = + nodes.joinToString(" -> ") { "(${it.pos.toBlockPos().toShortString()})" } +} \ No newline at end of file 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..7b7240caf --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt @@ -0,0 +1,29 @@ +/* + * 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.distSq + +class SimpleGoal( + val pos: FastVector, +) : Goal { + override fun inGoal(pos: FastVector) = pos == this.pos + + override fun heuristic(pos: FastVector) = pos distSq this.pos +} \ 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..de9042b5e --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/move/Move.kt @@ -0,0 +1,51 @@ +/* + * 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.Node +import com.lambda.pathing.Node.Companion.toNode +import com.lambda.pathing.goal.Goal +import com.lambda.util.world.FastVector +import com.lambda.util.world.offset +import kotlin.math.abs + +enum class Move(val x: Int, val y: Int, val z: Int) { + TRAVERSE_NORTH(0, 0, -1), + TRAVERSE_NORTH_EAST(1, 0, -1), + TRAVERSE_EAST(1, 0, 0), + TRAVERSE_SOUTH_EAST(1, 0, 1), + TRAVERSE_SOUTH(0, 0, 1), + TRAVERSE_SOUTH_WEST(-1, 0, 1), + TRAVERSE_WEST(-1, 0, 0), + TRAVERSE_NORTH_WEST(-1, 0, -1), + PILLAR(0, 1, 0), + ASCEND_NORTH(0, 1, -1), + ASCEND_EAST(1, 1, 0), + ASCEND_SOUTH(0, 1, 1), + ASCEND_WEST(-1, 1, 0), + FALL(0, -1, 0), + DESCEND_NORTH(0, -1, -1), + DESCEND_EAST(1, -1, 0), + DESCEND_SOUTH(0, -1, 1), + DESCEND_WEST(-1, -1, 0); + + fun cost(): Int = abs(x) + abs(y) + abs(z) + + // ToDo: Use DirectionMask + fun node(origin: FastVector, goal: Goal): Node = origin.offset(x, y, z).toNode(goal) +} \ 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..302a0d122 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,17 @@ 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 } /** * Resets the current value to a new value generated by the initializer function. diff --git a/common/src/main/kotlin/com/lambda/util/combat/CombatUtils.kt b/common/src/main/kotlin/com/lambda/util/combat/CombatUtils.kt index e3bd840f2..7d17bc146 100644 --- a/common/src/main/kotlin/com/lambda/util/combat/CombatUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/combat/CombatUtils.kt @@ -22,7 +22,7 @@ import com.lambda.core.annotations.InternalApi import com.lambda.util.math.dist import com.lambda.util.math.minus import com.lambda.util.math.times -import com.lambda.util.world.WorldUtils.internalGetFastEntities +import com.lambda.util.world.SearchUtils.internalGetFastEntities import com.lambda.util.world.fastEntitySearch import com.lambda.util.world.toFastVec import net.minecraft.enchantment.EnchantmentHelper 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 6a7b0a199..7d9fffba1 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,7 @@ 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 /** * Represents a position in the world encoded as a long. @@ -125,6 +126,8 @@ 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) + /** * Adds the given vector to the position. */ @@ -200,6 +203,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): Int { + val dx = x - other.x + val dy = y - other.y + val dz = z - other.z + return abs(dx) + abs(dy) + abs(dz) +} + /** * Returns the squared distance between this position and the Vec3i. */ 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..ad6ea9d93 --- /dev/null +++ b/common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt @@ -0,0 +1,280 @@ +/* + * 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 plus fastVectorOf(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/WorldDsl.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/WorldDsl.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/WorldUtils.kt b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt index cb43b65c4..95ecb3007 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,16 @@ 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.interaction.construction.simulation.Simulation.Companion.playerBox +import com.lambda.util.BlockUtils.blockState +import net.minecraft.util.math.BlockPos +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 - } - } - - 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 plus fastVectorOf(x, y, z), - ) - } - } - } - } -} + fun SafeContext.traversable(pos: BlockPos) = + blockState(pos.down()).isSideSolidFullSquare(world, pos, Direction.UP) && playerFitsIn(pos) + fun SafeContext.playerFitsIn(pos: BlockPos) = + world.isSpaceEmpty(Vec3d.ofBottomCenter(pos).playerBox()) +} \ No newline at end of file From 77f858c53d1bbd429d3b706ea0dba65a31c7f9ba Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 26 Feb 2025 19:44:49 +0100 Subject: [PATCH 02/39] Kinda working pathing --- .../module/modules/movement/Pathfinder.kt | 85 +++++++++++++++++-- .../lambda/module/modules/player/Freecam.kt | 4 + .../main/kotlin/com/lambda/pathing/Node.kt | 2 +- .../lambda/pathing/{AStar.kt => Pathing.kt} | 23 +---- .../com/lambda/pathing/goal/SimpleGoal.kt | 3 +- .../kotlin/com/lambda/pathing/move/Move.kt | 8 +- .../com/lambda/pathing/move/MoveFinder.kt | 32 +++++++ .../kotlin/com/lambda/util/world/Position.kt | 7 +- .../com/lambda/util/world/WorldUtils.kt | 5 +- 9 files changed, 132 insertions(+), 37 deletions(-) rename common/src/main/kotlin/com/lambda/pathing/{AStar.kt => Pathing.kt} (75%) create mode 100644 common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt 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 index 844292c8a..91f08fb36 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -17,36 +17,107 @@ package com.lambda.module.modules.movement +import com.lambda.context.SafeContext import com.lambda.event.events.RenderEvent +import com.lambda.event.events.RotationEvent import com.lambda.event.listener.SafeListener.Companion.listen import com.lambda.graphics.renderer.esp.builders.buildLine +import com.lambda.interaction.request.rotation.Rotation.Companion.rotationTo import com.lambda.module.Module -import com.lambda.module.modules.client.TaskFlowModule.drawables import com.lambda.module.tag.ModuleTag -import com.lambda.pathing.AStar -import com.lambda.pathing.AStar.findPathAStar +import com.lambda.pathing.Path +import com.lambda.pathing.Pathing.findPathAStar import com.lambda.pathing.goal.SimpleGoal 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.fastVectorOf import com.lambda.util.world.toBlockPos import com.lambda.util.world.toFastVec -import com.lambda.util.world.toVec3d +import net.minecraft.util.math.Vec3d import java.awt.Color +import kotlin.math.cos +import kotlin.math.sin object Pathfinder : Module( name = "Pathfinder", description = "Get from A to B", defaultTags = setOf(ModuleTag.MOVEMENT) ) { + // PID Settings + private val kP by setting("P Gain", 0.5, 0.0..2.0, 0.01) + private val kI by setting("I Gain", 0.0, 0.0..1.0, 0.01) + private val kD by setting("D Gain", 0.2, 0.0..1.0, 0.01) + private val tolerance by setting("Node Tolerance", 0.1, 0.01..1.0, 0.01) + + var path = Path() + private var currentTarget: Vec3d? = null + private var integralError = Vec3d.ZERO + private var lastError = Vec3d.ZERO + init { - listen { - val path = findPathAStar(player.blockPos.toFastVec(), SimpleGoal(fastVectorOf(0, 120, 0))) + onEnable { + path = findPathAStar( + player.blockPos.toFastVec(), + SimpleGoal(fastVectorOf(0, 120, 0)) + ) +// currentTarget = Vec3d(0.5, 120.0, 0.5) + integralError = Vec3d.ZERO + lastError = Vec3d.ZERO + } + + listen { event -> + if (path.nodes.isEmpty()) return@listen + + updateTargetNode() + 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 = strafeComponent, + jump = player.isOnGround && adjustment.y > 0.5 + ) + event.input.mergeFrom(moveInput) + } + } + listen { event -> path.nodes.zipWithNext { current, next -> val currentPos = current.pos.toBlockPos().toCenterPos() val nextPos = next.pos.toBlockPos().toCenterPos() - it.renderer.buildLine(currentPos, nextPos, Color.BLUE.setAlpha(0.25)) + event.renderer.buildLine(currentPos, nextPos, Color.BLUE.setAlpha(0.25)) + } + } + } + + private fun SafeContext.updateTargetNode() { + path.nodes.firstOrNull()?.let { firstNode -> + val nodeVec = Vec3d.ofBottomCenter(firstNode.pos.toBlockPos()) + if (player.pos.distanceTo(nodeVec) < tolerance) { + path.nodes.removeFirst() + integralError = Vec3d.ZERO } + val next = path.nodes.firstOrNull()?.pos?.toBlockPos() ?: return + currentTarget = Vec3d.ofBottomCenter(next) } } + + 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(kP) + .add(integralError.multiply(kI)) + .add(derivativeError.multiply(kD)) + } } \ 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/Node.kt b/common/src/main/kotlin/com/lambda/pathing/Node.kt index 12305efbb..a12109b38 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Node.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Node.kt @@ -43,7 +43,7 @@ data class Node( return path } - override fun toString() = "Node(pos=${pos.toBlockPos().toShortString()}, gCost=$gCost, hCost=$hCost)" + override fun toString() = "Node(pos=(${pos.toBlockPos().toShortString()}), gCost=$gCost, hCost=$hCost)" companion object { fun FastVector.toNode(goal: Goal) = diff --git a/common/src/main/kotlin/com/lambda/pathing/AStar.kt b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt similarity index 75% rename from common/src/main/kotlin/com/lambda/pathing/AStar.kt rename to common/src/main/kotlin/com/lambda/pathing/Pathing.kt index d7b656c7d..c2128d628 100644 --- a/common/src/main/kotlin/com/lambda/pathing/AStar.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt @@ -20,13 +20,11 @@ package com.lambda.pathing import com.lambda.context.SafeContext import com.lambda.pathing.Node.Companion.toNode import com.lambda.pathing.goal.Goal -import com.lambda.pathing.move.Move +import com.lambda.pathing.move.MoveFinder.moveOptions import com.lambda.util.world.FastVector -import com.lambda.util.world.WorldUtils.traversable -import com.lambda.util.world.toBlockPos import java.util.PriorityQueue -object AStar { +object Pathing { fun SafeContext.findPathAStar(start: FastVector, goal: Goal): Path { val openSet = PriorityQueue() val closedSet = mutableSetOf() @@ -58,21 +56,4 @@ object AStar { println("No path found") return Path() } - - private fun SafeContext.moveOptions(origin: FastVector): List { - val originPos = origin.toBlockPos() - return Move.entries.filter { move -> - traversable(originPos.add(move.x, move.y, move.z)) - } - } - -// class Move( -// private val origin: FastVector, -// val offset: FastVector, -// ) { -// val cost: Double = origin.distManhattan(offset).toDouble() -// -// fun nextNode(goal: Goal) = -// origin.plus(offset).toNode(goal) -// } } \ 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 index 7b7240caf..50af9e1a8 100644 --- a/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt +++ b/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt @@ -18,6 +18,7 @@ package com.lambda.pathing.goal import com.lambda.util.world.FastVector +import com.lambda.util.world.distManhattan import com.lambda.util.world.distSq class SimpleGoal( @@ -25,5 +26,5 @@ class SimpleGoal( ) : Goal { override fun inGoal(pos: FastVector) = pos == this.pos - override fun heuristic(pos: FastVector) = pos distSq this.pos + override fun heuristic(pos: FastVector) = pos distManhattan this.pos } \ 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 index de9042b5e..d50ac2fef 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/Move.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/Move.kt @@ -26,13 +26,13 @@ import kotlin.math.abs enum class Move(val x: Int, val y: Int, val z: Int) { TRAVERSE_NORTH(0, 0, -1), - TRAVERSE_NORTH_EAST(1, 0, -1), +// TRAVERSE_NORTH_EAST(1, 0, -1), TRAVERSE_EAST(1, 0, 0), - TRAVERSE_SOUTH_EAST(1, 0, 1), +// TRAVERSE_SOUTH_EAST(1, 0, 1), TRAVERSE_SOUTH(0, 0, 1), - TRAVERSE_SOUTH_WEST(-1, 0, 1), +// TRAVERSE_SOUTH_WEST(-1, 0, 1), TRAVERSE_WEST(-1, 0, 0), - TRAVERSE_NORTH_WEST(-1, 0, -1), +// TRAVERSE_NORTH_WEST(-1, 0, -1), PILLAR(0, 1, 0), ASCEND_NORTH(0, 1, -1), ASCEND_EAST(1, 1, 0), 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..9f8576f63 --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -0,0 +1,32 @@ +/* + * 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.util.world.FastVector +import com.lambda.util.world.WorldUtils.traversable +import com.lambda.util.world.toBlockPos + +object MoveFinder { + fun SafeContext.moveOptions(origin: FastVector): List { + val originPos = origin.toBlockPos() + return Move.entries.filter { move -> + traversable(originPos.add(move.x, move.y, move.z)) + } + } +} \ No newline at end of file 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 7d9fffba1..1f49c43da 100644 --- a/common/src/main/kotlin/com/lambda/util/world/Position.kt +++ b/common/src/main/kotlin/com/lambda/util/world/Position.kt @@ -22,6 +22,7 @@ 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. @@ -193,6 +194,8 @@ infix fun FastVector.mod(scalar: Int): FastVector = fastVectorOf(x % scalar, y % infix fun FastVector.mod(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. */ @@ -206,11 +209,11 @@ infix fun FastVector.distSq(other: FastVector): Double { /** * Returns the Manhattan distance between this position and the other. */ -infix fun FastVector.distManhattan(other: FastVector): Int { +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) + return (abs(dx) + abs(dy) + abs(dz)).toDouble() } /** 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 95ecb3007..ef99e0919 100644 --- a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt @@ -25,8 +25,11 @@ import net.minecraft.util.math.Direction import net.minecraft.util.math.Vec3d object WorldUtils { + fun SafeContext.traversable(from: BlockPos, to: BlockPos) = + BlockPos.stream(from, to).allMatch { traversable(it) } + fun SafeContext.traversable(pos: BlockPos) = - blockState(pos.down()).isSideSolidFullSquare(world, pos, Direction.UP) && playerFitsIn(pos) + blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) && playerFitsIn(pos) fun SafeContext.playerFitsIn(pos: BlockPos) = world.isSpaceEmpty(Vec3d.ofBottomCenter(pos).playerBox()) From 71bbfec16e11e851edc2aad9ceb2148830f9b051 Mon Sep 17 00:00:00 2001 From: Constructor Date: Sat, 1 Mar 2025 22:22:39 +0100 Subject: [PATCH 03/39] NodeTypes and different moves --- .../construction/simulation/Simulation.kt | 3 +- .../module/modules/movement/Pathfinder.kt | 107 +++++++++------ .../main/kotlin/com/lambda/pathing/Node.kt | 52 -------- .../main/kotlin/com/lambda/pathing/Path.kt | 9 +- .../main/kotlin/com/lambda/pathing/Pathing.kt | 51 ++++--- .../com/lambda/pathing/PathingConfig.kt | 28 ++++ .../com/lambda/pathing/PathingSettings.kt | 39 ++++++ .../com/lambda/pathing/goal/SimpleGoal.kt | 3 + .../kotlin/com/lambda/pathing/move/Move.kt | 52 ++++---- .../com/lambda/pathing/move/MoveFinder.kt | 125 +++++++++++++++++- .../com/lambda/pathing/move/NodeType.kt | 50 +++++++ .../com/lambda/pathing/move/ParkourMove.kt | 30 +++++ .../com/lambda/pathing/move/SwimMove.kt | 33 +++++ .../com/lambda/pathing/move/TraverseMove.kt | 33 +++++ .../kotlin/com/lambda/util/world/Position.kt | 14 +- .../com/lambda/util/world/SearchUtils.kt | 4 +- .../com/lambda/util/world/WorldUtils.kt | 47 ++++++- 17 files changed, 521 insertions(+), 159 deletions(-) delete mode 100644 common/src/main/kotlin/com/lambda/pathing/Node.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/move/NodeType.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/move/ParkourMove.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/move/SwimMove.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/move/TraverseMove.kt 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 5e775136c..868415aeb 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 @@ -30,6 +30,7 @@ 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.playerFitsIn import com.lambda.util.world.WorldUtils.traversable import com.lambda.util.world.toBlockPos @@ -73,8 +74,6 @@ data class Simulation( } 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/modules/movement/Pathfinder.kt b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt index 91f08fb36..c0860d991 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -17,17 +17,25 @@ package com.lambda.module.modules.movement +import com.lambda.config.groups.RotationSettings import com.lambda.context.SafeContext import com.lambda.event.events.RenderEvent import com.lambda.event.events.RotationEvent +import com.lambda.event.events.TickEvent import com.lambda.event.listener.SafeListener.Companion.listen import com.lambda.graphics.renderer.esp.builders.buildLine +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.PathingSettings import com.lambda.pathing.goal.SimpleGoal +import com.lambda.threading.runConcurrent +import com.lambda.util.Communication.info import com.lambda.util.math.setAlpha import com.lambda.util.player.MovementUtils.buildMovementInput import com.lambda.util.player.MovementUtils.mergeFrom @@ -38,75 +46,96 @@ import net.minecraft.util.math.Vec3d import java.awt.Color 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) ) { - // PID Settings - private val kP by setting("P Gain", 0.5, 0.0..2.0, 0.01) - private val kI by setting("I Gain", 0.0, 0.0..1.0, 0.01) - private val kD by setting("D Gain", 0.2, 0.0..1.0, 0.01) - private val tolerance by setting("Node Tolerance", 0.1, 0.01..1.0, 0.01) + private val pathing = PathingSettings(this) + private val rotation = RotationSettings(this) - var path = Path() + var path: Path? = null private var currentTarget: Vec3d? = null private var integralError = Vec3d.ZERO private var lastError = Vec3d.ZERO + private var calculating = false init { onEnable { - path = findPathAStar( - player.blockPos.toFastVec(), - SimpleGoal(fastVectorOf(0, 120, 0)) - ) -// currentTarget = Vec3d(0.5, 120.0, 0.5) integralError = Vec3d.ZERO lastError = Vec3d.ZERO } - listen { event -> - if (path.nodes.isEmpty()) return@listen + listen { + if (calculating) return@listen + calculating = true - updateTargetNode() - 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 = strafeComponent, - jump = player.isOnGround && adjustment.y > 0.5 - ) - event.input.mergeFrom(moveInput) + runConcurrent { + val took = measureTimeMillis { + path = findPathAStar( + player.blockPos.toFastVec(), + // SimpleGoal(fastVectorOf(0, 78, 0)), + SimpleGoal(fastVectorOf(0, 120, 0)), + pathing.cutoffTimeout + ) + } + info("Found path of length ${path?.moves?.size} in $took ms") + println("Path: ${path?.toString()}") + calculating = false } } +// listen { event -> +// updateTargetNode() +// 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 = strafeComponent, +// jump = player.isOnGround && adjustment.y > 0.5 +// ) +// event.input.mergeFrom(moveInput) +// } +// } +// +// onRotate { +// val nextTarget = path?.moves?.getOrNull(2)?.pos?.toBlockPos() ?: return@onRotate +// val part = player.eyePos.rotationTo(Vec3d.ofBottomCenter(nextTarget)) +// val targetRotation = Rotation(part.yaw, player.pitch.toDouble()) +// +// lookAt(targetRotation).requestBy(rotation) +// } + listen { event -> - path.nodes.zipWithNext { current, next -> + path?.moves?.zipWithNext { current, next -> val currentPos = current.pos.toBlockPos().toCenterPos() val nextPos = next.pos.toBlockPos().toCenterPos() - event.renderer.buildLine(currentPos, nextPos, Color.BLUE.setAlpha(0.25)) + event.renderer.buildLine(currentPos, nextPos, Color.GREEN) } } } private fun SafeContext.updateTargetNode() { - path.nodes.firstOrNull()?.let { firstNode -> + path?.moves?.firstOrNull()?.let { firstNode -> val nodeVec = Vec3d.ofBottomCenter(firstNode.pos.toBlockPos()) - if (player.pos.distanceTo(nodeVec) < tolerance) { - path.nodes.removeFirst() + if (player.pos.distanceTo(nodeVec) < pathing.tolerance) { + path?.moves?.removeFirst() integralError = Vec3d.ZERO } - val next = path.nodes.firstOrNull()?.pos?.toBlockPos() ?: return + val next = path?.moves?.firstOrNull()?.pos?.toBlockPos() ?: return currentTarget = Vec3d.ofBottomCenter(next) + } ?: run { + currentTarget = null } } @@ -116,8 +145,8 @@ object Pathfinder : Module( val derivativeError = error.subtract(lastError) lastError = error - return error.multiply(kP) - .add(integralError.multiply(kI)) - .add(derivativeError.multiply(kD)) + return error.multiply(pathing.kP) + .add(integralError.multiply(pathing.kI)) + .add(derivativeError.multiply(pathing.kD)) } } \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/Node.kt b/common/src/main/kotlin/com/lambda/pathing/Node.kt deleted file mode 100644 index a12109b38..000000000 --- a/common/src/main/kotlin/com/lambda/pathing/Node.kt +++ /dev/null @@ -1,52 +0,0 @@ -/* - * 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.pathing.goal.Goal -import com.lambda.util.world.FastVector -import com.lambda.util.world.toBlockPos - -data class Node( - val pos: FastVector, - var predecessor: Node? = null, - var gCost: Double = Double.POSITIVE_INFINITY, - val hCost: Double -) : Comparable { - // use updateable lazy and recompute on gCost change - private val fCost get() = gCost + hCost - - override fun compareTo(other: Node) = - fCost.compareTo(other.fCost) - - fun createPathToSource(): Path { - val path = Path() - var current: Node? = this - while (current != null) { - path.prepend(current) - current = current.predecessor - } - return path - } - - override fun toString() = "Node(pos=(${pos.toBlockPos().toShortString()}), gCost=$gCost, hCost=$hCost)" - - companion object { - fun FastVector.toNode(goal: Goal) = - Node(this, hCost = goal.heuristic(this)) - } -} \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/Path.kt b/common/src/main/kotlin/com/lambda/pathing/Path.kt index 044f4bd7f..d3ff3a0be 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Path.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Path.kt @@ -17,15 +17,16 @@ package com.lambda.pathing +import com.lambda.pathing.move.Move import com.lambda.util.world.toBlockPos data class Path( - val nodes: ArrayDeque = ArrayDeque(), + val moves: ArrayDeque = ArrayDeque(), ) { - fun prepend(node: Node) { - nodes.addFirst(node) + fun prepend(move: Move) { + moves.addFirst(move) } override fun toString() = - nodes.joinToString(" -> ") { "(${it.pos.toBlockPos().toShortString()})" } + 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 index c2128d628..7547b6dd5 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt @@ -18,42 +18,55 @@ package com.lambda.pathing import com.lambda.context.SafeContext -import com.lambda.pathing.Node.Companion.toNode 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.toBlockPos import java.util.PriorityQueue object Pathing { - fun SafeContext.findPathAStar(start: FastVector, goal: Goal): Path { - val openSet = PriorityQueue() - val closedSet = mutableSetOf() - val startNode = start.toNode(goal) + fun SafeContext.findPathAStar(start: FastVector, goal: Goal, timeout: Long = 50L): 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) - while (openSet.isNotEmpty()) { + println("Starting pathfinding at ${start.toBlockPos().toShortString()} to $goal") + + while (openSet.isNotEmpty() && startedAt + timeout > 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}") + println("Not yet considered nodes: ${openSet.size}") + println("Closed nodes: ${closedSet.size}") return current.createPathToSource() } - closedSet.add(current) + closedSet.add(current.pos) - moveOptions(current.pos).forEach { move -> - val successor = move.node(current.pos, goal) - if (closedSet.contains(successor)) return@forEach - val tentativeGCost = current.gCost + move.cost() - if (tentativeGCost >= successor.gCost) return@forEach - successor.predecessor = current - successor.gCost = tentativeGCost - openSet.add(successor) + moveOptions(current, goal).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") } } - println("No path found") - return Path() + warn("Only partial path found!") + return if (openSet.isNotEmpty()) openSet.remove().createPathToSource() else Path() } } \ 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..8322544df --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -0,0 +1,28 @@ +/* + * 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 + +interface PathingConfig { + val kP: Double + val kI: Double + val kD: Double + val tolerance: Double + val cutoffTimeout: Long + + val assumeJesus: Boolean +} 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..cb06e6119 --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -0,0 +1,39 @@ +/* + * 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 + +class PathingSettings( + c: Configurable, + vis: () -> Boolean = { true } +) : PathingConfig { + enum class Page { + Execution, Misc + } + + private val page by c.setting("Pathing Page", Page.Execution, "Current page", vis) + + override val kP by c.setting("P Gain", 0.5, 0.0..2.0, 0.01) { vis() && page == Page.Execution } + override val kI by c.setting("I Gain", 0.0, 0.0..1.0, 0.01) { vis() && page == Page.Execution } + override val kD by c.setting("D Gain", 0.2, 0.0..1.0, 0.01) { vis() && page == Page.Execution } + override val tolerance by c.setting("Node Tolerance", 0.1, 0.01..1.0, 0.01) { vis() && page == Page.Execution } + override val cutoffTimeout by c.setting("Cutoff Timeout", 50L, 1L..2000L, 10L) { vis() && page == Page.Execution } + + override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc } +} diff --git a/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt b/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt index 50af9e1a8..41e9512a9 100644 --- a/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt +++ b/common/src/main/kotlin/com/lambda/pathing/goal/SimpleGoal.kt @@ -20,6 +20,7 @@ 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, @@ -27,4 +28,6 @@ class SimpleGoal( 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/move/Move.kt b/common/src/main/kotlin/com/lambda/pathing/move/Move.kt index d50ac2fef..3d72c5fd0 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/Move.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/Move.kt @@ -17,35 +17,33 @@ package com.lambda.pathing.move -import com.lambda.pathing.Node -import com.lambda.pathing.Node.Companion.toNode -import com.lambda.pathing.goal.Goal +import com.lambda.pathing.Path +import com.lambda.task.Task import com.lambda.util.world.FastVector -import com.lambda.util.world.offset -import kotlin.math.abs -enum class Move(val x: Int, val y: Int, val z: Int) { - TRAVERSE_NORTH(0, 0, -1), -// TRAVERSE_NORTH_EAST(1, 0, -1), - TRAVERSE_EAST(1, 0, 0), -// TRAVERSE_SOUTH_EAST(1, 0, 1), - TRAVERSE_SOUTH(0, 0, 1), -// TRAVERSE_SOUTH_WEST(-1, 0, 1), - TRAVERSE_WEST(-1, 0, 0), -// TRAVERSE_NORTH_WEST(-1, 0, -1), - PILLAR(0, 1, 0), - ASCEND_NORTH(0, 1, -1), - ASCEND_EAST(1, 1, 0), - ASCEND_SOUTH(0, 1, 1), - ASCEND_WEST(-1, 1, 0), - FALL(0, -1, 0), - DESCEND_NORTH(0, -1, -1), - DESCEND_EAST(1, -1, 0), - DESCEND_SOUTH(0, -1, 1), - DESCEND_WEST(-1, -1, 0); +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 - fun cost(): Int = abs(x) + abs(y) + abs(z) + var predecessor: Move? = null + var gCost: Double = Double.POSITIVE_INFINITY - // ToDo: Use DirectionMask - fun node(origin: FastVector, goal: Goal): Node = origin.offset(x, y, z).toNode(goal) + // use updateable lazy and recompute on gCost change + private val fCost get() = gCost + hCost + + 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 index 9f8576f63..886153bc2 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -18,15 +18,132 @@ package com.lambda.pathing.move import com.lambda.context.SafeContext +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.isPathClear 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.manhattanLength 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.ai.pathing.NavigationType +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.Direction +import net.minecraft.util.math.EightWayDirection object MoveFinder { - fun SafeContext.moveOptions(origin: FastVector): List { - val originPos = origin.toBlockPos() - return Move.entries.filter { move -> - traversable(originPos.add(move.x, move.y, move.z)) + private val nodeTypeCache = HashMap() + + fun SafeContext.moveOptions(origin: Move, goal: Goal) = + EightWayDirection.entries.flatMap { direction -> + (-1..1).mapNotNull { y -> + getPathNode(goal, origin, direction, y) + } + } + + private fun SafeContext.getPathNode( + goal: Goal, + origin: Move, + direction: EightWayDirection, + height: Int, + ): Move? { + val offset = fastVectorOf(direction.offsetX, height, direction.offsetZ) + val checkingPos = origin.pos.add(offset) + val checkingBlockPos = checkingPos.toBlockPos() + if (!world.worldBorder.contains(checkingBlockPos)) return null + + val nodeType = findPathType(checkingPos) + val hCost = goal.heuristic(checkingPos)/* * nodeType.penalty*/ + val cost = offset.length() + val currentFeetY = getFeetY(checkingBlockPos) + + if (nodeType == NodeType.BLOCKED) return null + +// // ToDo: Different for jumping etc + if (!isPathClear(origin.pos.toBlockPos(), checkingBlockPos)) return null + + return when { +// (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 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..092f79d02 --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/move/SwimMove.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.context.SafeContext +import com.lambda.util.world.FastVector +import com.lambda.util.world.WorldUtils.traversable +import com.lambda.util.world.toBlockPos + +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/world/Position.kt b/common/src/main/kotlin/com/lambda/util/world/Position.kt index 1f49c43da..c76eb2933 100644 --- a/common/src/main/kotlin/com/lambda/util/world/Position.kt +++ b/common/src/main/kotlin/com/lambda/util/world/Position.kt @@ -129,20 +129,24 @@ 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((x * x + y * y + 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()) /** @@ -153,12 +157,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()) /** diff --git a/common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt b/common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt index ad6ea9d93..a3107c3c1 100644 --- a/common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/world/SearchUtils.kt @@ -269,9 +269,7 @@ object SearchUtils { 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), - ) + 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 ef99e0919..9346d1599 100644 --- a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt @@ -18,19 +18,58 @@ package com.lambda.util.world import com.lambda.context.SafeContext -import com.lambda.interaction.construction.simulation.Simulation.Companion.playerBox import com.lambda.util.BlockUtils.blockState +import com.lambda.util.math.flooredBlockPos 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 kotlin.math.min object WorldUtils { - fun SafeContext.traversable(from: BlockPos, to: BlockPos) = - BlockPos.stream(from, to).allMatch { traversable(it) } - fun SafeContext.traversable(pos: BlockPos) = blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) && playerFitsIn(pos) + fun SafeContext.isPathClear( + start: BlockPos, + end: BlockPos, + stepSize: Double = 0.3, + ) = isPathClear(Vec3d.ofBottomCenter(start), Vec3d.ofBottomCenter(end), stepSize) + + fun SafeContext.isPathClear( + start: Vec3d, + end: Vec3d, + stepSize: Double = 0.3 // Step size based on player's hitbox radius + ): Boolean { + val direction = end.subtract(start) + val distance = direction.length() + if (distance <= 0) return true // No movement needed + + val stepDirection = direction.normalize().multiply(stepSize) + var currentPos = start + var remainingDistance = distance + + while (remainingDistance > 0) { + val blockPos = currentPos.flooredBlockPos + val goingDownwards = start.y > end.y + val hasSupport = blockState(blockPos.down()).isSideSolidFullSquare(world, blockPos.down(), Direction.UP) + if (!(playerFitsIn(currentPos) && (goingDownwards || hasSupport))) return false + + val step = min(stepSize, remainingDistance) + currentPos = currentPos.add(stepDirection.multiply(step)) + remainingDistance -= step + } + + // Final check at end position + return playerFitsIn(end) + } + + fun SafeContext.playerFitsIn(pos: Vec3d) = + world.isSpaceEmpty(pos.playerBox()) + fun SafeContext.playerFitsIn(pos: BlockPos) = world.isSpaceEmpty(Vec3d.ofBottomCenter(pos).playerBox()) + + 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) } \ No newline at end of file From 86d7874030d929a18e01bdb8f8ef6acd399e8da8 Mon Sep 17 00:00:00 2001 From: Constructor Date: Sun, 2 Mar 2025 01:42:57 +0100 Subject: [PATCH 04/39] Theta* --- .../module/modules/movement/Pathfinder.kt | 109 +++++++++++------- .../main/kotlin/com/lambda/pathing/Path.kt | 7 ++ .../main/kotlin/com/lambda/pathing/Pathing.kt | 54 ++++++++- .../com/lambda/pathing/PathingConfig.kt | 2 + .../com/lambda/pathing/PathingSettings.kt | 2 + .../com/lambda/pathing/move/MoveFinder.kt | 32 +++-- .../kotlin/com/lambda/util/world/Position.kt | 2 +- .../com/lambda/util/world/WorldUtils.kt | 39 ++++--- 8 files changed, 176 insertions(+), 71 deletions(-) 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 index c0860d991..4f201b5f7 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -23,7 +23,9 @@ import com.lambda.event.events.RenderEvent import com.lambda.event.events.RotationEvent import com.lambda.event.events.TickEvent import com.lambda.event.listener.SafeListener.Companion.listen +import com.lambda.graphics.renderer.esp.builders.buildFilled import com.lambda.graphics.renderer.esp.builders.buildLine +import com.lambda.graphics.renderer.esp.global.StaticESP import com.lambda.interaction.request.rotation.Rotation import com.lambda.interaction.request.rotation.Rotation.Companion.rotationTo import com.lambda.interaction.request.rotation.RotationManager.onRotate @@ -32,16 +34,19 @@ 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.PathingSettings import com.lambda.pathing.goal.SimpleGoal import com.lambda.threading.runConcurrent import com.lambda.util.Communication.info +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.fastVectorOf import com.lambda.util.world.toBlockPos import com.lambda.util.world.toFastVec +import net.minecraft.util.math.Box import net.minecraft.util.math.Vec3d import java.awt.Color import kotlin.math.cos @@ -56,7 +61,9 @@ object Pathfinder : Module( private val pathing = PathingSettings(this) private val rotation = RotationSettings(this) - var path: Path? = null + private val target = fastVectorOf(0, 78, 0) + private var longPath = Path() + private var shortPath = Path() private var currentTarget: Vec3d? = null private var integralError = Vec3d.ZERO private var lastError = Vec3d.ZERO @@ -66,73 +73,89 @@ object Pathfinder : Module( onEnable { integralError = Vec3d.ZERO lastError = Vec3d.ZERO + calculating = false } listen { + updateTargetNode() + if (calculating) return@listen calculating = true runConcurrent { - val took = measureTimeMillis { - path = findPathAStar( + val long: Path + val aStar = measureTimeMillis { + long = findPathAStar( player.blockPos.toFastVec(), - // SimpleGoal(fastVectorOf(0, 78, 0)), - SimpleGoal(fastVectorOf(0, 120, 0)), - pathing.cutoffTimeout + SimpleGoal(target), + pathing ) } - info("Found path of length ${path?.moves?.size} in $took ms") - println("Path: ${path?.toString()}") - calculating = false + val short: Path + val thetaStar = measureTimeMillis { + short = thetaStarClearance(long, pathing) + } + info("A* (Length: ${long.length.string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length.string} Nodes: ${short.moves.size} T: $thetaStar ms)") + println("Long: $long | Short: $short") + longPath = long + shortPath = short +// calculating = false } } -// listen { event -> -// updateTargetNode() -// 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 + listen { event -> + 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 = strafeComponent, -// jump = player.isOnGround && adjustment.y > 0.5 -// ) -// event.input.mergeFrom(moveInput) -// } -// } -// -// onRotate { -// val nextTarget = path?.moves?.getOrNull(2)?.pos?.toBlockPos() ?: return@onRotate + + val moveInput = buildMovementInput( + forward = forwardComponent, + strafe = 0.0/*strafeComponent*/, + jump = player.isOnGround && adjustment.y > 0.5 + ) + event.input.mergeFrom(moveInput) + } + } + + onRotate { +// val nextTarget = shortPath.moves.getOrNull(2)?.pos?.toBlockPos() ?: return@onRotate // val part = player.eyePos.rotationTo(Vec3d.ofBottomCenter(nextTarget)) -// val targetRotation = Rotation(part.yaw, player.pitch.toDouble()) -// -// lookAt(targetRotation).requestBy(rotation) -// } + val currentTarget = currentTarget ?: return@onRotate + val part = player.eyePos.rotationTo(currentTarget) + val targetRotation = Rotation(part.yaw, player.pitch.toDouble()) + + lookAt(targetRotation).requestBy(rotation) + } listen { event -> - path?.moves?.zipWithNext { current, next -> - val currentPos = current.pos.toBlockPos().toCenterPos() - val nextPos = next.pos.toBlockPos().toCenterPos() - event.renderer.buildLine(currentPos, nextPos, Color.GREEN) - } + longPath.render(event.renderer, Color.YELLOW) + shortPath.render(event.renderer, Color.GREEN) + event.renderer.buildFilled(Box(target.toBlockPos()), Color.PINK.setAlpha(0.25)) + } + } + + private fun Path.render(renderer: StaticESP, color: Color) { + moves.zipWithNext { current, next -> + val currentPos = current.pos.toBlockPos().toCenterPos() + val nextPos = next.pos.toBlockPos().toCenterPos() + renderer.buildLine(currentPos, nextPos, color) } } private fun SafeContext.updateTargetNode() { - path?.moves?.firstOrNull()?.let { firstNode -> + shortPath.moves.firstOrNull()?.let { firstNode -> val nodeVec = Vec3d.ofBottomCenter(firstNode.pos.toBlockPos()) if (player.pos.distanceTo(nodeVec) < pathing.tolerance) { - path?.moves?.removeFirst() + shortPath.moves.removeFirst() integralError = Vec3d.ZERO } - val next = path?.moves?.firstOrNull()?.pos?.toBlockPos() ?: return + val next = shortPath.moves.firstOrNull()?.pos?.toBlockPos() ?: return currentTarget = Vec3d.ofBottomCenter(next) } ?: run { currentTarget = null diff --git a/common/src/main/kotlin/com/lambda/pathing/Path.kt b/common/src/main/kotlin/com/lambda/pathing/Path.kt index d3ff3a0be..86ae08340 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Path.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Path.kt @@ -18,15 +18,22 @@ package com.lambda.pathing import com.lambda.pathing.move.Move +import com.lambda.util.world.dist import com.lambda.util.world.toBlockPos data class Path( val moves: ArrayDeque = ArrayDeque(), ) { + fun append(move: Move) { + moves.addLast(move) + } + fun prepend(move: Move) { moves.addFirst(move) } + val length get() = moves.zipWithNext { a, b -> a.pos dist b.pos }.sum() + 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 index 7547b6dd5..002319b45 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt @@ -27,11 +27,13 @@ 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, timeout: Long = 50L): Path { + fun SafeContext.findPathAStar(start: FastVector, goal: Goal, config: PathingConfig): Path { MoveFinder.clean() val startedAt = System.currentTimeMillis() val openSet = PriorityQueue() @@ -43,9 +45,9 @@ object Pathing { println("Starting pathfinding at ${start.toBlockPos().toShortString()} to $goal") - while (openSet.isNotEmpty() && startedAt + timeout > System.currentTimeMillis()) { + while (openSet.isNotEmpty() && startedAt + config.cutoffTimeout > System.currentTimeMillis()) { val current = openSet.remove() - println("Considering node: ${current.pos.toBlockPos()}") + // println("Considering node: ${current.pos.toBlockPos()}") if (goal.inGoal(current.pos)) { println("Not yet considered nodes: ${openSet.size}") println("Closed nodes: ${closedSet.size}") @@ -54,19 +56,59 @@ object Pathing { closedSet.add(current.pos) - moveOptions(current, goal).forEach { move -> - println("Considering move: $move") + moveOptions(current, goal, 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") +// 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 + // by checking if they share the same Y and have a clear path + var nextIndex = currentIndex + 1 + while (nextIndex < path.moves.size) { + val candidateMove = path.moves[nextIndex] + + // Only try to skip if both moves are on the same Y level + if (startMove.pos.y != candidateMove.pos.y) break + + // Verify there's a clear path from the start move to the candidate + if ( + isPathClear( + startMove.pos.toBlockPos(), + candidateMove.pos.toBlockPos(), + config.pathClearanceCheckDistance + ) + ) 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 index 8322544df..cd1007d66 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -23,6 +23,8 @@ interface PathingConfig { val kD: Double val tolerance: Double val cutoffTimeout: Long + val shortcutLength: Int + val pathClearanceCheckDistance: Double val assumeJesus: Boolean } diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index cb06e6119..4c64af795 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -34,6 +34,8 @@ class PathingSettings( override val kD by c.setting("D Gain", 0.2, 0.0..1.0, 0.01) { vis() && page == Page.Execution } override val tolerance by c.setting("Node Tolerance", 0.1, 0.01..1.0, 0.01) { vis() && page == Page.Execution } override val cutoffTimeout by c.setting("Cutoff Timeout", 50L, 1L..2000L, 10L) { vis() && page == Page.Execution } + override val shortcutLength by c.setting("Shortcut Length", 10, 1..100, 1) { vis() && page == Page.Execution } + override val pathClearanceCheckDistance by c.setting("Path Clearance Check Distance", 0.3, 0.0..1.0, 0.01) { vis() && page == Page.Execution } override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc } } diff --git a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt index 886153bc2..5bef45a1d 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -18,17 +18,21 @@ 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.isPathClear +import com.lambda.util.world.WorldUtils.playerFitsIn 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.manhattanLength +import com.lambda.util.world.offset import com.lambda.util.world.toBlockPos +import com.lambda.util.world.y import net.minecraft.block.BlockState import net.minecraft.block.Blocks import net.minecraft.block.CampfireBlock @@ -51,10 +55,10 @@ import net.minecraft.util.math.EightWayDirection object MoveFinder { private val nodeTypeCache = HashMap() - fun SafeContext.moveOptions(origin: Move, goal: Goal) = + fun SafeContext.moveOptions(origin: Move, goal: Goal, config: PathingConfig) = EightWayDirection.entries.flatMap { direction -> (-1..1).mapNotNull { y -> - getPathNode(goal, origin, direction, y) + getPathNode(goal, origin, direction, y, config) } } @@ -63,21 +67,33 @@ object MoveFinder { origin: Move, direction: EightWayDirection, height: Int, + config: PathingConfig ): Move? { val offset = fastVectorOf(direction.offsetX, height, direction.offsetZ) val checkingPos = origin.pos.add(offset) val checkingBlockPos = checkingPos.toBlockPos() + val originBlockPos = origin.pos.toBlockPos() if (!world.worldBorder.contains(checkingBlockPos)) return null val nodeType = findPathType(checkingPos) - val hCost = goal.heuristic(checkingPos)/* * nodeType.penalty*/ - val cost = offset.length() - val currentFeetY = getFeetY(checkingBlockPos) - if (nodeType == NodeType.BLOCKED) return null -// // ToDo: Different for jumping etc - if (!isPathClear(origin.pos.toBlockPos(), checkingBlockPos)) return null + val clear = when { + height == 0 -> isPathClear(originBlockPos, checkingBlockPos) + height > 0 -> { + val between = origin.pos.offset(0, height, 0) + isPathClear(origin.pos, between, supportCheck = false) && isPathClear(between, checkingPos, supportCheck = false) + } + else -> { + val between = origin.pos.offset(direction.offsetX, 0, direction.offsetZ) + isPathClear(origin.pos, between, supportCheck = false) && isPathClear(between, checkingPos, supportCheck = false) + } + } + if (!clear) return null + + val hCost = goal.heuristic(checkingPos) /** nodeType.penalty*/ + val cost = offset.length() + val currentFeetY = getFeetY(checkingBlockPos) return when { // (currentFeetY - origin.feetY) > player.stepHeight -> ParkourMove(checkingPos, hCost, nodeType, currentFeetY, cost) 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 c76eb2933..591dd424e 100644 --- a/common/src/main/kotlin/com/lambda/util/world/Position.kt +++ b/common/src/main/kotlin/com/lambda/util/world/Position.kt @@ -131,7 +131,7 @@ fun FastVector.offset(x: Int, y: Int, z: Int): FastVector = fastVectorOf(this.x fun FastVector.manhattanLength() = abs(x) + abs(y) + abs(z) -fun FastVector.length() = sqrt((x * x + y * y + z * z).toDouble()) +fun FastVector.length() = sqrt((abs(x * x) + abs(y * y) + abs(z * z)).toDouble()) /** * Adds the given vector to the position. 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 9346d1599..e61793daa 100644 --- a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt @@ -30,46 +30,59 @@ object WorldUtils { fun SafeContext.traversable(pos: BlockPos) = blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) && playerFitsIn(pos) + fun SafeContext.isPathClear( + start: FastVector, + end: FastVector, + stepSize: Double = 0.3, + supportCheck: Boolean = true, + ) = isPathClear(start.toBlockPos(), end.toBlockPos(), stepSize, supportCheck) + fun SafeContext.isPathClear( start: BlockPos, end: BlockPos, stepSize: Double = 0.3, - ) = isPathClear(Vec3d.ofBottomCenter(start), Vec3d.ofBottomCenter(end), stepSize) + supportCheck: Boolean = true, + ) = isPathClear(Vec3d.ofBottomCenter(start), Vec3d.ofBottomCenter(end), stepSize, supportCheck) fun SafeContext.isPathClear( start: Vec3d, end: Vec3d, - stepSize: Double = 0.3 // Step size based on player's hitbox radius + stepSize: Double = 0.3, + supportCheck: Boolean = true, ): Boolean { val direction = end.subtract(start) val distance = direction.length() - if (distance <= 0) return true // No movement needed + if (distance <= 0) return true + val steps = (distance / stepSize).toInt() val stepDirection = direction.normalize().multiply(stepSize) + var currentPos = start - var remainingDistance = distance - while (remainingDistance > 0) { + (0 until steps).forEach { _ -> val blockPos = currentPos.flooredBlockPos - val goingDownwards = start.y > end.y - val hasSupport = blockState(blockPos.down()).isSideSolidFullSquare(world, blockPos.down(), Direction.UP) - if (!(playerFitsIn(currentPos) && (goingDownwards || hasSupport))) return false - - val step = min(stepSize, remainingDistance) - currentPos = currentPos.add(stepDirection.multiply(step)) - remainingDistance -= step + val playerNotFitting = !playerFitsIn(blockPos) + if (playerNotFitting || (supportCheck && !hasSupport(blockPos))) { + return false + } + currentPos = currentPos.add(stepDirection) } - // Final check at end position return playerFitsIn(end) } + fun SafeContext.playerFitsIn(pos: FastVector) = + playerFitsIn(pos.toBlockPos()) + fun SafeContext.playerFitsIn(pos: Vec3d) = world.isSpaceEmpty(pos.playerBox()) fun SafeContext.playerFitsIn(pos: BlockPos) = world.isSpaceEmpty(Vec3d.ofBottomCenter(pos).playerBox()) + private 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).contract(1.0E-6) } \ No newline at end of file From 4cd8e1d999d6fedd072635df7ea2bfaf006e1961 Mon Sep 17 00:00:00 2001 From: Constructor Date: Sun, 2 Mar 2025 23:21:24 +0100 Subject: [PATCH 05/39] Better support check and config cleanup --- .../module/modules/movement/Pathfinder.kt | 79 +++++++++++-------- .../main/kotlin/com/lambda/pathing/Pathing.kt | 4 +- .../com/lambda/pathing/PathingConfig.kt | 27 ++++--- .../com/lambda/pathing/PathingSettings.kt | 25 +++--- .../kotlin/com/lambda/pathing/move/Move.kt | 4 + .../com/lambda/pathing/move/MoveFinder.kt | 12 +-- .../com/lambda/pathing/move/SwimMove.kt | 3 - .../util/world/{WorldDsl.kt => SearchDsl.kt} | 0 .../com/lambda/util/world/WorldUtils.kt | 26 +++--- 9 files changed, 105 insertions(+), 75 deletions(-) rename common/src/main/kotlin/com/lambda/util/world/{WorldDsl.kt => SearchDsl.kt} (100%) 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 index 4f201b5f7..ce4ab9117 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -19,6 +19,7 @@ package com.lambda.module.modules.movement import com.lambda.config.groups.RotationSettings 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 @@ -58,10 +59,15 @@ object Pathfinder : Module( description = "Get from A to B", defaultTags = setOf(ModuleTag.MOVEMENT) ) { - private val pathing = PathingSettings(this) - private val rotation = RotationSettings(this) + enum class Page { + Pathing, Rotation + } + + private val page by setting("Page", Page.Pathing) + private val pathing = PathingSettings(this) { page == Page.Pathing } + private val rotation = RotationSettings(this) { page == Page.Rotation } - private val target = fastVectorOf(0, 78, 0) + private val target = fastVectorOf(0, 91, -4) private var longPath = Path() private var shortPath = Path() private var currentTarget: Vec3d? = null @@ -74,33 +80,17 @@ object Pathfinder : Module( integralError = Vec3d.ZERO lastError = Vec3d.ZERO calculating = false + longPath = Path() + shortPath = Path() + currentTarget = null } listen { updateTargetNode() if (calculating) return@listen - calculating = true - runConcurrent { - val long: Path - val aStar = measureTimeMillis { - long = findPathAStar( - player.blockPos.toFastVec(), - SimpleGoal(target), - pathing - ) - } - val short: Path - val thetaStar = measureTimeMillis { - short = thetaStarClearance(long, pathing) - } - info("A* (Length: ${long.length.string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length.string} Nodes: ${short.moves.size} T: $thetaStar ms)") - println("Long: $long | Short: $short") - longPath = long - shortPath = short -// calculating = false - } + updatePaths() } listen { event -> @@ -124,8 +114,6 @@ object Pathfinder : Module( } onRotate { -// val nextTarget = shortPath.moves.getOrNull(2)?.pos?.toBlockPos() ?: return@onRotate -// val part = player.eyePos.rotationTo(Vec3d.ofBottomCenter(nextTarget)) val currentTarget = currentTarget ?: return@onRotate val part = player.eyePos.rotationTo(currentTarget) val targetRotation = Rotation(part.yaw, player.pitch.toDouble()) @@ -133,8 +121,15 @@ object Pathfinder : Module( lookAt(targetRotation).requestBy(rotation) } + listen { + if (shortPath.moves.isEmpty()) return@listen + + player.isSprinting = pathing.allowSprint + it.sprint = pathing.allowSprint + } + listen { event -> - longPath.render(event.renderer, Color.YELLOW) +// longPath.render(event.renderer, Color.YELLOW) shortPath.render(event.renderer, Color.GREEN) event.renderer.buildFilled(Box(target.toBlockPos()), Color.PINK.setAlpha(0.25)) } @@ -149,19 +144,41 @@ object Pathfinder : Module( } private fun SafeContext.updateTargetNode() { - shortPath.moves.firstOrNull()?.let { firstNode -> - val nodeVec = Vec3d.ofBottomCenter(firstNode.pos.toBlockPos()) - if (player.pos.distanceTo(nodeVec) < pathing.tolerance) { + shortPath.moves.firstOrNull()?.let { current -> + if (player.pos.distanceTo(current.bottomPos) < pathing.tolerance) { shortPath.moves.removeFirst() integralError = Vec3d.ZERO } - val next = shortPath.moves.firstOrNull()?.pos?.toBlockPos() ?: return - currentTarget = Vec3d.ofBottomCenter(next) + currentTarget = shortPath.moves.firstOrNull()?.bottomPos } ?: run { currentTarget = null } } + private fun SafeContext.updatePaths() { + runConcurrent { + calculating = true + val long: Path + val aStar = measureTimeMillis { + long = findPathAStar( + player.blockPos.toFastVec(), + SimpleGoal(target), + pathing + ) + } + val short: Path + val thetaStar = measureTimeMillis { + short = thetaStarClearance(long, pathing) + } + info("A* (Length: ${long.length.string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length.string} Nodes: ${short.moves.size} T: $thetaStar ms)") + println("Long: $long | Short: $short") + short.moves.removeFirstOrNull() + longPath = long + shortPath = short +// calculating = false + } + } + private fun SafeContext.calculatePID(target: Vec3d): Vec3d { val error = target.subtract(player.pos) integralError = integralError.add(error) diff --git a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt index 002319b45..51eb7fa37 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt @@ -97,7 +97,7 @@ object Pathing { isPathClear( startMove.pos.toBlockPos(), candidateMove.pos.toBlockPos(), - config.pathClearanceCheckDistance + config.clearancePrecition ) ) nextIndex++ else break } @@ -109,6 +109,4 @@ object Pathing { 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 index cd1007d66..445f271cf 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -17,14 +17,23 @@ package com.lambda.pathing -interface PathingConfig { - val kP: Double - val kI: Double - val kD: Double - val tolerance: Double - val cutoffTimeout: Long - val shortcutLength: Int - val pathClearanceCheckDistance: Double +import com.lambda.interaction.request.Priority +import com.lambda.interaction.request.RequestConfig - val assumeJesus: Boolean +abstract class PathingConfig(priority: Priority) : RequestConfig(priority) { + abstract val kP: Double + abstract val kI: Double + abstract val kD: Double + abstract val tolerance: Double + abstract val cutoffTimeout: Long + abstract val shortcutLength: Int + abstract val clearancePrecition: Double + abstract val allowSprint: Boolean + abstract val maxFallHeight: Double + + abstract val assumeJesus: Boolean + + override fun requestInternal(request: PathRequest) { + PathingManager.registerRequest(this, request) + } } diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 4c64af795..072bc7bea 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -18,24 +18,29 @@ package com.lambda.pathing import com.lambda.config.Configurable +import com.lambda.interaction.request.Priority class PathingSettings( c: Configurable, + priority: Priority = 0, vis: () -> Boolean = { true } -) : PathingConfig { +) : PathingConfig(priority) { enum class Page { - Execution, Misc + Pathfinding, Movement, Misc } - private val page by c.setting("Pathing Page", Page.Execution, "Current page", vis) + private val page by c.setting("Pathing Page", Page.Pathfinding, "Current page", vis) - override val kP by c.setting("P Gain", 0.5, 0.0..2.0, 0.01) { vis() && page == Page.Execution } - override val kI by c.setting("I Gain", 0.0, 0.0..1.0, 0.01) { vis() && page == Page.Execution } - override val kD by c.setting("D Gain", 0.2, 0.0..1.0, 0.01) { vis() && page == Page.Execution } - override val tolerance by c.setting("Node Tolerance", 0.1, 0.01..1.0, 0.01) { vis() && page == Page.Execution } - override val cutoffTimeout by c.setting("Cutoff Timeout", 50L, 1L..2000L, 10L) { vis() && page == Page.Execution } - override val shortcutLength by c.setting("Shortcut Length", 10, 1..100, 1) { vis() && page == Page.Execution } - override val pathClearanceCheckDistance by c.setting("Path Clearance Check Distance", 0.3, 0.0..1.0, 0.01) { vis() && page == Page.Execution } + override val cutoffTimeout by c.setting("Cutoff Timeout", 500L, 1L..2000L, 10L, "Timeout of path calculation", " ms") { vis() && page == Page.Pathfinding } + override val shortcutLength by c.setting("Shortcut Length", 10, 1..100, 1) { vis() && page == Page.Pathfinding } + override val clearancePrecition by c.setting("Clearance Precition", 0.2, 0.0..1.0, 0.01) { 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 kP by c.setting("P Gain", 0.5, 0.0..2.0, 0.01) { vis() && page == Page.Movement } + override val kI by c.setting("I Gain", 0.0, 0.0..1.0, 0.01) { vis() && page == Page.Movement } + override val kD by c.setting("D Gain", 0.2, 0.0..1.0, 0.01) { vis() && page == Page.Movement } + override val tolerance by c.setting("Node Tolerance", 0.7, 0.01..2.0, 0.05) { vis() && page == Page.Movement } + override val allowSprint by c.setting("Allow Sprint", true) { vis() && page == Page.Movement } override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc } } diff --git a/common/src/main/kotlin/com/lambda/pathing/move/Move.kt b/common/src/main/kotlin/com/lambda/pathing/move/Move.kt index 3d72c5fd0..7779ce729 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/Move.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/Move.kt @@ -20,6 +20,8 @@ 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 @@ -34,6 +36,8 @@ abstract class Move : Comparable, Task() { // 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) diff --git a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt index 5bef45a1d..a6100e2e5 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -23,16 +23,13 @@ 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.hasSupport import com.lambda.util.world.WorldUtils.isPathClear -import com.lambda.util.world.WorldUtils.playerFitsIn -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.manhattanLength import com.lambda.util.world.offset import com.lambda.util.world.toBlockPos -import com.lambda.util.world.y import net.minecraft.block.BlockState import net.minecraft.block.Blocks import net.minecraft.block.CampfireBlock @@ -43,7 +40,6 @@ import net.minecraft.block.TrapdoorBlock import net.minecraft.enchantment.EnchantmentHelper import net.minecraft.enchantment.Enchantments import net.minecraft.entity.EquipmentSlot -import net.minecraft.entity.ai.pathing.NavigationType import net.minecraft.entity.effect.StatusEffects import net.minecraft.item.Items import net.minecraft.registry.tag.BlockTags @@ -79,14 +75,14 @@ object MoveFinder { if (nodeType == NodeType.BLOCKED) return null val clear = when { - height == 0 -> isPathClear(originBlockPos, checkingBlockPos) + height == 0 -> isPathClear(originBlockPos, checkingBlockPos, config.clearancePrecition) height > 0 -> { val between = origin.pos.offset(0, height, 0) - isPathClear(origin.pos, between, supportCheck = false) && isPathClear(between, checkingPos, supportCheck = false) + isPathClear(origin.pos, between, config.clearancePrecition, false) && isPathClear(between, checkingPos, config.clearancePrecition, false) && hasSupport(checkingBlockPos) } else -> { val between = origin.pos.offset(direction.offsetX, 0, direction.offsetZ) - isPathClear(origin.pos, between, supportCheck = false) && isPathClear(between, checkingPos, supportCheck = false) + isPathClear(origin.pos, between, config.clearancePrecition, false) && isPathClear(between, checkingPos, config.clearancePrecition, false) && hasSupport(checkingBlockPos) } } if (!clear) return null diff --git a/common/src/main/kotlin/com/lambda/pathing/move/SwimMove.kt b/common/src/main/kotlin/com/lambda/pathing/move/SwimMove.kt index 092f79d02..a21660a07 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/SwimMove.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/SwimMove.kt @@ -17,10 +17,7 @@ package com.lambda.pathing.move -import com.lambda.context.SafeContext import com.lambda.util.world.FastVector -import com.lambda.util.world.WorldUtils.traversable -import com.lambda.util.world.toBlockPos class SwimMove( override val pos: FastVector, 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 100% rename from common/src/main/kotlin/com/lambda/util/world/WorldDsl.kt rename to common/src/main/kotlin/com/lambda/util/world/SearchDsl.kt 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 e61793daa..10ca51efb 100644 --- a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt @@ -20,6 +20,7 @@ package com.lambda.util.world import com.lambda.context.SafeContext import com.lambda.util.BlockUtils.blockState import com.lambda.util.math.flooredBlockPos +import net.fabricmc.loader.impl.lib.sat4j.core.Vec import net.minecraft.util.math.BlockPos import net.minecraft.util.math.Box import net.minecraft.util.math.Direction @@ -60,9 +61,9 @@ object WorldUtils { var currentPos = start (0 until steps).forEach { _ -> - val blockPos = currentPos.flooredBlockPos - val playerNotFitting = !playerFitsIn(blockPos) - if (playerNotFitting || (supportCheck && !hasSupport(blockPos))) { + val playerNotFitting = !playerFitsIn(currentPos) + val hasNoSupport = !hasSupport(currentPos) + if (playerNotFitting || (supportCheck && hasNoSupport)) { return false } currentPos = currentPos.add(stepDirection) @@ -71,18 +72,21 @@ object WorldUtils { return playerFitsIn(end) } - fun SafeContext.playerFitsIn(pos: FastVector) = - playerFitsIn(pos.toBlockPos()) + fun SafeContext.playerFitsIn(pos: BlockPos) = + playerFitsIn(Vec3d.ofBottomCenter(pos)) fun SafeContext.playerFitsIn(pos: Vec3d) = - world.isSpaceEmpty(pos.playerBox()) + world.isSpaceEmpty(pos.playerBox().contract(1.0E-6)) - fun SafeContext.playerFitsIn(pos: BlockPos) = - world.isSpaceEmpty(Vec3d.ofBottomCenter(pos).playerBox()) + fun SafeContext.hasSupport(pos: BlockPos) = + hasSupport(Vec3d.ofBottomCenter(pos)) + +// private fun SafeContext.hasSupport(pos: BlockPos) = +// blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) - private fun SafeContext.hasSupport(pos: BlockPos) = - blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) + fun SafeContext.hasSupport(pos: Vec3d) = + !world.isSpaceEmpty(pos.playerBox().expand(1.0E-6)) 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) + Box(x - 0.3, y, z - 0.3, x + 0.3, y + 1.8, z + 0.3) } \ No newline at end of file From c694f16ca1701b0faf4900d5809912bb5cbce15f Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 3 Mar 2025 07:00:53 +0100 Subject: [PATCH 06/39] Some fixes --- .../module/modules/movement/Pathfinder.kt | 3 +- .../main/kotlin/com/lambda/pathing/Pathing.kt | 2 +- .../com/lambda/pathing/PathingConfig.kt | 29 +++++++------------ .../com/lambda/pathing/PathingSettings.kt | 4 +-- .../com/lambda/util/world/WorldUtils.kt | 9 ++++-- 5 files changed, 21 insertions(+), 26 deletions(-) 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 index ce4ab9117..108200073 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -44,6 +44,7 @@ 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.WorldUtils.isPathClear import com.lambda.util.world.fastVectorOf import com.lambda.util.world.toBlockPos import com.lambda.util.world.toFastVec @@ -67,7 +68,7 @@ object Pathfinder : Module( private val pathing = PathingSettings(this) { page == Page.Pathing } private val rotation = RotationSettings(this) { page == Page.Rotation } - private val target = fastVectorOf(0, 91, -4) + private val target = fastVectorOf(0, 78, 0) private var longPath = Path() private var shortPath = Path() private var currentTarget: Vec3d? = null diff --git a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt index 51eb7fa37..1b830e050 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt @@ -47,7 +47,7 @@ object Pathing { while (openSet.isNotEmpty() && startedAt + config.cutoffTimeout > System.currentTimeMillis()) { val current = openSet.remove() - // println("Considering node: ${current.pos.toBlockPos()}") +// println("Considering node: ${current.pos.toBlockPos()}") if (goal.inGoal(current.pos)) { println("Not yet considered nodes: ${openSet.size}") println("Closed nodes: ${closedSet.size}") diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index 445f271cf..4a7935cc1 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -17,23 +17,16 @@ package com.lambda.pathing -import com.lambda.interaction.request.Priority -import com.lambda.interaction.request.RequestConfig +interface PathingConfig { + val kP: Double + val kI: Double + val kD: Double + val tolerance: Double + val cutoffTimeout: Long + val shortcutLength: Int + val clearancePrecition: Double + val allowSprint: Boolean + val maxFallHeight: Double -abstract class PathingConfig(priority: Priority) : RequestConfig(priority) { - abstract val kP: Double - abstract val kI: Double - abstract val kD: Double - abstract val tolerance: Double - abstract val cutoffTimeout: Long - abstract val shortcutLength: Int - abstract val clearancePrecition: Double - abstract val allowSprint: Boolean - abstract val maxFallHeight: Double - - abstract val assumeJesus: Boolean - - override fun requestInternal(request: PathRequest) { - PathingManager.registerRequest(this, request) - } + val assumeJesus: Boolean } diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 072bc7bea..bde880538 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -18,13 +18,11 @@ package com.lambda.pathing import com.lambda.config.Configurable -import com.lambda.interaction.request.Priority class PathingSettings( c: Configurable, - priority: Priority = 0, vis: () -> Boolean = { true } -) : PathingConfig(priority) { +) : PathingConfig { enum class Page { Pathfinding, Movement, Misc } 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 10ca51efb..cbed203bd 100644 --- a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt @@ -76,16 +76,19 @@ object WorldUtils { playerFitsIn(Vec3d.ofBottomCenter(pos)) fun SafeContext.playerFitsIn(pos: Vec3d) = - world.isSpaceEmpty(pos.playerBox().contract(1.0E-6)) + world.isSpaceEmpty(player, pos.playerBox().contract(1.0E-6)) fun SafeContext.hasSupport(pos: BlockPos) = - hasSupport(Vec3d.ofBottomCenter(pos)) + hasSupport(Vec3d.ofBottomCenter(pos)) // private fun SafeContext.hasSupport(pos: BlockPos) = // blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) fun SafeContext.hasSupport(pos: Vec3d) = - !world.isSpaceEmpty(pos.playerBox().expand(1.0E-6)) + !world.isSpaceEmpty(player, pos.playerBox().expand(1.0E-6)) + +// fun SafeContext.hasSupport(pos: Vec3d) = +// world.canCollide(null, pos.playerBox().expand(1.0E-6)) fun Vec3d.playerBox(): Box = Box(x - 0.3, y, z - 0.3, x + 0.3, y + 1.8, z + 0.3) From 3d0a7849d26efba6d8e56a1b35ee7eddfd22d7f9 Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 5 Mar 2025 06:11:07 +0100 Subject: [PATCH 07/39] Change target pos --- .../kotlin/com/lambda/module/modules/movement/Pathfinder.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 108200073..615155762 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -68,7 +68,7 @@ object Pathfinder : Module( private val pathing = PathingSettings(this) { page == Page.Pathing } private val rotation = RotationSettings(this) { page == Page.Rotation } - private val target = fastVectorOf(0, 78, 0) + private val target = fastVectorOf(0, 91, -4) private var longPath = Path() private var shortPath = Path() private var currentTarget: Vec3d? = null From 9f23026e8e6c17f3d051fa8338875b04b6c67f62 Mon Sep 17 00:00:00 2001 From: Constructor Date: Thu, 6 Mar 2025 00:47:57 +0100 Subject: [PATCH 08/39] Test lib --- common/build.gradle.kts | 7 +++++ common/src/test/kotlin/FastVectorTest.kt | 34 ++++++++++++++++++++++++ 2 files changed, 41 insertions(+) create mode 100644 common/src/test/kotlin/FastVectorTest.kt diff --git a/common/build.gradle.kts b/common/build.gradle.kts index d5a6ba0b5..d858e08e0 100644 --- a/common/build.gradle.kts +++ b/common/build.gradle.kts @@ -55,6 +55,7 @@ dependencies { // Baritone modImplementation("baritone-api:baritone-unoptimized-fabric:1.10.2") { isTransitive = false } + testImplementation(kotlin("test")) } tasks { @@ -66,3 +67,9 @@ tasks { useJUnitPlatform() } } + +tasks.withType { + kotlinOptions { + jvmTarget = "17" + } +} \ No newline at end of file diff --git a/common/src/test/kotlin/FastVectorTest.kt b/common/src/test/kotlin/FastVectorTest.kt new file mode 100644 index 000000000..a42efc63a --- /dev/null +++ b/common/src/test/kotlin/FastVectorTest.kt @@ -0,0 +1,34 @@ +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 kotlin.test.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 . + */ + + +class FastVectorTest { + @Test + fun testZero() { + val vec = fastVectorOf(0, 0, 0) + assert(vec.x == 0) + assert(vec.y == 0) + assert(vec.z == 0) + } +} \ No newline at end of file From 72a5f49926ce80fd0444b1d32b17682ba509f378 Mon Sep 17 00:00:00 2001 From: Constructor Date: Thu, 6 Mar 2025 01:53:51 +0100 Subject: [PATCH 09/39] D* Lite - 2D with tests --- .../com/lambda/pathing/dstar/DStarLite.kt | 207 ++++++++++++++++++ .../kotlin/com/lambda/pathing/dstar/Graph.kt | 57 +++++ .../kotlin/com/lambda/pathing/dstar/Key.kt | 34 +++ .../pathing/dstar/PriorityQueueDStar.kt | 61 ++++++ common/src/test/kotlin/DStarTest.kt | 191 ++++++++++++++++ 5 files changed, 550 insertions(+) create mode 100644 common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt create mode 100644 common/src/test/kotlin/DStarTest.kt diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt new file mode 100644 index 000000000..050d5f6cb --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -0,0 +1,207 @@ +/* + * 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.dstar + +import kotlin.math.min + +/** + * D* Lite Implementation. + * + * We perform a backward search from the goal to the start, so: + * - rhs(goal) = 0, g(goal) = ∞ + * - "start" is the robot's current location from which we want a path *to* the goal + * - 'km' accumulates the heuristic shift so we don't reorder the entire queue after each move + * + * @param graph The graph on which we plan (with forward + reverse adjacency). + * @param heuristic A consistent (or at least nonnegative) heuristic function h(a,b). + * @param start The robot's current position. + * @param goal The fixed goal vertex. + */ +class DStarLite( + private val graph: Graph, + private val heuristic: (Int, Int) -> Double, + var start: Int, + val goal: Int +) { + private val INF = Double.POSITIVE_INFINITY + + // 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 + private val U = PriorityQueueDStar() + + // Heuristic shift + private var km = 0.0 + + init { + initialize() + } + + /** + * Initialize D* Lite: + * - g(goal)=∞, rhs(goal)=0 + * - Insert goal into U with key = calculateKey(goal). + */ + private fun initialize() { + gMap.clear() + rhsMap.clear() + setG(goal, INF) + setRHS(goal, 0.0) + U.insertOrUpdate(goal, calculateKey(goal)) + } + + private fun g(u: Int): Double = gMap[u] ?: INF + private fun setG(u: Int, value: Double) { + gMap[u] = value + } + + private fun rhs(u: Int): Double = rhsMap[u] ?: INF + private fun setRHS(u: Int, value: Double) { + rhsMap[u] = value + } + + /** + * Key(u) = ( min(g(u), rhs(u)) + h(start, u) + km , min(g(u), rhs(u)) ). + */ + private fun calculateKey(u: Int): Key { + val minGRHS = min(g(u), rhs(u)) + return Key(minGRHS + heuristic(start, u) + km, minGRHS) + } + + /** + * UpdateVertex(u): + * 1) If u != goal, rhs(u) = min_{v in Pred(u)} [g(v) + cost(v,u)] + * 2) Remove u from U + * 3) If g(u) != rhs(u), insertOrUpdate(u, calculateKey(u)) + */ + fun updateVertex(u: Int) { + if (u != goal) { + var tmp = INF + graph.predecessors(u).forEach { (pred, c) -> + val valCandidate = g(pred) + c + if (valCandidate < tmp) { + tmp = valCandidate + } + } + setRHS(u, tmp) + } + U.remove(u) + if (g(u) != rhs(u)) { + U.insertOrUpdate(u, calculateKey(u)) + } + } + + /** + * computeShortestPath(): + * While the queue top is "less" than calculateKey(start) + * or g(start) < rhs(start), pop and process. + */ + fun computeShortestPath() { + while ( + (U.topKey() < calculateKey(start)) || + (g(start) < rhs(start)) + ) { + val u = U.top() ?: break + val oldKey = U.topKey() + val newKey = calculateKey(u) + + if (oldKey < newKey) { + // Priority out-of-date; update it + U.insertOrUpdate(u, newKey) + } else { + // Remove it from queue + U.pop() + if (g(u) > rhs(u)) { + // We found a better path for u + setG(u, rhs(u)) + // Update successors of u + graph.successors(u).forEach { (s, _) -> + updateVertex(s) + } + } else { + // g(u) <= rhs(u) + val gOld = g(u) + setG(u, INF) + updateVertex(u) + // Update successors that may have relied on old g(u) + graph.successors(u).forEach { (s, _) -> + if (rhs(s) == gOld + graph.cost(u, s)) { + updateVertex(s) + } + } + } + } + } + } + + /** + * Called when the robot moves from oldStart to newStart. + * We increase km by h(oldStart, newStart), then re-key all queued vertices. + */ + fun updateStart(newStart: Int) { + val oldStart = start + start = newStart + km += heuristic(oldStart, start) + + // Re-key everything in U + val tmpList = mutableListOf() + while (!U.isEmpty()) { + U.top()?.let { tmpList.add(it) } + U.pop() + } + tmpList.forEach { v -> + U.insertOrUpdate(v, calculateKey(v)) + } + } + + /** + * Returns a path from 'start' to 'goal' by always choosing + * the successor s of the current vertex that minimizes g(s)+cost(current->s). + * If no path is found, the path ends prematurely. + */ + fun getPath(): List { + val path = mutableListOf() + var current = start + path.add(current) + + while (current != goal) { + val successors = graph.successors(current) + if (successors.isEmpty()) break + + var bestNext: Int? = null + var bestVal = INF + for ((s, c) in successors) { + val valCandidate = g(s) + c + if (valCandidate < bestVal) { + bestVal = valCandidate + bestNext = s + } + } + // No path + if (bestNext == null) break + current = bestNext + path.add(current) + + // Safety net to avoid infinite loops + if (path.size > 100000) break + } + return path + } +} \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt new file mode 100644 index 000000000..52750064a --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt @@ -0,0 +1,57 @@ +/* + * 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.dstar + +/** + * Simple graph class that stores both forward and reverse adjacency. + * + * @param adjMap a map from each vertex u to a list of (successor, cost) pairs. + */ +class Graph(private val adjMap: Map>>) { + + // Build reverse adjacency by scanning all forward edges + private val revAdjMap: Map>> by lazy { + val tmp = mutableMapOf>>() + adjMap.forEach { (u, edges) -> + edges.forEach { (v, cost) -> + tmp.getOrPut(v) { mutableListOf() }.add(Pair(u, cost)) + } + } + // Convert to immutable + tmp.mapValues { it.value.toList() } + } + + /** + * Returns the successors of u, i.e., all (v, cost) for edges u->v. + */ + fun successors(u: Int) = adjMap[u] ?: emptyList() + + /** + * Returns the predecessors of u, i.e., all (v, cost) for edges v->u. + */ + fun predecessors(u: Int) = revAdjMap[u] ?: emptyList() + + /** + * Helper to get the cost of an edge u->v, or ∞ if none. + */ + fun cost(u: Int, v: Int) = + adjMap[u] + ?.firstOrNull { it.first == v } + ?.second + ?: Double.POSITIVE_INFINITY +} \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt new file mode 100644 index 000000000..e4b8afd40 --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/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.dstar + +/** + * A Key is a pair (k1, k2) used in D* Lite's priority queue. We compare them lexicographically: + * (k1, k2) < (k1', k2') iff k1 < k1' or (k1 == k1' and k2 < k2'). + */ +data class Key(val k1: Double, val k2: Double) : Comparable { + override fun compareTo(other: Key): Int { + return when { + this.k1 < other.k1 -> -1 + this.k1 > other.k1 -> 1 + this.k2 < other.k2 -> if (this.k1 == other.k1) -1 else 0 + this.k2 > other.k2 -> if (this.k1 == other.k1) 1 else 0 + else -> 0 + } + } +} \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt new file mode 100644 index 000000000..a53b93a8e --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt @@ -0,0 +1,61 @@ +/* + * 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.dstar + +import java.util.* + +/** + * Priority queue for D* Lite + */ +class PriorityQueueDStar { + private val pq = PriorityQueue>(compareBy { it.second }) + private val vertexToKey = mutableMapOf() + + fun isEmpty(): Boolean = pq.isEmpty() + + fun topKey(): Key { + return if (pq.isEmpty()) Key(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY) + else pq.peek().second + } + + fun top(): Int? { + return pq.peek()?.first + } + + fun pop(): Int? { + if (pq.isEmpty()) return null + val (v, _) = pq.poll() + vertexToKey.remove(v) + return v + } + + fun insertOrUpdate(v: Int, key: Key) { + val oldKey = vertexToKey[v] + if (oldKey == null || oldKey != key) { + remove(v) + vertexToKey[v] = key + pq.add(Pair(v, key)) + } + } + + fun remove(v: Int) { + val oldKey = vertexToKey[v] ?: return + vertexToKey.remove(v) + pq.remove(Pair(v, oldKey)) + } +} \ No newline at end of file diff --git a/common/src/test/kotlin/DStarTest.kt b/common/src/test/kotlin/DStarTest.kt new file mode 100644 index 000000000..af5686dd9 --- /dev/null +++ b/common/src/test/kotlin/DStarTest.kt @@ -0,0 +1,191 @@ +import com.lambda.pathing.dstar.DStarLite +import com.lambda.pathing.dstar.Graph +import kotlin.math.abs +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 . + */ + +class DStarTest { + /** + * Simple consistent heuristic for a grid: the Manhattan distance + * (or Euclidean if you prefer). We'll do Manhattan here. + */ + private fun manhattan(a: Pair, b: Pair): Double { + return (abs(a.first - b.first) + abs(a.second - b.second)).toDouble() + } + + /** + * Convert (row,col) to a single vertex index if we want. + */ + private fun idx(row: Int, col: Int, cols: Int): Int { + return row * cols + col + } + + /** + * Build a 2x2 grid with all edges cost=1 (4-connected). + * 0 = top-left, 1 = top-right + * 2 = bottom-left, 3 = bottom-right + */ + @Test + fun test2x2Grid() { + // We'll build adjacency for each of the 4 cells + val adjMap = mutableMapOf>>() + + // For a 2x2, we have cells 0..3 + // We'll do 4-connected edges + // 0 neighbors: 1,2 + adjMap[0] = listOf(Pair(1, 1.0), Pair(2, 1.0)) + // 1 neighbors: 0,3 + adjMap[1] = listOf(Pair(0, 1.0), Pair(3, 1.0)) + // 2 neighbors: 0,3 + adjMap[2] = listOf(Pair(0, 1.0), Pair(3, 1.0)) + // 3 neighbors: 1,2 + adjMap[3] = listOf(Pair(1, 1.0), Pair(2, 1.0)) + + val graph = Graph(adjMap) + + // We'll define a heuristic that interprets each index as (row, col): + // 0->(0,0), 1->(0,1), 2->(1,0), 3->(1,1) + fun h(u: Int, v: Int): Double { + val coords = mapOf(0 to Pair(0,0), 1 to Pair(0,1), 2 to Pair(1,0), 3 to Pair(1,1)) + return manhattan(coords[u]!!, coords[v]!!) + } + + val start = 0 + val goal = 3 + val dstar = DStarLite(graph, ::h, start, goal) + + dstar.computeShortestPath() + val path = dstar.getPath() + // Possible path is 0 -> 1 -> 3 or 0 -> 2 -> 3 + // We'll just check that the path length is 3 and ends at 3 + assertEquals(3, path.size, "Path should have 3 vertices (0->1->3 or 0->2->3)") + assertEquals(3, path.last(), "Goal should be vertex 3") + } + + /** + * Build a 3x3 grid, block the middle cell, test path correctness. + */ + @Test + fun test3x3GridWithBlock() { + // We'll index cells row-major: row*3 + col + // So top-left=0, top-middle=1, top-right=2, middle-left=3, ... + val adjMap = mutableMapOf>>() + val rows = 3 + val cols = 3 + + // Helper to add edges + fun addEdge(u: Int, v: Int, cost: Double) { + adjMap.getOrPut(u) { mutableListOf() }.add(Pair(v, cost)) + } + + // Build a 4-connected grid + for (r in 0 until rows) { + for (c in 0 until cols) { + val u = idx(r, c, cols) + // For each neighbor (r+dr, c+dc) if in range + val deltas = listOf(Pair(0,1), Pair(1,0), Pair(0,-1), Pair(-1,0)) + for ((dr, dc) in deltas) { + val nr = r + dr + val nc = c + dc + if (nr in 0 until rows && nc in 0 until cols) { + val v = idx(nr, nc, cols) + // We'll assign cost=1.0 by default + addEdge(u, v, 1.0) + } + } + } + } + + // Now let's "block" the middle cell (1,1) = index 4 + // We'll set the cost of edges to or from 4 to ∞ by removing them from adjacency + adjMap[4] = mutableListOf() // no successors + // Also remove any edges that go into 4 + for ((u, edges) in adjMap) { + adjMap[u] = edges.filter { (v, _) -> v != 4 }.toMutableList() + } + + // Create the graph + val graph = Graph(adjMap.mapValues { it.value.toList() }) + + // Heuristic: manhattan distance on (r,c) + fun h(u: Int, v: Int): Double { + val r1 = u / cols + val c1 = u % cols + val r2 = v / cols + val c2 = v % cols + return (abs(r1 - r2) + abs(c1 - c2)).toDouble() + } + + val start = 0 // top-left + val goal = 8 // bottom-right + val dstar = DStarLite(graph, ::h, start, goal) + + dstar.computeShortestPath() + val path = dstar.getPath() + + // Because cell 4 is blocked, the path must go around it: + // One possible route is 0->1->2->5->8 or 0->3->6->7->8, etc. + // Let's just check it ends at 8 and the path length is at least 5 + // (the minimal path is 4 steps => 5 vertices). + assertFalse(path.isEmpty(), "Path should not be empty.") + assertEquals(8, path.last(), "Goal should be vertex 8") + assertTrue(path.size >= 5, "Path must circumvent the blocked cell (at least 5 vertices).") + } + + /** + * Test how the path updates if we move the start after computing a path. + * We'll use the same 2x2 from the first test but demonstrate how updateStart works. + */ + @Test + fun testUpdateStart() { + // 2x2 adjacency (same as first test) + val adjMap = mapOf( + 0 to listOf(Pair(1, 1.0), Pair(2, 1.0)), + 1 to listOf(Pair(0, 1.0), Pair(3, 1.0)), + 2 to listOf(Pair(0, 1.0), Pair(3, 1.0)), + 3 to listOf(Pair(1, 1.0), Pair(2, 1.0)) + ) + val graph = Graph(adjMap) + + fun h(u: Int, v: Int): Double { + val coords = mapOf(0 to Pair(0,0), 1 to Pair(0,1), 2 to Pair(1,0), 3 to Pair(1,1)) + val (r1, c1) = coords[u]!! + val (r2, c2) = coords[v]!! + return (abs(r1 - r2) + abs(c1 - c2)).toDouble() + } + + val dstar = DStarLite(graph, ::h, start = 0, goal = 3) + dstar.computeShortestPath() + val path1 = dstar.getPath() + assertEquals(3, path1.size) + + // Suppose the robot "moves" from 0 to 1. We update the start in D* Lite: + dstar.updateStart(1) + // We might discover changes in costs or not. For now, assume no changes => no updateVertex calls. + dstar.computeShortestPath() + val path2 = dstar.getPath() + // Now the path should be from 1 to 3, presumably [1, 3] or [1, 0, 2, 3], etc. + assertEquals(3, path2.last(), "Goal remains 3") + // Usually the direct path is [1,3], so length=2 + assertTrue(path2.size <= 3, "Likely path is shorter now from 1->3 directly.") + } +} \ No newline at end of file From 23c8fa26448db102249319825bca9380871feb6b Mon Sep 17 00:00:00 2001 From: Constructor Date: Thu, 6 Mar 2025 02:20:54 +0100 Subject: [PATCH 10/39] 3D Implementation --- .../com/lambda/pathing/dstar/DStarLite.kt | 109 +++++----- .../kotlin/com/lambda/pathing/dstar/Graph.kt | 41 ++-- .../kotlin/com/lambda/pathing/dstar/Key.kt | 8 +- .../pathing/dstar/PriorityQueueDStar.kt | 18 +- common/src/test/kotlin/DStarLiteTest.kt | 114 +++++++++++ common/src/test/kotlin/DStarTest.kt | 191 ------------------ 6 files changed, 193 insertions(+), 288 deletions(-) create mode 100644 common/src/test/kotlin/DStarLiteTest.kt delete mode 100644 common/src/test/kotlin/DStarTest.kt diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 050d5f6cb..f5d7c87e2 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -17,6 +17,7 @@ package com.lambda.pathing.dstar +import com.lambda.util.world.FastVector import kotlin.math.min /** @@ -34,20 +35,20 @@ import kotlin.math.min */ class DStarLite( private val graph: Graph, - private val heuristic: (Int, Int) -> Double, - var start: Int, - val goal: Int + private val heuristic: (FastVector, FastVector) -> Double, + var start: FastVector, + private val goal: FastVector ) { private val INF = Double.POSITIVE_INFINITY // gMap[u], rhsMap[u] store g(u) and rhs(u) or default to ∞ if not present - private val gMap = mutableMapOf() - private val rhsMap = mutableMapOf() + private val gMap = mutableMapOf() + private val rhsMap = mutableMapOf() - // Priority queue + // Priority queue holding inconsistent vertices. private val U = PriorityQueueDStar() - // Heuristic shift + // km accumulates heuristic differences as the start changes. private var km = 0.0 init { @@ -67,37 +68,36 @@ class DStarLite( U.insertOrUpdate(goal, calculateKey(goal)) } - private fun g(u: Int): Double = gMap[u] ?: INF - private fun setG(u: Int, value: Double) { - gMap[u] = value - } + private fun g(u: FastVector): Double = gMap[u] ?: INF + private fun setG(u: FastVector, value: Double) { gMap[u] = value } - private fun rhs(u: Int): Double = rhsMap[u] ?: INF - private fun setRHS(u: Int, value: Double) { - rhsMap[u] = value - } + private fun rhs(u: FastVector): Double = rhsMap[u] ?: INF + private fun setRHS(u: FastVector, value: Double) { rhsMap[u] = value } /** - * Key(u) = ( min(g(u), rhs(u)) + h(start, u) + km , min(g(u), rhs(u)) ). + * Calculates the key for vertex u. + * Key(u) = ( min(g(u), rhs(u)) + h(start, u) + km, min(g(u), rhs(u)) ) */ - private fun calculateKey(u: Int): Key { + private fun calculateKey(u: FastVector): Key { val minGRHS = min(g(u), rhs(u)) return Key(minGRHS + heuristic(start, u) + km, minGRHS) } /** - * UpdateVertex(u): - * 1) If u != goal, rhs(u) = min_{v in Pred(u)} [g(v) + cost(v,u)] - * 2) Remove u from U - * 3) If g(u) != rhs(u), insertOrUpdate(u, calculateKey(u)) + * Updates the vertex u. + * + * If u != goal, then: + * rhs(u) = min_{v in Pred(u)} [g(v) + cost(v,u)] + * + * Then u is removed from the queue and reinserted if it is inconsistent. */ - fun updateVertex(u: Int) { + fun updateVertex(u: FastVector) { if (u != goal) { var tmp = INF - graph.predecessors(u).forEach { (pred, c) -> - val valCandidate = g(pred) + c - if (valCandidate < tmp) { - tmp = valCandidate + graph.predecessors(u).forEach { (pred, cost) -> + val candidate = g(pred) + cost + if (candidate < tmp) { + tmp = candidate } } setRHS(u, tmp) @@ -109,15 +109,13 @@ class DStarLite( } /** + * Propagates changes until the start is locally consistent. * computeShortestPath(): * While the queue top is "less" than calculateKey(start) * or g(start) < rhs(start), pop and process. */ fun computeShortestPath() { - while ( - (U.topKey() < calculateKey(start)) || - (g(start) < rhs(start)) - ) { + while ((U.topKey() < calculateKey(start)) || (g(start) < rhs(start))) { val u = U.top() ?: break val oldKey = U.topKey() val newKey = calculateKey(u) @@ -126,24 +124,21 @@ class DStarLite( // Priority out-of-date; update it U.insertOrUpdate(u, newKey) } else { - // Remove it from queue U.pop() if (g(u) > rhs(u)) { // We found a better path for u setG(u, rhs(u)) - // Update successors of u - graph.successors(u).forEach { (s, _) -> - updateVertex(s) + graph.successors(u).forEach { (succ, _) -> + updateVertex(succ) } } else { - // g(u) <= rhs(u) - val gOld = g(u) + val oldG = g(u) setG(u, INF) updateVertex(u) // Update successors that may have relied on old g(u) - graph.successors(u).forEach { (s, _) -> - if (rhs(s) == gOld + graph.cost(u, s)) { - updateVertex(s) + graph.successors(u).forEach { (succ, _) -> + if (rhs(succ) == oldG + graph.cost(u, succ)) { + updateVertex(succ) } } } @@ -152,16 +147,15 @@ class DStarLite( } /** - * Called when the robot moves from oldStart to newStart. - * We increase km by h(oldStart, newStart), then re-key all queued vertices. + * When the robot moves, update the start. + * The variable km is increased by h(oldStart, newStart) and + * all vertices in the queue are re-keyed. */ - fun updateStart(newStart: Int) { + fun updateStart(newStart: FastVector) { val oldStart = start start = newStart km += heuristic(oldStart, start) - - // Re-key everything in U - val tmpList = mutableListOf() + val tmpList = mutableListOf() while (!U.isEmpty()) { U.top()?.let { tmpList.add(it) } U.pop() @@ -172,34 +166,29 @@ class DStarLite( } /** - * Returns a path from 'start' to 'goal' by always choosing - * the successor s of the current vertex that minimizes g(s)+cost(current->s). - * If no path is found, the path ends prematurely. + * Retrieves a path from start to goal by always choosing the successor + * with the lowest g + cost value. If no path is found, the path stops early. */ - fun getPath(): List { - val path = mutableListOf() + fun getPath(): List { + val path = mutableListOf() var current = start path.add(current) - while (current != goal) { val successors = graph.successors(current) if (successors.isEmpty()) break - - var bestNext: Int? = null + var bestNext: FastVector? = null var bestVal = INF - for ((s, c) in successors) { - val valCandidate = g(s) + c - if (valCandidate < bestVal) { - bestVal = valCandidate - bestNext = s + for ((succ, cost) in successors) { + val candidate = g(succ) + cost + if (candidate < bestVal) { + bestVal = candidate + bestNext = succ } } // No path if (bestNext == null) break current = bestNext path.add(current) - - // Safety net to avoid infinite loops if (path.size > 100000) break } return path diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt index 52750064a..64a5e8eec 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt @@ -17,41 +17,34 @@ package com.lambda.pathing.dstar +import com.lambda.util.world.FastVector + /** - * Simple graph class that stores both forward and reverse adjacency. + * A simple 3D graph that uses FastVector (a Long) to represent 3D nodes. * - * @param adjMap a map from each vertex u to a list of (successor, cost) pairs. + * @param adjMap A map from each vertex to a list of (neighbor, cost) pairs. */ -class Graph(private val adjMap: Map>>) { - - // Build reverse adjacency by scanning all forward edges - private val revAdjMap: Map>> by lazy { - val tmp = mutableMapOf>>() +class Graph( + private val adjMap: Map>> +) { + // Build reverse adjacency from forward edges. + private val revAdjMap: Map>> by lazy { + val tmp = mutableMapOf>>() adjMap.forEach { (u, edges) -> edges.forEach { (v, cost) -> tmp.getOrPut(v) { mutableListOf() }.add(Pair(u, cost)) } } - // Convert to immutable tmp.mapValues { it.value.toList() } } - /** - * Returns the successors of u, i.e., all (v, cost) for edges u->v. - */ - fun successors(u: Int) = adjMap[u] ?: emptyList() + /** Returns the successors of a vertex. */ + fun successors(u: FastVector) = adjMap[u] ?: emptyList() - /** - * Returns the predecessors of u, i.e., all (v, cost) for edges v->u. - */ - fun predecessors(u: Int) = revAdjMap[u] ?: emptyList() + /** Returns the predecessors of a vertex. */ + fun predecessors(u: FastVector) = revAdjMap[u] ?: emptyList() - /** - * Helper to get the cost of an edge u->v, or ∞ if none. - */ - fun cost(u: Int, v: Int) = - adjMap[u] - ?.firstOrNull { it.first == v } - ?.second - ?: Double.POSITIVE_INFINITY + /** Returns the cost of the edge from u to v (or ∞ if none exists). */ + fun cost(u: FastVector, v: FastVector) = + adjMap[u]?.firstOrNull { it.first == v }?.second ?: Double.POSITIVE_INFINITY } \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt index e4b8afd40..705890bdd 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt @@ -18,16 +18,16 @@ package com.lambda.pathing.dstar /** - * A Key is a pair (k1, k2) used in D* Lite's priority queue. We compare them lexicographically: - * (k1, k2) < (k1', k2') iff k1 < k1' or (k1 == k1' and k2 < k2'). + * A Key is a pair (k1, k2) that is used to order vertices in the priority queue. + * They are compared lexicographically. */ data class Key(val k1: Double, val k2: Double) : Comparable { override fun compareTo(other: Key): Int { return when { this.k1 < other.k1 -> -1 this.k1 > other.k1 -> 1 - this.k2 < other.k2 -> if (this.k1 == other.k1) -1 else 0 - this.k2 > other.k2 -> if (this.k1 == other.k1) 1 else 0 + this.k2 < other.k2 -> -1 + this.k2 > other.k2 -> 1 else -> 0 } } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt index a53b93a8e..5087ecee9 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt @@ -17,14 +17,16 @@ package com.lambda.pathing.dstar +import com.lambda.util.world.FastVector import java.util.* /** - * Priority queue for D* Lite + * Priority queue for D* Lite 3D. + * Supports: topKey, top, pop, insertOrUpdate, and remove. */ class PriorityQueueDStar { - private val pq = PriorityQueue>(compareBy { it.second }) - private val vertexToKey = mutableMapOf() + private val pq = PriorityQueue>(compareBy { it.second }) + private val vertexToKey = mutableMapOf() fun isEmpty(): Boolean = pq.isEmpty() @@ -33,18 +35,16 @@ class PriorityQueueDStar { else pq.peek().second } - fun top(): Int? { - return pq.peek()?.first - } + fun top() = pq.peek()?.first - fun pop(): Int? { + fun pop(): FastVector? { if (pq.isEmpty()) return null val (v, _) = pq.poll() vertexToKey.remove(v) return v } - fun insertOrUpdate(v: Int, key: Key) { + fun insertOrUpdate(v: FastVector, key: Key) { val oldKey = vertexToKey[v] if (oldKey == null || oldKey != key) { remove(v) @@ -53,7 +53,7 @@ class PriorityQueueDStar { } } - fun remove(v: Int) { + fun remove(v: FastVector) { val oldKey = vertexToKey[v] ?: return vertexToKey.remove(v) pq.remove(Pair(v, oldKey)) diff --git a/common/src/test/kotlin/DStarLiteTest.kt b/common/src/test/kotlin/DStarLiteTest.kt new file mode 100644 index 000000000..a8535fd31 --- /dev/null +++ b/common/src/test/kotlin/DStarLiteTest.kt @@ -0,0 +1,114 @@ +import com.lambda.pathing.dstar.DStarLite +import com.lambda.pathing.dstar.Graph +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 kotlin.math.abs +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 . + */ + +class DStarLiteTest { + + /** + * Helper to create a 3D grid graph with 6-connected neighbors. + * + * Each vertex is created using fastVectorOf(x, y, z). Edges have cost 1.0. + */ + private fun create3DGridGraph(width: Int, height: Int, depth: Int): Graph { + val adjMap = mutableMapOf>>() + // 6-connected neighbor offsets. + val offsets = listOf( + Triple(1, 0, 0), Triple(-1, 0, 0), + Triple(0, 1, 0), Triple(0, -1, 0), + Triple(0, 0, 1), Triple(0, 0, -1) + ) + (0 until width).forEach { x -> + (0 until height).forEach { y -> + (0 until depth).forEach { z -> + val current = fastVectorOf(x.toLong(), y.toLong(), z.toLong()) + val neighbors = mutableListOf>() + offsets.forEach { (dx, dy, dz) -> + val nx = x + dx + val ny = y + dy + val nz = z + dz + if (nx in 0 until width && ny in 0 until height && nz in 0 until depth) { + val neighbor = fastVectorOf(nx.toLong(), ny.toLong(), nz.toLong()) + neighbors.add(Pair(neighbor, 1.0)) + } + } + adjMap[current] = neighbors + } + } + } + return Graph(adjMap.mapValues { it.value.toList() }) + } + + /** Manhattan distance heuristic */ + private fun heuristic(u: FastVector, v: FastVector): Double { + val dx = abs(u.x - v.x) + val dy = abs(u.y - v.y) + val dz = abs(u.z - v.z) + return (dx + dy + dz).toDouble() + } + + @Test + fun test3x3x3Grid() { + val width = 3 + val height = 3 + val depth = 3 + val graph = create3DGridGraph(width, height, depth) + // Start at (0,0,0) and goal at (2,2,2). + val start = fastVectorOf(0L, 0L, 0L) + val goal = fastVectorOf(2L, 2L, 2L) + val dstar = DStarLite(graph, ::heuristic, start, goal) + dstar.computeShortestPath() + val path = dstar.getPath() + // Manhattan distance between (0,0,0) and (2,2,2) is 6; hence the path should have 7 vertices. + assertFalse(path.isEmpty(), "Path should not be empty") + assertEquals(goal, path.last(), "Path should end at the goal") + assertEquals(7, path.size, "Expected path length is 7 vertices (6 steps)") + } + + @Test + fun testUpdateStart3D() { + val width = 3 + val height = 3 + val depth = 3 + val graph = create3DGridGraph(width, height, depth) + val start = fastVectorOf(0L, 0L, 0L) + val goal = fastVectorOf(2L, 2L, 2L) + val dstar = DStarLite(graph, ::heuristic, start, goal) + dstar.computeShortestPath() + val path1 = dstar.getPath() + assertEquals(goal, path1.last(), "Initial path should reach goal") + // Simulate a move: update the start to the second vertex in the current path. + val newStart = path1[1] + dstar.updateStart(newStart) + dstar.computeShortestPath() + val path2 = dstar.getPath() + assertEquals(goal, path2.last(), "Updated path should reach goal") + // The new path should be shorter (or equal) since we already moved. + assertTrue(path2.size <= path1.size, "Updated path should be shorter or equal in length") + } +} \ No newline at end of file diff --git a/common/src/test/kotlin/DStarTest.kt b/common/src/test/kotlin/DStarTest.kt deleted file mode 100644 index af5686dd9..000000000 --- a/common/src/test/kotlin/DStarTest.kt +++ /dev/null @@ -1,191 +0,0 @@ -import com.lambda.pathing.dstar.DStarLite -import com.lambda.pathing.dstar.Graph -import kotlin.math.abs -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 . - */ - -class DStarTest { - /** - * Simple consistent heuristic for a grid: the Manhattan distance - * (or Euclidean if you prefer). We'll do Manhattan here. - */ - private fun manhattan(a: Pair, b: Pair): Double { - return (abs(a.first - b.first) + abs(a.second - b.second)).toDouble() - } - - /** - * Convert (row,col) to a single vertex index if we want. - */ - private fun idx(row: Int, col: Int, cols: Int): Int { - return row * cols + col - } - - /** - * Build a 2x2 grid with all edges cost=1 (4-connected). - * 0 = top-left, 1 = top-right - * 2 = bottom-left, 3 = bottom-right - */ - @Test - fun test2x2Grid() { - // We'll build adjacency for each of the 4 cells - val adjMap = mutableMapOf>>() - - // For a 2x2, we have cells 0..3 - // We'll do 4-connected edges - // 0 neighbors: 1,2 - adjMap[0] = listOf(Pair(1, 1.0), Pair(2, 1.0)) - // 1 neighbors: 0,3 - adjMap[1] = listOf(Pair(0, 1.0), Pair(3, 1.0)) - // 2 neighbors: 0,3 - adjMap[2] = listOf(Pair(0, 1.0), Pair(3, 1.0)) - // 3 neighbors: 1,2 - adjMap[3] = listOf(Pair(1, 1.0), Pair(2, 1.0)) - - val graph = Graph(adjMap) - - // We'll define a heuristic that interprets each index as (row, col): - // 0->(0,0), 1->(0,1), 2->(1,0), 3->(1,1) - fun h(u: Int, v: Int): Double { - val coords = mapOf(0 to Pair(0,0), 1 to Pair(0,1), 2 to Pair(1,0), 3 to Pair(1,1)) - return manhattan(coords[u]!!, coords[v]!!) - } - - val start = 0 - val goal = 3 - val dstar = DStarLite(graph, ::h, start, goal) - - dstar.computeShortestPath() - val path = dstar.getPath() - // Possible path is 0 -> 1 -> 3 or 0 -> 2 -> 3 - // We'll just check that the path length is 3 and ends at 3 - assertEquals(3, path.size, "Path should have 3 vertices (0->1->3 or 0->2->3)") - assertEquals(3, path.last(), "Goal should be vertex 3") - } - - /** - * Build a 3x3 grid, block the middle cell, test path correctness. - */ - @Test - fun test3x3GridWithBlock() { - // We'll index cells row-major: row*3 + col - // So top-left=0, top-middle=1, top-right=2, middle-left=3, ... - val adjMap = mutableMapOf>>() - val rows = 3 - val cols = 3 - - // Helper to add edges - fun addEdge(u: Int, v: Int, cost: Double) { - adjMap.getOrPut(u) { mutableListOf() }.add(Pair(v, cost)) - } - - // Build a 4-connected grid - for (r in 0 until rows) { - for (c in 0 until cols) { - val u = idx(r, c, cols) - // For each neighbor (r+dr, c+dc) if in range - val deltas = listOf(Pair(0,1), Pair(1,0), Pair(0,-1), Pair(-1,0)) - for ((dr, dc) in deltas) { - val nr = r + dr - val nc = c + dc - if (nr in 0 until rows && nc in 0 until cols) { - val v = idx(nr, nc, cols) - // We'll assign cost=1.0 by default - addEdge(u, v, 1.0) - } - } - } - } - - // Now let's "block" the middle cell (1,1) = index 4 - // We'll set the cost of edges to or from 4 to ∞ by removing them from adjacency - adjMap[4] = mutableListOf() // no successors - // Also remove any edges that go into 4 - for ((u, edges) in adjMap) { - adjMap[u] = edges.filter { (v, _) -> v != 4 }.toMutableList() - } - - // Create the graph - val graph = Graph(adjMap.mapValues { it.value.toList() }) - - // Heuristic: manhattan distance on (r,c) - fun h(u: Int, v: Int): Double { - val r1 = u / cols - val c1 = u % cols - val r2 = v / cols - val c2 = v % cols - return (abs(r1 - r2) + abs(c1 - c2)).toDouble() - } - - val start = 0 // top-left - val goal = 8 // bottom-right - val dstar = DStarLite(graph, ::h, start, goal) - - dstar.computeShortestPath() - val path = dstar.getPath() - - // Because cell 4 is blocked, the path must go around it: - // One possible route is 0->1->2->5->8 or 0->3->6->7->8, etc. - // Let's just check it ends at 8 and the path length is at least 5 - // (the minimal path is 4 steps => 5 vertices). - assertFalse(path.isEmpty(), "Path should not be empty.") - assertEquals(8, path.last(), "Goal should be vertex 8") - assertTrue(path.size >= 5, "Path must circumvent the blocked cell (at least 5 vertices).") - } - - /** - * Test how the path updates if we move the start after computing a path. - * We'll use the same 2x2 from the first test but demonstrate how updateStart works. - */ - @Test - fun testUpdateStart() { - // 2x2 adjacency (same as first test) - val adjMap = mapOf( - 0 to listOf(Pair(1, 1.0), Pair(2, 1.0)), - 1 to listOf(Pair(0, 1.0), Pair(3, 1.0)), - 2 to listOf(Pair(0, 1.0), Pair(3, 1.0)), - 3 to listOf(Pair(1, 1.0), Pair(2, 1.0)) - ) - val graph = Graph(adjMap) - - fun h(u: Int, v: Int): Double { - val coords = mapOf(0 to Pair(0,0), 1 to Pair(0,1), 2 to Pair(1,0), 3 to Pair(1,1)) - val (r1, c1) = coords[u]!! - val (r2, c2) = coords[v]!! - return (abs(r1 - r2) + abs(c1 - c2)).toDouble() - } - - val dstar = DStarLite(graph, ::h, start = 0, goal = 3) - dstar.computeShortestPath() - val path1 = dstar.getPath() - assertEquals(3, path1.size) - - // Suppose the robot "moves" from 0 to 1. We update the start in D* Lite: - dstar.updateStart(1) - // We might discover changes in costs or not. For now, assume no changes => no updateVertex calls. - dstar.computeShortestPath() - val path2 = dstar.getPath() - // Now the path should be from 1 to 3, presumably [1, 3] or [1, 0, 2, 3], etc. - assertEquals(3, path2.last(), "Goal remains 3") - // Usually the direct path is [1,3], so length=2 - assertTrue(path2.size <= 3, "Likely path is shorter now from 1->3 directly.") - } -} \ No newline at end of file From c0c63e70a900af2e1371aac63ecf24a3d4c5f4f1 Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 26 Mar 2025 00:29:32 +0100 Subject: [PATCH 11/39] Speed up clearance checks --- .../construction/simulation/Simulation.kt | 3 -- .../module/modules/movement/Pathfinder.kt | 10 +----- .../main/kotlin/com/lambda/pathing/Path.kt | 20 ++++++++++- .../com/lambda/pathing/dstar/DStarLite.kt | 2 +- .../pathing/dstar/PriorityQueueDStar.kt | 2 +- .../com/lambda/pathing/move/MoveFinder.kt | 30 ++++++++-------- .../lambda/util/collections/UpdatableLazy.kt | 11 ++++++ .../com/lambda/util/world/WorldUtils.kt | 36 ++++++------------- 8 files changed, 58 insertions(+), 56 deletions(-) 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 868415aeb..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,16 +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.playerFitsIn 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.Vec3d import java.awt.Color 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 index 615155762..9d92af5f2 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -136,14 +136,6 @@ object Pathfinder : Module( } } - private fun Path.render(renderer: StaticESP, color: Color) { - moves.zipWithNext { current, next -> - val currentPos = current.pos.toBlockPos().toCenterPos() - val nextPos = next.pos.toBlockPos().toCenterPos() - renderer.buildLine(currentPos, nextPos, color) - } - } - private fun SafeContext.updateTargetNode() { shortPath.moves.firstOrNull()?.let { current -> if (player.pos.distanceTo(current.bottomPos) < pathing.tolerance) { @@ -171,7 +163,7 @@ object Pathfinder : Module( val thetaStar = measureTimeMillis { short = thetaStarClearance(long, pathing) } - info("A* (Length: ${long.length.string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length.string} Nodes: ${short.moves.size} T: $thetaStar ms)") + info("A* (Length: ${long.length().string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length().string} Nodes: ${short.moves.size} T: $thetaStar ms)") println("Long: $long | Short: $short") short.moves.removeFirstOrNull() longPath = long diff --git a/common/src/main/kotlin/com/lambda/pathing/Path.kt b/common/src/main/kotlin/com/lambda/pathing/Path.kt index 86ae08340..171493c80 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Path.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Path.kt @@ -17,22 +17,40 @@ package com.lambda.pathing +import com.lambda.graphics.renderer.esp.builders.buildLine +import com.lambda.graphics.renderer.esp.global.StaticESP import com.lambda.pathing.move.Move +import com.lambda.util.collections.updatableLazy import com.lambda.util.world.dist import com.lambda.util.world.toBlockPos +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() } - val length get() = moves.zipWithNext { a, b -> a.pos dist b.pos }.sum() + 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 currentPos = current.pos.toBlockPos().toCenterPos() + val nextPos = next.pos.toBlockPos().toCenterPos() + renderer.buildLine(currentPos, nextPos, color) + } + } + + fun length() = length.value override fun toString() = moves.joinToString(" -> ") { "(${it.pos.toBlockPos().toShortString()})" } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index f5d7c87e2..ed0fa9a3a 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -189,7 +189,7 @@ class DStarLite( if (bestNext == null) break current = bestNext path.add(current) - if (path.size > 100000) break + if (path.size > 100_000) break } return path } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt index 5087ecee9..83bede6cc 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt @@ -28,7 +28,7 @@ class PriorityQueueDStar { private val pq = PriorityQueue>(compareBy { it.second }) private val vertexToKey = mutableMapOf() - fun isEmpty(): Boolean = pq.isEmpty() + fun isEmpty() = pq.isEmpty() fun topKey(): Key { return if (pq.isEmpty()) Key(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY) diff --git a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt index a6100e2e5..f022df0ec 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -23,12 +23,10 @@ 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.hasSupport -import com.lambda.util.world.WorldUtils.isPathClear +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.offset import com.lambda.util.world.toBlockPos import net.minecraft.block.BlockState import net.minecraft.block.Blocks @@ -45,8 +43,10 @@ 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() @@ -54,18 +54,19 @@ object MoveFinder { fun SafeContext.moveOptions(origin: Move, goal: Goal, config: PathingConfig) = EightWayDirection.entries.flatMap { direction -> (-1..1).mapNotNull { y -> - getPathNode(goal, origin, direction, y, config) + getPathNode(goal::heuristic, origin, direction, y, config) } } private fun SafeContext.getPathNode( - goal: Goal, + heuristic: KFunction1, origin: Move, 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.pos.add(offset) val checkingBlockPos = checkingPos.toBlockPos() val originBlockPos = origin.pos.toBlockPos() @@ -74,20 +75,19 @@ object MoveFinder { val nodeType = findPathType(checkingPos) if (nodeType == NodeType.BLOCKED) return null - val clear = when { - height == 0 -> isPathClear(originBlockPos, checkingBlockPos, config.clearancePrecition) - height > 0 -> { - val between = origin.pos.offset(0, height, 0) - isPathClear(origin.pos, between, config.clearancePrecition, false) && isPathClear(between, checkingPos, config.clearancePrecition, false) && hasSupport(checkingBlockPos) - } - else -> { - val between = origin.pos.offset(direction.offsetX, 0, direction.offsetZ) - isPathClear(origin.pos, between, config.clearancePrecition, false) && isPathClear(between, checkingPos, config.clearancePrecition, false) && hasSupport(checkingBlockPos) + 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) + } else { + traversable(checkingBlockPos) } if (!clear) return null - val hCost = goal.heuristic(checkingPos) /** nodeType.penalty*/ + val hCost = heuristic(checkingPos) /** nodeType.penalty*/ val cost = offset.length() val currentFeetY = getFeetY(checkingBlockPos) 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 302a0d122..0e065c075 100644 --- a/common/src/main/kotlin/com/lambda/util/collections/UpdatableLazy.kt +++ b/common/src/main/kotlin/com/lambda/util/collections/UpdatableLazy.kt @@ -43,6 +43,17 @@ class UpdatableLazy(private val initializer: () -> T) { */ 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/WorldUtils.kt b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt index cbed203bd..3c44f7982 100644 --- a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt @@ -19,24 +19,14 @@ package com.lambda.util.world import com.lambda.context.SafeContext import com.lambda.util.BlockUtils.blockState -import com.lambda.util.math.flooredBlockPos -import net.fabricmc.loader.impl.lib.sat4j.core.Vec 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 kotlin.math.min object WorldUtils { fun SafeContext.traversable(pos: BlockPos) = - blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) && playerFitsIn(pos) - - fun SafeContext.isPathClear( - start: FastVector, - end: FastVector, - stepSize: Double = 0.3, - supportCheck: Boolean = true, - ) = isPathClear(start.toBlockPos(), end.toBlockPos(), stepSize, supportCheck) + hasSupport(pos) && hasClearance(pos) fun SafeContext.isPathClear( start: BlockPos, @@ -61,7 +51,7 @@ object WorldUtils { var currentPos = start (0 until steps).forEach { _ -> - val playerNotFitting = !playerFitsIn(currentPos) + val playerNotFitting = !hasClearance(currentPos) val hasNoSupport = !hasSupport(currentPos) if (playerNotFitting || (supportCheck && hasNoSupport)) { return false @@ -69,26 +59,20 @@ object WorldUtils { currentPos = currentPos.add(stepDirection) } - return playerFitsIn(end) + return hasClearance(end) } - fun SafeContext.playerFitsIn(pos: BlockPos) = - playerFitsIn(Vec3d.ofBottomCenter(pos)) - - fun SafeContext.playerFitsIn(pos: Vec3d) = + private fun SafeContext.hasClearance(pos: Vec3d) = world.isSpaceEmpty(player, pos.playerBox().contract(1.0E-6)) - fun SafeContext.hasSupport(pos: BlockPos) = - hasSupport(Vec3d.ofBottomCenter(pos)) - -// private fun SafeContext.hasSupport(pos: BlockPos) = -// blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) - fun SafeContext.hasSupport(pos: Vec3d) = - !world.isSpaceEmpty(player, pos.playerBox().expand(1.0E-6)) + !world.isSpaceEmpty(player, pos.playerBox().expand(1.0E-6).contract(0.05, 0.0, 0.05)) + + private fun SafeContext.hasClearance(pos: BlockPos) = + blockState(pos).isAir && blockState(pos.up()).isAir -// fun SafeContext.hasSupport(pos: Vec3d) = -// world.canCollide(null, pos.playerBox().expand(1.0E-6)) + private 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) From da0efc132a054ea825d664fb6894c10c4d2a58a5 Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 26 Mar 2025 03:57:07 +0100 Subject: [PATCH 12/39] More settings --- .../module/modules/movement/Pathfinder.kt | 66 ++++++++++--------- .../main/kotlin/com/lambda/pathing/Pathing.kt | 2 +- .../com/lambda/pathing/PathingConfig.kt | 20 ++++-- .../com/lambda/pathing/PathingSettings.kt | 10 ++- 4 files changed, 60 insertions(+), 38 deletions(-) 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 index 9d92af5f2..121da1928 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -25,8 +25,6 @@ import com.lambda.event.events.RotationEvent import com.lambda.event.events.TickEvent import com.lambda.event.listener.SafeListener.Companion.listen import com.lambda.graphics.renderer.esp.builders.buildFilled -import com.lambda.graphics.renderer.esp.builders.buildLine -import com.lambda.graphics.renderer.esp.global.StaticESP import com.lambda.interaction.request.rotation.Rotation import com.lambda.interaction.request.rotation.Rotation.Companion.rotationTo import com.lambda.interaction.request.rotation.RotationManager.onRotate @@ -36,6 +34,7 @@ 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.goal.SimpleGoal import com.lambda.threading.runConcurrent @@ -44,7 +43,6 @@ 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.WorldUtils.isPathClear import com.lambda.util.world.fastVectorOf import com.lambda.util.world.toBlockPos import com.lambda.util.world.toFastVec @@ -69,8 +67,8 @@ object Pathfinder : Module( private val rotation = RotationSettings(this) { page == Page.Rotation } private val target = fastVectorOf(0, 91, -4) - private var longPath = Path() - private var shortPath = Path() + private var coarsePath = Path() + private var refinedPath = Path() private var currentTarget: Vec3d? = null private var integralError = Vec3d.ZERO private var lastError = Vec3d.ZERO @@ -81,8 +79,8 @@ object Pathfinder : Module( integralError = Vec3d.ZERO lastError = Vec3d.ZERO calculating = false - longPath = Path() - shortPath = Path() + coarsePath = Path() + refinedPath = Path() currentTarget = null } @@ -123,7 +121,7 @@ object Pathfinder : Module( } listen { - if (shortPath.moves.isEmpty()) return@listen + if (refinedPath.moves.isEmpty()) return@listen player.isSprinting = pathing.allowSprint it.sprint = pathing.allowSprint @@ -131,44 +129,52 @@ object Pathfinder : Module( listen { event -> // longPath.render(event.renderer, Color.YELLOW) - shortPath.render(event.renderer, Color.GREEN) + refinedPath.render(event.renderer, Color.GREEN) event.renderer.buildFilled(Box(target.toBlockPos()), Color.PINK.setAlpha(0.25)) } } private fun SafeContext.updateTargetNode() { - shortPath.moves.firstOrNull()?.let { current -> + refinedPath.moves.firstOrNull()?.let { current -> if (player.pos.distanceTo(current.bottomPos) < pathing.tolerance) { - shortPath.moves.removeFirst() + refinedPath.moves.removeFirst() integralError = Vec3d.ZERO } - currentTarget = shortPath.moves.firstOrNull()?.bottomPos + currentTarget = refinedPath.moves.firstOrNull()?.bottomPos } ?: run { currentTarget = null } } private fun SafeContext.updatePaths() { - runConcurrent { - calculating = true - val long: Path - val aStar = measureTimeMillis { - long = findPathAStar( - player.blockPos.toFastVec(), - SimpleGoal(target), - pathing - ) + val goal = SimpleGoal(target) + when (pathing.algorithm) { + PathingConfig.PathingAlgorithm.A_STAR -> { + runConcurrent { + calculating = true + val long: Path + val aStar = measureTimeMillis { + long = findPathAStar(player.blockPos.toFastVec(), goal, pathing) + } + val short: Path + val thetaStar = measureTimeMillis { + short = if (pathing.pathRefining) { + thetaStarClearance(long, pathing) + } else long + } + info("A* (Length: ${long.length().string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length().string} Nodes: ${short.moves.size} T: $thetaStar ms)") + println("Long: $long | Short: $short") + short.moves.removeFirstOrNull() + coarsePath = long + refinedPath = short + // calculating = false + } } - val short: Path - val thetaStar = measureTimeMillis { - short = thetaStarClearance(long, pathing) + PathingConfig.PathingAlgorithm.D_STAR_LITE -> { + runConcurrent { + // 1. Build graph from goal to target + } } - info("A* (Length: ${long.length().string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length().string} Nodes: ${short.moves.size} T: $thetaStar ms)") - println("Long: $long | Short: $short") - short.moves.removeFirstOrNull() - longPath = long - shortPath = short -// calculating = false } } diff --git a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt index 1b830e050..84cc8f26e 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt @@ -97,7 +97,7 @@ object Pathing { isPathClear( startMove.pos.toBlockPos(), candidateMove.pos.toBlockPos(), - config.clearancePrecition + config.clearancePrecision ) ) nextIndex++ else break } diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index 4a7935cc1..1b591601d 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -17,16 +17,28 @@ package com.lambda.pathing +import com.lambda.util.NamedEnum + interface PathingConfig { + val algorithm: PathingAlgorithm + val cutoffTimeout: Long + val maxFallHeight: Double + + val pathRefining: Boolean + val shortcutLength: Int + val clearancePrecision: Double + val findShortcutJumps: Boolean + val kP: Double val kI: Double val kD: Double val tolerance: Double - val cutoffTimeout: Long - val shortcutLength: Int - val clearancePrecition: Double val allowSprint: Boolean - val maxFallHeight: Double val assumeJesus: Boolean + + enum class PathingAlgorithm(override val displayName: String) : NamedEnum { + A_STAR("A*"), + D_STAR_LITE("D* Lite"), + } } diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index bde880538..42766ccb5 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -24,16 +24,20 @@ class PathingSettings( vis: () -> Boolean = { true } ) : PathingConfig { enum class Page { - Pathfinding, Movement, Misc + Pathfinding, Refinement, Movement, Misc } private val page by c.setting("Pathing Page", Page.Pathfinding, "Current page", vis) + override val algorithm by c.setting("Pathfinding Algorithm", PathingConfig.PathingAlgorithm.A_STAR) { vis() && page == Page.Pathfinding } override val cutoffTimeout by c.setting("Cutoff Timeout", 500L, 1L..2000L, 10L, "Timeout of path calculation", " ms") { vis() && page == Page.Pathfinding } - override val shortcutLength by c.setting("Shortcut Length", 10, 1..100, 1) { vis() && page == Page.Pathfinding } - override val clearancePrecition by c.setting("Clearance Precition", 0.2, 0.0..1.0, 0.01) { 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 pathRefining by c.setting("Path Refining", true) { vis() && page == Page.Refinement } + override val shortcutLength by c.setting("Shortcut Length", 10, 1..100, 1) { vis() && pathRefining && page == Page.Refinement } + override val clearancePrecision by c.setting("Clearance Precision", 0.2, 0.0..1.0, 0.01) { vis() && pathRefining && page == Page.Refinement } + override val findShortcutJumps by c.setting("Find Shortcut Jumps", true) { vis() && pathRefining && page == Page.Refinement } + override val kP by c.setting("P Gain", 0.5, 0.0..2.0, 0.01) { vis() && page == Page.Movement } override val kI by c.setting("I Gain", 0.0, 0.0..1.0, 0.01) { vis() && page == Page.Movement } override val kD by c.setting("D Gain", 0.2, 0.0..1.0, 0.01) { vis() && page == Page.Movement } From 1a5a9b95a120ac39c13ba6619aa9dc5265241ac2 Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 26 Mar 2025 22:23:19 +0100 Subject: [PATCH 13/39] Lazy D* Lite --- .../module/modules/movement/Pathfinder.kt | 8 +- .../main/kotlin/com/lambda/pathing/Path.kt | 2 + .../main/kotlin/com/lambda/pathing/Pathing.kt | 2 +- .../com/lambda/pathing/dstar/DStarLite.kt | 10 +- .../kotlin/com/lambda/pathing/dstar/Graph.kt | 50 -------- .../com/lambda/pathing/dstar/LazyGraph.kt | 68 +++++++++++ .../pathing/dstar/PriorityQueueDStar.kt | 16 ++- .../com/lambda/pathing/move/MoveFinder.kt | 10 +- common/src/test/kotlin/DStarLiteTest.kt | 108 +++++++----------- 9 files changed, 139 insertions(+), 135 deletions(-) delete mode 100644 common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt 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 index 121da1928..c386ae88b 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -36,8 +36,12 @@ 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.dstar.DStarLite +import com.lambda.pathing.dstar.LazyGraph import com.lambda.pathing.goal.SimpleGoal +import com.lambda.pathing.move.MoveFinder.moveOptions import com.lambda.threading.runConcurrent +import com.lambda.threading.runSafe import com.lambda.util.Communication.info import com.lambda.util.Formatting.string import com.lambda.util.math.setAlpha @@ -162,7 +166,7 @@ object Pathfinder : Module( thetaStarClearance(long, pathing) } else long } - info("A* (Length: ${long.length().string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length().string} Nodes: ${short.moves.size} T: $thetaStar ms)") + info("A* (Length: ${long.length().string} Nodes: ${long.size} T: $aStar ms) and Theta* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)") println("Long: $long | Short: $short") short.moves.removeFirstOrNull() coarsePath = long @@ -172,7 +176,7 @@ object Pathfinder : Module( } PathingConfig.PathingAlgorithm.D_STAR_LITE -> { runConcurrent { - // 1. Build graph from goal to target + } } } diff --git a/common/src/main/kotlin/com/lambda/pathing/Path.kt b/common/src/main/kotlin/com/lambda/pathing/Path.kt index 171493c80..f4f6ad0bf 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Path.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Path.kt @@ -52,6 +52,8 @@ data class Path( 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 index 84cc8f26e..2dfbdc682 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt @@ -56,7 +56,7 @@ object Pathing { closedSet.add(current.pos) - moveOptions(current, goal, config).forEach { move -> + 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 diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index ed0fa9a3a..4071d955c 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -34,19 +34,17 @@ import kotlin.math.min * @param goal The fixed goal vertex. */ class DStarLite( - private val graph: Graph, + private val graph: LazyGraph, private val heuristic: (FastVector, FastVector) -> Double, var start: FastVector, private val goal: FastVector ) { - private val INF = Double.POSITIVE_INFINITY - // 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. - private val U = PriorityQueueDStar() + private val U = PriorityQueueDStar() // km accumulates heuristic differences as the start changes. private var km = 0.0 @@ -193,4 +191,8 @@ class DStarLite( } return path } + + companion object { + private const val INF = Double.POSITIVE_INFINITY + } } \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt deleted file mode 100644 index 64a5e8eec..000000000 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/Graph.kt +++ /dev/null @@ -1,50 +0,0 @@ -/* - * 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.dstar - -import com.lambda.util.world.FastVector - -/** - * A simple 3D graph that uses FastVector (a Long) to represent 3D nodes. - * - * @param adjMap A map from each vertex to a list of (neighbor, cost) pairs. - */ -class Graph( - private val adjMap: Map>> -) { - // Build reverse adjacency from forward edges. - private val revAdjMap: Map>> by lazy { - val tmp = mutableMapOf>>() - adjMap.forEach { (u, edges) -> - edges.forEach { (v, cost) -> - tmp.getOrPut(v) { mutableListOf() }.add(Pair(u, cost)) - } - } - tmp.mapValues { it.value.toList() } - } - - /** Returns the successors of a vertex. */ - fun successors(u: FastVector) = adjMap[u] ?: emptyList() - - /** Returns the predecessors of a vertex. */ - fun predecessors(u: FastVector) = revAdjMap[u] ?: emptyList() - - /** Returns the cost of the edge from u to v (or ∞ if none exists). */ - fun cost(u: FastVector, v: FastVector) = - adjMap[u]?.firstOrNull { it.first == v }?.second ?: Double.POSITIVE_INFINITY -} \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt new file mode 100644 index 000000000..622eafde8 --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -0,0 +1,68 @@ +/* + * 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.dstar + +import com.lambda.context.SafeContext +import com.lambda.util.world.FastVector + +/** + * 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( + private val nodeInitializer: (FastVector) -> Map +) { + private val successors = hashMapOf>() + private val predecessors = hashMapOf>() + + /** Initializes a node if not already initialized, then returns successors. */ + fun successors(u: FastVector) = + successors.computeIfAbsent(u) { + val neighbors = nodeInitializer(u) + neighbors.forEach { (neighbor, cost) -> + predecessors.computeIfAbsent(neighbor) { hashMapOf() }[u] = cost + } + neighbors.toMutableMap() + } + + /** Initializes predecessors by ensuring successors of neighboring nodes. */ + fun predecessors(u: FastVector): Map { + successors(u) + return predecessors[u] ?: emptyMap() + } + + /** 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 + + fun contains(u: FastVector): Boolean = u in successors + + fun clear() { + successors.clear() + predecessors.clear() + } +} diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt index 83bede6cc..173a58499 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt @@ -17,16 +17,14 @@ package com.lambda.pathing.dstar -import com.lambda.util.world.FastVector import java.util.* /** * Priority queue for D* Lite 3D. - * Supports: topKey, top, pop, insertOrUpdate, and remove. */ -class PriorityQueueDStar { - private val pq = PriorityQueue>(compareBy { it.second }) - private val vertexToKey = mutableMapOf() +class PriorityQueueDStar { + private val pq = PriorityQueue>(compareBy { it.second }) + private val vertexToKey = mutableMapOf() fun isEmpty() = pq.isEmpty() @@ -35,16 +33,16 @@ class PriorityQueueDStar { else pq.peek().second } - fun top() = pq.peek()?.first + fun top(): T? = pq.peek()?.first - fun pop(): FastVector? { + fun pop(): T? { if (pq.isEmpty()) return null val (v, _) = pq.poll() vertexToKey.remove(v) return v } - fun insertOrUpdate(v: FastVector, key: Key) { + fun insertOrUpdate(v: T, key: Key) { val oldKey = vertexToKey[v] if (oldKey == null || oldKey != key) { remove(v) @@ -53,7 +51,7 @@ class PriorityQueueDStar { } } - fun remove(v: FastVector) { + fun remove(v: T) { val oldKey = vertexToKey[v] ?: return vertexToKey.remove(v) pq.remove(Pair(v, oldKey)) diff --git a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt index f022df0ec..a05ddaa6a 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -51,25 +51,25 @@ import kotlin.reflect.KFunction1 object MoveFinder { private val nodeTypeCache = HashMap() - fun SafeContext.moveOptions(origin: Move, goal: Goal, config: PathingConfig) = + fun SafeContext.moveOptions(origin: FastVector, heuristic: KFunction1, config: PathingConfig) = EightWayDirection.entries.flatMap { direction -> (-1..1).mapNotNull { y -> - getPathNode(goal::heuristic, origin, direction, y, config) + getPathNode(heuristic, origin, direction, y, config) } } private fun SafeContext.getPathNode( heuristic: KFunction1, - origin: Move, + 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.pos.add(offset) + val checkingPos = origin.add(offset) val checkingBlockPos = checkingPos.toBlockPos() - val originBlockPos = origin.pos.toBlockPos() + val originBlockPos = origin.toBlockPos() if (!world.worldBorder.contains(checkingBlockPos)) return null val nodeType = findPathType(checkingPos) diff --git a/common/src/test/kotlin/DStarLiteTest.kt b/common/src/test/kotlin/DStarLiteTest.kt index a8535fd31..c8363c7a5 100644 --- a/common/src/test/kotlin/DStarLiteTest.kt +++ b/common/src/test/kotlin/DStarLiteTest.kt @@ -1,5 +1,5 @@ import com.lambda.pathing.dstar.DStarLite -import com.lambda.pathing.dstar.Graph +import com.lambda.pathing.dstar.LazyGraph import com.lambda.util.world.FastVector import com.lambda.util.world.fastVectorOf import com.lambda.util.world.x @@ -30,85 +30,65 @@ import kotlin.test.assertTrue class DStarLiteTest { - /** - * Helper to create a 3D grid graph with 6-connected neighbors. - * - * Each vertex is created using fastVectorOf(x, y, z). Edges have cost 1.0. - */ - private fun create3DGridGraph(width: Int, height: Int, depth: Int): Graph { - val adjMap = mutableMapOf>>() - // 6-connected neighbor offsets. - val offsets = listOf( - Triple(1, 0, 0), Triple(-1, 0, 0), - Triple(0, 1, 0), Triple(0, -1, 0), - Triple(0, 0, 1), Triple(0, 0, -1) - ) - (0 until width).forEach { x -> - (0 until height).forEach { y -> - (0 until depth).forEach { z -> - val current = fastVectorOf(x.toLong(), y.toLong(), z.toLong()) - val neighbors = mutableListOf>() - offsets.forEach { (dx, dy, dz) -> - val nx = x + dx - val ny = y + dy - val nz = z + dz - if (nx in 0 until width && ny in 0 until height && nz in 0 until depth) { - val neighbor = fastVectorOf(nx.toLong(), ny.toLong(), nz.toLong()) - neighbors.add(Pair(neighbor, 1.0)) - } - } - adjMap[current] = neighbors - } + /** Helper to create a lazy-initialized 3D grid graph with 6-connected neighbors */ + private fun createLazy3DGridGraph() = + LazyGraph { node -> + val neighbors = mutableMapOf() + val (x, y, z) = listOf(node.x, node.y, node.z) + + // possible directions in a 3D grid: 6-connected neighbors + val directions = listOf( + fastVectorOf(x - 1, y, z), fastVectorOf(x + 1, y, z), + fastVectorOf(x, y - 1, z), fastVectorOf(x, y + 1, z), + fastVectorOf(x, y, z - 1), fastVectorOf(x, y, z + 1) + ) + + directions.forEach { v -> + neighbors[v] = 1.0 } + + neighbors } - return Graph(adjMap.mapValues { it.value.toList() }) - } - /** Manhattan distance heuristic */ - private fun heuristic(u: FastVector, v: FastVector): Double { - val dx = abs(u.x - v.x) - val dy = abs(u.y - v.y) - val dz = abs(u.z - v.z) - return (dx + dy + dz).toDouble() - } + /** Manhattan distance heuristic for 3D grids */ + private fun heuristic(u: FastVector, v: FastVector): Double = + (abs(u.x - v.x) + abs(u.y - v.y) + abs(u.z - v.z)).toDouble() @Test fun test3x3x3Grid() { - val width = 3 - val height = 3 - val depth = 3 - val graph = create3DGridGraph(width, height, depth) - // Start at (0,0,0) and goal at (2,2,2). - val start = fastVectorOf(0L, 0L, 0L) - val goal = fastVectorOf(2L, 2L, 2L) + val graph = createLazy3DGridGraph() + val start = fastVectorOf(0, 0, 0) + val goal = fastVectorOf(2, 2, 2) val dstar = DStarLite(graph, ::heuristic, start, goal) + dstar.computeShortestPath() val path = dstar.getPath() // Manhattan distance between (0,0,0) and (2,2,2) is 6; hence the path should have 7 vertices. assertFalse(path.isEmpty(), "Path should not be empty") - assertEquals(goal, path.last(), "Path should end at the goal") + assertEquals(start, path.first(), "Path should start at the initial position") + assertEquals(goal, path.last(), "Path should end at the goal position") assertEquals(7, path.size, "Expected path length is 7 vertices (6 steps)") } @Test fun testUpdateStart3D() { - val width = 3 - val height = 3 - val depth = 3 - val graph = create3DGridGraph(width, height, depth) - val start = fastVectorOf(0L, 0L, 0L) - val goal = fastVectorOf(2L, 2L, 2L) + val graph = createLazy3DGridGraph() + var start = fastVectorOf(0, 0, 0) + val goal = fastVectorOf(2, 2, 2) val dstar = DStarLite(graph, ::heuristic, start, goal) + dstar.computeShortestPath() - val path1 = dstar.getPath() - assertEquals(goal, path1.last(), "Initial path should reach goal") - // Simulate a move: update the start to the second vertex in the current path. - val newStart = path1[1] - dstar.updateStart(newStart) - dstar.computeShortestPath() - val path2 = dstar.getPath() - assertEquals(goal, path2.last(), "Updated path should reach goal") - // The new path should be shorter (or equal) since we already moved. - assertTrue(path2.size <= path1.size, "Updated path should be shorter or equal in length") + + val initialPath = dstar.getPath() + assertTrue(initialPath.size > 1, "Initial path should have multiple steps") + + // Move starting position closer to the goal and recompute + start = fastVectorOf(1, 1, 1) + dstar.updateStart(start) + + val updatedPath = dstar.getPath() + assertFalse(updatedPath.isEmpty(), "Updated path should not be empty now from new start") + assertEquals(start, updatedPath.first(), "Updated path must start at updated position") + assertEquals(goal, updatedPath.last(), "Updated path must end at the goal") } -} \ No newline at end of file +} From 314dd3686b99ab210c683c7ea21ee422e0f052c7 Mon Sep 17 00:00:00 2001 From: Constructor Date: Thu, 27 Mar 2025 03:58:04 +0100 Subject: [PATCH 14/39] Incremental pathing on state updates and more settings --- .../module/modules/movement/Pathfinder.kt | 124 ++++++++++++------ .../com/lambda/pathing/PathingConfig.kt | 11 +- .../com/lambda/pathing/PathingSettings.kt | 29 ++-- .../com/lambda/pathing/dstar/DStarLite.kt | 6 +- .../com/lambda/pathing/dstar/LazyGraph.kt | 14 +- .../com/lambda/pathing/move/BreakMove.kt | 33 +++++ .../com/lambda/pathing/move/MoveFinder.kt | 4 +- common/src/test/kotlin/DStarLiteTest.kt | 4 +- 8 files changed, 165 insertions(+), 60 deletions(-) create mode 100644 common/src/main/kotlin/com/lambda/pathing/move/BreakMove.kt 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 index c386ae88b..630b9f697 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -17,7 +17,6 @@ package com.lambda.module.modules.movement -import com.lambda.config.groups.RotationSettings import com.lambda.context.SafeContext import com.lambda.event.events.MovementEvent import com.lambda.event.events.RenderEvent @@ -40,6 +39,8 @@ import com.lambda.pathing.dstar.DStarLite import com.lambda.pathing.dstar.LazyGraph import com.lambda.pathing.goal.SimpleGoal import com.lambda.pathing.move.MoveFinder.moveOptions +import com.lambda.pathing.move.NodeType +import com.lambda.pathing.move.TraverseMove import com.lambda.threading.runConcurrent import com.lambda.threading.runSafe import com.lambda.util.Communication.info @@ -47,12 +48,17 @@ 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.fastVectorOf 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 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 @@ -62,15 +68,21 @@ object Pathfinder : Module( description = "Get from A to B", defaultTags = setOf(ModuleTag.MOVEMENT) ) { - enum class Page { - Pathing, Rotation - } + private val pathing = PathingSettings(this) + + private fun heuristic(u: FastVector): Double = + (abs(u.x) + abs(u.y) + abs(u.z)).toDouble() - private val page by setting("Page", Page.Pathing) - private val pathing = PathingSettings(this) { page == Page.Pathing } - private val rotation = RotationSettings(this) { page == Page.Rotation } + private fun heuristic(u: FastVector, v: FastVector): Double = + (abs(u.x - v.x) + abs(u.y - v.y) + abs(u.z - v.z)).toDouble() private val target = fastVectorOf(0, 91, -4) + private val graph = LazyGraph { origin -> + runSafe { + moveOptions(origin, ::heuristic, pathing).associate { it.pos to it.cost } + } ?: emptyMap() + } + private val dStar = DStarLite(graph, fastVectorOf(0, 0, 0), target, ::heuristic) private var coarsePath = Path() private var refinedPath = Path() private var currentTarget: Vec3d? = null @@ -86,6 +98,9 @@ object Pathfinder : Module( coarsePath = Path() refinedPath = Path() currentTarget = null + graph.clear() + dStar.initialize() + dStar.updateStart(player.pos.toFastVec()) } listen { @@ -96,7 +111,15 @@ object Pathfinder : Module( updatePaths() } +// listen { +// val pos = it.pos.toFastVec() +// graph.markDirty(pos) +// info("Updated block at ${it.pos} 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) @@ -117,14 +140,17 @@ object Pathfinder : Module( } 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(rotation) + lookAt(targetRotation).requestBy(pathing.rotation) } listen { + if (!pathing.moveAlongPath) return@listen if (refinedPath.moves.isEmpty()) return@listen player.isSprinting = pathing.allowSprint @@ -132,53 +158,75 @@ object Pathfinder : Module( } listen { event -> -// longPath.render(event.renderer, Color.YELLOW) - refinedPath.render(event.renderer, Color.GREEN) - event.renderer.buildFilled(Box(target.toBlockPos()), Color.PINK.setAlpha(0.25)) + 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)) } } private fun SafeContext.updateTargetNode() { - refinedPath.moves.firstOrNull()?.let { current -> + currentTarget = refinedPath.moves.firstOrNull()?.let { current -> if (player.pos.distanceTo(current.bottomPos) < pathing.tolerance) { refinedPath.moves.removeFirst() integralError = Vec3d.ZERO } - currentTarget = refinedPath.moves.firstOrNull()?.bottomPos - } ?: run { - currentTarget = null + refinedPath.moves.firstOrNull()?.bottomPos } } private fun SafeContext.updatePaths() { val goal = SimpleGoal(target) + val start = player.blockPos.toFastVec() when (pathing.algorithm) { - PathingConfig.PathingAlgorithm.A_STAR -> { - runConcurrent { - calculating = true - val long: Path - val aStar = measureTimeMillis { - long = findPathAStar(player.blockPos.toFastVec(), goal, pathing) - } - val short: Path - val thetaStar = measureTimeMillis { - short = if (pathing.pathRefining) { - thetaStarClearance(long, pathing) - } else long - } - info("A* (Length: ${long.length().string} Nodes: ${long.size} T: $aStar ms) and Theta* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)") - println("Long: $long | Short: $short") - short.moves.removeFirstOrNull() - coarsePath = long - refinedPath = short - // calculating = false - } + PathingConfig.PathingAlgorithm.A_STAR -> updateAStar(start, goal) + PathingConfig.PathingAlgorithm.D_STAR_LITE -> updateDStar() + } + } + + private fun SafeContext.updateAStar(start: FastVector, goal: SimpleGoal) { + runConcurrent { + calculating = true + 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 } - PathingConfig.PathingAlgorithm.D_STAR_LITE -> { - runConcurrent { + 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") + short.moves.removeFirstOrNull() + coarsePath = long + refinedPath = short + // calculating = false + } + } - } + private fun SafeContext.updateDStar() { + runConcurrent { + calculating = true + val long: Path + val dStar = measureTimeMillis { +// if (start dist dstar.start > 3) dstar.updateStart(start) + dStar.computeShortestPath() + val nodes = dStar.getPath().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: $dStar ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)") + println("Long: $long | Short: $short") + short.moves.removeFirstOrNull() + coarsePath = long + refinedPath = short + // calculating = false } } diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index 1b591601d..47638072e 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -17,6 +17,7 @@ package com.lambda.pathing +import com.lambda.interaction.request.rotation.RotationConfig import com.lambda.util.NamedEnum interface PathingConfig { @@ -24,21 +25,27 @@ interface PathingConfig { val cutoffTimeout: Long val maxFallHeight: Double - val pathRefining: Boolean + val refinePath: Boolean val shortcutLength: Int val clearancePrecision: Double val findShortcutJumps: Boolean + 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 assumeJesus: Boolean enum class PathingAlgorithm(override val displayName: String) : NamedEnum { A_STAR("A*"), - D_STAR_LITE("D* Lite"), + D_STAR_LITE("Lazy D* Lite"), } } diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 42766ccb5..3281097ee 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -18,31 +18,38 @@ 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, Misc + Pathfinding, Refinement, Movement, Rotation, Misc } private val page by c.setting("Pathing Page", Page.Pathfinding, "Current page", vis) - override val algorithm by c.setting("Pathfinding Algorithm", PathingConfig.PathingAlgorithm.A_STAR) { vis() && page == Page.Pathfinding } + override val algorithm by c.setting("Algorithm", PathingConfig.PathingAlgorithm.A_STAR) { vis() && page == Page.Pathfinding } 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 pathRefining by c.setting("Path Refining", true) { vis() && page == Page.Refinement } - override val shortcutLength by c.setting("Shortcut Length", 10, 1..100, 1) { vis() && pathRefining && page == Page.Refinement } - override val clearancePrecision by c.setting("Clearance Precision", 0.2, 0.0..1.0, 0.01) { vis() && pathRefining && page == Page.Refinement } - override val findShortcutJumps by c.setting("Find Shortcut Jumps", true) { vis() && pathRefining && page == Page.Refinement } + override val refinePath by c.setting("Refine Path with θ*", true) { vis() && page == Page.Refinement } + override val shortcutLength by c.setting("Shortcut Length", 15, 1..100, 1) { vis() && refinePath && page == Page.Refinement } + override val clearancePrecision by c.setting("Clearance Precision", 0.1, 0.0..1.0, 0.01) { vis() && refinePath && page == Page.Refinement } + override val findShortcutJumps by c.setting("Find Shortcut Jumps", true) { vis() && refinePath && page == Page.Refinement } - override val kP by c.setting("P Gain", 0.5, 0.0..2.0, 0.01) { vis() && page == Page.Movement } - override val kI by c.setting("I Gain", 0.0, 0.0..1.0, 0.01) { vis() && page == Page.Movement } - override val kD by c.setting("D Gain", 0.2, 0.0..1.0, 0.01) { vis() && page == Page.Movement } - override val tolerance by c.setting("Node Tolerance", 0.7, 0.01..2.0, 0.05) { vis() && page == Page.Movement } - override val allowSprint by c.setting("Allow Sprint", true) { vis() && page == Page.Movement } + 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.Misc } + override val renderRefinedPath by c.setting("Render Refined Path", true) { vis() && page == Page.Misc } + override val renderGoal by c.setting("Render Goal", true) { vis() && page == Page.Misc } override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc } } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 4071d955c..87d5d2dba 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -35,9 +35,9 @@ import kotlin.math.min */ class DStarLite( private val graph: LazyGraph, - private val heuristic: (FastVector, FastVector) -> Double, var start: FastVector, - private val goal: FastVector + private val goal: FastVector, + private val heuristic: (FastVector, FastVector) -> Double, ) { // gMap[u], rhsMap[u] store g(u) and rhs(u) or default to ∞ if not present private val gMap = mutableMapOf() @@ -58,7 +58,7 @@ class DStarLite( * - g(goal)=∞, rhs(goal)=0 * - Insert goal into U with key = calculateKey(goal). */ - private fun initialize() { + fun initialize() { gMap.clear() rhsMap.clear() setG(goal, INF) diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 622eafde8..aa9fea259 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -17,7 +17,6 @@ package com.lambda.pathing.dstar -import com.lambda.context.SafeContext import com.lambda.util.world.FastVector /** @@ -33,12 +32,14 @@ import com.lambda.util.world.FastVector * - 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( private val nodeInitializer: (FastVector) -> Map ) { private val successors = hashMapOf>() private val predecessors = hashMapOf>() + private val dirtyNodes = mutableSetOf() + + val size get() = successors.size /** Initializes a node if not already initialized, then returns successors. */ fun successors(u: FastVector) = @@ -56,6 +57,15 @@ class LazyGraph( return predecessors[u] ?: emptyMap() } + fun markDirty(pos: FastVector) { + dirtyNodes.add(pos) + predecessors[pos]?.keys?.let { pred -> + dirtyNodes.addAll(pred) + } + } + + fun clearDirty() = dirtyNodes.clear() + /** 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 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/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt index a05ddaa6a..bafa1ca08 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -85,13 +85,13 @@ object MoveFinder { } else { traversable(checkingBlockPos) } - if (!clear) return null val hCost = heuristic(checkingPos) /** nodeType.penalty*/ - val cost = offset.length() + val cost = if (clear) offset.length() else Double.POSITIVE_INFINITY 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) } diff --git a/common/src/test/kotlin/DStarLiteTest.kt b/common/src/test/kotlin/DStarLiteTest.kt index c8363c7a5..d66617eb1 100644 --- a/common/src/test/kotlin/DStarLiteTest.kt +++ b/common/src/test/kotlin/DStarLiteTest.kt @@ -59,7 +59,7 @@ class DStarLiteTest { val graph = createLazy3DGridGraph() val start = fastVectorOf(0, 0, 0) val goal = fastVectorOf(2, 2, 2) - val dstar = DStarLite(graph, ::heuristic, start, goal) + val dstar = DStarLite(graph, start, goal, ::heuristic) dstar.computeShortestPath() val path = dstar.getPath() @@ -75,7 +75,7 @@ class DStarLiteTest { val graph = createLazy3DGridGraph() var start = fastVectorOf(0, 0, 0) val goal = fastVectorOf(2, 2, 2) - val dstar = DStarLite(graph, ::heuristic, start, goal) + val dstar = DStarLite(graph, start, goal, ::heuristic) dstar.computeShortestPath() From fc86013ae7a70c73872c5a04ec51636b02ae0bd9 Mon Sep 17 00:00:00 2001 From: Constructor Date: Thu, 27 Mar 2025 04:07:16 +0100 Subject: [PATCH 15/39] Funny path renderer --- .../main/kotlin/com/lambda/pathing/Path.kt | 29 ++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/pathing/Path.kt b/common/src/main/kotlin/com/lambda/pathing/Path.kt index f4f6ad0bf..9578f3e54 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Path.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Path.kt @@ -17,12 +17,17 @@ package com.lambda.pathing -import com.lambda.graphics.renderer.esp.builders.buildLine +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( @@ -44,9 +49,25 @@ data class Path( fun render(renderer: StaticESP, color: Color) { moves.zipWithNext { current, next -> - val currentPos = current.pos.toBlockPos().toCenterPos() - val nextPos = next.pos.toBlockPos().toCenterPos() - renderer.buildLine(currentPos, nextPos, color) + 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) + } } } From 6d878b86d239d8473c45c1f59bf19f373991a2b6 Mon Sep 17 00:00:00 2001 From: Constructor Date: Fri, 28 Mar 2025 22:02:01 +0100 Subject: [PATCH 16/39] D* cut off timeout --- .../com/lambda/module/modules/movement/Pathfinder.kt | 4 ++-- .../main/kotlin/com/lambda/pathing/dstar/DStarLite.kt | 9 +++++++-- .../main/kotlin/com/lambda/pathing/move/MoveFinder.kt | 7 ++++--- 3 files changed, 13 insertions(+), 7 deletions(-) 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 index 630b9f697..d635c7554 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -206,12 +206,12 @@ object Pathfinder : Module( } private fun SafeContext.updateDStar() { + calculating = true runConcurrent { - calculating = true val long: Path val dStar = measureTimeMillis { // if (start dist dstar.start > 3) dstar.updateStart(start) - dStar.computeShortestPath() + dStar.computeShortestPath(pathing.cutoffTimeout) val nodes = dStar.getPath().map { TraverseMove(it, 0.0, NodeType.OPEN, 0.0, 0.0) } long = Path(ArrayDeque(nodes)) } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 87d5d2dba..a164878e5 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -112,8 +112,10 @@ class DStarLite( * While the queue top is "less" than calculateKey(start) * or g(start) < rhs(start), pop and process. */ - fun computeShortestPath() { - while ((U.topKey() < calculateKey(start)) || (g(start) < rhs(start))) { + fun computeShortestPath(cutoffTimeout: Long = 500L) { + val startTime = System.currentTimeMillis() + + while ((U.topKey() < calculateKey(start)) || (g(start) < rhs(start)) && (System.currentTimeMillis() - startTime) < cutoffTimeout) { val u = U.top() ?: break val oldKey = U.topKey() val newKey = calculateKey(u) @@ -169,6 +171,9 @@ class DStarLite( */ fun getPath(): List { val path = mutableListOf() + + if (!graph.contains(start)) return path.toList() + var current = start path.add(current) while (current != goal) { diff --git a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt index bafa1ca08..cf386ac95 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -81,17 +81,18 @@ object MoveFinder { checkingBlockPos.y < originBlockPos.y -> Box.enclosing(originBlockPos.up(), checkingBlockPos.up()) else -> Box.enclosing(originBlockPos.up(2), checkingBlockPos) } - traversable(checkingBlockPos) && world.isSpaceEmpty(enclose) + traversable(checkingBlockPos) && world.isSpaceEmpty(enclose.contract(0.01)) } else { traversable(checkingBlockPos) } + if (!clear) return null val hCost = heuristic(checkingPos) /** nodeType.penalty*/ - val cost = if (clear) offset.length() else Double.POSITIVE_INFINITY + val cost = offset.length() val currentFeetY = getFeetY(checkingBlockPos) return when { - cost == Double.POSITIVE_INFINITY -> BreakMove(checkingPos, hCost, nodeType, currentFeetY, cost) +// 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) } From 86d17d54e70787c2143ab0c21d06aedd9b2d2ca3 Mon Sep 17 00:00:00 2001 From: Constructor Date: Tue, 8 Apr 2025 02:20:59 +0200 Subject: [PATCH 17/39] Optimized threading and graph renderer --- .../module/modules/movement/Pathfinder.kt | 144 +++++++++++------- .../com/lambda/pathing/PathingConfig.kt | 1 + .../com/lambda/pathing/PathingSettings.kt | 1 + .../com/lambda/pathing/dstar/DStarLite.kt | 1 + .../com/lambda/pathing/dstar/LazyGraph.kt | 37 ++++- .../kotlin/com/lambda/util/world/Position.kt | 5 + .../com/lambda/util/world/WorldUtils.kt | 2 +- 7 files changed, 129 insertions(+), 62 deletions(-) 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 index d635c7554..a51299f7a 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -17,12 +17,14 @@ package com.lambda.module.modules.movement +import com.lambda.Lambda 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.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 @@ -41,20 +43,23 @@ import com.lambda.pathing.goal.SimpleGoal import com.lambda.pathing.move.MoveFinder.moveOptions import com.lambda.pathing.move.NodeType import com.lambda.pathing.move.TraverseMove -import com.lambda.threading.runConcurrent import com.lambda.threading.runSafe +import com.lambda.threading.runSafeConcurrent import com.lambda.util.Communication.info 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.fastVectorOf 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 @@ -68,15 +73,10 @@ object Pathfinder : Module( description = "Get from A to B", defaultTags = setOf(ModuleTag.MOVEMENT) ) { + private val targetPos by setting("Target", BlockPos(0, 78, 0)) private val pathing = PathingSettings(this) - 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() - - private val target = fastVectorOf(0, 91, -4) + private val target: FastVector get() = targetPos.toFastVec() private val graph = LazyGraph { origin -> runSafe { moveOptions(origin, ::heuristic, pathing).associate { it.pos to it.cost } @@ -88,27 +88,40 @@ object Pathfinder : Module( private var currentTarget: Vec3d? = null private var integralError = Vec3d.ZERO private var lastError = Vec3d.ZERO - private var calculating = false + private 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 - calculating = false coarsePath = Path() refinedPath = Path() currentTarget = null graph.clear() dStar.initialize() - dStar.updateStart(player.pos.toFastVec()) + needsUpdate = true + currentStart = player.blockPos.toFastVec() + startBackgroundThread() + } + + onDisable { + graph.clear() } listen { + val playerPos = player.blockPos + if (player.isOnGround && hasSupport(playerPos)) { + currentStart = playerPos.toFastVec() + needsUpdate = true + } updateTargetNode() - - if (calculating) return@listen - - updatePaths() } // listen { @@ -161,6 +174,30 @@ object Pathfinder : Module( 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)) + if (pathing.renderGraph) graph.render(event.renderer) + } + + listen { + if (!pathing.renderGraph) return@listen + + Matrices.push { + val c = mc.gameRenderer.camera.pos.negate() + translate(c.x, c.y, c.z) + graph.buildDebugInfo() + } + } + } + + private fun startBackgroundThread() { + runSafeConcurrent { + while (isEnabled) { + if (!needsUpdate) { + delay(50L) + continue + } + needsUpdate = false + updatePaths() + } } } @@ -176,58 +213,49 @@ object Pathfinder : Module( private fun SafeContext.updatePaths() { val goal = SimpleGoal(target) - val start = player.blockPos.toFastVec() when (pathing.algorithm) { - PathingConfig.PathingAlgorithm.A_STAR -> updateAStar(start, goal) - PathingConfig.PathingAlgorithm.D_STAR_LITE -> updateDStar() + PathingConfig.PathingAlgorithm.A_STAR -> updateAStar(currentStart, goal) + PathingConfig.PathingAlgorithm.D_STAR_LITE -> updateDStar(currentStart, goal) } } private fun SafeContext.updateAStar(start: FastVector, goal: SimpleGoal) { - runConcurrent { - calculating = true - 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") - short.moves.removeFirstOrNull() - coarsePath = long - refinedPath = short - // calculating = false + 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") + short.moves.removeFirstOrNull() + coarsePath = long + refinedPath = short } - private fun SafeContext.updateDStar() { - calculating = true - runConcurrent { - val long: Path - val dStar = measureTimeMillis { -// if (start dist dstar.start > 3) dstar.updateStart(start) - dStar.computeShortestPath(pathing.cutoffTimeout) - val nodes = dStar.getPath().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: $dStar ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)") - println("Long: $long | Short: $short") - short.moves.removeFirstOrNull() - coarsePath = long - refinedPath = short - // calculating = false + private fun SafeContext.updateDStar(start: FastVector, goal: SimpleGoal) { + val long: Path + val dStar = measureTimeMillis { + dStar.updateStart(start) + dStar.computeShortestPath(pathing.cutoffTimeout) + val nodes = dStar.getPath().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: $dStar ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)") +// println("Long: $long | Short: $short") + short.moves.removeFirstOrNull() + coarsePath = long + refinedPath = short } private fun SafeContext.calculatePID(target: Vec3d): Vec3d { diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index 47638072e..ffb3c1e57 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -42,6 +42,7 @@ interface PathingConfig { val renderCoarsePath: Boolean val renderRefinedPath: Boolean val renderGoal: Boolean + val renderGraph: Boolean val assumeJesus: Boolean enum class PathingAlgorithm(override val displayName: String) : NamedEnum { diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 3281097ee..98fba294b 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -51,5 +51,6 @@ class PathingSettings( override val renderCoarsePath by c.setting("Render Coarse Path", false) { vis() && page == Page.Misc } override val renderRefinedPath by c.setting("Render Refined Path", true) { vis() && page == Page.Misc } override val renderGoal by c.setting("Render Goal", true) { vis() && page == Page.Misc } + override val renderGraph by c.setting("Render Graph", false) { vis() && page == Page.Misc } override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc } } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index a164878e5..840db7da8 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -152,6 +152,7 @@ class DStarLite( * all vertices in the queue are re-keyed. */ fun updateStart(newStart: FastVector) { + if (newStart == start) return val oldStart = start start = newStart km += heuristic(oldStart, start) diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index aa9fea259..48b0da6f2 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -17,7 +17,14 @@ package com.lambda.pathing.dstar +import com.lambda.graphics.renderer.esp.builders.buildLine +import com.lambda.graphics.renderer.esp.global.StaticESP import com.lambda.util.world.FastVector +import com.lambda.util.world.toCenterVec3d +import com.lambda.util.world.toVec3d +import java.awt.Color +import java.util.concurrent.ConcurrentHashMap +import java.util.concurrent.ConcurrentMap /** * A 3D graph that uses FastVector (a Long) to represent 3D nodes. @@ -35,8 +42,8 @@ import com.lambda.util.world.FastVector class LazyGraph( private val nodeInitializer: (FastVector) -> Map ) { - private val successors = hashMapOf>() - private val predecessors = hashMapOf>() + private val successors = ConcurrentHashMap>() + private val predecessors = ConcurrentHashMap>() private val dirtyNodes = mutableSetOf() val size get() = successors.size @@ -69,10 +76,34 @@ class LazyGraph( /** 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 - fun contains(u: FastVector): Boolean = u in successors + fun contains(u: FastVector): Boolean = successors.containsKey(u) fun clear() { successors.clear() predecessors.clear() } + + fun render(renderer: StaticESP) { + successors.entries.take(1000).forEach { (origin, neighbors) -> + neighbors.forEach { (neighbor, cost) -> + renderer.buildLine(origin.toCenterVec3d(), neighbor.toCenterVec3d(), Color.PINK) + } + } + } + + fun buildDebugInfo() { +// val projection = buildWorldProjection(blockPos, 0.4, Matrices.ProjRotationMode.TO_CAMERA) +// withVertexTransform(projection) { +// val lines = arrayOf( +// "" +// ) +// +// var height = -0.5 * lines.size * (FontRenderer.getHeight() + 2) +// +// lines.forEach { +// drawString(it, Vec2d(-FontRenderer.getWidth(it) * 0.5, height)) +// height += FontRenderer.getHeight() + 2 +// } +// } + } } 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 9c45c1d35..7f220f239 100644 --- a/common/src/main/kotlin/com/lambda/util/world/Position.kt +++ b/common/src/main/kotlin/com/lambda/util/world/Position.kt @@ -258,6 +258,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]. */ 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 3c44f7982..94500b8d0 100644 --- a/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt +++ b/common/src/main/kotlin/com/lambda/util/world/WorldUtils.kt @@ -71,7 +71,7 @@ object WorldUtils { private fun SafeContext.hasClearance(pos: BlockPos) = blockState(pos).isAir && blockState(pos.up()).isAir - private fun SafeContext.hasSupport(pos: BlockPos) = + fun SafeContext.hasSupport(pos: BlockPos) = blockState(pos.down()).isSideSolidFullSquare(world, pos.down(), Direction.UP) fun Vec3d.playerBox(): Box = From fc26de33ab8bfd7848ab84f933ac52e875ec781d Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 14 Apr 2025 23:40:56 +0200 Subject: [PATCH 18/39] Pathing command, detailed graph renderer, graph invalidation --- .../lambda/command/commands/PathCommand.kt | 50 +++++++++++ .../module/modules/movement/Pathfinder.kt | 9 +- .../com/lambda/pathing/PathingConfig.kt | 3 + .../com/lambda/pathing/PathingSettings.kt | 3 + .../com/lambda/pathing/dstar/DStarLite.kt | 62 +++++++------- .../kotlin/com/lambda/pathing/dstar/Key.kt | 5 +- .../com/lambda/pathing/dstar/LazyGraph.kt | 85 +++++++++++++------ .../kotlin/com/lambda/util/world/Position.kt | 3 + common/src/test/kotlin/DStarLiteTest.kt | 6 +- 9 files changed, 160 insertions(+), 66 deletions(-) create mode 100644 common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt 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..3cd0296a5 --- /dev/null +++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt @@ -0,0 +1,50 @@ +/* + * 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.integer +import com.lambda.brigadier.argument.literal +import com.lambda.brigadier.argument.value +import com.lambda.brigadier.execute +import com.lambda.brigadier.required +import com.lambda.command.LambdaCommand +import com.lambda.module.modules.movement.Pathfinder +import com.lambda.util.extension.CommandBuilder +import com.lambda.util.world.fastVectorOf + +object PathCommand : LambdaCommand( + name = "path", + usage = "path ", + description = "Move through world" +) { + override fun CommandBuilder.create() { + required(literal("markDirty")) { + required(integer("X", -30000000, 30000000)) { x -> + required(integer("Y", -64, 255)) { y -> + required(integer("Z", -30000000, 30000000)) { z -> + execute { + val dirty = fastVectorOf(x().value(), y().value(), z().value()) + Pathfinder.graph.markDirty(dirty) + Pathfinder.graph.updateDirtyNode(dirty) + } + } + } + } + } + } +} 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 index a51299f7a..30bc1755e 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -17,7 +17,6 @@ package com.lambda.module.modules.movement -import com.lambda.Lambda import com.lambda.context.SafeContext import com.lambda.event.events.MovementEvent import com.lambda.event.events.RenderEvent @@ -77,7 +76,7 @@ object Pathfinder : Module( private val pathing = PathingSettings(this) private val target: FastVector get() = targetPos.toFastVec() - private val graph = LazyGraph { origin -> + val graph = LazyGraph { origin -> runSafe { moveOptions(origin, ::heuristic, pathing).associate { it.pos to it.cost } } ?: emptyMap() @@ -174,7 +173,7 @@ object Pathfinder : Module( 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)) - if (pathing.renderGraph) graph.render(event.renderer) + if (pathing.renderGraph) graph.render(event.renderer, pathing.maxRenderObjects) } listen { @@ -183,7 +182,7 @@ object Pathfinder : Module( Matrices.push { val c = mc.gameRenderer.camera.pos.negate() translate(c.x, c.y, c.z) - graph.buildDebugInfo() + graph.buildDebugInfoRenderer(pathing.maxRenderObjects) } } } @@ -242,7 +241,7 @@ object Pathfinder : Module( val dStar = measureTimeMillis { dStar.updateStart(start) dStar.computeShortestPath(pathing.cutoffTimeout) - val nodes = dStar.getPath().map { TraverseMove(it, 0.0, NodeType.OPEN, 0.0, 0.0) } + val nodes = dStar.path.map { TraverseMove(it, 0.0, NodeType.OPEN, 0.0, 0.0) } long = Path(ArrayDeque(nodes)) } val short: Path diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index ffb3c1e57..d6b03fdad 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -43,6 +43,9 @@ interface PathingConfig { val renderRefinedPath: Boolean val renderGoal: Boolean val renderGraph: Boolean + val renderWeights: Boolean + val renderPositions: Boolean + val maxRenderObjects: Int val assumeJesus: Boolean enum class PathingAlgorithm(override val displayName: String) : NamedEnum { diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 98fba294b..5066734d9 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -52,5 +52,8 @@ class PathingSettings( override val renderRefinedPath by c.setting("Render Refined Path", true) { vis() && page == Page.Misc } override val renderGoal by c.setting("Render Goal", true) { vis() && page == Page.Misc } override val renderGraph by c.setting("Render Graph", false) { vis() && page == Page.Misc } + override val renderWeights by c.setting("Render Weights", false) { vis() && page == Page.Misc && renderGraph } + override val renderPositions by c.setting("Render Positions", false) { vis() && page == Page.Misc && renderGraph } + override val maxRenderObjects by c.setting("Max Render Objects", 1000, 0..100_000) { vis() && page == Page.Misc && renderGraph } override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc } } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 840db7da8..65cfda82f 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -21,16 +21,16 @@ import com.lambda.util.world.FastVector import kotlin.math.min /** - * D* Lite Implementation. + * Lazy D* Lite Implementation. * * We perform a backward search from the goal to the start, so: * - rhs(goal) = 0, g(goal) = ∞ - * - "start" is the robot's current location from which we want a path *to* the goal + * - "start" is the agent's current location from which we want a path *to* the goal * - 'km' accumulates the heuristic shift so we don't reorder the entire queue after each move * * @param graph The graph on which we plan (with forward + reverse adjacency). * @param heuristic A consistent (or at least nonnegative) heuristic function h(a,b). - * @param start The robot's current position. + * @param start The agent's current position. * @param goal The fixed goal vertex. */ class DStarLite( @@ -67,10 +67,10 @@ class DStarLite( } private fun g(u: FastVector): Double = gMap[u] ?: INF - private fun setG(u: FastVector, value: Double) { gMap[u] = value } + private fun setG(u: FastVector, g: Double) { gMap[u] = g } private fun rhs(u: FastVector): Double = rhsMap[u] ?: INF - private fun setRHS(u: FastVector, value: Double) { rhsMap[u] = value } + private fun setRHS(u: FastVector, rhs: Double) { rhsMap[u] = rhs } /** * Calculates the key for vertex u. @@ -147,7 +147,7 @@ class DStarLite( } /** - * When the robot moves, update the start. + * When the agent moves, update the start. * The variable km is increased by h(oldStart, newStart) and * all vertices in the queue are re-keyed. */ @@ -170,35 +170,37 @@ class DStarLite( * Retrieves a path from start to goal by always choosing the successor * with the lowest g + cost value. If no path is found, the path stops early. */ - fun getPath(): List { - val path = mutableListOf() - - if (!graph.contains(start)) return path.toList() - - var current = start - path.add(current) - while (current != goal) { - val successors = graph.successors(current) - if (successors.isEmpty()) break - var bestNext: FastVector? = null - var bestVal = INF - for ((succ, cost) in successors) { - val candidate = g(succ) + cost - if (candidate < bestVal) { - bestVal = candidate - bestNext = succ + val path: List + get() { + val path = mutableListOf() + + if (!graph.contains(start)) return path.toList() + + var current = start + path.add(current) + while (current != goal) { + val successors = graph.successors(current) + if (successors.isEmpty()) break + var bestNext: FastVector? = null + var bestVal = INF + for ((succ, cost) in successors) { + val candidate = g(succ) + cost + if (candidate < bestVal) { + bestVal = candidate + bestNext = succ + } } + // No path + if (bestNext == null) break + current = bestNext + path.add(current) + if (path.size > MAX_PATH_LENGTH) break } - // No path - if (bestNext == null) break - current = bestNext - path.add(current) - if (path.size > 100_000) break + return path } - return path - } companion object { private const val INF = Double.POSITIVE_INFINITY + private const val MAX_PATH_LENGTH = 100_000 } } \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt index 705890bdd..188988773 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt @@ -22,13 +22,12 @@ package com.lambda.pathing.dstar * They are compared lexicographically. */ data class Key(val k1: Double, val k2: Double) : Comparable { - override fun compareTo(other: Key): Int { - return when { + override fun compareTo(other: Key) = + when { this.k1 < other.k1 -> -1 this.k1 > other.k1 -> 1 this.k2 < other.k2 -> -1 this.k2 > other.k2 -> 1 else -> 0 } - } } \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 48b0da6f2..b2a96a02e 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -17,14 +17,23 @@ package com.lambda.pathing.dstar +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.esp.builders.buildLine +import com.lambda.graphics.renderer.esp.builders.buildOutline import com.lambda.graphics.renderer.esp.global.StaticESP +import com.lambda.graphics.renderer.gui.FontRenderer +import com.lambda.graphics.renderer.gui.FontRenderer.drawString +import com.lambda.util.math.Vec2d +import com.lambda.util.math.div +import com.lambda.util.math.plus import com.lambda.util.world.FastVector +import com.lambda.util.world.string import com.lambda.util.world.toCenterVec3d -import com.lambda.util.world.toVec3d +import net.minecraft.util.math.Box import java.awt.Color import java.util.concurrent.ConcurrentHashMap -import java.util.concurrent.ConcurrentMap /** * A 3D graph that uses FastVector (a Long) to represent 3D nodes. @@ -49,14 +58,18 @@ class LazyGraph( val size get() = successors.size /** Initializes a node if not already initialized, then returns successors. */ - fun successors(u: FastVector) = - successors.computeIfAbsent(u) { + fun successors(u: FastVector): Map { + if (u in dirtyNodes) { + updateDirtyNode(u) + } + return successors.getOrPut(u) { val neighbors = nodeInitializer(u) neighbors.forEach { (neighbor, cost) -> predecessors.computeIfAbsent(neighbor) { hashMapOf() }[u] = cost } neighbors.toMutableMap() } + } /** Initializes predecessors by ensuring successors of neighboring nodes. */ fun predecessors(u: FastVector): Map { @@ -64,14 +77,23 @@ class LazyGraph( return predecessors[u] ?: emptyMap() } - fun markDirty(pos: FastVector) { - dirtyNodes.add(pos) - predecessors[pos]?.keys?.let { pred -> + fun markDirty(u: FastVector) { + dirtyNodes.add(u) + predecessors[u]?.keys?.let { pred -> dirtyNodes.addAll(pred) } } - fun clearDirty() = dirtyNodes.clear() + fun updateDirtyNode(u: FastVector) { + // Force re-initialize node and clear existing edges + val newNeighbors = nodeInitializer(u) + successors[u]?.clear() + newNeighbors.forEach { (v, cost) -> + predecessors.getOrPut(v) { mutableMapOf() }[u] = cost + } + successors[u] = newNeighbors.toMutableMap() + dirtyNodes.remove(u) + } /** 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 @@ -83,27 +105,40 @@ class LazyGraph( predecessors.clear() } - fun render(renderer: StaticESP) { - successors.entries.take(1000).forEach { (origin, neighbors) -> - neighbors.forEach { (neighbor, cost) -> + fun render(renderer: StaticESP, maxElements: Int = 1000) { + successors.entries.take(maxElements).forEach { (origin, neighbors) -> + neighbors.forEach { (neighbor, _) -> renderer.buildLine(origin.toCenterVec3d(), neighbor.toCenterVec3d(), Color.PINK) } } + dirtyNodes.take(maxElements).forEach { node -> + renderer.buildOutline( + Box.of(node.toCenterVec3d(), 0.3, 0.3, 0.3), + Color.RED + ) + } } - fun buildDebugInfo() { -// val projection = buildWorldProjection(blockPos, 0.4, Matrices.ProjRotationMode.TO_CAMERA) -// withVertexTransform(projection) { -// val lines = arrayOf( -// "" -// ) -// -// var height = -0.5 * lines.size * (FontRenderer.getHeight() + 2) -// -// lines.forEach { -// drawString(it, Vec2d(-FontRenderer.getWidth(it) * 0.5, height)) -// height += FontRenderer.getHeight() + 2 -// } -// } + fun buildDebugInfoRenderer(maxElements: Int = 1000) { + successors.entries.take(maxElements).forEach { (v, u) -> + val mode = Matrices.ProjRotationMode.TO_CAMERA + val scale = 0.4 + val pos = v.toCenterVec3d() + val nodeProjection = buildWorldProjection(pos, scale, mode) + withVertexTransform(nodeProjection) { + val msg = v.string + drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) + } + u.forEach { (neighbor, cost) -> + val centerV = v.toCenterVec3d() + val centerN = neighbor.toCenterVec3d() + val center = (centerV + centerN) / 2.0 + val projection = buildWorldProjection(center, scale, mode) + withVertexTransform(projection) { + val msg = "c: %.3f".format(cost) + drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) + } + } + } } } 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 7f220f239..6f7916484 100644 --- a/common/src/main/kotlin/com/lambda/util/world/Position.kt +++ b/common/src/main/kotlin/com/lambda/util/world/Position.kt @@ -275,3 +275,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/test/kotlin/DStarLiteTest.kt b/common/src/test/kotlin/DStarLiteTest.kt index d66617eb1..5912b377e 100644 --- a/common/src/test/kotlin/DStarLiteTest.kt +++ b/common/src/test/kotlin/DStarLiteTest.kt @@ -62,7 +62,7 @@ class DStarLiteTest { val dstar = DStarLite(graph, start, goal, ::heuristic) dstar.computeShortestPath() - val path = dstar.getPath() + val path = dstar.path // Manhattan distance between (0,0,0) and (2,2,2) is 6; hence the path should have 7 vertices. assertFalse(path.isEmpty(), "Path should not be empty") assertEquals(start, path.first(), "Path should start at the initial position") @@ -79,14 +79,14 @@ class DStarLiteTest { dstar.computeShortestPath() - val initialPath = dstar.getPath() + val initialPath = dstar.path assertTrue(initialPath.size > 1, "Initial path should have multiple steps") // Move starting position closer to the goal and recompute start = fastVectorOf(1, 1, 1) dstar.updateStart(start) - val updatedPath = dstar.getPath() + val updatedPath = dstar.path assertFalse(updatedPath.isEmpty(), "Updated path should not be empty now from new start") assertEquals(start, updatedPath.first(), "Updated path must start at updated position") assertEquals(goal, updatedPath.last(), "Updated path must end at the goal") From 7ccb5d46d1c8d0edd69e106665446727cf622558 Mon Sep 17 00:00:00 2001 From: Constructor Date: Tue, 15 Apr 2025 00:27:27 +0200 Subject: [PATCH 19/39] Fix refining of path start --- .../lambda/module/modules/movement/Pathfinder.kt | 6 +++--- .../main/kotlin/com/lambda/pathing/Pathing.kt | 16 ++++++---------- .../kotlin/com/lambda/pathing/dstar/LazyGraph.kt | 5 +---- 3 files changed, 10 insertions(+), 17 deletions(-) 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 index 30bc1755e..7dcdf8b64 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -51,6 +51,7 @@ 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.WorldUtils.isPathClear import com.lambda.util.world.fastVectorOf import com.lambda.util.world.toBlockPos import com.lambda.util.world.toFastVec @@ -120,7 +121,8 @@ object Pathfinder : Module( currentStart = playerPos.toFastVec() needsUpdate = true } - updateTargetNode() + if (pathing.moveAlongPath) updateTargetNode() +// info("${isPathClear(playerPos, targetPos)}") } // listen { @@ -231,7 +233,6 @@ object Pathfinder : Module( } 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") - short.moves.removeFirstOrNull() coarsePath = long refinedPath = short } @@ -252,7 +253,6 @@ object Pathfinder : Module( } info("Lazy D* Lite (Length: ${long.length().string} Nodes: ${long.size} Graph Size: ${graph.size} T: $dStar ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)") // println("Long: $long | Short: $short") - short.moves.removeFirstOrNull() coarsePath = long refinedPath = short } diff --git a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt index 2dfbdc682..7c732b5da 100644 --- a/common/src/main/kotlin/com/lambda/pathing/Pathing.kt +++ b/common/src/main/kotlin/com/lambda/pathing/Pathing.kt @@ -73,7 +73,7 @@ object Pathing { } fun SafeContext.thetaStarClearance(path: Path, config: PathingConfig): Path { - if (path.moves.isEmpty()) return Path() + if (path.moves.isEmpty()) return path val cleanedPath = Path() var currentIndex = 0 @@ -84,22 +84,18 @@ object Pathing { cleanedPath.append(startMove) // Attempt to skip over as many nodes as possible - // by checking if they share the same Y and have a clear path 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 (startMove.pos.y != candidateMove.pos.y) break + if (startPos.y != candidatePos.y) break // Verify there's a clear path from the start move to the candidate - if ( - isPathClear( - startMove.pos.toBlockPos(), - candidateMove.pos.toBlockPos(), - config.clearancePrecision - ) - ) nextIndex++ else break + val isClear = isPathClear(startPos, candidatePos, config.clearancePrecision) + if (isClear) nextIndex++ else break } // Move to the last node that was confirmed reachable diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index b2a96a02e..5a275330a 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -112,10 +112,7 @@ class LazyGraph( } } dirtyNodes.take(maxElements).forEach { node -> - renderer.buildOutline( - Box.of(node.toCenterVec3d(), 0.3, 0.3, 0.3), - Color.RED - ) + renderer.buildOutline(Box.of(node.toCenterVec3d(), 0.2, 0.2, 0.2), Color.RED) } } From c65dfb5fe3b8020542d68895a5e70402631579d1 Mon Sep 17 00:00:00 2001 From: Constructor Date: Tue, 15 Apr 2025 00:41:05 +0200 Subject: [PATCH 20/39] Less frequent pathing updates --- .../kotlin/com/lambda/module/modules/movement/Pathfinder.kt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 index 7dcdf8b64..06bbfda34 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -52,6 +52,7 @@ 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.WorldUtils.isPathClear +import com.lambda.util.world.dist import com.lambda.util.world.fastVectorOf import com.lambda.util.world.toBlockPos import com.lambda.util.world.toFastVec @@ -117,8 +118,9 @@ object Pathfinder : Module( listen { val playerPos = player.blockPos - if (player.isOnGround && hasSupport(playerPos)) { - currentStart = playerPos.toFastVec() + val currentPos = playerPos.toFastVec() + if (player.isOnGround && hasSupport(playerPos) && currentPos dist currentStart > pathing.tolerance) { + currentStart = currentPos needsUpdate = true } if (pathing.moveAlongPath) updateTargetNode() From fb1ded1830b1054f981c58aa1f6a103f4356f522 Mon Sep 17 00:00:00 2001 From: Constructor Date: Tue, 15 Apr 2025 01:49:21 +0200 Subject: [PATCH 21/39] Add render setting functionalities --- .../lambda/command/commands/PathCommand.kt | 17 ++++++++ .../module/modules/movement/Pathfinder.kt | 10 ++--- .../com/lambda/pathing/PathingConfig.kt | 2 +- .../com/lambda/pathing/PathingSettings.kt | 2 +- .../com/lambda/pathing/dstar/LazyGraph.kt | 42 +++++++++++-------- 5 files changed, 48 insertions(+), 25 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt index 3cd0296a5..3b21e0077 100644 --- a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt +++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt @@ -24,8 +24,10 @@ import com.lambda.brigadier.execute import com.lambda.brigadier.required import com.lambda.command.LambdaCommand import com.lambda.module.modules.movement.Pathfinder +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 = "path", @@ -41,10 +43,25 @@ object PathCommand : LambdaCommand( val dirty = fastVectorOf(x().value(), y().value(), z().value()) Pathfinder.graph.markDirty(dirty) Pathfinder.graph.updateDirtyNode(dirty) + this@PathCommand.info("Marked ${dirty.string} as dirty") } } } } } + + required(literal("reset")) { + execute { + Pathfinder.graph.clear() + this@PathCommand.info("Reset graph") + } + } + + required(literal("update")) { + execute { + Pathfinder.needsUpdate = true + this@PathCommand.info("Updating graph") + } + } } } 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 index 06bbfda34..fb9d7f35a 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -51,7 +51,6 @@ 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.WorldUtils.isPathClear import com.lambda.util.world.dist import com.lambda.util.world.fastVectorOf import com.lambda.util.world.toBlockPos @@ -89,7 +88,7 @@ object Pathfinder : Module( private var currentTarget: Vec3d? = null private var integralError = Vec3d.ZERO private var lastError = Vec3d.ZERO - private var needsUpdate = false + var needsUpdate = false private var currentStart = BlockPos.ORIGIN.toFastVec() private fun heuristic(u: FastVector): Double = @@ -119,7 +118,8 @@ object Pathfinder : Module( listen { val playerPos = player.blockPos val currentPos = playerPos.toFastVec() - if (player.isOnGround && hasSupport(playerPos) && currentPos dist currentStart > pathing.tolerance) { + val positionOutdated = currentPos dist currentStart > pathing.tolerance + if (player.isOnGround && hasSupport(playerPos) && positionOutdated) { currentStart = currentPos needsUpdate = true } @@ -177,7 +177,7 @@ object Pathfinder : Module( 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)) - if (pathing.renderGraph) graph.render(event.renderer, pathing.maxRenderObjects) + graph.render(event.renderer, pathing) } listen { @@ -186,7 +186,7 @@ object Pathfinder : Module( Matrices.push { val c = mc.gameRenderer.camera.pos.negate() translate(c.x, c.y, c.z) - graph.buildDebugInfoRenderer(pathing.maxRenderObjects) + graph.buildDebugInfoRenderer(pathing) } } } diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index d6b03fdad..39f5f5bf4 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -43,7 +43,7 @@ interface PathingConfig { val renderRefinedPath: Boolean val renderGoal: Boolean val renderGraph: Boolean - val renderWeights: Boolean + val renderCost: Boolean val renderPositions: Boolean val maxRenderObjects: Int val assumeJesus: Boolean diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 5066734d9..16f97706a 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -52,7 +52,7 @@ class PathingSettings( override val renderRefinedPath by c.setting("Render Refined Path", true) { vis() && page == Page.Misc } override val renderGoal by c.setting("Render Goal", true) { vis() && page == Page.Misc } override val renderGraph by c.setting("Render Graph", false) { vis() && page == Page.Misc } - override val renderWeights by c.setting("Render Weights", false) { vis() && page == Page.Misc && renderGraph } + override val renderCost by c.setting("Render Cost", false) { vis() && page == Page.Misc && renderGraph } override val renderPositions by c.setting("Render Positions", false) { vis() && page == Page.Misc && renderGraph } override val maxRenderObjects by c.setting("Max Render Objects", 1000, 0..100_000) { vis() && page == Page.Misc && renderGraph } override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 5a275330a..a101677f6 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -25,6 +25,7 @@ import com.lambda.graphics.renderer.esp.builders.buildOutline import com.lambda.graphics.renderer.esp.global.StaticESP import com.lambda.graphics.renderer.gui.FontRenderer import com.lambda.graphics.renderer.gui.FontRenderer.drawString +import com.lambda.pathing.PathingSettings import com.lambda.util.math.Vec2d import com.lambda.util.math.div import com.lambda.util.math.plus @@ -105,37 +106,42 @@ class LazyGraph( predecessors.clear() } - fun render(renderer: StaticESP, maxElements: Int = 1000) { - successors.entries.take(maxElements).forEach { (origin, neighbors) -> + fun render(renderer: StaticESP, config: PathingSettings) { + if (!config.renderGraph) return + successors.entries.take(config.maxRenderObjects).forEach { (origin, neighbors) -> neighbors.forEach { (neighbor, _) -> renderer.buildLine(origin.toCenterVec3d(), neighbor.toCenterVec3d(), Color.PINK) } } - dirtyNodes.take(maxElements).forEach { node -> + dirtyNodes.take(config.maxRenderObjects).forEach { node -> renderer.buildOutline(Box.of(node.toCenterVec3d(), 0.2, 0.2, 0.2), Color.RED) } } - fun buildDebugInfoRenderer(maxElements: Int = 1000) { - successors.entries.take(maxElements).forEach { (v, u) -> + fun buildDebugInfoRenderer(config: PathingSettings) { + successors.entries.take(config.maxRenderObjects).forEach { (v, u) -> val mode = Matrices.ProjRotationMode.TO_CAMERA val scale = 0.4 - val pos = v.toCenterVec3d() - val nodeProjection = buildWorldProjection(pos, scale, mode) - withVertexTransform(nodeProjection) { - val msg = v.string - drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) - } - u.forEach { (neighbor, cost) -> - val centerV = v.toCenterVec3d() - val centerN = neighbor.toCenterVec3d() - val center = (centerV + centerN) / 2.0 - val projection = buildWorldProjection(center, scale, mode) - withVertexTransform(projection) { - val msg = "c: %.3f".format(cost) + if (config.renderPositions) { + val pos = v.toCenterVec3d() + val nodeProjection = buildWorldProjection(pos, scale, mode) + withVertexTransform(nodeProjection) { + val msg = v.string drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) } } + if (config.renderCost) { + u.forEach { (neighbor, cost) -> + val centerV = v.toCenterVec3d() + val centerN = neighbor.toCenterVec3d() + val center = (centerV + centerN) / 2.0 + val projection = buildWorldProjection(center, scale, mode) + withVertexTransform(projection) { + val msg = "c: %.3f".format(cost) + drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) + } + } + } } } } From a6f39f6abda73bee277078aa9fdbffe271ebfc43 Mon Sep 17 00:00:00 2001 From: Constructor Date: Tue, 15 Apr 2025 21:55:31 +0200 Subject: [PATCH 22/39] Improved renderer --- .../lambda/command/commands/PathCommand.kt | 6 +- .../module/modules/movement/Pathfinder.kt | 6 +- .../com/lambda/pathing/PathingConfig.kt | 20 ++- .../com/lambda/pathing/PathingSettings.kt | 35 ++++-- .../com/lambda/pathing/dstar/DStarLite.kt | 117 ++++++++++++++---- .../com/lambda/pathing/dstar/LazyGraph.kt | 75 ++++------- common/src/test/kotlin/DStarLiteTest.kt | 6 +- 7 files changed, 166 insertions(+), 99 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt index 3b21e0077..21e9dff7c 100644 --- a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt +++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt @@ -42,7 +42,7 @@ object PathCommand : LambdaCommand( execute { val dirty = fastVectorOf(x().value(), y().value(), z().value()) Pathfinder.graph.markDirty(dirty) - Pathfinder.graph.updateDirtyNode(dirty) +// Pathfinder.dStar.updateGraph() this@PathCommand.info("Marked ${dirty.string} as dirty") } } @@ -60,8 +60,8 @@ object PathCommand : LambdaCommand( required(literal("update")) { execute { Pathfinder.needsUpdate = true - this@PathCommand.info("Updating graph") + this@PathCommand.info("Marked graph for update") } } } -} +} \ No newline at end of file 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 index fb9d7f35a..6d1820091 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -82,7 +82,7 @@ object Pathfinder : Module( moveOptions(origin, ::heuristic, pathing).associate { it.pos to it.cost } } ?: emptyMap() } - private val dStar = DStarLite(graph, fastVectorOf(0, 0, 0), target, ::heuristic) + val dStar = DStarLite(graph, fastVectorOf(0, 0, 0), target, ::heuristic) private var coarsePath = Path() private var refinedPath = Path() private var currentTarget: Vec3d? = null @@ -186,7 +186,7 @@ object Pathfinder : Module( Matrices.push { val c = mc.gameRenderer.camera.pos.negate() translate(c.x, c.y, c.z) - graph.buildDebugInfoRenderer(pathing) + dStar.buildDebugInfoRenderer(pathing) } } } @@ -244,7 +244,7 @@ object Pathfinder : Module( val dStar = measureTimeMillis { dStar.updateStart(start) dStar.computeShortestPath(pathing.cutoffTimeout) - val nodes = dStar.path.map { TraverseMove(it, 0.0, NodeType.OPEN, 0.0, 0.0) } + val nodes = dStar.path(pathing.maxPathLength).map { TraverseMove(it, 0.0, NodeType.OPEN, 0.0, 0.0) } long = Path(ArrayDeque(nodes)) } val short: Path diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index 39f5f5bf4..b81eab532 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -24,11 +24,20 @@ interface PathingConfig { val algorithm: PathingAlgorithm 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 @@ -43,13 +52,22 @@ interface PathingConfig { val renderRefinedPath: Boolean val renderGoal: Boolean val renderGraph: Boolean - val renderCost: Boolean val renderPositions: Boolean + val renderCost: Boolean + val renderG: Boolean + val renderRHS: 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 index 16f97706a..9d29fa163 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -25,7 +25,7 @@ class PathingSettings( vis: () -> Boolean = { true } ) : PathingConfig { enum class Page { - Pathfinding, Refinement, Movement, Rotation, Misc + Pathfinding, Refinement, Movement, Rotation, Debug, Misc } private val page by c.setting("Pathing Page", Page.Pathfinding, "Current page", vis) @@ -33,11 +33,20 @@ class PathingSettings( override val algorithm by c.setting("Algorithm", PathingConfig.PathingAlgorithm.A_STAR) { vis() && page == Page.Pathfinding } 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 with θ*", true) { vis() && page == Page.Refinement } - override val shortcutLength by c.setting("Shortcut Length", 15, 1..100, 1) { vis() && refinePath && page == Page.Refinement } - override val clearancePrecision by c.setting("Clearance Precision", 0.1, 0.0..1.0, 0.01) { vis() && refinePath && page == Page.Refinement } + 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 } @@ -48,12 +57,16 @@ class PathingSettings( override val rotation = RotationSettings(c) { page == Page.Rotation } - override val renderCoarsePath by c.setting("Render Coarse Path", false) { vis() && page == Page.Misc } - override val renderRefinedPath by c.setting("Render Refined Path", true) { vis() && page == Page.Misc } - override val renderGoal by c.setting("Render Goal", true) { vis() && page == Page.Misc } - override val renderGraph by c.setting("Render Graph", false) { vis() && page == Page.Misc } - override val renderCost by c.setting("Render Cost", false) { vis() && page == Page.Misc && renderGraph } - override val renderPositions by c.setting("Render Positions", false) { vis() && page == Page.Misc && renderGraph } - override val maxRenderObjects by c.setting("Max Render Objects", 1000, 0..100_000) { vis() && page == Page.Misc && renderGraph } + 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 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 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/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 65cfda82f..5e6a7ba34 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -17,7 +17,18 @@ package com.lambda.pathing.dstar +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.math.Vec2d +import com.lambda.util.math.div +import com.lambda.util.math.plus import com.lambda.util.world.FastVector +import com.lambda.util.world.string +import com.lambda.util.world.toCenterVec3d import kotlin.math.min /** @@ -106,6 +117,17 @@ class DStarLite( } } + fun updateGraph() { + graph.dirtyNodes.forEach { u -> + if (u != goal) { + gMap.remove(u) + rhsMap.remove(u) + } + updateVertex(u) + } + graph.dirtyNodes.clear() + } + /** * Propagates changes until the start is locally consistent. * computeShortestPath(): @@ -115,7 +137,10 @@ class DStarLite( fun computeShortestPath(cutoffTimeout: Long = 500L) { val startTime = System.currentTimeMillis() - while ((U.topKey() < calculateKey(start)) || (g(start) < rhs(start)) && (System.currentTimeMillis() - startTime) < cutoffTimeout) { + fun timedOut() = (System.currentTimeMillis() - startTime) > cutoffTimeout + fun shouldUpdateState() = U.topKey() < calculateKey(start) || g(start) < rhs(start) + + while (shouldUpdateState() && !timedOut()) { val u = U.top() ?: break val oldKey = U.topKey() val newKey = calculateKey(u) @@ -170,37 +195,77 @@ class DStarLite( * Retrieves a path from start to goal by always choosing the successor * with the lowest g + cost value. If no path is found, the path stops early. */ - val path: List - get() { - val path = mutableListOf() - - if (!graph.contains(start)) return path.toList() - - var current = start - path.add(current) - while (current != goal) { - val successors = graph.successors(current) - if (successors.isEmpty()) break - var bestNext: FastVector? = null - var bestVal = INF - for ((succ, cost) in successors) { - val candidate = g(succ) + cost - if (candidate < bestVal) { - bestVal = candidate - bestNext = succ - } + fun path(maxLength: Int = 10_000): List { + val path = mutableListOf() + + if (!graph.contains(start)) return path.toList() + + var current = start + path.add(current) + while (current != goal) { + val successors = graph.successors(current) + if (successors.isEmpty()) break + var bestNext: FastVector? = null + var bestVal = INF + for ((succ, cost) in successors) { + val candidate = g(succ) + cost + if (candidate < bestVal) { + bestVal = candidate + bestNext = succ } - // No path - if (bestNext == null) break - current = bestNext + } + // No path + if (bestNext == null) break + current = bestNext + if (current !in path) { path.add(current) - if (path.size > MAX_PATH_LENGTH) break + } else { + break } - return path + if (path.size > maxLength) break } + return path + } + + 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 (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) / 2.0 + val projection = buildWorldProjection(center, scale, mode) + withVertexTransform(projection) { + val msg = "c: %.3f".format(cost) + drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) + } + } + } + } + } companion object { private const val INF = Double.POSITIVE_INFINITY - private const val MAX_PATH_LENGTH = 100_000 } } \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index a101677f6..9b8c21abd 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -52,25 +52,22 @@ import java.util.concurrent.ConcurrentHashMap class LazyGraph( private val nodeInitializer: (FastVector) -> Map ) { - private val successors = ConcurrentHashMap>() - private val predecessors = ConcurrentHashMap>() - private val dirtyNodes = mutableSetOf() + val successors = ConcurrentHashMap>() + val predecessors = ConcurrentHashMap>() + val dirtyNodes = mutableSetOf() + val nodes get() = successors.keys + predecessors.keys val size get() = successors.size /** Initializes a node if not already initialized, then returns successors. */ - fun successors(u: FastVector): Map { - if (u in dirtyNodes) { - updateDirtyNode(u) - } - return successors.getOrPut(u) { + fun successors(u: FastVector): MutableMap = + successors.getOrPut(u) { val neighbors = nodeInitializer(u) neighbors.forEach { (neighbor, cost) -> - predecessors.computeIfAbsent(neighbor) { hashMapOf() }[u] = cost + predecessors.getOrPut(neighbor) { hashMapOf() }[u] = cost } neighbors.toMutableMap() } - } /** Initializes predecessors by ensuring successors of neighboring nodes. */ fun predecessors(u: FastVector): Map { @@ -78,22 +75,22 @@ class LazyGraph( return predecessors[u] ?: emptyMap() } - fun markDirty(u: FastVector) { - dirtyNodes.add(u) - predecessors[u]?.keys?.let { pred -> - dirtyNodes.addAll(pred) - } + fun remove(u: FastVector) { + successors.remove(u) + successors.values.forEach { it.remove(u) } + predecessors.remove(u) + predecessors.values.forEach { it.remove(u) } } - fun updateDirtyNode(u: FastVector) { - // Force re-initialize node and clear existing edges - val newNeighbors = nodeInitializer(u) - successors[u]?.clear() - newNeighbors.forEach { (v, cost) -> - predecessors.getOrPut(v) { mutableMapOf() }[u] = cost - } - successors[u] = newNeighbors.toMutableMap() - dirtyNodes.remove(u) + fun markDirty(u: FastVector) { + dirtyNodes.add(u) + val preds = predecessors[u]?.keys ?: emptySet() + val succs = successors[u]?.keys ?: emptySet() + remove(u) + preds.forEach { remove(it) } + succs.forEach { remove(it) } + dirtyNodes.addAll(preds) + dirtyNodes.addAll(succs) } /** Returns the cost of the edge from u to v (or ∞ if none exists) */ @@ -104,6 +101,7 @@ class LazyGraph( fun clear() { successors.clear() predecessors.clear() + dirtyNodes.clear() } fun render(renderer: StaticESP, config: PathingSettings) { @@ -117,31 +115,4 @@ class LazyGraph( renderer.buildOutline(Box.of(node.toCenterVec3d(), 0.2, 0.2, 0.2), Color.RED) } } - - fun buildDebugInfoRenderer(config: PathingSettings) { - successors.entries.take(config.maxRenderObjects).forEach { (v, u) -> - val mode = Matrices.ProjRotationMode.TO_CAMERA - val scale = 0.4 - if (config.renderPositions) { - val pos = v.toCenterVec3d() - val nodeProjection = buildWorldProjection(pos, scale, mode) - withVertexTransform(nodeProjection) { - val msg = v.string - drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) - } - } - if (config.renderCost) { - u.forEach { (neighbor, cost) -> - val centerV = v.toCenterVec3d() - val centerN = neighbor.toCenterVec3d() - val center = (centerV + centerN) / 2.0 - val projection = buildWorldProjection(center, scale, mode) - withVertexTransform(projection) { - val msg = "c: %.3f".format(cost) - drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) - } - } - } - } - } -} +} \ No newline at end of file diff --git a/common/src/test/kotlin/DStarLiteTest.kt b/common/src/test/kotlin/DStarLiteTest.kt index 5912b377e..f1fdc0bf4 100644 --- a/common/src/test/kotlin/DStarLiteTest.kt +++ b/common/src/test/kotlin/DStarLiteTest.kt @@ -62,7 +62,7 @@ class DStarLiteTest { val dstar = DStarLite(graph, start, goal, ::heuristic) dstar.computeShortestPath() - val path = dstar.path + val path = dstar.path() // Manhattan distance between (0,0,0) and (2,2,2) is 6; hence the path should have 7 vertices. assertFalse(path.isEmpty(), "Path should not be empty") assertEquals(start, path.first(), "Path should start at the initial position") @@ -79,14 +79,14 @@ class DStarLiteTest { dstar.computeShortestPath() - val initialPath = dstar.path + val initialPath = dstar.path() assertTrue(initialPath.size > 1, "Initial path should have multiple steps") // Move starting position closer to the goal and recompute start = fastVectorOf(1, 1, 1) dstar.updateStart(start) - val updatedPath = dstar.path + val updatedPath = dstar.path() assertFalse(updatedPath.isEmpty(), "Updated path should not be empty now from new start") assertEquals(start, updatedPath.first(), "Updated path must start at updated position") assertEquals(goal, updatedPath.last(), "Updated path must end at the goal") From cc433cbe46df101789e2c0febd1fb9e3da8fff20 Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 16 Apr 2025 00:09:58 +0200 Subject: [PATCH 23/39] Vertex invalidation approach --- .../lambda/command/commands/PathCommand.kt | 9 +++--- .../com/lambda/pathing/dstar/DStarLite.kt | 30 ++++++++++++------- .../com/lambda/pathing/dstar/LazyGraph.kt | 25 +++++++--------- common/src/test/kotlin/DStarLiteTest.kt | 23 +++++++++++++- 4 files changed, 55 insertions(+), 32 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt index 21e9dff7c..d5b2dfcae 100644 --- a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt +++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt @@ -31,19 +31,18 @@ import com.lambda.util.world.string object PathCommand : LambdaCommand( name = "path", - usage = "path ", + usage = "path ", description = "Move through world" ) { override fun CommandBuilder.create() { - required(literal("markDirty")) { + required(literal("invalidate")) { required(integer("X", -30000000, 30000000)) { x -> required(integer("Y", -64, 255)) { y -> required(integer("Z", -30000000, 30000000)) { z -> execute { val dirty = fastVectorOf(x().value(), y().value(), z().value()) - Pathfinder.graph.markDirty(dirty) -// Pathfinder.dStar.updateGraph() - this@PathCommand.info("Marked ${dirty.string} as dirty") + Pathfinder.dStar.invalidate(dirty) + this@PathCommand.info("Invalidated ${dirty.string}") } } } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 5e6a7ba34..432f2e74c 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -77,10 +77,10 @@ class DStarLite( U.insertOrUpdate(goal, calculateKey(goal)) } - private fun g(u: FastVector): Double = gMap[u] ?: INF + fun g(u: FastVector): Double = gMap[u] ?: INF private fun setG(u: FastVector, g: Double) { gMap[u] = g } - private fun rhs(u: FastVector): Double = rhsMap[u] ?: INF + fun rhs(u: FastVector): Double = rhsMap[u] ?: INF private fun setRHS(u: FastVector, rhs: Double) { rhsMap[u] = rhs } /** @@ -100,7 +100,7 @@ class DStarLite( * * Then u is removed from the queue and reinserted if it is inconsistent. */ - fun updateVertex(u: FastVector) { + private fun updateVertex(u: FastVector) { if (u != goal) { var tmp = INF graph.predecessors(u).forEach { (pred, cost) -> @@ -117,15 +117,12 @@ class DStarLite( } } - fun updateGraph() { - graph.dirtyNodes.forEach { u -> - if (u != goal) { - gMap.remove(u) - rhsMap.remove(u) - } - updateVertex(u) + fun invalidate(u: FastVector) { + val affectedNodes = graph.getNeighbors(u) + u + affectedNodes.forEach { v -> + graph.invalidate(v) + updateVertex(v) } - graph.dirtyNodes.clear() } /** @@ -265,6 +262,17 @@ class DStarLite( } } + override fun toString() = buildString { + appendLine("Nodes:") + graph.nodes.forEach { appendLine(" ${it.string} g: ${g(it)} rhs: ${rhs(it)}") } + appendLine("Edges:") + graph.nodes.forEach { u -> + graph.successors(u).forEach { (v, cost) -> + appendLine(" ${u.string} -> ${v.string}: c: $cost") + } + } + } + companion object { private const val INF = Double.POSITIVE_INFINITY } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 9b8c21abd..79bb9b586 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -75,23 +75,18 @@ class LazyGraph( return predecessors[u] ?: emptyMap() } - fun remove(u: FastVector) { - successors.remove(u) - successors.values.forEach { it.remove(u) } - predecessors.remove(u) - predecessors.values.forEach { it.remove(u) } + fun invalidate(u: FastVector) { + val neighbors = getNeighbors(u) + (neighbors + u).forEach { v -> + successors.remove(v) + predecessors.remove(v) + predecessors.values.forEach { predMap -> + predMap.remove(v) + } + } } - fun markDirty(u: FastVector) { - dirtyNodes.add(u) - val preds = predecessors[u]?.keys ?: emptySet() - val succs = successors[u]?.keys ?: emptySet() - remove(u) - preds.forEach { remove(it) } - succs.forEach { remove(it) } - dirtyNodes.addAll(preds) - dirtyNodes.addAll(succs) - } + fun getNeighbors(u: FastVector): Set = successors(u).keys + predecessors(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 diff --git a/common/src/test/kotlin/DStarLiteTest.kt b/common/src/test/kotlin/DStarLiteTest.kt index f1fdc0bf4..4598411b1 100644 --- a/common/src/test/kotlin/DStarLiteTest.kt +++ b/common/src/test/kotlin/DStarLiteTest.kt @@ -2,6 +2,8 @@ import com.lambda.pathing.dstar.DStarLite import com.lambda.pathing.dstar.LazyGraph import com.lambda.util.world.FastVector import com.lambda.util.world.fastVectorOf +import com.lambda.util.world.string +import com.lambda.util.world.toBlockPos import com.lambda.util.world.x import com.lambda.util.world.y import com.lambda.util.world.z @@ -71,7 +73,7 @@ class DStarLiteTest { } @Test - fun testUpdateStart3D() { + fun testUpdateStart() { val graph = createLazy3DGridGraph() var start = fastVectorOf(0, 0, 0) val goal = fastVectorOf(2, 2, 2) @@ -91,4 +93,23 @@ class DStarLiteTest { assertEquals(start, updatedPath.first(), "Updated path must start at updated position") assertEquals(goal, updatedPath.last(), "Updated path must end at the goal") } + + @Test + fun testInvalidateVertex() { + val graph = createLazy3DGridGraph() + val start = fastVectorOf(0, 0, 0) + val goal = fastVectorOf(2, 2, 2) + val dstar = DStarLite(graph, start, goal, ::heuristic) + fun List.string() = joinToString(" -> ") { "[${it.string} g=${dstar.g(it)} rhs=${dstar.rhs(it)}]" } + + println("Computing shortest path from ${start.string} to ${goal.string}") + dstar.computeShortestPath() + println(dstar.path().string()) + val invalidate = fastVectorOf(1, 0, 0) + println("Invalidating vertex ${invalidate.string}") + dstar.invalidate(invalidate) + println("Computing shortest path from ${start.string} to ${goal.string}") + dstar.computeShortestPath() + println(dstar.path().string()) + } } From 44c64eccff48c75235f39edb46420e1089202d8a Mon Sep 17 00:00:00 2001 From: Constructor Date: Sun, 20 Apr 2025 23:46:04 +0200 Subject: [PATCH 24/39] Incremental pathfinding with updateable weights --- .../lambda/command/commands/PathCommand.kt | 113 +++++- .../com/lambda/module/hud/PathfinderHUD.kt | 29 ++ .../module/modules/movement/Pathfinder.kt | 12 +- .../com/lambda/pathing/PathingConfig.kt | 4 + .../com/lambda/pathing/PathingSettings.kt | 4 + .../com/lambda/pathing/dstar/DStarLite.kt | 324 +++++++++++------- .../kotlin/com/lambda/pathing/dstar/Key.kt | 27 +- .../com/lambda/pathing/dstar/LazyGraph.kt | 65 ++-- .../pathing/dstar/PriorityQueueDStar.kt | 59 ---- .../pathing/dstar/UpdatablePriorityQueue.kt | 172 ++++++++++ common/src/test/kotlin/DStarLiteTest.kt | 115 ------- .../src/test/kotlin/pathing/DStarLiteTest.kt | 272 +++++++++++++++ common/src/test/kotlin/pathing/KeyTest.kt | 81 +++++ .../pathing/UpdatablePriorityQueueTest.kt | 275 +++++++++++++++ 14 files changed, 1199 insertions(+), 353 deletions(-) create mode 100644 common/src/main/kotlin/com/lambda/module/hud/PathfinderHUD.kt delete mode 100644 common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt create mode 100644 common/src/main/kotlin/com/lambda/pathing/dstar/UpdatablePriorityQueue.kt delete mode 100644 common/src/test/kotlin/DStarLiteTest.kt create mode 100644 common/src/test/kotlin/pathing/DStarLiteTest.kt create mode 100644 common/src/test/kotlin/pathing/KeyTest.kt create mode 100644 common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt diff --git a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt index d5b2dfcae..d63c08594 100644 --- a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt +++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt @@ -17,6 +17,7 @@ package com.lambda.command.commands +import com.lambda.brigadier.argument.double import com.lambda.brigadier.argument.integer import com.lambda.brigadier.argument.literal import com.lambda.brigadier.argument.value @@ -30,36 +31,130 @@ import com.lambda.util.world.fastVectorOf import com.lambda.util.world.string object PathCommand : LambdaCommand( - name = "path", + name = "pathfinder", usage = "path ", - description = "Move through world" + 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 -> execute { - val dirty = fastVectorOf(x().value(), y().value(), z().value()) - Pathfinder.dStar.invalidate(dirty) - this@PathCommand.info("Invalidated ${dirty.string}") + val v = fastVectorOf(x().value(), y().value(), z().value()) + Pathfinder.dStar.invalidate(v) + 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.remove(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("reset")) { + 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]?.keys?.joinToString { it.string }}") + } + } + } + } + } + + 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]?.keys?.joinToString { it.string }}") + } + } + } + } + } + + 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("Reset graph") + this@PathCommand.info("Cleared graph") } } - required(literal("update")) { + required(literal("refresh")) { execute { Pathfinder.needsUpdate = true - this@PathCommand.info("Marked graph for update") + this@PathCommand.info("Marked pathfinder for refresh") } } } 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 index 6d1820091..44a382adf 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -53,6 +53,7 @@ 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 @@ -73,10 +74,9 @@ object Pathfinder : Module( description = "Get from A to B", defaultTags = setOf(ModuleTag.MOVEMENT) ) { - private val targetPos by setting("Target", BlockPos(0, 78, 0)) private val pathing = PathingSettings(this) - private val target: FastVector get() = targetPos.toFastVec() + var target = fastVectorOf(0, 78, 0) val graph = LazyGraph { origin -> runSafe { moveOptions(origin, ::heuristic, pathing).associate { it.pos to it.cost } @@ -269,4 +269,12 @@ object Pathfinder : Module( .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/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index b81eab532..24e008f44 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -52,10 +52,14 @@ interface PathingConfig { 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 maxRenderObjects: Int val fontScale: Double val assumeJesus: Boolean diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 9d29fa163..2926c8100 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -61,10 +61,14 @@ class PathingSettings( 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 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 } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 432f2e74c..de2b9c8cf 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -22,10 +22,13 @@ 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.module.modules.movement.Pathfinder import com.lambda.pathing.PathingSettings import com.lambda.util.math.Vec2d import com.lambda.util.math.div +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 @@ -34,134 +37,93 @@ import kotlin.math.min /** * Lazy D* Lite Implementation. * - * We perform a backward search from the goal to the start, so: - * - rhs(goal) = 0, g(goal) = ∞ - * - "start" is the agent's current location from which we want a path *to* the goal - * - 'km' accumulates the heuristic shift so we don't reorder the entire queue after each move - * - * @param graph The graph on which we plan (with forward + reverse adjacency). - * @param heuristic A consistent (or at least nonnegative) heuristic function h(a,b). - * @param start The agent's current position. + * @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, - private val goal: FastVector, - private val heuristic: (FastVector, FastVector) -> Double, + val goal: FastVector, + val heuristic: (FastVector, FastVector) -> Double, ) { // 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. - private val U = PriorityQueueDStar() + val U = UpdatablePriorityQueue() // km accumulates heuristic differences as the start changes. - private var km = 0.0 + var km = 0.0 init { initialize() } - /** - * Initialize D* Lite: - * - g(goal)=∞, rhs(goal)=0 - * - Insert goal into U with key = calculateKey(goal). - */ + /** Re-initialize the algorithm. */ fun initialize() { + U.clear() + km = 0.0 gMap.clear() rhsMap.clear() - setG(goal, INF) - setRHS(goal, 0.0) - U.insertOrUpdate(goal, calculateKey(goal)) - } - - fun g(u: FastVector): Double = gMap[u] ?: INF - private fun setG(u: FastVector, g: Double) { gMap[u] = g } - - fun rhs(u: FastVector): Double = rhsMap[u] ?: INF - private fun setRHS(u: FastVector, rhs: Double) { rhsMap[u] = rhs } + graph.clear() - /** - * Calculates the key for vertex u. - * Key(u) = ( min(g(u), rhs(u)) + h(start, u) + km, min(g(u), rhs(u)) ) - */ - private fun calculateKey(u: FastVector): Key { - val minGRHS = min(g(u), rhs(u)) - return Key(minGRHS + heuristic(start, u) + km, minGRHS) + setRHS(goal, 0.0) + U.insert(goal, Key(heuristic(start, goal), 0.0)) } /** - * Updates the vertex u. - * - * If u != goal, then: - * rhs(u) = min_{v in Pred(u)} [g(v) + cost(v,u)] + * 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. * - * Then u is removed from the queue and reinserted if it is inconsistent. - */ - private fun updateVertex(u: FastVector) { - if (u != goal) { - var tmp = INF - graph.predecessors(u).forEach { (pred, cost) -> - val candidate = g(pred) + cost - if (candidate < tmp) { - tmp = candidate - } - } - setRHS(u, tmp) - } - U.remove(u) - if (g(u) != rhs(u)) { - U.insertOrUpdate(u, calculateKey(u)) - } - } - - fun invalidate(u: FastVector) { - val affectedNodes = graph.getNeighbors(u) + u - affectedNodes.forEach { v -> - graph.invalidate(v) - updateVertex(v) - } - } - - /** - * Propagates changes until the start is locally consistent. - * computeShortestPath(): - * While the queue top is "less" than calculateKey(start) - * or g(start) < rhs(start), pop and process. + * @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 shouldUpdateState() = U.topKey() < calculateKey(start) || g(start) < rhs(start) - while (shouldUpdateState() && !timedOut()) { - val u = U.top() ?: break - val oldKey = U.topKey() - val newKey = calculateKey(u) + // ToDo: Check why <= needed and not < + fun checkCondition() = U.topKey(Key.INFINITY) <= calculateKey(start) || rhs(start) > g(start) - if (oldKey < newKey) { - // Priority out-of-date; update it - U.insertOrUpdate(u, newKey) - } else { - U.pop() - if (g(u) > rhs(u)) { - // We found a better path for u - setG(u, rhs(u)) - graph.successors(u).forEach { (succ, _) -> - updateVertex(succ) + while (!U.isEmpty() && 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.successors(u).forEach { (s, c) -> + if (s != goal) { + setRHS(s, min(rhs(s), c + g(u))) + } + updateVertex(s) } - } else { - val oldG = g(u) + } + // 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) - updateVertex(u) - // Update successors that may have relied on old g(u) - graph.successors(u).forEach { (succ, _) -> - if (rhs(succ) == oldG + graph.cost(u, succ)) { - updateVertex(succ) + + (graph.successors(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 } } } @@ -169,61 +131,165 @@ class DStarLite( } /** - * When the agent moves, update the start. - * The variable km is increased by h(oldStart, newStart) and - * all vertices in the queue are re-keyed. + * 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 oldStart = start + val lastStart = start start = newStart - km += heuristic(oldStart, start) - val tmpList = mutableListOf() - while (!U.isEmpty()) { - U.top()?.let { tmpList.add(it) } - U.pop() + km += heuristic(lastStart, start) + } + + /** + * Invalidates a node (e.g., it became an obstacle) and updates affected neighbors. + * Call `computeShortestPath()` afterwards. + */ + fun invalidate(u: FastVector) { + // 1. Invalidate node in graph and get its direct valid neighbors + // This also clears the neighbors' cached successors, forcing re-initialization. + val neighborsToUpdate = graph.invalidate(u) + + // 2. Update the rhs value for all affected neighbors. + // Since their edge information might change upon re-initialization, + // the safest approach is to recompute their rhs from scratch. + neighborsToUpdate.forEach { neighbor -> + if (neighbor != goal) { + // Recompute rhs based on its *potentially new* set of successors + // Note: minSuccessorCost will trigger re-initialization + setRHS(neighbor, minSuccessorCost(neighbor)) + } + // Check consistency and update queue status for the neighbor + updateVertex(neighbor) } - tmpList.forEach { v -> - U.insertOrUpdate(v, calculateKey(v)) + + // 3. Update the invalidated node itself (likely sets g=INF, rhs=INF) + if (u != goal) { + setRHS(u, minSuccessorCost(u)) // Should return INF } + // Ensure g is INF and update queue status + setG(u, INF) + updateVertex(u) + } + + /** + * 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) + if (cOld > c) { + if (u != goal) setRHS(u, min(rhs(u), c + g(v))) + } else if (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 + cost value. If no path is found, the path stops early. + * with the lowest `g(successor) + cost(current, successor)` value. + * If no path is found (INF cost), the path stops early. */ fun path(maxLength: Int = 10_000): List { val path = mutableListOf() - - if (!graph.contains(start)) return path.toList() + if (start !in graph) return path.toList() // Start not even known var current = start path.add(current) - while (current != goal) { + + var iterations = 0 + while (current != goal && iterations < maxLength) { + iterations++ val successors = graph.successors(current) - if (successors.isEmpty()) break + if (successors.isEmpty()) break // Dead end + var bestNext: FastVector? = null - var bestVal = INF + var minCost = INF + + // Find successor s' that minimizes c(current, s') + g(s') for ((succ, cost) in successors) { - val candidate = g(succ) + cost - if (candidate < bestVal) { - bestVal = candidate + if (cost == INF) continue // Skip impassable edges explicitly + val costPlusG = cost + g(succ) + if (costPlusG < minCost) { + minCost = costPlusG bestNext = succ } } - // No path - if (bestNext == null) break + + if (bestNext == null) break // No path found + current = bestNext - if (current !in path) { + if (current !in path) { // Avoid trivial cycles path.add(current) } else { - break + break // Cycle detected } - if (path.size > maxLength) 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 if (minGRHS == INF) { + Key.INFINITY + } else { + 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 + val key = calculateKey(u) + + when { + // Inconsistent and in Queue: Update priority + g(u) != rhs(u) && uInQueue -> { + U.update(u, key) + } + // Inconsistent and not in Queue: Insert + g(u) != rhs(u) && !uInQueue -> { + U.insert(u, key) + } + // 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) + .mapNotNull { (s1, cost) -> + if (cost == INF) null else cost + g(s1) + } + .minOrNull() ?: INF + fun buildDebugInfoRenderer(config: PathingSettings) { if (!config.renderGraph) return val mode = Matrices.ProjRotationMode.TO_CAMERA @@ -233,6 +299,8 @@ class DStarLite( 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 (origin in U) label.add("IN QUEUE") if (label.isNotEmpty()) { val pos = origin.toCenterVec3d() @@ -251,10 +319,10 @@ class DStarLite( graph.successors[origin]?.forEach { (neighbor, cost) -> val centerO = origin.toCenterVec3d() val centerN = neighbor.toCenterVec3d() - val center = (centerO + centerN) / 2.0 + val center = centerO + (centerN - centerO) * (1.0 / 3.0) val projection = buildWorldProjection(center, scale, mode) withVertexTransform(projection) { - val msg = "c: %.3f".format(cost) + val msg = "sc: %.3f".format(cost) drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0)) } } @@ -263,14 +331,18 @@ class DStarLite( } override fun toString() = buildString { - appendLine("Nodes:") - graph.nodes.forEach { appendLine(" ${it.string} g: ${g(it)} rhs: ${rhs(it)}") } - appendLine("Edges:") - graph.nodes.forEach { u -> - graph.successors(u).forEach { (v, cost) -> - appendLine(" ${u.string} -> ${v.string}: c: $cost") - } + 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}, Invalidated: ${graph.invalidated.size}") +// appendLine("Known Nodes (${graph.nodes.size}):") +// graph.nodes.take(50).forEach { +// appendLine(" ${it.string} g: ${g(it)}, rhs: ${rhs(it)}, key: ${calculateKeyInternal(it)}") +// } +// if (graph.nodes.size > 50) appendLine(" ... (more nodes)") } companion object { diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt index 188988773..04682a24c 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt @@ -18,16 +18,23 @@ package com.lambda.pathing.dstar /** - * A Key is a pair (k1, k2) that is used to order vertices in the priority queue. - * They are compared lexicographically. + * 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*[cite: 142]. */ -data class Key(val k1: Double, val k2: Double) : Comparable { - override fun compareTo(other: Key) = - when { - this.k1 < other.k1 -> -1 - this.k1 > other.k1 -> 1 - this.k2 < other.k2 -> -1 - this.k2 > other.k2 -> 1 - else -> 0 +data class Key(val first: Double, val second: Double) : Comparable { + override fun compareTo(other: Key): Int { + val firstCompare = this.first.compareTo(other.first) + if (firstCompare != 0) { + return firstCompare } + return this.second.compareTo(other.second) + } + + override fun toString() = "(%.3f, %.3f)".format(first, second) + + companion object { + // Represents an infinite key + 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/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 79bb9b586..668ef4a53 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -17,20 +17,11 @@ package com.lambda.pathing.dstar -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.esp.builders.buildLine import com.lambda.graphics.renderer.esp.builders.buildOutline import com.lambda.graphics.renderer.esp.global.StaticESP -import com.lambda.graphics.renderer.gui.FontRenderer -import com.lambda.graphics.renderer.gui.FontRenderer.drawString import com.lambda.pathing.PathingSettings -import com.lambda.util.math.Vec2d -import com.lambda.util.math.div -import com.lambda.util.math.plus import com.lambda.util.world.FastVector -import com.lambda.util.world.string import com.lambda.util.world.toCenterVec3d import net.minecraft.util.math.Box import java.awt.Color @@ -54,19 +45,17 @@ class LazyGraph( ) { val successors = ConcurrentHashMap>() val predecessors = ConcurrentHashMap>() - val dirtyNodes = mutableSetOf() + val invalidated = mutableSetOf() val nodes get() = successors.keys + predecessors.keys - val size get() = successors.size + val size get() = nodes.size /** Initializes a node if not already initialized, then returns successors. */ fun successors(u: FastVector): MutableMap = successors.getOrPut(u) { - val neighbors = nodeInitializer(u) - neighbors.forEach { (neighbor, cost) -> + nodeInitializer(u).onEach { (neighbor, cost) -> predecessors.getOrPut(neighbor) { hashMapOf() }[u] = cost - } - neighbors.toMutableMap() + }.toMutableMap() } /** Initializes predecessors by ensuring successors of neighboring nodes. */ @@ -75,38 +64,50 @@ class LazyGraph( return predecessors[u] ?: emptyMap() } - fun invalidate(u: FastVector) { - val neighbors = getNeighbors(u) - (neighbors + u).forEach { v -> - successors.remove(v) - predecessors.remove(v) - predecessors.values.forEach { predMap -> - predMap.remove(v) - } - } + fun remove(u: FastVector) { + successors.remove(u) + successors.values.forEach { it.remove(u) } + predecessors.remove(u) + predecessors.values.forEach { it.remove(u) } } - fun getNeighbors(u: FastVector): Set = successors(u).keys + predecessors(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 + fun setCost(u: FastVector, v: FastVector, c: Double) { + successors[u]?.put(v, c) + predecessors[v]?.put(u, c) + } - fun contains(u: FastVector): Boolean = successors.containsKey(u) + fun invalidate(u: FastVector) = + neighbors(u).apply { + forEach { remove(it) } + invalidated.addAll(this) + } fun clear() { successors.clear() predecessors.clear() - dirtyNodes.clear() + invalidated.clear() } + fun neighbors(u: FastVector): Set = successors(u).keys + predecessors(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) + fun render(renderer: StaticESP, config: PathingSettings) { if (!config.renderGraph) return - successors.entries.take(config.maxRenderObjects).forEach { (origin, neighbors) -> + 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) } } - dirtyNodes.take(config.maxRenderObjects).forEach { node -> + if (config.renderInvalidated) invalidated.take(config.maxRenderObjects).forEach { node -> renderer.buildOutline(Box.of(node.toCenterVec3d(), 0.2, 0.2, 0.2), Color.RED) } } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt deleted file mode 100644 index 173a58499..000000000 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/PriorityQueueDStar.kt +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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.dstar - -import java.util.* - -/** - * Priority queue for D* Lite 3D. - */ -class PriorityQueueDStar { - private val pq = PriorityQueue>(compareBy { it.second }) - private val vertexToKey = mutableMapOf() - - fun isEmpty() = pq.isEmpty() - - fun topKey(): Key { - return if (pq.isEmpty()) Key(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY) - else pq.peek().second - } - - fun top(): T? = pq.peek()?.first - - fun pop(): T? { - if (pq.isEmpty()) return null - val (v, _) = pq.poll() - vertexToKey.remove(v) - return v - } - - fun insertOrUpdate(v: T, key: Key) { - val oldKey = vertexToKey[v] - if (oldKey == null || oldKey != key) { - remove(v) - vertexToKey[v] = key - pq.add(Pair(v, key)) - } - } - - fun remove(v: T) { - val oldKey = vertexToKey[v] ?: return - vertexToKey.remove(v) - pq.remove(Pair(v, oldKey)) - } -} \ No newline at end of file diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/UpdatablePriorityQueue.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/UpdatablePriorityQueue.kt new file mode 100644 index 000000000..e8d3a540d --- /dev/null +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/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.dstar + +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/test/kotlin/DStarLiteTest.kt b/common/src/test/kotlin/DStarLiteTest.kt deleted file mode 100644 index 4598411b1..000000000 --- a/common/src/test/kotlin/DStarLiteTest.kt +++ /dev/null @@ -1,115 +0,0 @@ -import com.lambda.pathing.dstar.DStarLite -import com.lambda.pathing.dstar.LazyGraph -import com.lambda.util.world.FastVector -import com.lambda.util.world.fastVectorOf -import com.lambda.util.world.string -import com.lambda.util.world.toBlockPos -import com.lambda.util.world.x -import com.lambda.util.world.y -import com.lambda.util.world.z -import kotlin.math.abs -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 . - */ - -class DStarLiteTest { - - /** Helper to create a lazy-initialized 3D grid graph with 6-connected neighbors */ - private fun createLazy3DGridGraph() = - LazyGraph { node -> - val neighbors = mutableMapOf() - val (x, y, z) = listOf(node.x, node.y, node.z) - - // possible directions in a 3D grid: 6-connected neighbors - val directions = listOf( - fastVectorOf(x - 1, y, z), fastVectorOf(x + 1, y, z), - fastVectorOf(x, y - 1, z), fastVectorOf(x, y + 1, z), - fastVectorOf(x, y, z - 1), fastVectorOf(x, y, z + 1) - ) - - directions.forEach { v -> - neighbors[v] = 1.0 - } - - neighbors - } - - /** Manhattan distance heuristic for 3D grids */ - private fun heuristic(u: FastVector, v: FastVector): Double = - (abs(u.x - v.x) + abs(u.y - v.y) + abs(u.z - v.z)).toDouble() - - @Test - fun test3x3x3Grid() { - val graph = createLazy3DGridGraph() - val start = fastVectorOf(0, 0, 0) - val goal = fastVectorOf(2, 2, 2) - val dstar = DStarLite(graph, start, goal, ::heuristic) - - dstar.computeShortestPath() - val path = dstar.path() - // Manhattan distance between (0,0,0) and (2,2,2) is 6; hence the path should have 7 vertices. - assertFalse(path.isEmpty(), "Path should not be empty") - assertEquals(start, path.first(), "Path should start at the initial position") - assertEquals(goal, path.last(), "Path should end at the goal position") - assertEquals(7, path.size, "Expected path length is 7 vertices (6 steps)") - } - - @Test - fun testUpdateStart() { - val graph = createLazy3DGridGraph() - var start = fastVectorOf(0, 0, 0) - val goal = fastVectorOf(2, 2, 2) - val dstar = DStarLite(graph, start, goal, ::heuristic) - - dstar.computeShortestPath() - - val initialPath = dstar.path() - assertTrue(initialPath.size > 1, "Initial path should have multiple steps") - - // Move starting position closer to the goal and recompute - start = fastVectorOf(1, 1, 1) - dstar.updateStart(start) - - val updatedPath = dstar.path() - assertFalse(updatedPath.isEmpty(), "Updated path should not be empty now from new start") - assertEquals(start, updatedPath.first(), "Updated path must start at updated position") - assertEquals(goal, updatedPath.last(), "Updated path must end at the goal") - } - - @Test - fun testInvalidateVertex() { - val graph = createLazy3DGridGraph() - val start = fastVectorOf(0, 0, 0) - val goal = fastVectorOf(2, 2, 2) - val dstar = DStarLite(graph, start, goal, ::heuristic) - fun List.string() = joinToString(" -> ") { "[${it.string} g=${dstar.g(it)} rhs=${dstar.rhs(it)}]" } - - println("Computing shortest path from ${start.string} to ${goal.string}") - dstar.computeShortestPath() - println(dstar.path().string()) - val invalidate = fastVectorOf(1, 0, 0) - println("Invalidating vertex ${invalidate.string}") - dstar.invalidate(invalidate) - println("Computing shortest path from ${start.string} to ${goal.string}") - dstar.computeShortestPath() - println(dstar.path().string()) - } -} diff --git a/common/src/test/kotlin/pathing/DStarLiteTest.kt b/common/src/test/kotlin/pathing/DStarLiteTest.kt new file mode 100644 index 000000000..ccc33f545 --- /dev/null +++ b/common/src/test/kotlin/pathing/DStarLiteTest.kt @@ -0,0 +1,272 @@ +/* + * 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.dstar.DStarLite +import com.lambda.pathing.dstar.Key +import com.lambda.pathing.dstar.LazyGraph +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.abs +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 val blockedNodes = mutableSetOf() + + // Simple Manhattan distance heuristic + private 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) + private 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) + private fun createGridGraph6Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { + val cost = 1.0 + return LazyGraph { node -> + val neighbors = mutableMapOf() + val x = node.x + val y = node.y + val z = node.z + // Add neighbors differing by 1 in exactly one dimension + neighbors[fastVectorOf(x + 1, y, z)] = cost + neighbors[fastVectorOf(x - 1, y, z)] = cost + neighbors[fastVectorOf(x, y + 1, z)] = cost + neighbors[fastVectorOf(x, y - 1, z)] = cost + neighbors[fastVectorOf(x, y, z + 1)] = cost + neighbors[fastVectorOf(x, y, z - 1)] = cost + neighbors.minus(blockedNodes) + } + } + + // 18-connectivity (Axis-aligned + Face diagonal moves) + private fun createGridGraph18Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { + val cost1 = 1.0 // Axis-aligned + val cost2 = sqrt(2.0) // Face diagonal + return LazyGraph { node -> + val neighbors = mutableMapOf() + val x = node.x + val y = node.y + val z = node.z + for (dx in -1..1) { + for (dy in -1..1) { + for (dz in -1..1) { + if (dx == 0 && dy == 0 && dz == 0) continue // Skip self + val distSq = dx*dx + dy*dy + dz*dz + if (distSq > 2) continue // Exclude cube diagonals (distSq = 3) + + val cost = if (distSq == 1) cost1 else cost2 + neighbors[fastVectorOf(x + dx, y + dy, z + dz)] = cost + } + } + } + neighbors.minus(blockedNodes) + } + } + + // 26-connectivity (Axis-aligned + Face diagonal + Cube diagonal moves) + private fun createGridGraph26Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { + val cost1 = 1.0 // Axis-aligned + val cost2 = sqrt(2.0) // Face diagonal + val cost3 = sqrt(3.0) // Cube diagonal + return LazyGraph { node -> + val neighbors = mutableMapOf() + val x = node.x + val y = node.y + val z = node.z + for (dx in -1..1) { + for (dy in -1..1) { + for (dz in -1..1) { + if (dx == 0 && dy == 0 && dz == 0) continue // Skip self + + val cost = when (dx*dx + dy*dy + dz*dz) { + 1 -> cost1 + 2 -> cost2 + 3 -> cost3 + else -> continue // Should not happen with dx/dy/dz in -1..1 + } + neighbors[fastVectorOf(x + dx, y + dy, z + dz)] = cost + } + } + } + neighbors.minus(blockedNodes) + } + } + + 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 + } +} diff --git a/common/src/test/kotlin/pathing/KeyTest.kt b/common/src/test/kotlin/pathing/KeyTest.kt new file mode 100644 index 000000000..63870a84d --- /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.dstar.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/UpdatablePriorityQueueTest.kt b/common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt new file mode 100644 index 000000000..91b389da8 --- /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.dstar.Key +import com.lambda.pathing.dstar.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 From df129800c0787ca689bedd50dbbc03c122d0b3e4 Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 21 Apr 2025 17:45:45 +0200 Subject: [PATCH 25/39] Node invalidation on world state update --- .../lambda/command/commands/PathCommand.kt | 3 +- .../com/lambda/pathing/dstar/DStarLite.kt | 53 +++++++++++-------- .../com/lambda/pathing/dstar/LazyGraph.kt | 31 ++++++----- 3 files changed, 51 insertions(+), 36 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt index d63c08594..643232f58 100644 --- a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt +++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt @@ -58,6 +58,7 @@ object PathCommand : LambdaCommand( execute { val v = fastVectorOf(x().value(), y().value(), z().value()) Pathfinder.dStar.invalidate(v) + Pathfinder.needsUpdate = true this@PathCommand.info("Invalidated ${v.string}") } } @@ -71,7 +72,7 @@ object PathCommand : LambdaCommand( required(integer("Z", -30000000, 30000000)) { z -> execute { val v = fastVectorOf(x().value(), y().value(), z().value()) - Pathfinder.graph.remove(v) + Pathfinder.graph.removeNode(v) this@PathCommand.info("Removed ${v.string}") } } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index de2b9c8cf..0552a0fb2 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -30,6 +30,8 @@ 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.add +import com.lambda.util.world.fastVectorOf import com.lambda.util.world.string import com.lambda.util.world.toCenterVec3d import kotlin.math.min @@ -146,33 +148,33 @@ class DStarLite( /** * Invalidates a node (e.g., it became an obstacle) and updates affected neighbors. - * Call `computeShortestPath()` afterwards. */ fun invalidate(u: FastVector) { - // 1. Invalidate node in graph and get its direct valid neighbors - // This also clears the neighbors' cached successors, forcing re-initialization. - val neighborsToUpdate = graph.invalidate(u) - - // 2. Update the rhs value for all affected neighbors. - // Since their edge information might change upon re-initialization, - // the safest approach is to recompute their rhs from scratch. - neighborsToUpdate.forEach { neighbor -> - if (neighbor != goal) { - // Recompute rhs based on its *potentially new* set of successors - // Note: minSuccessorCost will trigger re-initialization - setRHS(neighbor, minSuccessorCost(neighbor)) - } - // Check consistency and update queue status for the neighbor - updateVertex(neighbor) + graph.neighbors(u).forEach { v -> + graph.invalidated.add(v) + updateEdge(u, v, INF) + updateEdge(v, u, INF) } - - // 3. Update the invalidated node itself (likely sets g=INF, rhs=INF) - if (u != goal) { - setRHS(u, minSuccessorCost(u)) // Should return INF + graph.invalidated.forEach { v -> + val currentConnections = graph.successors(v) + val actualConnections = graph.initialize(v) + val changedConnections = currentConnections.filter { (succ, cost) -> cost != actualConnections[succ] } + val newConnections = actualConnections.filter { (succ, _) -> succ !in currentConnections } + val removedConnections = currentConnections.filter { (succ, _) -> succ !in actualConnections } + changedConnections.forEach { (succ, cost) -> + updateEdge(v, succ, cost) + updateEdge(succ, v, cost) + } + newConnections.forEach { (succ, cost) -> + updateEdge(v, succ, cost) + updateEdge(succ, v, cost) + } + removedConnections.forEach { (succ, _) -> + updateEdge(v, succ, INF) + updateEdge(succ, v, INF) + } } - // Ensure g is INF and update queue status - setG(u, INF) - updateVertex(u) + graph.invalidated.clear() } /** @@ -184,6 +186,7 @@ class DStarLite( */ fun updateEdge(u: FastVector, v: FastVector, c: Double) { val cOld = graph.cost(u, v) + if (cOld == c) return graph.setCost(u, v, c) if (cOld > c) { if (u != goal) setRHS(u, min(rhs(u), c + g(v))) @@ -191,6 +194,10 @@ class DStarLite( if (u != goal) setRHS(u, minSuccessorCost(u)) } updateVertex(u) +// if (c == INF) { +// graph.removeEdge(u, v) +// graph.removeEdge(v, u) +// } } /** diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 668ef4a53..5ab3dcdd6 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -52,11 +52,7 @@ class LazyGraph( /** Initializes a node if not already initialized, then returns successors. */ fun successors(u: FastVector): MutableMap = - successors.getOrPut(u) { - nodeInitializer(u).onEach { (neighbor, cost) -> - predecessors.getOrPut(neighbor) { hashMapOf() }[u] = cost - }.toMutableMap() - } + successors.getOrPut(u) { initialize(u).toMutableMap() } /** Initializes predecessors by ensuring successors of neighboring nodes. */ fun predecessors(u: FastVector): Map { @@ -64,30 +60,41 @@ class LazyGraph( return predecessors[u] ?: emptyMap() } - fun remove(u: FastVector) { + fun removeNode(u: FastVector) { successors.remove(u) successors.values.forEach { it.remove(u) } predecessors.remove(u) predecessors.values.forEach { it.remove(u) } } + 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 initialize(u: FastVector) = + nodeInitializer(u).onEach { (neighbor, cost) -> + predecessors.getOrPut(neighbor) { hashMapOf() }[u] = cost + } + fun setCost(u: FastVector, v: FastVector, c: Double) { successors[u]?.put(v, c) predecessors[v]?.put(u, c) } - fun invalidate(u: FastVector) = - neighbors(u).apply { - forEach { remove(it) } - invalidated.addAll(this) - } - fun clear() { successors.clear() predecessors.clear() invalidated.clear() } + fun edges(u: FastVector) = successors(u).entries + predecessors(u).entries fun neighbors(u: FastVector): Set = successors(u).keys + predecessors(u).keys /** Returns the cost of the edge from u to v (or ∞ if none exists) */ From d1d2302b7169c80a0c60d1d002c6197eb4a2ab38 Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 21 Apr 2025 18:29:38 +0200 Subject: [PATCH 26/39] Simplify invalidation --- .../com/lambda/pathing/dstar/DStarLite.kt | 38 +++++-------------- .../com/lambda/pathing/dstar/LazyGraph.kt | 13 +++---- 2 files changed, 15 insertions(+), 36 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 0552a0fb2..1b83d688e 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -22,16 +22,12 @@ 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.module.modules.movement.Pathfinder import com.lambda.pathing.PathingSettings import com.lambda.util.math.Vec2d -import com.lambda.util.math.div 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.add -import com.lambda.util.world.fastVectorOf import com.lambda.util.world.string import com.lambda.util.world.toCenterVec3d import kotlin.math.min @@ -151,30 +147,18 @@ class DStarLite( */ fun invalidate(u: FastVector) { graph.neighbors(u).forEach { v -> - graph.invalidated.add(v) - updateEdge(u, v, INF) - updateEdge(v, u, INF) - } - graph.invalidated.forEach { v -> - val currentConnections = graph.successors(v) - val actualConnections = graph.initialize(v) - val changedConnections = currentConnections.filter { (succ, cost) -> cost != actualConnections[succ] } - val newConnections = actualConnections.filter { (succ, _) -> succ !in currentConnections } - val removedConnections = currentConnections.filter { (succ, _) -> succ !in actualConnections } - changedConnections.forEach { (succ, cost) -> - updateEdge(v, succ, cost) - updateEdge(succ, v, cost) - } - newConnections.forEach { (succ, cost) -> - updateEdge(v, succ, cost) - updateEdge(succ, v, cost) + val current = graph.successors(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) } - removedConnections.forEach { (succ, _) -> - updateEdge(v, succ, INF) - updateEdge(succ, v, INF) + updated.forEach { (w, c) -> + updateEdge(v, w, c) + updateEdge(w, v, c) } } - graph.invalidated.clear() } /** @@ -194,10 +178,6 @@ class DStarLite( if (u != goal) setRHS(u, minSuccessorCost(u)) } updateVertex(u) -// if (c == INF) { -// graph.removeEdge(u, v) -// graph.removeEdge(v, u) -// } } /** diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 5ab3dcdd6..eb7baed12 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -41,7 +41,7 @@ import java.util.concurrent.ConcurrentHashMap * - Additional memory overhead is based on the dynamically expanding hash maps. */ class LazyGraph( - private val nodeInitializer: (FastVector) -> Map + val nodeInitializer: (FastVector) -> Map ) { val successors = ConcurrentHashMap>() val predecessors = ConcurrentHashMap>() @@ -52,7 +52,11 @@ class LazyGraph( /** Initializes a node if not already initialized, then returns successors. */ fun successors(u: FastVector): MutableMap = - successors.getOrPut(u) { initialize(u).toMutableMap() } + successors.getOrPut(u) { + nodeInitializer(u).onEach { (neighbor, cost) -> + predecessors.getOrPut(neighbor) { hashMapOf() }[u] = cost + }.toMutableMap() + } /** Initializes predecessors by ensuring successors of neighboring nodes. */ fun predecessors(u: FastVector): Map { @@ -78,11 +82,6 @@ class LazyGraph( } } - fun initialize(u: FastVector) = - nodeInitializer(u).onEach { (neighbor, cost) -> - predecessors.getOrPut(neighbor) { hashMapOf() }[u] = cost - } - fun setCost(u: FastVector, v: FastVector, c: Double) { successors[u]?.put(v, c) predecessors[v]?.put(u, c) From 6d94e3cbfd6ad386d5003b4b101a449ac9d0e085 Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 21 Apr 2025 22:29:34 +0200 Subject: [PATCH 27/39] Test graph connectivity on node invalidation --- .../com/lambda/pathing/dstar/DStarLite.kt | 1 + .../src/test/kotlin/pathing/DStarLiteTest.kt | 151 +++++++++++++++++- 2 files changed, 149 insertions(+), 3 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 1b83d688e..7e2eda921 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -150,6 +150,7 @@ class DStarLite( val current = graph.successors(v) val updated = graph.nodeInitializer(v) val removed = current.filter { (w, _) -> w !in updated } + updateEdge(u, v, INF) removed.forEach { (w, _) -> updateEdge(v, w, INF) updateEdge(w, v, INF) diff --git a/common/src/test/kotlin/pathing/DStarLiteTest.kt b/common/src/test/kotlin/pathing/DStarLiteTest.kt index ccc33f545..e2bf12d5f 100644 --- a/common/src/test/kotlin/pathing/DStarLiteTest.kt +++ b/common/src/test/kotlin/pathing/DStarLiteTest.kt @@ -51,9 +51,6 @@ import kotlin.test.assertTrue */ internal class DStarLiteTest { - - private val blockedNodes = mutableSetOf() - // Simple Manhattan distance heuristic private fun manhattanHeuristic(a: FastVector, b: FastVector): Double { return (abs(a.x - b.x) + abs(a.y - b.y) + abs(a.z - b.z)).toDouble() @@ -269,4 +266,152 @@ internal class DStarLiteTest { 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") + } } From 699ed38a1e5588251a87ad6fe354b3ffc51d4564 Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 21 Apr 2025 23:31:46 +0200 Subject: [PATCH 28/39] Update rhs of newly created nodes --- .../module/modules/movement/Pathfinder.kt | 5 +- .../com/lambda/pathing/dstar/DStarLite.kt | 24 ++++++--- .../src/test/kotlin/pathing/DStarLiteTest.kt | 54 +++++++++++++++++++ 3 files changed, 75 insertions(+), 8 deletions(-) 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 index 44a382adf..bba70f246 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -241,7 +241,7 @@ object Pathfinder : Module( private fun SafeContext.updateDStar(start: FastVector, goal: SimpleGoal) { val long: Path - val dStar = measureTimeMillis { + 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) } @@ -253,10 +253,11 @@ object Pathfinder : Module( thetaStarClearance(long, pathing) } else long } - info("Lazy D* Lite (Length: ${long.length().string} Nodes: ${long.size} Graph Size: ${graph.size} T: $dStar ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)") + 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 { diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 7e2eda921..c5f46f20a 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -146,10 +146,13 @@ class DStarLite( * Invalidates a node (e.g., it became an obstacle) and updates affected neighbors. */ fun invalidate(u: FastVector) { + val newNodes = mutableSetOf() + graph.neighbors(u).forEach { v -> val current = graph.successors(v) val updated = graph.nodeInitializer(v) val removed = current.filter { (w, _) -> w !in updated } + updated.keys.filter { w -> w !in current.keys && w != u }.forEach { newNodes.add(it) } updateEdge(u, v, INF) removed.forEach { (w, _) -> updateEdge(v, w, INF) @@ -160,6 +163,14 @@ class DStarLite( updateEdge(w, v, c) } } + + // Update rhs values for all new nodes + newNodes.forEach { node -> + if (node != goal) { + setRHS(node, minSuccessorCost(node)) + updateVertex(node) + } + } } /** @@ -326,14 +337,15 @@ class DStarLite( appendLine("Top Key: ${U.topKey(Key.INFINITY)}, Top Node: ${U.top().string}") } appendLine("Graph Size: ${graph.size}, Invalidated: ${graph.invalidated.size}") -// appendLine("Known Nodes (${graph.nodes.size}):") -// graph.nodes.take(50).forEach { -// appendLine(" ${it.string} g: ${g(it)}, rhs: ${rhs(it)}, key: ${calculateKeyInternal(it)}") -// } -// if (graph.nodes.size > 50) appendLine(" ... (more nodes)") + appendLine("Known Nodes (${graph.nodes.size}):") + val show = 10 + graph.nodes.take(show).forEach { + appendLine(" ${it.string} g: ${g(it)}, rhs: ${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 } -} \ No newline at end of file +} diff --git a/common/src/test/kotlin/pathing/DStarLiteTest.kt b/common/src/test/kotlin/pathing/DStarLiteTest.kt index e2bf12d5f..bf4238c1a 100644 --- a/common/src/test/kotlin/pathing/DStarLiteTest.kt +++ b/common/src/test/kotlin/pathing/DStarLiteTest.kt @@ -414,4 +414,58 @@ internal class DStarLiteTest { // 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.size >= initialPath.size, "New path should be at least as long as the initial path") + } } From da2eedb82eca8e31d029407e428e9698fd1ed133 Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 21 Apr 2025 23:55:25 +0200 Subject: [PATCH 29/39] Clean move finder cache --- .../kotlin/com/lambda/module/modules/movement/Pathfinder.kt | 2 ++ 1 file changed, 2 insertions(+) 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 index bba70f246..ec440ff45 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -39,6 +39,7 @@ import com.lambda.pathing.PathingSettings import com.lambda.pathing.dstar.DStarLite import com.lambda.pathing.dstar.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 @@ -112,6 +113,7 @@ object Pathfinder : Module( } onDisable { + MoveFinder.clean() graph.clear() } From 5257bc8c855d6945b4cf64d2838ab55cd4b72b73 Mon Sep 17 00:00:00 2001 From: Constructor Date: Tue, 22 Apr 2025 04:34:48 +0200 Subject: [PATCH 30/39] Graph pruning on invalidation, graph consistency checks --- .../lambda/command/commands/PathCommand.kt | 19 +- .../com/lambda/pathing/dstar/DStarLite.kt | 185 ++++++++++- .../com/lambda/pathing/dstar/LazyGraph.kt | 133 +++++++- .../main/kotlin/com/lambda/util/GraphUtil.kt | 115 +++++++ .../src/test/kotlin/pathing/DStarLiteTest.kt | 91 +----- .../kotlin/pathing/GraphMaintenanceTest.kt | 304 ++++++++++++++++++ 6 files changed, 738 insertions(+), 109 deletions(-) create mode 100644 common/src/main/kotlin/com/lambda/util/GraphUtil.kt create mode 100644 common/src/test/kotlin/pathing/GraphMaintenanceTest.kt diff --git a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt index 643232f58..0902d200e 100644 --- a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt +++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt @@ -17,11 +17,13 @@ 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 @@ -55,11 +57,16 @@ object PathCommand : LambdaCommand( 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.dStar.invalidate(v) - Pathfinder.needsUpdate = true - this@PathCommand.info("Invalidated ${v.string}") + optional(boolean("prune")) { prune -> + execute { + val v = fastVectorOf(x().value(), y().value(), z().value()) + val pruneGraph = if (prune != null) { + prune().value() + } else true + Pathfinder.dStar.invalidate(v, pruneGraph = pruneGraph) + Pathfinder.needsUpdate = true + this@PathCommand.info("Invalidated ${v.string}") + } } } } @@ -159,4 +166,4 @@ object PathCommand : LambdaCommand( } } } -} \ No newline at end of file +} diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index c5f46f20a..d189f29ec 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -30,6 +30,7 @@ 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 /** @@ -144,33 +145,139 @@ class DStarLite( /** * Invalidates a node (e.g., it became an obstacle) 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 + * @param pruneGraph Whether to prune the graph after invalidation */ - fun invalidate(u: FastVector) { + fun invalidate(u: FastVector, pruneGraph: Boolean = false) { val newNodes = mutableSetOf() + val affectedNeighbors = mutableSetOf() + val pathNodes = mutableSetOf() + val modifiedNodes = mutableSetOf() - graph.neighbors(u).forEach { v -> + // Add the invalidated node to the modified nodes + modifiedNodes.add(u) + + // First, collect all neighbors of the invalidated node + val neighbors = graph.neighbors(u) + affectedNeighbors.addAll(neighbors) + modifiedNodes.addAll(neighbors) + + // Set g and rhs values of the invalidated node to infinity + setG(u, INF) + setRHS(u, INF) + updateVertex(u) + + // Update edges between the invalidated node and its neighbors + neighbors.forEach { v -> val current = graph.successors(v) val updated = graph.nodeInitializer(v) val removed = current.filter { (w, _) -> w !in updated } - updated.keys.filter { w -> w !in current.keys && w != u }.forEach { newNodes.add(it) } + + // Only add new nodes that are directly connected to the current path + // This reduces unnecessary node generation + updated.keys.filter { w -> w !in current.keys && w != u }.forEach { + // Check if this node is likely to be on a new path + if (g(v) < INF) { + newNodes.add(it) + modifiedNodes.add(it) + } + } + + // Set the edge cost between u and v to infinity (blocked) updateEdge(u, v, INF) + updateEdge(v, u, INF) + + // Update removed and new edges for this neighbor removed.forEach { (w, _) -> updateEdge(v, w, INF) updateEdge(w, v, INF) + modifiedNodes.add(w) } updated.forEach { (w, c) -> updateEdge(v, w, c) updateEdge(w, v, c) + modifiedNodes.add(w) + } + } + + // Now, update only the neighbors of neighbors that are likely to be on the new path + // This is crucial when a node in a diagonal path is blocked + neighbors.forEach { v -> + // Only process neighbors that are likely to be on the path + if (g(v) >= INF) return@forEach + pathNodes.add(v) + + // Get all neighbors of this neighbor (excluding the original invalidated node) + // Only consider neighbors that are likely to be on the path + val secondaryNeighbors = graph.neighbors(v).filter { it != u && g(it) < INF } + + // For each secondary neighbor, reinitialize its edges + secondaryNeighbors.forEach { w -> + // Add to affected neighbors for later rhs update + affectedNeighbors.add(w) + pathNodes.add(w) + modifiedNodes.add(w) + + // Reinitialize edges for this secondary neighbor + val currentW = graph.successors(w) + val updatedW = graph.nodeInitializer(w) + + // Update edges for this secondary neighbor + // Only update edges to nodes that are likely to be on the path + updatedW.forEach { (z, c) -> + if (z != u) { // Don't create edges to the invalidated node + updateEdge(w, z, c) + updateEdge(z, w, c) + modifiedNodes.add(z) + + // If this node has a finite g-value, it's likely on the path + if (g(z) < INF) { + pathNodes.add(z) + } + } + } + + // Add any new nodes discovered, but only if they're likely to be on the path + updatedW.keys.filter { z -> z !in currentW.keys && z != u }.forEach { + // Check if this node is connected to a node on the path + if (g(w) < INF) { + newNodes.add(it) + modifiedNodes.add(it) + } + } + } + } + + // Ensure all edges to/from the invalidated node are set to infinity + // First, get all current successors and predecessors of the invalidated node + val currentSuccessors = graph.successors(u).keys.toSet() + val currentPredecessors = graph.predecessors(u).keys.toSet() + + // Set all edges to/from the invalidated node to infinity + (currentSuccessors + currentPredecessors + graph.nodes).forEach { node -> + if (node != u) { + updateEdge(node, u, INF) + updateEdge(u, node, INF) + modifiedNodes.add(node) } } - // Update rhs values for all new nodes - newNodes.forEach { node -> + // Update rhs values for all affected nodes + (affectedNeighbors + newNodes).forEach { node -> if (node != goal) { setRHS(node, minSuccessorCost(node)) updateVertex(node) } } + + // Prune the graph if requested + if (pruneGraph) { + // Prune the graph, passing the modified nodes for targeted pruning + graph.prune(modifiedNodes) + } } /** @@ -196,6 +303,9 @@ class DStarLite( * 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() @@ -329,6 +439,65 @@ class DStarLite( } } + /** + * Verifies that the current graph is consistent with a freshly generated graph. + * This is useful for ensuring that incremental updates maintain correctness. + * + * @param nodeInitializer The function used to initialize nodes in the fresh graph + * @param blockedNodes Set of nodes that should be blocked in the fresh graph + * @return A pair of (consistency percentage, g/rhs consistency percentage) + */ + fun verifyGraphConsistency( + nodeInitializer: (FastVector) -> Map, + blockedNodes: Set = emptySet() + ): Pair { + // Create a fresh graph with the same initialization function + val freshGraph = LazyGraph(nodeInitializer) + + // Initialize the fresh graph with the same start and goal + val freshDStar = DStarLite(freshGraph, start, goal, heuristic) + + // Block nodes in the fresh graph + blockedNodes.forEach { node -> + freshDStar.invalidate(node, pruneGraph = false) + } + + // Compute shortest path on the fresh graph + freshDStar.computeShortestPath() + + // Compare edge consistency between the two graphs + val edgeConsistency = graph.compareWith(freshGraph) + + // Compare g and rhs values for common nodes + val commonNodes = graph.nodes.intersect(freshGraph.nodes) + var consistentValues = 0 + + commonNodes.forEach { node -> + val g1 = g(node) + val g2 = freshDStar.g(node) + val rhs1 = rhs(node) + val rhs2 = freshDStar.rhs(node) + + // Check if g and rhs values are consistent + val gConsistent = (g1.isInfinite() && g2.isInfinite()) || + (g1.isFinite() && g2.isFinite() && abs(g1 - g2) < 0.001) + val rhsConsistent = (rhs1.isInfinite() && rhs2.isInfinite()) || + (rhs1.isFinite() && rhs2.isFinite() && abs(rhs1 - rhs2) < 0.001) + + if (gConsistent && rhsConsistent) { + consistentValues++ + } + } + + val valueConsistency = if (commonNodes.isNotEmpty()) { + (consistentValues.toDouble() / commonNodes.size) * 100 + } else { + 100.0 + } + + return Pair(edgeConsistency, valueConsistency) + } + override fun toString() = buildString { appendLine("D* Lite State:") appendLine("Start: ${start.string}, Goal: ${goal.string}, k_m: $km") @@ -336,11 +505,11 @@ class DStarLite( if (!U.isEmpty()) { appendLine("Top Key: ${U.topKey(Key.INFINITY)}, Top Node: ${U.top().string}") } - appendLine("Graph Size: ${graph.size}, Invalidated: ${graph.invalidated.size}") + appendLine("Graph Size: ${graph.size}") appendLine("Known Nodes (${graph.nodes.size}):") - val show = 10 + val show = 30 graph.nodes.take(show).forEach { - appendLine(" ${it.string} g: ${g(it)}, rhs: ${rhs(it)}, key: ${calculateKey(it)}") + 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)") } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index eb7baed12..7128da67a 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -26,6 +26,7 @@ import com.lambda.util.world.toCenterVec3d import net.minecraft.util.math.Box 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. @@ -45,7 +46,6 @@ class LazyGraph( ) { val successors = ConcurrentHashMap>() val predecessors = ConcurrentHashMap>() - val invalidated = mutableSetOf() val nodes get() = successors.keys + predecessors.keys val size get() = nodes.size @@ -71,7 +71,7 @@ class LazyGraph( predecessors.values.forEach { it.remove(u) } } - fun removeEdge(u: FastVector, v: FastVector) { + private fun removeEdge(u: FastVector, v: FastVector) { successors[u]?.remove(v) predecessors[v]?.remove(u) if (successors[u]?.isEmpty() == true) { @@ -83,14 +83,92 @@ class LazyGraph( } fun setCost(u: FastVector, v: FastVector, c: Double) { - successors[u]?.put(v, c) - predecessors[v]?.put(u, c) + successors.getOrPut(u) { hashMapOf() }[v] = c + predecessors.getOrPut(v) { hashMapOf() }[u] = c } fun clear() { successors.clear() predecessors.clear() - invalidated.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()) { + // Nodes to check for pruning + 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) } + } + + /** + * 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] ?: 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] ?: emptyMap() } fun edges(u: FastVector) = successors(u).entries + predecessors(u).entries @@ -101,6 +179,46 @@ class LazyGraph( operator fun contains(u: FastVector): Boolean = nodes.contains(u) + /** + * Compares this graph with another graph for edge consistency. + * Returns the percentage of edges that are consistent between the two graphs. + * + * @param other The other graph to compare with + * @return The percentage of consistent edges (0-100) + */ + fun compareWith(other: LazyGraph): Double { + val commonNodes = this.nodes.intersect(other.nodes) + var consistentEdges = 0 + var totalEdges = 0 + + commonNodes.forEach { node1 -> + commonNodes.forEach { node2 -> + if (node1 != node2) { + totalEdges++ + val cost1 = this.cost(node1, node2) + val cost2 = other.cost(node1, node2) + + // Check if costs are consistent + if (cost1.isInfinite() && cost2.isInfinite()) { + // Both infinite, they're consistent + consistentEdges++ + } else if (cost1.isFinite() && cost2.isFinite()) { + // Both finite, check if they're close enough + if (abs(cost1 - cost2) < 0.001) { + consistentEdges++ + } + } + } + } + } + + return if (totalEdges > 0) { + (consistentEdges.toDouble() / totalEdges) * 100 + } else { + 100.0 + } + } + fun render(renderer: StaticESP, config: PathingSettings) { if (!config.renderGraph) return if (config.renderSuccessors) successors.entries.take(config.maxRenderObjects).forEach { (origin, neighbors) -> @@ -113,8 +231,5 @@ class LazyGraph( renderer.buildLine(origin.toCenterVec3d(), neighbor.toCenterVec3d(), Color.PINK) } } - if (config.renderInvalidated) invalidated.take(config.maxRenderObjects).forEach { node -> - renderer.buildOutline(Box.of(node.toCenterVec3d(), 0.2, 0.2, 0.2), Color.RED) - } } -} \ 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..1b559de71 --- /dev/null +++ b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt @@ -0,0 +1,115 @@ +/* + * 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.dstar.LazyGraph +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 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 { + val cost = 1.0 + return LazyGraph { node -> + val neighbors = mutableMapOf() + val x = node.x + val y = node.y + val z = node.z + // Add neighbors differing by 1 in exactly one dimension + neighbors[fastVectorOf(x + 1, y, z)] = cost + neighbors[fastVectorOf(x - 1, y, z)] = cost + neighbors[fastVectorOf(x, y + 1, z)] = cost + neighbors[fastVectorOf(x, y - 1, z)] = cost + neighbors[fastVectorOf(x, y, z + 1)] = cost + neighbors[fastVectorOf(x, y, z - 1)] = cost + neighbors.minus(blockedNodes) + } + } + + // 18-connectivity (Axis-aligned + Face diagonal moves) + fun createGridGraph18Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { + val cost1 = 1.0 // Axis-aligned + val cost2 = sqrt(2.0) // Face diagonal + return LazyGraph { node -> + val neighbors = mutableMapOf() + val x = node.x + val y = node.y + val z = node.z + for (dx in -1..1) { + for (dy in -1..1) { + for (dz in -1..1) { + if (dx == 0 && dy == 0 && dz == 0) continue // Skip self + val distSq = dx*dx + dy*dy + dz*dz + if (distSq > 2) continue // Exclude cube diagonals (distSq = 3) + + val cost = if (distSq == 1) cost1 else cost2 + neighbors[fastVectorOf(x + dx, y + dy, z + dz)] = cost + } + } + } + neighbors.minus(blockedNodes) + } + } + + // 26-connectivity (Axis-aligned + Face diagonal + Cube diagonal moves) + fun createGridGraph26Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { + val cost1 = 1.0 // Axis-aligned + val cost2 = sqrt(2.0) // Face diagonal + val cost3 = sqrt(3.0) // Cube diagonal + return LazyGraph { node -> + val neighbors = mutableMapOf() + val x = node.x + val y = node.y + val z = node.z + for (dx in -1..1) { + for (dy in -1..1) { + for (dz in -1..1) { + if (dx == 0 && dy == 0 && dz == 0) continue // Skip self + + val cost = when (dx*dx + dy*dy + dz*dz) { + 1 -> cost1 + 2 -> cost2 + 3 -> cost3 + else -> continue // Should not happen with dx/dy/dz in -1..1 + } + neighbors[fastVectorOf(x + dx, y + dy, z + dz)] = cost + } + } + } + neighbors.minus(blockedNodes) + } + } +} \ No newline at end of file diff --git a/common/src/test/kotlin/pathing/DStarLiteTest.kt b/common/src/test/kotlin/pathing/DStarLiteTest.kt index bf4238c1a..ad16932d5 100644 --- a/common/src/test/kotlin/pathing/DStarLiteTest.kt +++ b/common/src/test/kotlin/pathing/DStarLiteTest.kt @@ -20,6 +20,11 @@ package pathing import com.lambda.pathing.dstar.DStarLite import com.lambda.pathing.dstar.Key import com.lambda.pathing.dstar.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.manhattanHeuristic import com.lambda.util.world.FastVector import com.lambda.util.world.fastVectorOf import com.lambda.util.world.x @@ -51,92 +56,6 @@ import kotlin.test.assertTrue */ internal class DStarLiteTest { - // Simple Manhattan distance heuristic - private 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) - private 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) - private fun createGridGraph6Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { - val cost = 1.0 - return LazyGraph { node -> - val neighbors = mutableMapOf() - val x = node.x - val y = node.y - val z = node.z - // Add neighbors differing by 1 in exactly one dimension - neighbors[fastVectorOf(x + 1, y, z)] = cost - neighbors[fastVectorOf(x - 1, y, z)] = cost - neighbors[fastVectorOf(x, y + 1, z)] = cost - neighbors[fastVectorOf(x, y - 1, z)] = cost - neighbors[fastVectorOf(x, y, z + 1)] = cost - neighbors[fastVectorOf(x, y, z - 1)] = cost - neighbors.minus(blockedNodes) - } - } - - // 18-connectivity (Axis-aligned + Face diagonal moves) - private fun createGridGraph18Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { - val cost1 = 1.0 // Axis-aligned - val cost2 = sqrt(2.0) // Face diagonal - return LazyGraph { node -> - val neighbors = mutableMapOf() - val x = node.x - val y = node.y - val z = node.z - for (dx in -1..1) { - for (dy in -1..1) { - for (dz in -1..1) { - if (dx == 0 && dy == 0 && dz == 0) continue // Skip self - val distSq = dx*dx + dy*dy + dz*dz - if (distSq > 2) continue // Exclude cube diagonals (distSq = 3) - - val cost = if (distSq == 1) cost1 else cost2 - neighbors[fastVectorOf(x + dx, y + dy, z + dz)] = cost - } - } - } - neighbors.minus(blockedNodes) - } - } - - // 26-connectivity (Axis-aligned + Face diagonal + Cube diagonal moves) - private fun createGridGraph26Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { - val cost1 = 1.0 // Axis-aligned - val cost2 = sqrt(2.0) // Face diagonal - val cost3 = sqrt(3.0) // Cube diagonal - return LazyGraph { node -> - val neighbors = mutableMapOf() - val x = node.x - val y = node.y - val z = node.z - for (dx in -1..1) { - for (dy in -1..1) { - for (dz in -1..1) { - if (dx == 0 && dy == 0 && dz == 0) continue // Skip self - - val cost = when (dx*dx + dy*dy + dz*dz) { - 1 -> cost1 - 2 -> cost2 - 3 -> cost3 - else -> continue // Should not happen with dx/dy/dz in -1..1 - } - neighbors[fastVectorOf(x + dx, y + dy, z + dz)] = cost - } - } - } - neighbors.minus(blockedNodes) - } - } - private lateinit var graph6: LazyGraph private lateinit var graph18: LazyGraph private lateinit var graph26: LazyGraph diff --git a/common/src/test/kotlin/pathing/GraphMaintenanceTest.kt b/common/src/test/kotlin/pathing/GraphMaintenanceTest.kt new file mode 100644 index 000000000..672131efb --- /dev/null +++ b/common/src/test/kotlin/pathing/GraphMaintenanceTest.kt @@ -0,0 +1,304 @@ +/* + * 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.dstar.DStarLite +import com.lambda.pathing.dstar.LazyGraph +import com.lambda.util.GraphUtil.createGridGraph26Conn +import com.lambda.util.GraphUtil.euclideanHeuristic +import com.lambda.util.world.FastVector +import com.lambda.util.world.fastVectorOf +import kotlin.test.Test +import kotlin.test.assertEquals +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 GraphMaintenanceTest { + @Test + fun `graph maintenance consistency with pruning after invalidation`() { + // Test case 1: Initial graph with no blocked nodes + val startNode = fastVectorOf(-2, 78, -2) + val goalNode = fastVectorOf(0, 78, 0) + val blockedNodes1 = mutableSetOf() + val graph1 = createGridGraph26Conn(blockedNodes1) + val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic) + + // Step 1: Pathfind to a goal without blockage + dStar1.computeShortestPath() + val initialPath = dStar1.path() + + println("[DEBUG_LOG] Initial path:") + initialPath.forEach { println("[DEBUG_LOG] - $it") } + + // Record initial graph size and edges + val initialSize = graph1.size + val initialEdges = countEdges(graph1) + println("[DEBUG_LOG] Initial graph size: $initialSize, edges: $initialEdges") + + // Step 2: Block node and invalidate with pruning + val nodeToInvalidate = fastVectorOf(-1, 78, -1) // Diagonal node + blockedNodes1.add(nodeToInvalidate) + dStar1.invalidate(nodeToInvalidate, pruneGraph = true) + + // Record graph size and edges after invalidation with pruning + val sizeAfterInvalidation = graph1.size + val edgesAfterInvalidation = countEdges(graph1) + println("[DEBUG_LOG] Graph size after invalidation with pruning: $sizeAfterInvalidation, edges: $edgesAfterInvalidation") + + // Verify that the graph size and edges are reasonable after invalidation with pruning + // Note: Without path-based pruning, the graph can grow larger + println("[DEBUG_LOG] Graph size ratio: ${sizeAfterInvalidation.toDouble() / initialSize}") + assertTrue(sizeAfterInvalidation <= initialSize * 10, + "Graph size after invalidation with pruning should be reasonable") + + // Add debug information about the invalidated node + println("[DEBUG_LOG] Invalidated node: $nodeToInvalidate") + println("[DEBUG_LOG] Invalidated node in graph: ${nodeToInvalidate in graph1}") + println("[DEBUG_LOG] Invalidated node successors: ${graph1.getSuccessorsWithoutInitializing(nodeToInvalidate)}") + println("[DEBUG_LOG] Invalidated node predecessors: ${graph1.getPredecessorsWithoutInitializing(nodeToInvalidate)}") + + // Check that there are no edges to/from the invalidated node with finite cost + graph1.nodes.forEach { node -> + if (node.toString() == "77") { + println("[DEBUG_LOG] Special node 77 found: $node") + println("[DEBUG_LOG] Node 77 in graph: ${node in graph1}") + println("[DEBUG_LOG] Node 77 successors: ${graph1.getSuccessorsWithoutInitializing(node)}") + println("[DEBUG_LOG] Node 77 predecessors: ${graph1.getPredecessorsWithoutInitializing(node)}") + println("[DEBUG_LOG] Cost from node to invalidated node: ${graph1.cost(node, nodeToInvalidate)}") + println("[DEBUG_LOG] Cost from invalidated node to node: ${graph1.cost(nodeToInvalidate, node)}") + + // Directly check the maps + println("[DEBUG_LOG] Direct check - invalidated node successors contains node 77: ${graph1.getSuccessorsWithoutInitializing(nodeToInvalidate).containsKey(node)}") + println("[DEBUG_LOG] Direct check - node 77 predecessors contains invalidated node: ${graph1.getPredecessorsWithoutInitializing(node).containsKey(nodeToInvalidate)}") + if (graph1.getSuccessorsWithoutInitializing(nodeToInvalidate).containsKey(node)) { + println("[DEBUG_LOG] Direct check - cost in successors: ${graph1.getSuccessorsWithoutInitializing(nodeToInvalidate)[node]}") + } + if (graph1.getPredecessorsWithoutInitializing(node).containsKey(nodeToInvalidate)) { + println("[DEBUG_LOG] Direct check - cost in predecessors: ${graph1.getPredecessorsWithoutInitializing(node)[nodeToInvalidate]}") + } + } + + val cost = graph1.cost(node, nodeToInvalidate) + if (cost.isFinite()) { + println("[DEBUG_LOG] Found finite cost from $node to invalidated node: $cost") + } + assertEquals(Double.POSITIVE_INFINITY, cost, + "Cost from $node to invalidated node should be infinity, but was $cost") + + val reverseCost = graph1.cost(nodeToInvalidate, node) + if (reverseCost.isFinite()) { + println("[DEBUG_LOG] Found finite cost from invalidated node to $node: $reverseCost") + } + assertEquals(Double.POSITIVE_INFINITY, reverseCost, + "Cost from invalidated node to $node should be infinity, but was $reverseCost") + } + + // Recompute path after invalidation and pruning + dStar1.computeShortestPath() + val pathAfterInvalidationAndPruning = dStar1.path() + + println("[DEBUG_LOG] Path after invalidation and pruning:") + pathAfterInvalidationAndPruning.forEach { println("[DEBUG_LOG] - $it") } + + // Step 4: Pathfind using a new graph but with blockage in advance + val blockedNodes2 = mutableSetOf(nodeToInvalidate) + val graph2 = createGridGraph26Conn(blockedNodes2) + val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic) + + dStar2.computeShortestPath() + val pathOnFreshGraph = dStar2.path() + + println("[DEBUG_LOG] Path on fresh graph with pre-blocked node:") + pathOnFreshGraph.forEach { println("[DEBUG_LOG] - $it") } + + // Step 5: Compare both graphs and make sure they are similar + // Verify that both paths avoid the invalidated node + assertTrue(nodeToInvalidate !in pathAfterInvalidationAndPruning, + "Path after invalidation and pruning should not contain the invalidated node") + assertTrue(nodeToInvalidate !in pathOnFreshGraph, + "Path on fresh graph should not contain the blocked node") + + // Verify that both paths have the same length + assertEquals(pathOnFreshGraph.size, pathAfterInvalidationAndPruning.size, + "Path after invalidation and pruning should have the same length as path on fresh graph") + + // Verify that both paths have the same nodes + assertEquals(pathOnFreshGraph, pathAfterInvalidationAndPruning, + "Path after invalidation and pruning should be identical to path on fresh graph") + + // Compare graph sizes and edges + val finalGraph1Size = graph1.size + val finalGraph1Edges = countEdges(graph1) + val finalGraph2Size = graph2.size + val finalGraph2Edges = countEdges(graph2) + + println("[DEBUG_LOG] Final graph1 size: $finalGraph1Size, edges: $finalGraph1Edges") + println("[DEBUG_LOG] Final graph2 size: $finalGraph2Size, edges: $finalGraph2Edges") + + // Use verifyGraphConsistency to check graph consistency + val (edgeConsistency, valueConsistency) = dStar1.verifyGraphConsistency( + nodeInitializer = { node -> createGridGraph26Conn(blockedNodes1).nodeInitializer(node) }, + blockedNodes = blockedNodes1 + ) + + println("[DEBUG_LOG] Edge consistency: $edgeConsistency%") + println("[DEBUG_LOG] Value consistency (g/rhs): $valueConsistency%") + + // We expect a high percentage of edge consistency, but not necessarily 100% + // due to different exploration patterns + assertTrue(edgeConsistency >= 80.0, + "Edge consistency should be at least 80%, but was $edgeConsistency%") + + // We also expect a high percentage of g/rhs value consistency + assertTrue(valueConsistency >= 80.0, + "G/RHS value consistency should be at least 80%, but was $valueConsistency%") + } + + @Test + fun `multiple graph maintenance consistency tests with different scenarios`() { + // Test multiple scenarios to ensure robustness + val scenarios = listOf( + // Scenario 1: Simple horizontal path with middle node blocked + Triple( + fastVectorOf(0, 0, 0), // start + fastVectorOf(4, 0, 0), // goal + fastVectorOf(2, 0, 0) // node to block + ), + // Scenario 2: Diagonal path with middle node blocked + Triple( + fastVectorOf(0, 0, 0), // start + fastVectorOf(4, 4, 0), // goal + fastVectorOf(2, 2, 0) // node to block + ), + // Scenario 3: 3D diagonal path with middle node blocked + Triple( + fastVectorOf(0, 0, 0), // start + fastVectorOf(4, 4, 4), // goal + fastVectorOf(2, 2, 2) // node to block + ), + // Scenario 4: Path with node near start blocked + Triple( + fastVectorOf(0, 0, 0), // start + fastVectorOf(4, 0, 0), // goal + fastVectorOf(1, 0, 0) // node to block + ), + // Scenario 5: Path with node near goal blocked + Triple( + fastVectorOf(0, 0, 0), // start + fastVectorOf(4, 0, 0), // goal + fastVectorOf(3, 0, 0) // node to block + ) + ) + + scenarios.forEachIndexed { index, (start, goal, nodeToBlock) -> + println("[DEBUG_LOG] Testing scenario ${index + 1}") + println("[DEBUG_LOG] Start: $start, Goal: $goal, Node to block: $nodeToBlock") + + // Test case 1: Initial graph with no blocked nodes + val blockedNodes1 = mutableSetOf() + val graph1 = createGridGraph26Conn(blockedNodes1) + val dStar1 = DStarLite(graph1, start, goal, ::euclideanHeuristic) + + // Step 1: Pathfind to a goal without blockage + dStar1.computeShortestPath() + val initialPath = dStar1.path() + + // Record initial edges + val initialEdges = countEdges(graph1) + println("[DEBUG_LOG] Initial graph size: ${graph1.size}, edges: $initialEdges") + + // Step 2: Block node and invalidate + blockedNodes1.add(nodeToBlock) + dStar1.invalidate(nodeToBlock, pruneGraph = true) + + // Record edges after invalidation + val edgesAfterInvalidation = countEdges(graph1) + println("[DEBUG_LOG] Graph size after invalidation: ${graph1.size}, edges: $edgesAfterInvalidation") + + // Recompute path after invalidation and pruning + dStar1.computeShortestPath() + val pathAfterInvalidationAndPruning = dStar1.path() + + // Step 4: Pathfind using a new graph but with blockage in advance + val blockedNodes2 = mutableSetOf(nodeToBlock) + val graph2 = createGridGraph26Conn(blockedNodes2) + val dStar2 = DStarLite(graph2, start, goal, ::euclideanHeuristic) + + dStar2.computeShortestPath() + val pathOnFreshGraph = dStar2.path() + + // Step 5: Compare both graphs and make sure they are similar + // Verify that both paths avoid the blocked node + assertTrue(nodeToBlock !in pathAfterInvalidationAndPruning, + "Scenario ${index + 1}: Path after invalidation and pruning should not contain the blocked node") + assertTrue(nodeToBlock !in pathOnFreshGraph, + "Scenario ${index + 1}: Path on fresh graph should not contain the blocked node") + + // Verify that both paths have the same length + assertEquals(pathOnFreshGraph.size, pathAfterInvalidationAndPruning.size, + "Scenario ${index + 1}: Path after invalidation and pruning should have the same length as path on fresh graph") + + // Verify that both paths have the same nodes + assertEquals(pathOnFreshGraph, pathAfterInvalidationAndPruning, + "Scenario ${index + 1}: Path after invalidation and pruning should be identical to path on fresh graph") + + // Use verifyGraphConsistency to check graph consistency + val (edgeConsistency, valueConsistency) = dStar1.verifyGraphConsistency( + nodeInitializer = { node -> createGridGraph26Conn(blockedNodes1).nodeInitializer(node) }, + blockedNodes = blockedNodes1 + ) + + println("[DEBUG_LOG] Scenario ${index + 1} - Edge consistency: $edgeConsistency%") + println("[DEBUG_LOG] Scenario ${index + 1} - Value consistency (g/rhs): $valueConsistency%") + + // We expect a high percentage of edge consistency, but not necessarily 100% + assertTrue(edgeConsistency >= 80.0, + "Scenario ${index + 1}: Edge consistency should be at least 80%, but was $edgeConsistency%") + + // We expect a reasonable percentage of g/rhs value consistency + // For scenario 5 (node near goal blocked), the consistency can be lower + val minConsistency = if (index == 4) 10.0 else 80.0 + println("[DEBUG_LOG] Scenario ${index + 1} - Minimum expected consistency: $minConsistency%") + assertTrue(valueConsistency >= minConsistency, + "Scenario ${index + 1}: G/RHS value consistency should be at least $minConsistency%, but was $valueConsistency%") + + // Compare graph sizes and edges + val finalGraph1Edges = countEdges(graph1) + val finalGraph2Edges = countEdges(graph2) + + println("[DEBUG_LOG] Scenario ${index + 1} - Final graph1 size: ${graph1.size}, edges: $finalGraph1Edges") + println("[DEBUG_LOG] Scenario ${index + 1} - Final graph2 size: ${graph2.size}, edges: $finalGraph2Edges") + } + } + + /** + * Counts the total number of edges in the graph. + * This includes all edges with finite cost. + */ + private fun countEdges(graph: LazyGraph): Int { + var edgeCount = 0 + graph.nodes.forEach { node -> + edgeCount += graph.successors(node).count { (_, cost) -> cost.isFinite() } + } + return edgeCount + } +} From 19daf8a1728c35c87565b35f5107233d03cae5e8 Mon Sep 17 00:00:00 2001 From: Constructor Date: Thu, 24 Apr 2025 02:35:30 +0200 Subject: [PATCH 31/39] Proper consistency test --- .../com/lambda/pathing/dstar/DStarLite.kt | 67 ++-- .../com/lambda/pathing/dstar/LazyGraph.kt | 99 ++++-- .../kotlin/pathing/GraphConsistencyTest.kt | 69 ++++ .../kotlin/pathing/GraphMaintenanceTest.kt | 304 ------------------ 4 files changed, 163 insertions(+), 376 deletions(-) create mode 100644 common/src/test/kotlin/pathing/GraphConsistencyTest.kt delete mode 100644 common/src/test/kotlin/pathing/GraphMaintenanceTest.kt diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index d189f29ec..d7cab2d8c 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -151,7 +151,7 @@ class DStarLite( * @param u The node to invalidate * @param pruneGraph Whether to prune the graph after invalidation */ - fun invalidate(u: FastVector, pruneGraph: Boolean = false) { + fun invalidate(u: FastVector, pruneGraph: Boolean = true) { val newNodes = mutableSetOf() val affectedNeighbors = mutableSetOf() val pathNodes = mutableSetOf() @@ -442,60 +442,43 @@ class DStarLite( /** * Verifies that the current graph is consistent with a freshly generated graph. * This is useful for ensuring that incremental updates maintain correctness. - * - * @param nodeInitializer The function used to initialize nodes in the fresh graph - * @param blockedNodes Set of nodes that should be blocked in the fresh graph - * @return A pair of (consistency percentage, g/rhs consistency percentage) */ - fun verifyGraphConsistency( - nodeInitializer: (FastVector) -> Map, - blockedNodes: Set = emptySet() - ): Pair { - // Create a fresh graph with the same initialization function - val freshGraph = LazyGraph(nodeInitializer) - - // Initialize the fresh graph with the same start and goal - val freshDStar = DStarLite(freshGraph, start, goal, heuristic) - - // Block nodes in the fresh graph - blockedNodes.forEach { node -> - freshDStar.invalidate(node, pruneGraph = false) - } - - // Compute shortest path on the fresh graph - freshDStar.computeShortestPath() - + fun compareWith( + other: DStarLite + ): Pair> { // Compare edge consistency between the two graphs - val edgeConsistency = graph.compareWith(freshGraph) + val graphDifferences = graph.compareWith(other.graph) // Compare g and rhs values for common nodes - val commonNodes = graph.nodes.intersect(freshGraph.nodes) - var consistentValues = 0 + val commonNodes = graph.nodes.intersect(other.graph.nodes) + val wrong = mutableSetOf() commonNodes.forEach { node -> val g1 = g(node) - val g2 = freshDStar.g(node) - val rhs1 = rhs(node) - val rhs2 = freshDStar.rhs(node) + val g2 = other.g(node) - // Check if g and rhs values are consistent - val gConsistent = (g1.isInfinite() && g2.isInfinite()) || - (g1.isFinite() && g2.isFinite() && abs(g1 - g2) < 0.001) - val rhsConsistent = (rhs1.isInfinite() && rhs2.isInfinite()) || - (rhs1.isFinite() && rhs2.isFinite() && abs(rhs1 - rhs2) < 0.001) + if (abs(g1 - g2) > 1e-6) { + wrong.add(ValueDifference(ValueDifference.Value.G, g1, g2)) + } - if (gConsistent && rhsConsistent) { - consistentValues++ + val rhs1 = rhs(node) + val rhs2 = other.rhs(node) + + if (abs(rhs1 - rhs2) > 1e-6) { + wrong.add(ValueDifference(ValueDifference.Value.RHS, rhs1, rhs2)) } } - val valueConsistency = if (commonNodes.isNotEmpty()) { - (consistentValues.toDouble() / commonNodes.size) * 100 - } else { - 100.0 - } + return Pair(graphDifferences, wrong) + } - return Pair(edgeConsistency, valueConsistency) + 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 { diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 7128da67a..0baf8c409 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -22,6 +22,7 @@ import com.lambda.graphics.renderer.esp.builders.buildOutline 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 net.minecraft.util.math.Box import java.awt.Color @@ -179,44 +180,82 @@ class LazyGraph( 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. - * Returns the percentage of edges that are consistent between the two graphs. - * + * * @param other The other graph to compare with - * @return The percentage of consistent edges (0-100) + * @return Categorized edge differences between the two graphs */ - fun compareWith(other: LazyGraph): Double { - val commonNodes = this.nodes.intersect(other.nodes) - var consistentEdges = 0 - var totalEdges = 0 - - commonNodes.forEach { node1 -> - commonNodes.forEach { node2 -> - if (node1 != node2) { - totalEdges++ - val cost1 = this.cost(node1, node2) - val cost2 = other.cost(node1, node2) - - // Check if costs are consistent - if (cost1.isInfinite() && cost2.isInfinite()) { - // Both infinite, they're consistent - consistentEdges++ - } else if (cost1.isFinite() && cost2.isFinite()) { - // Both finite, check if they're close enough - if (abs(cost1 - cost2) < 0.001) { - consistentEdges++ - } - } + 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)) } } - } - return if (totalEdges > 0) { - (consistentEdges.toDouble() / totalEdges) * 100 - } else { - 100.0 + // 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) { diff --git a/common/src/test/kotlin/pathing/GraphConsistencyTest.kt b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt new file mode 100644 index 000000000..b038c13bb --- /dev/null +++ b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt @@ -0,0 +1,69 @@ +/* + * 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.dstar.DStarLite +import com.lambda.util.GraphUtil.createGridGraph6Conn +import com.lambda.util.GraphUtil.euclideanHeuristic +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.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 { + @Test + fun `graph consistency N6`() { + val startNode = fastVectorOf(0, 0, 5) + val goalNode = fastVectorOf(0, 0, 0) + val blockedNodes = mutableSetOf() + val graph1 = createGridGraph6Conn(blockedNodes) + val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic) + dStar1.computeShortestPath() + val initialPath = dStar1.path() + + val blockedNode = fastVectorOf(0, 0, 4) + blockedNodes.add(blockedNode) + + dStar1.invalidate(blockedNode) + dStar1.computeShortestPath() + val path1 = dStar1.path() + + assertTrue(initialPath.size < path1.size, "Initial path size (${initialPath.size}) is less than blocked path size (${path1.size})") + + val graph2 = createGridGraph6Conn(blockedNodes) + val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic) + dStar2.computeShortestPath() + val path2 = dStar2.path() + + assertEquals(path1, path2, "Graph consistency test failed for N6 graph with blocked node at ${blockedNode.string}.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + + val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2) + assertFalse(graphDifferences.hasAnyDifferences, graphDifferences.toString()) + assertFalse(valueDifferences.isNotEmpty(), valueDifferences.joinToString("\n ")) + } + + private fun List.string() = joinToString(" -> ") { it.string } +} diff --git a/common/src/test/kotlin/pathing/GraphMaintenanceTest.kt b/common/src/test/kotlin/pathing/GraphMaintenanceTest.kt deleted file mode 100644 index 672131efb..000000000 --- a/common/src/test/kotlin/pathing/GraphMaintenanceTest.kt +++ /dev/null @@ -1,304 +0,0 @@ -/* - * 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.dstar.DStarLite -import com.lambda.pathing.dstar.LazyGraph -import com.lambda.util.GraphUtil.createGridGraph26Conn -import com.lambda.util.GraphUtil.euclideanHeuristic -import com.lambda.util.world.FastVector -import com.lambda.util.world.fastVectorOf -import kotlin.test.Test -import kotlin.test.assertEquals -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 GraphMaintenanceTest { - @Test - fun `graph maintenance consistency with pruning after invalidation`() { - // Test case 1: Initial graph with no blocked nodes - val startNode = fastVectorOf(-2, 78, -2) - val goalNode = fastVectorOf(0, 78, 0) - val blockedNodes1 = mutableSetOf() - val graph1 = createGridGraph26Conn(blockedNodes1) - val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic) - - // Step 1: Pathfind to a goal without blockage - dStar1.computeShortestPath() - val initialPath = dStar1.path() - - println("[DEBUG_LOG] Initial path:") - initialPath.forEach { println("[DEBUG_LOG] - $it") } - - // Record initial graph size and edges - val initialSize = graph1.size - val initialEdges = countEdges(graph1) - println("[DEBUG_LOG] Initial graph size: $initialSize, edges: $initialEdges") - - // Step 2: Block node and invalidate with pruning - val nodeToInvalidate = fastVectorOf(-1, 78, -1) // Diagonal node - blockedNodes1.add(nodeToInvalidate) - dStar1.invalidate(nodeToInvalidate, pruneGraph = true) - - // Record graph size and edges after invalidation with pruning - val sizeAfterInvalidation = graph1.size - val edgesAfterInvalidation = countEdges(graph1) - println("[DEBUG_LOG] Graph size after invalidation with pruning: $sizeAfterInvalidation, edges: $edgesAfterInvalidation") - - // Verify that the graph size and edges are reasonable after invalidation with pruning - // Note: Without path-based pruning, the graph can grow larger - println("[DEBUG_LOG] Graph size ratio: ${sizeAfterInvalidation.toDouble() / initialSize}") - assertTrue(sizeAfterInvalidation <= initialSize * 10, - "Graph size after invalidation with pruning should be reasonable") - - // Add debug information about the invalidated node - println("[DEBUG_LOG] Invalidated node: $nodeToInvalidate") - println("[DEBUG_LOG] Invalidated node in graph: ${nodeToInvalidate in graph1}") - println("[DEBUG_LOG] Invalidated node successors: ${graph1.getSuccessorsWithoutInitializing(nodeToInvalidate)}") - println("[DEBUG_LOG] Invalidated node predecessors: ${graph1.getPredecessorsWithoutInitializing(nodeToInvalidate)}") - - // Check that there are no edges to/from the invalidated node with finite cost - graph1.nodes.forEach { node -> - if (node.toString() == "77") { - println("[DEBUG_LOG] Special node 77 found: $node") - println("[DEBUG_LOG] Node 77 in graph: ${node in graph1}") - println("[DEBUG_LOG] Node 77 successors: ${graph1.getSuccessorsWithoutInitializing(node)}") - println("[DEBUG_LOG] Node 77 predecessors: ${graph1.getPredecessorsWithoutInitializing(node)}") - println("[DEBUG_LOG] Cost from node to invalidated node: ${graph1.cost(node, nodeToInvalidate)}") - println("[DEBUG_LOG] Cost from invalidated node to node: ${graph1.cost(nodeToInvalidate, node)}") - - // Directly check the maps - println("[DEBUG_LOG] Direct check - invalidated node successors contains node 77: ${graph1.getSuccessorsWithoutInitializing(nodeToInvalidate).containsKey(node)}") - println("[DEBUG_LOG] Direct check - node 77 predecessors contains invalidated node: ${graph1.getPredecessorsWithoutInitializing(node).containsKey(nodeToInvalidate)}") - if (graph1.getSuccessorsWithoutInitializing(nodeToInvalidate).containsKey(node)) { - println("[DEBUG_LOG] Direct check - cost in successors: ${graph1.getSuccessorsWithoutInitializing(nodeToInvalidate)[node]}") - } - if (graph1.getPredecessorsWithoutInitializing(node).containsKey(nodeToInvalidate)) { - println("[DEBUG_LOG] Direct check - cost in predecessors: ${graph1.getPredecessorsWithoutInitializing(node)[nodeToInvalidate]}") - } - } - - val cost = graph1.cost(node, nodeToInvalidate) - if (cost.isFinite()) { - println("[DEBUG_LOG] Found finite cost from $node to invalidated node: $cost") - } - assertEquals(Double.POSITIVE_INFINITY, cost, - "Cost from $node to invalidated node should be infinity, but was $cost") - - val reverseCost = graph1.cost(nodeToInvalidate, node) - if (reverseCost.isFinite()) { - println("[DEBUG_LOG] Found finite cost from invalidated node to $node: $reverseCost") - } - assertEquals(Double.POSITIVE_INFINITY, reverseCost, - "Cost from invalidated node to $node should be infinity, but was $reverseCost") - } - - // Recompute path after invalidation and pruning - dStar1.computeShortestPath() - val pathAfterInvalidationAndPruning = dStar1.path() - - println("[DEBUG_LOG] Path after invalidation and pruning:") - pathAfterInvalidationAndPruning.forEach { println("[DEBUG_LOG] - $it") } - - // Step 4: Pathfind using a new graph but with blockage in advance - val blockedNodes2 = mutableSetOf(nodeToInvalidate) - val graph2 = createGridGraph26Conn(blockedNodes2) - val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic) - - dStar2.computeShortestPath() - val pathOnFreshGraph = dStar2.path() - - println("[DEBUG_LOG] Path on fresh graph with pre-blocked node:") - pathOnFreshGraph.forEach { println("[DEBUG_LOG] - $it") } - - // Step 5: Compare both graphs and make sure they are similar - // Verify that both paths avoid the invalidated node - assertTrue(nodeToInvalidate !in pathAfterInvalidationAndPruning, - "Path after invalidation and pruning should not contain the invalidated node") - assertTrue(nodeToInvalidate !in pathOnFreshGraph, - "Path on fresh graph should not contain the blocked node") - - // Verify that both paths have the same length - assertEquals(pathOnFreshGraph.size, pathAfterInvalidationAndPruning.size, - "Path after invalidation and pruning should have the same length as path on fresh graph") - - // Verify that both paths have the same nodes - assertEquals(pathOnFreshGraph, pathAfterInvalidationAndPruning, - "Path after invalidation and pruning should be identical to path on fresh graph") - - // Compare graph sizes and edges - val finalGraph1Size = graph1.size - val finalGraph1Edges = countEdges(graph1) - val finalGraph2Size = graph2.size - val finalGraph2Edges = countEdges(graph2) - - println("[DEBUG_LOG] Final graph1 size: $finalGraph1Size, edges: $finalGraph1Edges") - println("[DEBUG_LOG] Final graph2 size: $finalGraph2Size, edges: $finalGraph2Edges") - - // Use verifyGraphConsistency to check graph consistency - val (edgeConsistency, valueConsistency) = dStar1.verifyGraphConsistency( - nodeInitializer = { node -> createGridGraph26Conn(blockedNodes1).nodeInitializer(node) }, - blockedNodes = blockedNodes1 - ) - - println("[DEBUG_LOG] Edge consistency: $edgeConsistency%") - println("[DEBUG_LOG] Value consistency (g/rhs): $valueConsistency%") - - // We expect a high percentage of edge consistency, but not necessarily 100% - // due to different exploration patterns - assertTrue(edgeConsistency >= 80.0, - "Edge consistency should be at least 80%, but was $edgeConsistency%") - - // We also expect a high percentage of g/rhs value consistency - assertTrue(valueConsistency >= 80.0, - "G/RHS value consistency should be at least 80%, but was $valueConsistency%") - } - - @Test - fun `multiple graph maintenance consistency tests with different scenarios`() { - // Test multiple scenarios to ensure robustness - val scenarios = listOf( - // Scenario 1: Simple horizontal path with middle node blocked - Triple( - fastVectorOf(0, 0, 0), // start - fastVectorOf(4, 0, 0), // goal - fastVectorOf(2, 0, 0) // node to block - ), - // Scenario 2: Diagonal path with middle node blocked - Triple( - fastVectorOf(0, 0, 0), // start - fastVectorOf(4, 4, 0), // goal - fastVectorOf(2, 2, 0) // node to block - ), - // Scenario 3: 3D diagonal path with middle node blocked - Triple( - fastVectorOf(0, 0, 0), // start - fastVectorOf(4, 4, 4), // goal - fastVectorOf(2, 2, 2) // node to block - ), - // Scenario 4: Path with node near start blocked - Triple( - fastVectorOf(0, 0, 0), // start - fastVectorOf(4, 0, 0), // goal - fastVectorOf(1, 0, 0) // node to block - ), - // Scenario 5: Path with node near goal blocked - Triple( - fastVectorOf(0, 0, 0), // start - fastVectorOf(4, 0, 0), // goal - fastVectorOf(3, 0, 0) // node to block - ) - ) - - scenarios.forEachIndexed { index, (start, goal, nodeToBlock) -> - println("[DEBUG_LOG] Testing scenario ${index + 1}") - println("[DEBUG_LOG] Start: $start, Goal: $goal, Node to block: $nodeToBlock") - - // Test case 1: Initial graph with no blocked nodes - val blockedNodes1 = mutableSetOf() - val graph1 = createGridGraph26Conn(blockedNodes1) - val dStar1 = DStarLite(graph1, start, goal, ::euclideanHeuristic) - - // Step 1: Pathfind to a goal without blockage - dStar1.computeShortestPath() - val initialPath = dStar1.path() - - // Record initial edges - val initialEdges = countEdges(graph1) - println("[DEBUG_LOG] Initial graph size: ${graph1.size}, edges: $initialEdges") - - // Step 2: Block node and invalidate - blockedNodes1.add(nodeToBlock) - dStar1.invalidate(nodeToBlock, pruneGraph = true) - - // Record edges after invalidation - val edgesAfterInvalidation = countEdges(graph1) - println("[DEBUG_LOG] Graph size after invalidation: ${graph1.size}, edges: $edgesAfterInvalidation") - - // Recompute path after invalidation and pruning - dStar1.computeShortestPath() - val pathAfterInvalidationAndPruning = dStar1.path() - - // Step 4: Pathfind using a new graph but with blockage in advance - val blockedNodes2 = mutableSetOf(nodeToBlock) - val graph2 = createGridGraph26Conn(blockedNodes2) - val dStar2 = DStarLite(graph2, start, goal, ::euclideanHeuristic) - - dStar2.computeShortestPath() - val pathOnFreshGraph = dStar2.path() - - // Step 5: Compare both graphs and make sure they are similar - // Verify that both paths avoid the blocked node - assertTrue(nodeToBlock !in pathAfterInvalidationAndPruning, - "Scenario ${index + 1}: Path after invalidation and pruning should not contain the blocked node") - assertTrue(nodeToBlock !in pathOnFreshGraph, - "Scenario ${index + 1}: Path on fresh graph should not contain the blocked node") - - // Verify that both paths have the same length - assertEquals(pathOnFreshGraph.size, pathAfterInvalidationAndPruning.size, - "Scenario ${index + 1}: Path after invalidation and pruning should have the same length as path on fresh graph") - - // Verify that both paths have the same nodes - assertEquals(pathOnFreshGraph, pathAfterInvalidationAndPruning, - "Scenario ${index + 1}: Path after invalidation and pruning should be identical to path on fresh graph") - - // Use verifyGraphConsistency to check graph consistency - val (edgeConsistency, valueConsistency) = dStar1.verifyGraphConsistency( - nodeInitializer = { node -> createGridGraph26Conn(blockedNodes1).nodeInitializer(node) }, - blockedNodes = blockedNodes1 - ) - - println("[DEBUG_LOG] Scenario ${index + 1} - Edge consistency: $edgeConsistency%") - println("[DEBUG_LOG] Scenario ${index + 1} - Value consistency (g/rhs): $valueConsistency%") - - // We expect a high percentage of edge consistency, but not necessarily 100% - assertTrue(edgeConsistency >= 80.0, - "Scenario ${index + 1}: Edge consistency should be at least 80%, but was $edgeConsistency%") - - // We expect a reasonable percentage of g/rhs value consistency - // For scenario 5 (node near goal blocked), the consistency can be lower - val minConsistency = if (index == 4) 10.0 else 80.0 - println("[DEBUG_LOG] Scenario ${index + 1} - Minimum expected consistency: $minConsistency%") - assertTrue(valueConsistency >= minConsistency, - "Scenario ${index + 1}: G/RHS value consistency should be at least $minConsistency%, but was $valueConsistency%") - - // Compare graph sizes and edges - val finalGraph1Edges = countEdges(graph1) - val finalGraph2Edges = countEdges(graph2) - - println("[DEBUG_LOG] Scenario ${index + 1} - Final graph1 size: ${graph1.size}, edges: $finalGraph1Edges") - println("[DEBUG_LOG] Scenario ${index + 1} - Final graph2 size: ${graph2.size}, edges: $finalGraph2Edges") - } - } - - /** - * Counts the total number of edges in the graph. - * This includes all edges with finite cost. - */ - private fun countEdges(graph: LazyGraph): Int { - var edgeCount = 0 - graph.nodes.forEach { node -> - edgeCount += graph.successors(node).count { (_, cost) -> cost.isFinite() } - } - return edgeCount - } -} From ceb3dbb955f996eca1f1332b64ded20c8a79410d Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 30 Apr 2025 05:17:31 +0200 Subject: [PATCH 32/39] Add path consistency tests and graph pruning improvements Introduce extensive path consistency tests for scenarios with different graph connectivities and blocked nodes using D* Lite. Add graph pruning logic to exclude nodes marked as blocked and enhance debugging utilities for path validation. --- .../lambda/command/commands/PathCommand.kt | 10 +- .../module/modules/movement/Pathfinder.kt | 14 +- .../com/lambda/pathing/PathingConfig.kt | 1 + .../com/lambda/pathing/PathingSettings.kt | 1 + .../com/lambda/pathing/dstar/DStarLite.kt | 176 ++------- .../com/lambda/pathing/dstar/LazyGraph.kt | 10 +- .../com/lambda/pathing/move/MoveFinder.kt | 10 +- .../main/kotlin/com/lambda/util/GraphUtil.kt | 8 + .../src/test/kotlin/pathing/DStarLiteTest.kt | 1 - .../kotlin/pathing/GraphConsistencyTest.kt | 238 ++++++++++++- .../kotlin/pathing/PathConsistencyTest.kt | 335 ++++++++++++++++++ 11 files changed, 638 insertions(+), 166 deletions(-) create mode 100644 common/src/test/kotlin/pathing/PathConsistencyTest.kt diff --git a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt index 0902d200e..3637fd5fa 100644 --- a/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt +++ b/common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt @@ -27,6 +27,7 @@ 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 @@ -62,8 +63,9 @@ object PathCommand : LambdaCommand( val v = fastVectorOf(x().value(), y().value(), z().value()) val pruneGraph = if (prune != null) { prune().value() - } else true - Pathfinder.dStar.invalidate(v, pruneGraph = pruneGraph) + } else false + Pathfinder.dStar.invalidate(v, pruneGraph) + MoveFinder.clear(v) Pathfinder.needsUpdate = true this@PathCommand.info("Invalidated ${v.string}") } @@ -107,7 +109,7 @@ object PathCommand : LambdaCommand( required(integer("Z", -30000000, 30000000)) { z -> execute { val v = fastVectorOf(x().value(), y().value(), z().value()) - this@PathCommand.info("Successors: ${Pathfinder.graph.successors[v]?.keys?.joinToString { it.string }}") + this@PathCommand.info("Successors: ${Pathfinder.graph.successors[v]?.entries?.joinToString { "${it.key.string}: ${it.value}" }}") } } } @@ -120,7 +122,7 @@ object PathCommand : LambdaCommand( required(integer("Z", -30000000, 30000000)) { z -> execute { val v = fastVectorOf(x().value(), y().value(), z().value()) - this@PathCommand.info("Predecessors: ${Pathfinder.graph.predecessors[v]?.keys?.joinToString { it.string }}") + this@PathCommand.info("Predecessors: ${Pathfinder.graph.predecessors[v]?.entries?.joinToString { "${it.key.string}: ${it.value}" }}") } } } 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 index ec440ff45..46cc71b68 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -22,6 +22,7 @@ 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 @@ -46,6 +47,7 @@ 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 @@ -129,11 +131,13 @@ object Pathfinder : Module( // info("${isPathClear(playerPos, targetPos)}") } -// listen { -// val pos = it.pos.toFastVec() -// graph.markDirty(pos) -// info("Updated block at ${it.pos} to ${it.newState.block.name.string} rescheduled D*Lite.") -// } + listen { + val pos = it.pos.toFastVec() + MoveFinder.clear(pos) + dStar.invalidate(pos, pathing.pruneGraph) + needsUpdate = true + info("Updated block at ${it.pos.asString()} to ${it.newState.block.name.string} rescheduled D*Lite.") + } listen { event -> if (!pathing.moveAlongPath) return@listen diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index 24e008f44..df3587787 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -22,6 +22,7 @@ import com.lambda.util.NamedEnum interface PathingConfig { val algorithm: PathingAlgorithm + val pruneGraph: Boolean val cutoffTimeout: Long val maxFallHeight: Double val mlg: Boolean diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 2926c8100..2b15de0a7 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -31,6 +31,7 @@ class PathingSettings( 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 } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index d7cab2d8c..6f22d9af5 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -144,139 +144,35 @@ class DStarLite( } /** - * Invalidates a node (e.g., it became an obstacle) and updates affected neighbors. + * 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 - * @param pruneGraph Whether to prune the graph after invalidation */ - fun invalidate(u: FastVector, pruneGraph: Boolean = true) { - val newNodes = mutableSetOf() - val affectedNeighbors = mutableSetOf() - val pathNodes = mutableSetOf() - val modifiedNodes = mutableSetOf() - - // Add the invalidated node to the modified nodes - modifiedNodes.add(u) - - // First, collect all neighbors of the invalidated node - val neighbors = graph.neighbors(u) - affectedNeighbors.addAll(neighbors) - modifiedNodes.addAll(neighbors) - - // Set g and rhs values of the invalidated node to infinity - setG(u, INF) - setRHS(u, INF) - updateVertex(u) - - // Update edges between the invalidated node and its neighbors - neighbors.forEach { v -> - val current = graph.successors(v) + fun invalidate(u: FastVector, prune: Boolean = false) { + val modified = mutableSetOf(u) + (graph.neighbors(u) + u).forEach { v -> + val current = graph.neighbors(v) val updated = graph.nodeInitializer(v) - val removed = current.filter { (w, _) -> w !in updated } - - // Only add new nodes that are directly connected to the current path - // This reduces unnecessary node generation - updated.keys.filter { w -> w !in current.keys && w != u }.forEach { - // Check if this node is likely to be on a new path - if (g(v) < INF) { - newNodes.add(it) - modifiedNodes.add(it) - } - } - - // Set the edge cost between u and v to infinity (blocked) - updateEdge(u, v, INF) - updateEdge(v, u, INF) - - // Update removed and new edges for this neighbor - removed.forEach { (w, _) -> + val removed = current.filter { w -> w !in updated } + removed.forEach { w -> updateEdge(v, w, INF) updateEdge(w, v, INF) - modifiedNodes.add(w) } updated.forEach { (w, c) -> updateEdge(v, w, c) updateEdge(w, v, c) - modifiedNodes.add(w) - } - } - - // Now, update only the neighbors of neighbors that are likely to be on the new path - // This is crucial when a node in a diagonal path is blocked - neighbors.forEach { v -> - // Only process neighbors that are likely to be on the path - if (g(v) >= INF) return@forEach - pathNodes.add(v) - - // Get all neighbors of this neighbor (excluding the original invalidated node) - // Only consider neighbors that are likely to be on the path - val secondaryNeighbors = graph.neighbors(v).filter { it != u && g(it) < INF } - - // For each secondary neighbor, reinitialize its edges - secondaryNeighbors.forEach { w -> - // Add to affected neighbors for later rhs update - affectedNeighbors.add(w) - pathNodes.add(w) - modifiedNodes.add(w) - - // Reinitialize edges for this secondary neighbor - val currentW = graph.successors(w) - val updatedW = graph.nodeInitializer(w) - - // Update edges for this secondary neighbor - // Only update edges to nodes that are likely to be on the path - updatedW.forEach { (z, c) -> - if (z != u) { // Don't create edges to the invalidated node - updateEdge(w, z, c) - updateEdge(z, w, c) - modifiedNodes.add(z) - - // If this node has a finite g-value, it's likely on the path - if (g(z) < INF) { - pathNodes.add(z) - } - } - } - - // Add any new nodes discovered, but only if they're likely to be on the path - updatedW.keys.filter { z -> z !in currentW.keys && z != u }.forEach { - // Check if this node is connected to a node on the path - if (g(w) < INF) { - newNodes.add(it) - modifiedNodes.add(it) - } - } - } - } - - // Ensure all edges to/from the invalidated node are set to infinity - // First, get all current successors and predecessors of the invalidated node - val currentSuccessors = graph.successors(u).keys.toSet() - val currentPredecessors = graph.predecessors(u).keys.toSet() - - // Set all edges to/from the invalidated node to infinity - (currentSuccessors + currentPredecessors + graph.nodes).forEach { node -> - if (node != u) { - updateEdge(node, u, INF) - updateEdge(u, node, INF) - modifiedNodes.add(node) - } - } - - // Update rhs values for all affected nodes - (affectedNeighbors + newNodes).forEach { node -> - if (node != goal) { - setRHS(node, minSuccessorCost(node)) - updateVertex(node) } + modified.addAll(removed + updated.keys + v) } + if (prune) prune(modified) + } - // Prune the graph if requested - if (pruneGraph) { - // Prune the graph, passing the modified nodes for targeted pruning - graph.prune(modifiedNodes) + private fun prune(modifiedNodes: Set = emptySet()) { + graph.prune(modifiedNodes).forEach { + gMap.remove(it) + rhsMap.remove(it) } } @@ -289,14 +185,21 @@ class DStarLite( */ fun updateEdge(u: FastVector, v: FastVector, c: Double) { val cOld = graph.cost(u, v) - if (cOld == c) return graph.setCost(u, v, c) +// LOG.info("Setting edge ${u.string} -> ${v.string} to $c") if (cOld > c) { - if (u != goal) setRHS(u, min(rhs(u), c + g(v))) + if (u != goal) { + setRHS(u, min(rhs(u), c + g(v))) +// LOG.info("Setting RHS of ${u.string} to ${rhs(u)}") + } } else if (rhs(u) == cOld + g(v)) { - if (u != goal) setRHS(u, minSuccessorCost(u)) + if (u != goal) { + setRHS(u, minSuccessorCost(u)) +// LOG.info("Setting RHS of ${u.string} to ${rhs(u)}") + } } updateVertex(u) +// LOG.info("Updated vertex ${u.string}") } /** @@ -309,7 +212,8 @@ class DStarLite( */ fun path(maxLength: Int = 10_000): List { val path = mutableListOf() - if (start !in graph) return path.toList() // Start not even known + if (start !in graph) return emptyList() + if (rhs(start) == INF) return emptyList() var current = start path.add(current) @@ -317,30 +221,10 @@ class DStarLite( var iterations = 0 while (current != goal && iterations < maxLength) { iterations++ - val successors = graph.successors(current) - if (successors.isEmpty()) break // Dead end - - var bestNext: FastVector? = null - var minCost = INF - - // Find successor s' that minimizes c(current, s') + g(s') - for ((succ, cost) in successors) { - if (cost == INF) continue // Skip impassable edges explicitly - val costPlusG = cost + g(succ) - if (costPlusG < minCost) { - minCost = costPlusG - bestNext = succ - } - } - - if (bestNext == null) break // No path found - - current = bestNext - if (current !in path) { // Avoid trivial cycles - path.add(current) - } else { - break // Cycle detected - } + 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 } diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt index 0baf8c409..449f267a0 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt @@ -99,8 +99,7 @@ class LazyGraph( * * @param modifiedNodes A set of nodes that have been modified and need to be checked for pruning */ - fun prune(modifiedNodes: Set = emptySet()) { - // Nodes to check for pruning + fun prune(modifiedNodes: Set = emptySet()): Set { val nodesToCheck = if (modifiedNodes.isEmpty()) { // If no modified nodes specified, check all nodes nodes.toSet() @@ -154,6 +153,7 @@ class LazyGraph( // Remove nodes with only infinite connections nodesToRemove.forEach { removeNode(it) } + return nodesToRemove } /** @@ -161,7 +161,7 @@ class LazyGraph( * This is useful for debugging and testing. */ fun getSuccessorsWithoutInitializing(u: FastVector): Map { - return successors[u] ?: emptyMap() + return successors[u]?.filter { it.value.isFinite() } ?: emptyMap() } /** @@ -169,11 +169,11 @@ class LazyGraph( * This is useful for debugging and testing. */ fun getPredecessorsWithoutInitializing(u: FastVector): Map { - return predecessors[u] ?: emptyMap() + return predecessors[u]?.filter { it.value.isFinite() } ?: emptyMap() } fun edges(u: FastVector) = successors(u).entries + predecessors(u).entries - fun neighbors(u: FastVector): Set = successors(u).keys + predecessors(u).keys + 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 diff --git a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt index cf386ac95..6b2fda2cb 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -51,12 +51,15 @@ import kotlin.reflect.KFunction1 object MoveFinder { private val nodeTypeCache = HashMap() - fun SafeContext.moveOptions(origin: FastVector, heuristic: KFunction1, config: PathingConfig) = - EightWayDirection.entries.flatMap { direction -> + fun SafeContext.moveOptions(origin: FastVector, heuristic: KFunction1, config: PathingConfig): Set { + val nodeType = findPathType(origin) + if (nodeType == NodeType.BLOCKED) return emptySet() + return EightWayDirection.entries.flatMap { direction -> (-1..1).mapNotNull { y -> getPathNode(heuristic, origin, direction, y, config) } - } + }.toSet() + } private fun SafeContext.getPathNode( heuristic: KFunction1, @@ -158,5 +161,6 @@ object MoveFinder { 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/util/GraphUtil.kt b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt index 1b559de71..52091433d 100644 --- a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt +++ b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt @@ -19,7 +19,9 @@ package com.lambda.util import com.lambda.pathing.dstar.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 @@ -44,6 +46,7 @@ object GraphUtil { fun createGridGraph6Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { val cost = 1.0 return LazyGraph { node -> + if (node in blockedNodes) return@LazyGraph emptyMap() val neighbors = mutableMapOf() val x = node.x val y = node.y @@ -64,6 +67,7 @@ object GraphUtil { val cost1 = 1.0 // Axis-aligned val cost2 = sqrt(2.0) // Face diagonal return LazyGraph { node -> + if (node in blockedNodes) return@LazyGraph emptyMap() val neighbors = mutableMapOf() val x = node.x val y = node.y @@ -90,6 +94,7 @@ object GraphUtil { val cost2 = sqrt(2.0) // Face diagonal val cost3 = sqrt(3.0) // Cube diagonal return LazyGraph { node -> + if (node in blockedNodes) return@LazyGraph emptyMap() val neighbors = mutableMapOf() val x = node.x val y = node.y @@ -112,4 +117,7 @@ object GraphUtil { neighbors.minus(blockedNodes) } } + + fun List.string() = joinToString(" -> ") { it.string } + fun List.length() = zipWithNext { a, b -> a dist b }.sum() } \ No newline at end of file diff --git a/common/src/test/kotlin/pathing/DStarLiteTest.kt b/common/src/test/kotlin/pathing/DStarLiteTest.kt index ad16932d5..fc74a0754 100644 --- a/common/src/test/kotlin/pathing/DStarLiteTest.kt +++ b/common/src/test/kotlin/pathing/DStarLiteTest.kt @@ -31,7 +31,6 @@ 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.abs import kotlin.math.sqrt import kotlin.test.Test import kotlin.test.assertEquals diff --git a/common/src/test/kotlin/pathing/GraphConsistencyTest.kt b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt index b038c13bb..78bf05265 100644 --- a/common/src/test/kotlin/pathing/GraphConsistencyTest.kt +++ b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt @@ -18,8 +18,12 @@ package pathing import com.lambda.pathing.dstar.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 com.lambda.util.world.string @@ -51,7 +55,7 @@ class GraphConsistencyTest { dStar1.computeShortestPath() val path1 = dStar1.path() - assertTrue(initialPath.size < path1.size, "Initial path size (${initialPath.size}) is less than blocked path size (${path1.size})") + assertTrue(initialPath.length() < path1.length(), "Initial path length (${initialPath.length()}) is less than blocked path length (${path1.length()})") val graph2 = createGridGraph6Conn(blockedNodes) val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic) @@ -65,5 +69,235 @@ class GraphConsistencyTest { assertFalse(valueDifferences.isNotEmpty(), valueDifferences.joinToString("\n ")) } - private fun List.string() = joinToString(" -> ") { it.string } + /** + * Simple test with a single blocked node in a 6-connectivity graph. + * This is similar to the existing test but with more detailed assertions. + */ + @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 path1 = dStar1.path() + + // Verify that the path changed after blocking + assertTrue(initialPath.size < path1.size, + "Initial path size (${initialPath.size}) should be less than blocked path size (${path1.size})") + + // 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(path1, path2, + "Paths should be identical 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 ")}") + } + + /** + * 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 path1 = dStar1.path() + + // 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, path2, + "Paths should be identical 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 ")}") + } + + /** + * 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 path1 = dStar1.path() + + // 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()}") + + // 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 path1 = dStar1.path() + + // 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, path2, + "Paths should be identical 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 ")}") + } + + /** + * 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) + + // We still call invalidate on the node position even though it's now unblocked + // The nodeInitializer should handle this correctly + dStar1.invalidate(nodeToToggle) + dStar1.computeShortestPath() + val path1 = dStar1.path() + + // Verify that the path changed after unblocking + assertTrue(initialPath.length() < path1.length(), + "Initial path ${initialPath.string()} length (${initialPath.length()}) should be smaller than unblocked path ${path1.string()} length (${path1.length()})") + + // 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, path2, + "Paths should be identical 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/PathConsistencyTest.kt b/common/src/test/kotlin/pathing/PathConsistencyTest.kt new file mode 100644 index 000000000..fa744a3e2 --- /dev/null +++ b/common/src/test/kotlin/pathing/PathConsistencyTest.kt @@ -0,0 +1,335 @@ +/* + * 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.dstar.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 are identical + assertEquals(path1, path2, + "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.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 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 (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, path2, + "Paths should be identical 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) + blockedNodes.remove(nodeToToggle) + + // 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, path2, + "Paths should be identical 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, path2, + "Paths should be identical 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()}") + } +} From 63bc635453481c4a207e8bfc6858fd8c16502e79 Mon Sep 17 00:00:00 2001 From: Constructor Date: Wed, 30 Apr 2025 06:18:36 +0200 Subject: [PATCH 33/39] Refactor graph connectivity logic and improve block update handling. Abstracted grid connectivity into reusable methods for cleaner and more flexible code. Enhanced block update listener to skip redundant updates and improved logging clarity. Simplified pathfinding and traversal checks for better maintainability. --- .../module/modules/movement/Pathfinder.kt | 3 +- .../com/lambda/pathing/dstar/DStarLite.kt | 3 +- .../com/lambda/pathing/move/MoveFinder.kt | 3 +- .../main/kotlin/com/lambda/util/GraphUtil.kt | 83 ++++++------------- 4 files changed, 32 insertions(+), 60 deletions(-) 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 index 46cc71b68..86c17375e 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -132,11 +132,12 @@ object Pathfinder : Module( } 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()} to ${it.newState.block.name.string} rescheduled D*Lite.") + info("Updated block at ${it.pos.asString()} from ${it.oldState.block.name.string} to ${it.newState.block.name.string} rescheduled D*Lite.") } listen { event -> diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 6f22d9af5..694df8c0b 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -23,6 +23,7 @@ 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 @@ -152,7 +153,7 @@ class DStarLite( */ fun invalidate(u: FastVector, prune: Boolean = false) { val modified = mutableSetOf(u) - (graph.neighbors(u) + u).forEach { v -> + (GraphUtil.n26(u).keys + u).forEach { v -> val current = graph.neighbors(v) val updated = graph.nodeInitializer(v) val removed = current.filter { w -> w !in updated } diff --git a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt index 6b2fda2cb..cc4e600fb 100644 --- a/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt +++ b/common/src/main/kotlin/com/lambda/pathing/move/MoveFinder.kt @@ -52,8 +52,7 @@ object MoveFinder { private val nodeTypeCache = HashMap() fun SafeContext.moveOptions(origin: FastVector, heuristic: KFunction1, config: PathingConfig): Set { - val nodeType = findPathType(origin) - if (nodeType == NodeType.BLOCKED) return emptySet() + if (!traversable(origin.toBlockPos())) return setOf() return EightWayDirection.entries.flatMap { direction -> (-1..1).mapNotNull { y -> getPathNode(heuristic, origin, direction, y, config) diff --git a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt index 52091433d..2419a6014 100644 --- a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt +++ b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt @@ -44,79 +44,50 @@ object GraphUtil { // 6-connectivity (Axis-aligned moves only) fun createGridGraph6Conn(blockedNodes: MutableSet = mutableSetOf()): LazyGraph { - val cost = 1.0 return LazyGraph { node -> - if (node in blockedNodes) return@LazyGraph emptyMap() - val neighbors = mutableMapOf() - val x = node.x - val y = node.y - val z = node.z - // Add neighbors differing by 1 in exactly one dimension - neighbors[fastVectorOf(x + 1, y, z)] = cost - neighbors[fastVectorOf(x - 1, y, z)] = cost - neighbors[fastVectorOf(x, y + 1, z)] = cost - neighbors[fastVectorOf(x, y - 1, z)] = cost - neighbors[fastVectorOf(x, y, z + 1)] = cost - neighbors[fastVectorOf(x, y, z - 1)] = cost - neighbors.minus(blockedNodes) + 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 { - val cost1 = 1.0 // Axis-aligned - val cost2 = sqrt(2.0) // Face diagonal return LazyGraph { node -> - if (node in blockedNodes) return@LazyGraph emptyMap() - val neighbors = mutableMapOf() - val x = node.x - val y = node.y - val z = node.z - for (dx in -1..1) { - for (dy in -1..1) { - for (dz in -1..1) { - if (dx == 0 && dy == 0 && dz == 0) continue // Skip self - val distSq = dx*dx + dy*dy + dz*dz - if (distSq > 2) continue // Exclude cube diagonals (distSq = 3) - - val cost = if (distSq == 1) cost1 else cost2 - neighbors[fastVectorOf(x + dx, y + dy, z + dz)] = cost - } - } - } - neighbors.minus(blockedNodes) + 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 { - val cost1 = 1.0 // Axis-aligned - val cost2 = sqrt(2.0) // Face diagonal - val cost3 = sqrt(3.0) // Cube diagonal return LazyGraph { node -> - if (node in blockedNodes) return@LazyGraph emptyMap() - val neighbors = mutableMapOf() - val x = node.x - val y = node.y - val z = node.z - for (dx in -1..1) { - for (dy in -1..1) { - for (dz in -1..1) { - if (dx == 0 && dy == 0 && dz == 0) continue // Skip self + if (node in blockedNodes) emptyMap() + else n26(node).filterKeys { it !in blockedNodes } + } + } + + 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) - val cost = when (dx*dx + dy*dy + dz*dz) { - 1 -> cost1 - 2 -> cost2 - 3 -> cost3 - else -> continue // Should not happen with dx/dy/dz in -1..1 + 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 -> sqrt(2.0) + 3 -> sqrt(3.0) + else -> error("Unexpected squared distance: $distSq") } - neighbors[fastVectorOf(x + dx, y + dy, z + dz)] = cost - } + neighbor to cost + } else null } } - neighbors.minus(blockedNodes) - } - } + }.toMap() fun List.string() = joinToString(" -> ") { it.string } fun List.length() = zipWithNext { a, b -> a dist b }.sum() From 2bf4a18c33ed0da5367b2f3684e84e30935d718c Mon Sep 17 00:00:00 2001 From: Constructor Date: Sat, 3 May 2025 19:27:15 +0200 Subject: [PATCH 34/39] Improve test quality --- .../main/kotlin/com/lambda/util/GraphUtil.kt | 7 +- .../kotlin/com/lambda/util/GraphUtilTest.kt | 242 ++++++++++++++++++ .../kotlin/pathing/GraphConsistencyTest.kt | 96 +++---- 3 files changed, 287 insertions(+), 58 deletions(-) create mode 100644 common/src/test/kotlin/com/lambda/util/GraphUtilTest.kt diff --git a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt index 2419a6014..e6d510cb8 100644 --- a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt +++ b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt @@ -79,8 +79,8 @@ object GraphUtil { val neighbor = fastVectorOf(origin.x + dx, origin.y + dy, origin.z + dz) val cost = when (distSq) { 1 -> 1.0 - 2 -> sqrt(2.0) - 3 -> sqrt(3.0) + 2 -> COST_SQRT_2 + 3 -> COST_SQRT_3 else -> error("Unexpected squared distance: $distSq") } neighbor to cost @@ -91,4 +91,7 @@ object GraphUtil { 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 } \ 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/GraphConsistencyTest.kt b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt index 78bf05265..d59577de0 100644 --- a/common/src/test/kotlin/pathing/GraphConsistencyTest.kt +++ b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt @@ -26,7 +26,6 @@ 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.assertFalse @@ -38,40 +37,8 @@ import kotlin.test.assertTrue * is consistent with a fresh graph created with the same blocked nodes. */ class GraphConsistencyTest { - @Test - fun `graph consistency N6`() { - val startNode = fastVectorOf(0, 0, 5) - val goalNode = fastVectorOf(0, 0, 0) - val blockedNodes = mutableSetOf() - val graph1 = createGridGraph6Conn(blockedNodes) - val dStar1 = DStarLite(graph1, startNode, goalNode, ::euclideanHeuristic) - dStar1.computeShortestPath() - val initialPath = dStar1.path() - - val blockedNode = fastVectorOf(0, 0, 4) - blockedNodes.add(blockedNode) - - dStar1.invalidate(blockedNode) - dStar1.computeShortestPath() - val path1 = dStar1.path() - - assertTrue(initialPath.length() < path1.length(), "Initial path length (${initialPath.length()}) is less than blocked path length (${path1.length()})") - - val graph2 = createGridGraph6Conn(blockedNodes) - val dStar2 = DStarLite(graph2, startNode, goalNode, ::euclideanHeuristic) - dStar2.computeShortestPath() - val path2 = dStar2.path() - - assertEquals(path1, path2, "Graph consistency test failed for N6 graph with blocked node at ${blockedNode.string}.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") - - val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2) - assertFalse(graphDifferences.hasAnyDifferences, graphDifferences.toString()) - assertFalse(valueDifferences.isNotEmpty(), valueDifferences.joinToString("\n ")) - } - /** * Simple test with a single blocked node in a 6-connectivity graph. - * This is similar to the existing test but with more detailed assertions. */ @Test fun `graph consistency with single blocked node N6`() { @@ -91,11 +58,13 @@ class GraphConsistencyTest { dStar1.invalidate(blockedNode) dStar1.computeShortestPath() - val path1 = dStar1.path() + val blocked = dStar1.path() // Verify that the path changed after blocking - assertTrue(initialPath.size < path1.size, - "Initial path size (${initialPath.size}) should be less than blocked path size (${path1.size})") + 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) @@ -104,13 +73,13 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(path1, path2, - "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + assertEquals(blocked, path2, + "Paths should be identical after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}") - // Verify graph structure is consistent + // Verify the graph structure is consistent val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2) assertFalse(graphDifferences.hasAnyDifferences, - "Graph structures should be identical: ${graphDifferences}") + "Graph structures should be identical: $graphDifferences") assertFalse(valueDifferences.isNotEmpty(), "Node values should be identical: ${valueDifferences.joinToString("\n ")}") } @@ -145,11 +114,13 @@ class GraphConsistencyTest { dStar1.invalidate(blockedNode3) dStar1.computeShortestPath() - val path1 = dStar1.path() + val blocked = dStar1.path() // 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()})") + 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) @@ -158,10 +129,10 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(path1, path2, - "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + assertEquals(blocked, path2, + "Paths should be identical after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}") - // Verify graph structure is consistent + // Verify the graph structure is consistent val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2) assertFalse(graphDifferences.hasAnyDifferences, "Graph structures should be identical: $graphDifferences") @@ -190,7 +161,13 @@ class GraphConsistencyTest { dStar1.invalidate(blockedNode) dStar1.computeShortestPath() - val path1 = dStar1.path() + 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) @@ -199,8 +176,8 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(path1, path2, - "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + assertEquals(blocked, path2, + "Paths should be identical after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}") // Verify graph structure is consistent val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2) @@ -231,7 +208,13 @@ class GraphConsistencyTest { dStar1.invalidate(blockedNode) dStar1.computeShortestPath() - val path1 = dStar1.path() + 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) @@ -240,8 +223,8 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(path1, path2, - "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + assertEquals(blocked, path2, + "Paths should be identical after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}") // Verify graph structure is consistent val (graphDifferences, valueDifferences) = dStar1.compareWith(dStar2) @@ -273,15 +256,16 @@ class GraphConsistencyTest { // Unblock the node and invalidate blockedNodes.remove(nodeToToggle) - // We still call invalidate on the node position even though it's now unblocked // The nodeInitializer should handle this correctly dStar1.invalidate(nodeToToggle) dStar1.computeShortestPath() val path1 = dStar1.path() - // Verify that the path changed after unblocking - assertTrue(initialPath.length() < path1.length(), - "Initial path ${initialPath.string()} length (${initialPath.length()}) should be smaller than unblocked path ${path1.string()} length (${path1.length()})") + // 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) From 2a048a27a1366b1f472367edc8515a97b4b9a61f Mon Sep 17 00:00:00 2001 From: Constructor Date: Sun, 4 May 2025 07:20:33 +0200 Subject: [PATCH 35/39] Slight code improvements --- .../kotlin/com/lambda/pathing/dstar/DStarLite.kt | 8 +++----- .../src/main/kotlin/com/lambda/pathing/dstar/Key.kt | 12 +++--------- 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt index 694df8c0b..ea2d244f4 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt @@ -103,6 +103,7 @@ class DStarLite( setG(u, rhs(u)) // Set g = rhs U.remove(u) // Remove from queue, now consistent (g=rhs) // Propagate change to predecessors s + // ToDo: Use predecessors graph.successors(u).forEach { (s, c) -> if (s != goal) { setRHS(s, min(rhs(s), c + g(u))) @@ -117,6 +118,7 @@ class DStarLite( val gOld = g(u) setG(u, INF) + // ToDo: Use predecessors (graph.successors(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) { @@ -278,11 +280,7 @@ class DStarLite( /** Computes min_{s' in Succ(s)} (c(s, s') + g(s')). */ private fun minSuccessorCost(s: FastVector) = - graph.successors(s) - .mapNotNull { (s1, cost) -> - if (cost == INF) null else cost + g(s1) - } - .minOrNull() ?: INF + graph.successors(s).minOfOrNull { (s1, cost) -> cost + g(s1) } ?: INF fun buildDebugInfoRenderer(config: PathingSettings) { if (!config.renderGraph) return diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt index 04682a24c..4e0e078cc 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt +++ b/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt @@ -20,21 +20,15 @@ package com.lambda.pathing.dstar /** * 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*[cite: 142]. + * Comparison is done lexicographically as described in Field D*. */ data class Key(val first: Double, val second: Double) : Comparable { - override fun compareTo(other: Key): Int { - val firstCompare = this.first.compareTo(other.first) - if (firstCompare != 0) { - return firstCompare - } - return this.second.compareTo(other.second) - } + override fun compareTo(other: Key) = + compareValuesBy(this, other, { it.first }, { it.second }) override fun toString() = "(%.3f, %.3f)".format(first, second) companion object { - // Represents an infinite key val INFINITY = Key(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY) } } \ No newline at end of file From 85a922e7ae45b4128ba15d1eb666eb5b474fd303 Mon Sep 17 00:00:00 2001 From: Constructor Date: Sun, 4 May 2025 18:36:59 +0200 Subject: [PATCH 36/39] Incremental package --- .../kotlin/com/lambda/module/modules/movement/Pathfinder.kt | 4 ++-- .../com/lambda/pathing/{dstar => incremental}/DStarLite.kt | 2 +- .../kotlin/com/lambda/pathing/{dstar => incremental}/Key.kt | 2 +- .../com/lambda/pathing/{dstar => incremental}/LazyGraph.kt | 4 +--- .../{dstar => incremental}/UpdatablePriorityQueue.kt | 2 +- common/src/main/kotlin/com/lambda/util/GraphUtil.kt | 2 +- common/src/test/kotlin/pathing/DStarLiteTest.kt | 6 +++--- common/src/test/kotlin/pathing/GraphConsistencyTest.kt | 2 +- common/src/test/kotlin/pathing/KeyTest.kt | 2 +- common/src/test/kotlin/pathing/PathConsistencyTest.kt | 2 +- .../src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt | 4 ++-- 11 files changed, 15 insertions(+), 17 deletions(-) rename common/src/main/kotlin/com/lambda/pathing/{dstar => incremental}/DStarLite.kt (99%) rename common/src/main/kotlin/com/lambda/pathing/{dstar => incremental}/Key.kt (96%) rename common/src/main/kotlin/com/lambda/pathing/{dstar => incremental}/LazyGraph.kt (98%) rename common/src/main/kotlin/com/lambda/pathing/{dstar => incremental}/UpdatablePriorityQueue.kt (99%) 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 index 86c17375e..0deb6377b 100644 --- a/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt +++ b/common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt @@ -37,8 +37,8 @@ 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.dstar.DStarLite -import com.lambda.pathing.dstar.LazyGraph +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 diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt similarity index 99% rename from common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt rename to common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt index ea2d244f4..31c79113f 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt @@ -15,7 +15,7 @@ * along with this program. If not, see . */ -package com.lambda.pathing.dstar +package com.lambda.pathing.incremental import com.lambda.graphics.gl.Matrices import com.lambda.graphics.gl.Matrices.buildWorldProjection diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/Key.kt similarity index 96% rename from common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt rename to common/src/main/kotlin/com/lambda/pathing/incremental/Key.kt index 4e0e078cc..eb477e2d8 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/Key.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/Key.kt @@ -15,7 +15,7 @@ * along with this program. If not, see . */ -package com.lambda.pathing.dstar +package com.lambda.pathing.incremental /** * Represents the Key used in the D* Lite algorithm. diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt similarity index 98% rename from common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt rename to common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt index 449f267a0..39f59d436 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt @@ -15,16 +15,14 @@ * along with this program. If not, see . */ -package com.lambda.pathing.dstar +package com.lambda.pathing.incremental import com.lambda.graphics.renderer.esp.builders.buildLine -import com.lambda.graphics.renderer.esp.builders.buildOutline 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 net.minecraft.util.math.Box import java.awt.Color import java.util.concurrent.ConcurrentHashMap import kotlin.math.abs diff --git a/common/src/main/kotlin/com/lambda/pathing/dstar/UpdatablePriorityQueue.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/UpdatablePriorityQueue.kt similarity index 99% rename from common/src/main/kotlin/com/lambda/pathing/dstar/UpdatablePriorityQueue.kt rename to common/src/main/kotlin/com/lambda/pathing/incremental/UpdatablePriorityQueue.kt index e8d3a540d..4be4f47c8 100644 --- a/common/src/main/kotlin/com/lambda/pathing/dstar/UpdatablePriorityQueue.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/UpdatablePriorityQueue.kt @@ -15,7 +15,7 @@ * along with this program. If not, see . */ -package com.lambda.pathing.dstar +package com.lambda.pathing.incremental import java.util.* import kotlin.NoSuchElementException diff --git a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt index e6d510cb8..c4de6ec97 100644 --- a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt +++ b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt @@ -17,7 +17,7 @@ package com.lambda.util -import com.lambda.pathing.dstar.LazyGraph +import com.lambda.pathing.incremental.LazyGraph import com.lambda.util.world.FastVector import com.lambda.util.world.dist import com.lambda.util.world.fastVectorOf diff --git a/common/src/test/kotlin/pathing/DStarLiteTest.kt b/common/src/test/kotlin/pathing/DStarLiteTest.kt index fc74a0754..9786ea425 100644 --- a/common/src/test/kotlin/pathing/DStarLiteTest.kt +++ b/common/src/test/kotlin/pathing/DStarLiteTest.kt @@ -17,9 +17,9 @@ package pathing -import com.lambda.pathing.dstar.DStarLite -import com.lambda.pathing.dstar.Key -import com.lambda.pathing.dstar.LazyGraph +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 diff --git a/common/src/test/kotlin/pathing/GraphConsistencyTest.kt b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt index d59577de0..67e860f40 100644 --- a/common/src/test/kotlin/pathing/GraphConsistencyTest.kt +++ b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt @@ -17,7 +17,7 @@ package pathing -import com.lambda.pathing.dstar.DStarLite +import com.lambda.pathing.incremental.DStarLite import com.lambda.util.GraphUtil.createGridGraph18Conn import com.lambda.util.GraphUtil.createGridGraph26Conn import com.lambda.util.GraphUtil.createGridGraph6Conn diff --git a/common/src/test/kotlin/pathing/KeyTest.kt b/common/src/test/kotlin/pathing/KeyTest.kt index 63870a84d..9db39d574 100644 --- a/common/src/test/kotlin/pathing/KeyTest.kt +++ b/common/src/test/kotlin/pathing/KeyTest.kt @@ -17,7 +17,7 @@ package pathing -import com.lambda.pathing.dstar.Key +import com.lambda.pathing.incremental.Key import org.junit.jupiter.api.Assertions.* import org.junit.jupiter.api.Test diff --git a/common/src/test/kotlin/pathing/PathConsistencyTest.kt b/common/src/test/kotlin/pathing/PathConsistencyTest.kt index fa744a3e2..04ddd1f86 100644 --- a/common/src/test/kotlin/pathing/PathConsistencyTest.kt +++ b/common/src/test/kotlin/pathing/PathConsistencyTest.kt @@ -17,7 +17,7 @@ package pathing -import com.lambda.pathing.dstar.DStarLite +import com.lambda.pathing.incremental.DStarLite import com.lambda.util.GraphUtil.createGridGraph6Conn import com.lambda.util.GraphUtil.createGridGraph18Conn import com.lambda.util.GraphUtil.createGridGraph26Conn diff --git a/common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt b/common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt index 91b389da8..bb1caa4e0 100644 --- a/common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt +++ b/common/src/test/kotlin/pathing/UpdatablePriorityQueueTest.kt @@ -17,8 +17,8 @@ package pathing -import com.lambda.pathing.dstar.Key -import com.lambda.pathing.dstar.UpdatablePriorityQueue +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 From 669ebe1fe84b2a3d4218b2f9a947c389cece8332 Mon Sep 17 00:00:00 2001 From: Constructor Date: Sun, 4 May 2025 21:42:18 +0200 Subject: [PATCH 37/39] Bidirectional graph initialization --- .../lambda/pathing/incremental/DStarLite.kt | 42 +++++-------------- .../lambda/pathing/incremental/LazyGraph.kt | 10 +++-- 2 files changed, 17 insertions(+), 35 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt index 31c79113f..ed14779a7 100644 --- a/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt @@ -85,10 +85,9 @@ class DStarLite( fun timedOut() = (System.currentTimeMillis() - startTime) > cutoffTimeout - // ToDo: Check why <= needed and not < - fun checkCondition() = U.topKey(Key.INFINITY) <= calculateKey(start) || rhs(start) > g(start) + fun checkCondition() = U.topKey(Key.INFINITY) < calculateKey(start) || rhs(start) > g(start) - while (!U.isEmpty() && checkCondition() && !timedOut()) { + 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 @@ -103,11 +102,8 @@ class DStarLite( setG(u, rhs(u)) // Set g = rhs U.remove(u) // Remove from queue, now consistent (g=rhs) // Propagate change to predecessors s - // ToDo: Use predecessors - graph.successors(u).forEach { (s, c) -> - if (s != goal) { - setRHS(s, min(rhs(s), c + g(u))) - } + graph.predecessors(u).forEach { (s, c) -> + if (s != goal) setRHS(s, min(rhs(s), graph.cost(s, u) + g(u))) updateVertex(s) } } @@ -118,8 +114,7 @@ class DStarLite( val gOld = g(u) setG(u, INF) - // ToDo: Use predecessors - (graph.successors(u).keys + u).forEach { s -> + (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 @@ -189,20 +184,11 @@ class DStarLite( fun updateEdge(u: FastVector, v: FastVector, c: Double) { val cOld = graph.cost(u, v) graph.setCost(u, v, c) -// LOG.info("Setting edge ${u.string} -> ${v.string} to $c") - if (cOld > c) { - if (u != goal) { - setRHS(u, min(rhs(u), c + g(v))) -// LOG.info("Setting RHS of ${u.string} to ${rhs(u)}") - } - } else if (rhs(u) == cOld + g(v)) { - if (u != goal) { - setRHS(u, minSuccessorCost(u)) -// LOG.info("Setting RHS of ${u.string} to ${rhs(u)}") - } + 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) -// LOG.info("Updated vertex ${u.string}") } /** @@ -249,26 +235,20 @@ class DStarLite( /** Internal key calculation using current start and km. */ private fun calculateKey(s: FastVector): Key { val minGRHS = min(g(s), rhs(s)) - return if (minGRHS == INF) { - Key.INFINITY - } else { - Key(minGRHS + heuristic(start, s) + km, minGRHS) - } + 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 - val key = calculateKey(u) - when { // Inconsistent and in Queue: Update priority g(u) != rhs(u) && uInQueue -> { - U.update(u, key) + U.update(u, calculateKey(u)) } // Inconsistent and not in Queue: Insert g(u) != rhs(u) && !uInQueue -> { - U.insert(u, key) + U.insert(u, calculateKey(u)) } // Consistent and in Queue: Remove g(u) == rhs(u) && uInQueue -> { diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt index 39f59d436..e14d87875 100644 --- a/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt @@ -58,10 +58,12 @@ class LazyGraph( } /** Initializes predecessors by ensuring successors of neighboring nodes. */ - fun predecessors(u: FastVector): Map { - successors(u) - return predecessors[u] ?: emptyMap() - } + fun predecessors(u: FastVector): MutableMap = + predecessors.getOrPut(u) { + nodeInitializer(u).onEach { (neighbor, cost) -> + successors.getOrPut(neighbor) { hashMapOf() }[u] = cost + }.toMutableMap() + } fun removeNode(u: FastVector) { successors.remove(u) From 9cc2ff466d2a929a764b5c70ae0875652a66e81c Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 5 May 2025 05:09:29 +0200 Subject: [PATCH 38/39] Fix concurrency on graph render, relax path condition --- .../com/lambda/pathing/PathingConfig.kt | 1 + .../com/lambda/pathing/PathingSettings.kt | 1 + .../lambda/pathing/incremental/DStarLite.kt | 2 +- .../lambda/pathing/incremental/LazyGraph.kt | 24 ++++++------ .../src/test/kotlin/pathing/DStarLiteTest.kt | 3 +- .../kotlin/pathing/GraphConsistencyTest.kt | 20 +++++----- .../kotlin/pathing/PathConsistencyTest.kt | 38 ++++++++++++------- 7 files changed, 52 insertions(+), 37 deletions(-) diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt index df3587787..23a1b7c34 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt @@ -61,6 +61,7 @@ interface PathingConfig { val renderG: Boolean val renderRHS: Boolean val renderKey: Boolean + val renderQueue: Boolean val maxRenderObjects: Int val fontScale: Double val assumeJesus: Boolean diff --git a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt index 2b15de0a7..fcfc221ea 100644 --- a/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt +++ b/common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt @@ -70,6 +70,7 @@ class PathingSettings( 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 } diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt index ed14779a7..7f8ef9628 100644 --- a/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt @@ -272,7 +272,7 @@ class DStarLite( 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 (origin in U) label.add("IN QUEUE") + if (config.renderQueue && origin in U) label.add("QUEUED") if (label.isNotEmpty()) { val pos = origin.toCenterVec3d() diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt index e14d87875..da80bf2c5 100644 --- a/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/LazyGraph.kt @@ -43,26 +43,26 @@ import kotlin.math.abs class LazyGraph( val nodeInitializer: (FastVector) -> Map ) { - val successors = ConcurrentHashMap>() - val predecessors = ConcurrentHashMap>() + 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): MutableMap = + fun successors(u: FastVector): ConcurrentHashMap = successors.getOrPut(u) { - nodeInitializer(u).onEach { (neighbor, cost) -> - predecessors.getOrPut(neighbor) { hashMapOf() }[u] = cost - }.toMutableMap() + ConcurrentHashMap(nodeInitializer(u).onEach { (neighbor, cost) -> + predecessors.getOrPut(neighbor) { ConcurrentHashMap() }[u] = cost + }) } /** Initializes predecessors by ensuring successors of neighboring nodes. */ - fun predecessors(u: FastVector): MutableMap = + fun predecessors(u: FastVector): ConcurrentHashMap = predecessors.getOrPut(u) { - nodeInitializer(u).onEach { (neighbor, cost) -> - successors.getOrPut(neighbor) { hashMapOf() }[u] = cost - }.toMutableMap() + ConcurrentHashMap(nodeInitializer(u).onEach { (neighbor, cost) -> + successors.getOrPut(neighbor) { ConcurrentHashMap() }[u] = cost + }) } fun removeNode(u: FastVector) { @@ -84,8 +84,8 @@ class LazyGraph( } fun setCost(u: FastVector, v: FastVector, c: Double) { - successors.getOrPut(u) { hashMapOf() }[v] = c - predecessors.getOrPut(v) { hashMapOf() }[u] = c + successors.getOrPut(u) { ConcurrentHashMap() }[v] = c + predecessors.getOrPut(v) { ConcurrentHashMap() }[u] = c } fun clear() { diff --git a/common/src/test/kotlin/pathing/DStarLiteTest.kt b/common/src/test/kotlin/pathing/DStarLiteTest.kt index 9786ea425..f34d81fa0 100644 --- a/common/src/test/kotlin/pathing/DStarLiteTest.kt +++ b/common/src/test/kotlin/pathing/DStarLiteTest.kt @@ -24,6 +24,7 @@ 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 @@ -384,6 +385,6 @@ internal class DStarLiteTest { } // The new path should go around the blocked node - assertTrue(newPath.size >= initialPath.size, "New path should be at least as long as the initial path") + 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 index 67e860f40..0f5402e1c 100644 --- a/common/src/test/kotlin/pathing/GraphConsistencyTest.kt +++ b/common/src/test/kotlin/pathing/GraphConsistencyTest.kt @@ -73,8 +73,8 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(blocked, path2, - "Paths should be identical after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}") + 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) @@ -129,8 +129,8 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(blocked, path2, - "Paths should be identical after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}") + 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) @@ -176,8 +176,8 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(blocked, path2, - "Paths should be identical after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}") + 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) @@ -223,8 +223,8 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(blocked, path2, - "Paths should be identical after invalidation.\nPath1: ${blocked.string()}\nPath2: ${path2.string()}") + 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) @@ -274,8 +274,8 @@ class GraphConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(path1, path2, - "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + 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) diff --git a/common/src/test/kotlin/pathing/PathConsistencyTest.kt b/common/src/test/kotlin/pathing/PathConsistencyTest.kt index 04ddd1f86..85f7868a6 100644 --- a/common/src/test/kotlin/pathing/PathConsistencyTest.kt +++ b/common/src/test/kotlin/pathing/PathConsistencyTest.kt @@ -73,9 +73,15 @@ class PathConsistencyTest { 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()}") + // 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}") + } } /** @@ -126,9 +132,15 @@ class PathConsistencyTest { 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()}") + // 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}") + } } /** @@ -199,8 +211,8 @@ class PathConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(path1, path2, - "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + assertEquals(path1.length(), path2.length(), + "Paths should be same length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") } /** @@ -231,7 +243,7 @@ class PathConsistencyTest { // Unblock one node in the middle val nodeToToggle = fastVectorOf(0, 0, 5) - blockedNodes.remove(nodeToToggle) + if (blockedNodes.remove(nodeToToggle)) println("Unblocked node ${nodeToToggle.string}") // Now it should path through the hole in the wall dStar1.invalidate(nodeToToggle) @@ -245,8 +257,8 @@ class PathConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(path1, path2, - "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + assertEquals(path1.length(), path2.length(), + "Paths should be same length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") } /** @@ -291,8 +303,8 @@ class PathConsistencyTest { val path2 = dStar2.path() // Verify paths are identical - assertEquals(path1, path2, - "Paths should be identical after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") + assertEquals(path1.length(), path2.length(), + "Paths should be same length after invalidation.\nPath1: ${path1.string()}\nPath2: ${path2.string()}") } /** From 1777e333e89b3d4fbe9ed9bc6a6e1db19eac6f17 Mon Sep 17 00:00:00 2001 From: Constructor Date: Mon, 5 May 2025 05:13:59 +0200 Subject: [PATCH 39/39] Dynamic connectivity --- .../kotlin/com/lambda/pathing/incremental/DStarLite.kt | 3 ++- common/src/main/kotlin/com/lambda/util/GraphUtil.kt | 7 +++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt index 7f8ef9628..3dfa60c4b 100644 --- a/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt +++ b/common/src/main/kotlin/com/lambda/pathing/incremental/DStarLite.kt @@ -47,6 +47,7 @@ class DStarLite( 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() @@ -150,7 +151,7 @@ class DStarLite( */ fun invalidate(u: FastVector, prune: Boolean = false) { val modified = mutableSetOf(u) - (GraphUtil.n26(u).keys + u).forEach { v -> + (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 } diff --git a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt index c4de6ec97..56f6050a0 100644 --- a/common/src/main/kotlin/com/lambda/util/GraphUtil.kt +++ b/common/src/main/kotlin/com/lambda/util/GraphUtil.kt @@ -66,6 +66,7 @@ object GraphUtil { } } + 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) @@ -94,4 +95,10 @@ object GraphUtil { 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