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.

Author: Avanatiker

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}" }}")
                         }
                     }
                 }

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<WorldEvent.BlockUpdate.Client> {
-//            val pos = it.pos.toFastVec()
-//            graph.markDirty(pos)
-//            info("Updated block at ${it.pos} to ${it.newState.block.name.string} rescheduled D*Lite.")
-//        }
+        listen<WorldEvent.BlockUpdate.Client> {
+            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<RotationEvent.StrafeInput> { event ->
             if (!pathing.moveAlongPath) return@listen

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

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 }

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<FastVector>()
-        val affectedNeighbors = mutableSetOf<FastVector>()
-        val pathNodes = mutableSetOf<FastVector>()
-        val modifiedNodes = mutableSetOf<FastVector>()
-
-        // 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<FastVector> = 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,38 +212,19 @@ class DStarLite(
      */
     fun path(maxLength: Int = 10_000): List<FastVector> {
         val path = mutableListOf<FastVector>()
-        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)
 
         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
     }

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<FastVector> = emptySet()) {
-        // Nodes to check for pruning
+    fun prune(modifiedNodes: Set<FastVector> = emptySet()): Set<FastVector> {
         val nodesToCheck = if (modifiedNodes.isEmpty()) {
             // If no modified nodes specified, check all nodes
             nodes.toSet()
@@ -154,26 +153,27 @@ class LazyGraph(
 
         // Remove nodes with only infinite connections
         nodesToRemove.forEach { removeNode(it) }
+        return nodesToRemove
     }
 
     /**
      * Returns the successors of a node without initializing it if it doesn't exist.
      * This is useful for debugging and testing.
      */
     fun getSuccessorsWithoutInitializing(u: FastVector): Map<FastVector, Double> {
-        return successors[u] ?: emptyMap()
+        return successors[u]?.filter { it.value.isFinite() } ?: emptyMap()
     }
 
     /**
      * Returns the predecessors of a node without initializing it if it doesn't exist.
      * This is useful for debugging and testing.
      */
     fun getPredecessorsWithoutInitializing(u: FastVector): Map<FastVector, Double> {
-        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<FastVector> = successors(u).keys + predecessors(u).keys
+    fun neighbors(u: FastVector): Set<FastVector> = 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

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

@@ -51,12 +51,15 @@ import kotlin.reflect.KFunction1
 object MoveFinder {
     private val nodeTypeCache = HashMap<FastVector, NodeType>()
 
-    fun SafeContext.moveOptions(origin: FastVector, heuristic: KFunction1<FastVector, Double>, config: PathingConfig) =
-        EightWayDirection.entries.flatMap { direction ->
+    fun SafeContext.moveOptions(origin: FastVector, heuristic: KFunction1<FastVector, Double>, config: PathingConfig): Set<Move> {
+        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<FastVector, Double>,
@@ -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()
 }