Test graph connectivity on node invalidation

Author: Avanatiker

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)

common/src/test/kotlin/pathing/DStarLiteTest.kt

@@ -51,9 +51,6 @@ import kotlin.test.assertTrue
  */
 
 internal class DStarLiteTest {
-
-    private val blockedNodes = mutableSetOf<FastVector>()
-
     // 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<FastVector>()
+        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<FastVector>()
+        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<FastVector>()
+        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<FastVector>()
+        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")
+    }
 }