Update rhs of newly created nodes

Author: Avanatiker

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 {

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<FastVector>()
+
         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
     }
-}
+}

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<FastVector>()
+        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")
+    }
 }