Vertex invalidation approach

Author: Avanatiker

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 <markDirty>",
+    usage = "path <invalidate | reset | update>",
     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}")
                         }
                     }
                 }

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
     }

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<FastVector> = 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

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