Lazy D* Lite

Author: Avanatiker

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

@@ -36,8 +36,12 @@ import com.lambda.pathing.Pathing.findPathAStar
 import com.lambda.pathing.Pathing.thetaStarClearance
 import com.lambda.pathing.PathingConfig
 import com.lambda.pathing.PathingSettings
+import com.lambda.pathing.dstar.DStarLite
+import com.lambda.pathing.dstar.LazyGraph
 import com.lambda.pathing.goal.SimpleGoal
+import com.lambda.pathing.move.MoveFinder.moveOptions
 import com.lambda.threading.runConcurrent
+import com.lambda.threading.runSafe
 import com.lambda.util.Communication.info
 import com.lambda.util.Formatting.string
 import com.lambda.util.math.setAlpha
@@ -162,7 +166,7 @@ object Pathfinder : Module(
                             thetaStarClearance(long, pathing)
                         } else long
                     }
-                    info("A* (Length: ${long.length().string} Nodes: ${long.moves.size} T: $aStar ms) and Theta* (Length: ${short.length().string} Nodes: ${short.moves.size} T: $thetaStar ms)")
+                    info("A* (Length: ${long.length().string} Nodes: ${long.size} T: $aStar ms) and Theta* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)")
                     println("Long: $long | Short: $short")
                     short.moves.removeFirstOrNull()
                     coarsePath = long
@@ -172,7 +176,7 @@ object Pathfinder : Module(
             }
             PathingConfig.PathingAlgorithm.D_STAR_LITE -> {
                 runConcurrent {
-                    // 1. Build graph from goal to target
+
                 }
             }
         }

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

@@ -52,6 +52,8 @@ data class Path(
 
     fun length() = length.value
 
+    val size get() = moves.size
+
     override fun toString() =
         moves.joinToString(" -> ") { "(${it.pos.toBlockPos().toShortString()})" }
 }

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

@@ -56,7 +56,7 @@ object Pathing {
 
             closedSet.add(current.pos)
 
-            moveOptions(current, goal, config).forEach { move ->
+            moveOptions(current.pos, goal::heuristic, config).forEach { move ->
 //                println("Considering move: $move")
                 if (closedSet.contains(move.pos)) return@forEach
                 val tentativeGCost = current.gCost + move.cost

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

@@ -34,19 +34,17 @@ import kotlin.math.min
  * @param goal       The fixed goal vertex.
  */
 class DStarLite(
-    private val graph: Graph,
+    private val graph: LazyGraph,
     private val heuristic: (FastVector, FastVector) -> Double,
     var start: FastVector,
     private val goal: FastVector
 ) {
-    private val INF = Double.POSITIVE_INFINITY
-
     // gMap[u], rhsMap[u] store g(u) and rhs(u) or default to ∞ if not present
     private val gMap = mutableMapOf<FastVector, Double>()
     private val rhsMap = mutableMapOf<FastVector, Double>()
 
     // Priority queue holding inconsistent vertices.
-    private val U = PriorityQueueDStar()
+    private val U = PriorityQueueDStar<FastVector>()
 
     // km accumulates heuristic differences as the start changes.
     private var km = 0.0
@@ -193,4 +191,8 @@ class DStarLite(
         }
         return path
     }
+
+    companion object {
+        private const val INF = Double.POSITIVE_INFINITY
+    }
 }

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

@@ -0,0 +1,68 @@
+/*
+ * Copyright 2025 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.pathing.dstar
+
+import com.lambda.context.SafeContext
+import com.lambda.util.world.FastVector
+
+/**
+ * A 3D graph that uses FastVector (a Long) to represent 3D nodes.
+ *
+ * Runtime Complexity:
+ * - successor(u): O(N), where N is the number of neighbors of node u, due to node initialization.
+ * - predecessors(u): O(N), similar reasoning to successors(u).
+ * - cost(u, v): O(1), constant time lookup after initialization.
+ * - contains(u): O(1), hash map lookup.
+ *
+ * Space Complexity:
+ * - O(V + E), where V = number of nodes (vertices) initialized and E = number of edges stored.
+ * - Additional memory overhead is based on the dynamically expanding hash maps.
+ */
+
+class LazyGraph(
+    private val nodeInitializer: (FastVector) -> Map<FastVector, Double>
+) {
+    private val successors = hashMapOf<FastVector, MutableMap<FastVector, Double>>()
+    private val predecessors = hashMapOf<FastVector, MutableMap<FastVector, Double>>()
+
+    /** Initializes a node if not already initialized, then returns successors. */
+    fun successors(u: FastVector) =
+        successors.computeIfAbsent(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()
+    }
+
+    /** 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
+
+    fun contains(u: FastVector): Boolean = u in successors
+
+    fun clear() {
+        successors.clear()
+        predecessors.clear()
+    }
+}

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

@@ -17,16 +17,14 @@
 
 package com.lambda.pathing.dstar
 
-import com.lambda.util.world.FastVector
 import java.util.*
 
 /**
  * Priority queue for D* Lite 3D.
- * Supports: topKey, top, pop, insertOrUpdate, and remove.
  */
-class PriorityQueueDStar {
-    private val pq = PriorityQueue<Pair<FastVector, Key>>(compareBy { it.second })
-    private val vertexToKey = mutableMapOf<FastVector, Key>()
+class PriorityQueueDStar<T : Any> {
+    private val pq = PriorityQueue<Pair<T, Key>>(compareBy { it.second })
+    private val vertexToKey = mutableMapOf<T, Key>()
 
     fun isEmpty() = pq.isEmpty()
 
@@ -35,16 +33,16 @@ class PriorityQueueDStar {
         else pq.peek().second
     }
 
-    fun top() = pq.peek()?.first
+    fun top(): T? = pq.peek()?.first
 
-    fun pop(): FastVector? {
+    fun pop(): T? {
         if (pq.isEmpty()) return null
         val (v, _) = pq.poll()
         vertexToKey.remove(v)
         return v
     }
 
-    fun insertOrUpdate(v: FastVector, key: Key) {
+    fun insertOrUpdate(v: T, key: Key) {
         val oldKey = vertexToKey[v]
         if (oldKey == null || oldKey != key) {
             remove(v)
@@ -53,7 +51,7 @@ class PriorityQueueDStar {
         }
     }
 
-    fun remove(v: FastVector) {
+    fun remove(v: T) {
         val oldKey = vertexToKey[v] ?: return
         vertexToKey.remove(v)
         pq.remove(Pair(v, oldKey))

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

@@ -51,25 +51,25 @@ import kotlin.reflect.KFunction1
 object MoveFinder {
     private val nodeTypeCache = HashMap<FastVector, NodeType>()
 
-    fun SafeContext.moveOptions(origin: Move, goal: Goal, config: PathingConfig) =
+    fun SafeContext.moveOptions(origin: FastVector, heuristic: KFunction1<FastVector, Double>, config: PathingConfig) =
         EightWayDirection.entries.flatMap { direction ->
             (-1..1).mapNotNull { y ->
-                getPathNode(goal::heuristic, origin, direction, y, config)
+                getPathNode(heuristic, origin, direction, y, config)
             }
         }
 
     private fun SafeContext.getPathNode(
         heuristic: KFunction1<FastVector, Double>,
-        origin: Move,
+        origin: FastVector,
         direction: EightWayDirection,
         height: Int,
         config: PathingConfig
     ): Move? {
         val offset = fastVectorOf(direction.offsetX, height, direction.offsetZ)
         val diagonal = direction.ordinal.mod(2) == 1
-        val checkingPos = origin.pos.add(offset)
+        val checkingPos = origin.add(offset)
         val checkingBlockPos = checkingPos.toBlockPos()
-        val originBlockPos = origin.pos.toBlockPos()
+        val originBlockPos = origin.toBlockPos()
         if (!world.worldBorder.contains(checkingBlockPos)) return null
 
         val nodeType = findPathType(checkingPos)