Pathing command, detailed graph renderer, graph invalidation

Author: Avanatiker

common/src/main/kotlin/com/lambda/command/commands/PathCommand.kt

@@ -0,0 +1,50 @@
+/*
+ * Copyright 2024 Lambda
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+package com.lambda.command.commands
+
+import com.lambda.brigadier.argument.integer
+import com.lambda.brigadier.argument.literal
+import com.lambda.brigadier.argument.value
+import com.lambda.brigadier.execute
+import com.lambda.brigadier.required
+import com.lambda.command.LambdaCommand
+import com.lambda.module.modules.movement.Pathfinder
+import com.lambda.util.extension.CommandBuilder
+import com.lambda.util.world.fastVectorOf
+
+object PathCommand : LambdaCommand(
+    name = "path",
+    usage = "path <markDirty>",
+    description = "Move through world"
+) {
+    override fun CommandBuilder.create() {
+        required(literal("markDirty")) {
+            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.graph.updateDirtyNode(dirty)
+                        }
+                    }
+                }
+            }
+        }
+    }
+}

common/src/main/kotlin/com/lambda/module/modules/movement/Pathfinder.kt

@@ -17,7 +17,6 @@
 
 package com.lambda.module.modules.movement
 
-import com.lambda.Lambda
 import com.lambda.context.SafeContext
 import com.lambda.event.events.MovementEvent
 import com.lambda.event.events.RenderEvent
@@ -77,7 +76,7 @@ object Pathfinder : Module(
     private val pathing = PathingSettings(this)
 
     private val target: FastVector get() = targetPos.toFastVec()
-    private val graph = LazyGraph { origin ->
+    val graph = LazyGraph { origin ->
         runSafe {
             moveOptions(origin, ::heuristic, pathing).associate { it.pos to it.cost }
         } ?: emptyMap()
@@ -174,7 +173,7 @@ object Pathfinder : Module(
             if (pathing.renderCoarsePath) coarsePath.render(event.renderer, Color.YELLOW)
             if (pathing.renderRefinedPath) refinedPath.render(event.renderer, Color.GREEN)
             if (pathing.renderGoal) event.renderer.buildFilled(Box(target.toBlockPos()), Color.PINK.setAlpha(0.25))
-            if (pathing.renderGraph) graph.render(event.renderer)
+            if (pathing.renderGraph) graph.render(event.renderer, pathing.maxRenderObjects)
         }
 
         listen<RenderEvent.World> {
@@ -183,7 +182,7 @@ object Pathfinder : Module(
             Matrices.push {
                 val c = mc.gameRenderer.camera.pos.negate()
                 translate(c.x, c.y, c.z)
-                graph.buildDebugInfo()
+                graph.buildDebugInfoRenderer(pathing.maxRenderObjects)
             }
         }
     }
@@ -242,7 +241,7 @@ object Pathfinder : Module(
         val dStar = measureTimeMillis {
             dStar.updateStart(start)
             dStar.computeShortestPath(pathing.cutoffTimeout)
-            val nodes = dStar.getPath().map { TraverseMove(it, 0.0, NodeType.OPEN, 0.0, 0.0) }
+            val nodes = dStar.path.map { TraverseMove(it, 0.0, NodeType.OPEN, 0.0, 0.0) }
             long = Path(ArrayDeque(nodes))
         }
         val short: Path

common/src/main/kotlin/com/lambda/pathing/PathingConfig.kt

@@ -43,6 +43,9 @@ interface PathingConfig {
     val renderRefinedPath: Boolean
     val renderGoal: Boolean
     val renderGraph: Boolean
+    val renderWeights: Boolean
+    val renderPositions: Boolean
+    val maxRenderObjects: Int
     val assumeJesus: Boolean
 
     enum class PathingAlgorithm(override val displayName: String) : NamedEnum {

common/src/main/kotlin/com/lambda/pathing/PathingSettings.kt

@@ -52,5 +52,8 @@ class PathingSettings(
     override val renderRefinedPath by c.setting("Render Refined Path", true) { vis() && page == Page.Misc }
     override val renderGoal by c.setting("Render Goal", true) { vis() && page == Page.Misc }
     override val renderGraph by c.setting("Render Graph", false) { vis() && page == Page.Misc }
+    override val renderWeights by c.setting("Render Weights", false) { vis() && page == Page.Misc && renderGraph }
+    override val renderPositions by c.setting("Render Positions", false) { vis() && page == Page.Misc && renderGraph }
+    override val maxRenderObjects by c.setting("Max Render Objects", 1000, 0..100_000) { vis() && page == Page.Misc && renderGraph }
     override val assumeJesus by c.setting("Assume Jesus", false) { vis() && page == Page.Misc }
 }

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

@@ -21,16 +21,16 @@ import com.lambda.util.world.FastVector
 import kotlin.math.min
 
 /**
- * D* Lite Implementation.
+ * Lazy D* Lite Implementation.
  *
  * We perform a backward search from the goal to the start, so:
  *  - rhs(goal) = 0, g(goal) = ∞
- *  - "start" is the robot's current location from which we want a path *to* the goal
+ *  - "start" is the agent's current location from which we want a path *to* the goal
  *  - 'km' accumulates the heuristic shift so we don't reorder the entire queue after each move
  *
  * @param graph      The graph on which we plan (with forward + reverse adjacency).
  * @param heuristic  A consistent (or at least nonnegative) heuristic function h(a,b).
- * @param start      The robot's current position.
+ * @param start      The agent's current position.
  * @param goal       The fixed goal vertex.
  */
 class DStarLite(
@@ -67,10 +67,10 @@ class DStarLite(
     }
 
     private fun g(u: FastVector): Double = gMap[u] ?: INF
-    private fun setG(u: FastVector, value: Double) { gMap[u] = value }
+    private fun setG(u: FastVector, g: Double) { gMap[u] = g }
 
     private fun rhs(u: FastVector): Double = rhsMap[u] ?: INF
-    private fun setRHS(u: FastVector, value: Double) { rhsMap[u] = value }
+    private fun setRHS(u: FastVector, rhs: Double) { rhsMap[u] = rhs }
 
     /**
      * Calculates the key for vertex u.
@@ -147,7 +147,7 @@ class DStarLite(
     }
 
     /**
-     * When the robot moves, update the start.
+     * When the agent moves, update the start.
      * The variable km is increased by h(oldStart, newStart) and
      * all vertices in the queue are re-keyed.
      */
@@ -170,35 +170,37 @@ class DStarLite(
      * Retrieves a path from start to goal by always choosing the successor
      * with the lowest g + cost value. If no path is found, the path stops early.
      */
-    fun getPath(): List<FastVector> {
-        val path = mutableListOf<FastVector>()
-
-        if (!graph.contains(start)) return path.toList()
-
-        var current = start
-        path.add(current)
-        while (current != goal) {
-            val successors = graph.successors(current)
-            if (successors.isEmpty()) break
-            var bestNext: FastVector? = null
-            var bestVal = INF
-            for ((succ, cost) in successors) {
-                val candidate = g(succ) + cost
-                if (candidate < bestVal) {
-                    bestVal = candidate
-                    bestNext = succ
+    val path: List<FastVector>
+        get() {
+            val path = mutableListOf<FastVector>()
+
+            if (!graph.contains(start)) return path.toList()
+
+            var current = start
+            path.add(current)
+            while (current != goal) {
+                val successors = graph.successors(current)
+                if (successors.isEmpty()) break
+                var bestNext: FastVector? = null
+                var bestVal = INF
+                for ((succ, cost) in successors) {
+                    val candidate = g(succ) + cost
+                    if (candidate < bestVal) {
+                        bestVal = candidate
+                        bestNext = succ
+                    }
                 }
+                // No path
+                if (bestNext == null) break
+                current = bestNext
+                path.add(current)
+                if (path.size > MAX_PATH_LENGTH) break
             }
-            // No path
-            if (bestNext == null) break
-            current = bestNext
-            path.add(current)
-            if (path.size > 100_000) break
+            return path
         }
-        return path
-    }
 
     companion object {
         private const val INF = Double.POSITIVE_INFINITY
+        private const val MAX_PATH_LENGTH = 100_000
     }
 }

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

@@ -22,13 +22,12 @@ package com.lambda.pathing.dstar
  * They are compared lexicographically.
  */
 data class Key(val k1: Double, val k2: Double) : Comparable<Key> {
-    override fun compareTo(other: Key): Int {
-        return when {
+    override fun compareTo(other: Key) =
+        when {
             this.k1 < other.k1 -> -1
             this.k1 > other.k1 -> 1
             this.k2 < other.k2 -> -1
             this.k2 > other.k2 -> 1
             else -> 0
         }
-    }
 }

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

@@ -17,14 +17,23 @@
 
 package com.lambda.pathing.dstar
 
+import com.lambda.graphics.gl.Matrices
+import com.lambda.graphics.gl.Matrices.buildWorldProjection
+import com.lambda.graphics.gl.Matrices.withVertexTransform
 import com.lambda.graphics.renderer.esp.builders.buildLine
+import com.lambda.graphics.renderer.esp.builders.buildOutline
 import com.lambda.graphics.renderer.esp.global.StaticESP
+import com.lambda.graphics.renderer.gui.FontRenderer
+import com.lambda.graphics.renderer.gui.FontRenderer.drawString
+import com.lambda.util.math.Vec2d
+import com.lambda.util.math.div
+import com.lambda.util.math.plus
 import com.lambda.util.world.FastVector
+import com.lambda.util.world.string
 import com.lambda.util.world.toCenterVec3d
-import com.lambda.util.world.toVec3d
+import net.minecraft.util.math.Box
 import java.awt.Color
 import java.util.concurrent.ConcurrentHashMap
-import java.util.concurrent.ConcurrentMap
 
 /**
  * A 3D graph that uses FastVector (a Long) to represent 3D nodes.
@@ -49,29 +58,42 @@ class LazyGraph(
     val size get() = successors.size
 
     /** Initializes a node if not already initialized, then returns successors. */
-    fun successors(u: FastVector) =
-        successors.computeIfAbsent(u) {
+    fun successors(u: FastVector): Map<FastVector, Double> {
+        if (u in dirtyNodes) {
+            updateDirtyNode(u)
+        }
+        return successors.getOrPut(u) {
             val neighbors = nodeInitializer(u)
             neighbors.forEach { (neighbor, cost) ->
                 predecessors.computeIfAbsent(neighbor) { hashMapOf() }[u] = cost
             }
             neighbors.toMutableMap()
         }
+    }
 
     /** Initializes predecessors by ensuring successors of neighboring nodes. */
     fun predecessors(u: FastVector): Map<FastVector, Double> {
         successors(u)
         return predecessors[u] ?: emptyMap()
     }
 
-    fun markDirty(pos: FastVector) {
-        dirtyNodes.add(pos)
-        predecessors[pos]?.keys?.let { pred ->
+    fun markDirty(u: FastVector) {
+        dirtyNodes.add(u)
+        predecessors[u]?.keys?.let { pred ->
             dirtyNodes.addAll(pred)
         }
     }
 
-    fun clearDirty() = dirtyNodes.clear()
+    fun updateDirtyNode(u: FastVector) {
+        // Force re-initialize node and clear existing edges
+        val newNeighbors = nodeInitializer(u)
+        successors[u]?.clear()
+        newNeighbors.forEach { (v, cost) ->
+            predecessors.getOrPut(v) { mutableMapOf() }[u] = cost
+        }
+        successors[u] = newNeighbors.toMutableMap()
+        dirtyNodes.remove(u)
+    }
 
     /** 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
@@ -83,27 +105,40 @@ class LazyGraph(
         predecessors.clear()
     }
 
-    fun render(renderer: StaticESP) {
-        successors.entries.take(1000).forEach { (origin, neighbors) ->
-            neighbors.forEach { (neighbor, cost) ->
+    fun render(renderer: StaticESP, maxElements: Int = 1000) {
+        successors.entries.take(maxElements).forEach { (origin, neighbors) ->
+            neighbors.forEach { (neighbor, _) ->
                 renderer.buildLine(origin.toCenterVec3d(), neighbor.toCenterVec3d(), Color.PINK)
             }
         }
+        dirtyNodes.take(maxElements).forEach { node ->
+            renderer.buildOutline(
+                Box.of(node.toCenterVec3d(), 0.3, 0.3, 0.3),
+                Color.RED
+            )
+        }
     }
 
-    fun buildDebugInfo() {
-//        val projection = buildWorldProjection(blockPos, 0.4, Matrices.ProjRotationMode.TO_CAMERA)
-//        withVertexTransform(projection) {
-//            val lines = arrayOf(
-//                ""
-//            )
-//
-//            var height = -0.5 * lines.size * (FontRenderer.getHeight() + 2)
-//
-//            lines.forEach {
-//                drawString(it, Vec2d(-FontRenderer.getWidth(it) * 0.5, height))
-//                height += FontRenderer.getHeight() + 2
-//            }
-//        }
+    fun buildDebugInfoRenderer(maxElements: Int = 1000) {
+        successors.entries.take(maxElements).forEach { (v, u) ->
+            val mode = Matrices.ProjRotationMode.TO_CAMERA
+            val scale = 0.4
+            val pos = v.toCenterVec3d()
+            val nodeProjection = buildWorldProjection(pos, scale, mode)
+            withVertexTransform(nodeProjection) {
+                val msg = v.string
+                drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0))
+            }
+            u.forEach { (neighbor, cost) ->
+                val centerV = v.toCenterVec3d()
+                val centerN = neighbor.toCenterVec3d()
+                val center = (centerV + centerN) / 2.0
+                val projection = buildWorldProjection(center, scale, mode)
+                withVertexTransform(projection) {
+                    val msg = "c: %.3f".format(cost)
+                    drawString(msg, Vec2d(-FontRenderer.getWidth(msg) * 0.5, 0.0))
+                }
+            }
+        }
     }
 }

common/src/main/kotlin/com/lambda/util/world/Position.kt

@@ -275,3 +275,6 @@ internal fun Long.bitSetTo(value: Long, position: Int, length: Int): Long {
     val mask = (1L shl length) - 1L
     return this and (mask shl position).inv() or (value and mask shl position)
 }
+
+val FastVector.string: String
+    get() = "($x, $y, $z)"