Node invalidation on world state update

Author: Avanatiker

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

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,13 +186,18 @@ 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)))
         } else if (rhs(u) == cOld + g(v)) {
             if (u != goal) setRHS(u, minSuccessorCost(u))
         }
         updateVertex(u)
+//        if (c == INF) {
+//            graph.removeEdge(u, v)
+//            graph.removeEdge(v, u)
+//        }
     }
 
     /**

common/src/main/kotlin/com/lambda/pathing/dstar/LazyGraph.kt

@@ -52,42 +52,49 @@ class LazyGraph(
 
     /** Initializes a node if not already initialized, then returns successors. */
     fun successors(u: FastVector): MutableMap<FastVector, Double> =
-        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<FastVector, Double> {
         successors(u)
         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<FastVector> = successors(u).keys + predecessors(u).keys
 
     /** Returns the cost of the edge from u to v (or ∞ if none exists) */