abstracted navigation rerouting (#2597)

Co-authored-by: hannibal2 <24389977+hannibal00212@users.noreply.github.com>

2 files changed, 77 insertions, 10 deletions

diff --git a/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt b/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt
index 4a245a894..0d15b99ed 100644
--- a/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt
+++ b/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt
@@ -1,10 +1,10 @@
 package at.hannibal2.skyhanni.utils
 
+import at.hannibal2.skyhanni.data.model.DijkstraTree
 import at.hannibal2.skyhanni.data.model.Graph
 import at.hannibal2.skyhanni.data.model.GraphNode
-import at.hannibal2.skyhanni.data.model.findAllShortestDistances
-import at.hannibal2.skyhanni.data.model.findDijkstraDistances
 import at.hannibal2.skyhanni.data.model.findPathToDestination
+import java.util.PriorityQueue
 import java.util.Stack
 
 object GraphUtils {
@@ -12,11 +12,10 @@ object GraphUtils {
      * Find the fastest path from [closestNode] to *any* node that matches [condition].
      */
     fun findFastestPath(
-        graph: Graph,
         closestNode: GraphNode,
         condition: (GraphNode) -> Boolean,
     ): Pair<Graph, Double>? {
-        val distances = graph.findDijkstraDistances(closestNode, condition)
+        val distances = findDijkstraDistances(closestNode, condition)
         val entry = distances.lastVisitedNode.takeIf(condition)
         return entry?.let {
             distances.findPathToDestination(it)
@@ -34,7 +33,7 @@ object GraphUtils {
         val paths = mutableMapOf<GraphNode, Graph>()
 
         val map = mutableMapOf<GraphNode, Double>()
-        val distances = graph.findAllShortestDistances(closestNode)
+        val distances = findAllShortestDistances(closestNode)
         for (graphNode in graph.nodes) {
             if (!condition(graphNode)) continue
             val (path, distance) = distances.findPathToDestination(graphNode)
@@ -65,4 +64,66 @@ object GraphUtils {
         }
         return allClusters
     }
+
+    fun findShortestPathAsGraph(start: GraphNode, end: GraphNode): Graph = findShortestPathAsGraphWithDistance(start, end).first
+
+    /**
+     * Find a tree of distances to the [start] node using dijkstra's algorithm.
+     */
+    fun findDijkstraDistances(
+        start: GraphNode,
+        /**
+         * Bail out early before collecting all the distances to all nodes in the graph. This will not collect valid distance data for *all*
+         * nodes for which bailout matches, but only the closest one.
+         */
+        bailout: (GraphNode) -> Boolean,
+    ): DijkstraTree {
+        val distances = mutableMapOf<GraphNode, Double>()
+        val previous = mutableMapOf<GraphNode, GraphNode>()
+        val visited = mutableSetOf<GraphNode>()
+        val queue = PriorityQueue<GraphNode>(compareBy { distances.getOrDefault(it, Double.MAX_VALUE) })
+        var lastVisitedNode: GraphNode = start
+
+        distances[start] = 0.0
+        queue.add(start)
+
+        while (queue.isNotEmpty()) {
+            val current = queue.poll()
+            lastVisitedNode = current
+            if (bailout(current)) break
+
+            visited.add(current)
+
+            current.neighbours.forEach { (neighbour, weight) ->
+                if (neighbour !in visited) {
+                    val newDistance = distances.getValue(current) + weight
+                    if (newDistance < distances.getOrDefault(neighbour, Double.MAX_VALUE)) {
+                        distances[neighbour] = newDistance
+                        previous[neighbour] = current
+                        queue.add(neighbour)
+                    }
+                }
+            }
+        }
+
+        return DijkstraTree(
+            start,
+            distances,
+            previous,
+            lastVisitedNode,
+        )
+    }
+
+    fun findAllShortestDistances(start: GraphNode): DijkstraTree {
+        return findDijkstraDistances(start) { false }
+    }
+
+    fun findShortestPathAsGraphWithDistance(start: GraphNode, end: GraphNode): Pair<Graph, Double> {
+        val distances = findDijkstraDistances(start) { it == end }
+        return distances.findPathToDestination(end)
+    }
+
+    fun findShortestPath(start: GraphNode, end: GraphNode): List<LorenzVec> = findShortestPathAsGraph(start, end).toPositionsList()
+
+    fun findShortestDistance(start: GraphNode, end: GraphNode): Double = findShortestPathAsGraphWithDistance(start, end).second
 }
diff --git a/src/main/java/at/hannibal2/skyhanni/utils/RenderUtils.kt b/src/main/java/at/hannibal2/skyhanni/utils/RenderUtils.kt
index 805db90f5..d22ecc85e 100644
--- a/src/main/java/at/hannibal2/skyhanni/utils/RenderUtils.kt
+++ b/src/main/java/at/hannibal2/skyhanni/utils/RenderUtils.kt
@@ -5,7 +5,6 @@ import at.hannibal2.skyhanni.data.GuiEditManager
 import at.hannibal2.skyhanni.data.GuiEditManager.getAbsX
 import at.hannibal2.skyhanni.data.GuiEditManager.getAbsY
 import at.hannibal2.skyhanni.data.model.Graph
-import at.hannibal2.skyhanni.data.model.toPositionsList
 import at.hannibal2.skyhanni.events.GuiContainerEvent
 import at.hannibal2.skyhanni.events.GuiRenderItemEvent
 import at.hannibal2.skyhanni.events.LorenzRenderWorldEvent
@@ -1280,7 +1279,8 @@ object RenderUtils {
         waypointColor: Color =
             (path.lastOrNull()?.name?.getFirstColorCode()?.toLorenzColor() ?: LorenzColor.WHITE).toColor(),
         bezierPoint: Double = 1.0,
-        showNoteNames: Boolean = false
+        showNoteNames: Boolean = false,
+        markLastBlock: Boolean = true,
     ) {
         if (path.isEmpty()) return
         val points = if (startAtEye) {
@@ -1307,8 +1307,10 @@ object RenderUtils {
                 this.drawDynamicText(it.position, it.name!!, textSize)
             }
         }
-        val last = path.last()
-        drawWaypointFilled(last.position, waypointColor, seeThroughBlocks = true)
+        if (markLastBlock) {
+            val last = path.last()
+            drawWaypointFilled(last.position, waypointColor, seeThroughBlocks = true)
+        }
     }
 
     class LineDrawer @PublishedApi internal constructor(val tessellator: Tessellator) {
@@ -1586,7 +1588,11 @@ object RenderUtils {
 
     // TODO nea please merge with 'draw3DLine'
     fun LorenzRenderWorldEvent.draw3DLine_nea(
-        p1: LorenzVec, p2: LorenzVec, color: Color, lineWidth: Int, depth: Boolean,
+        p1: LorenzVec,
+        p2: LorenzVec,
+        color: Color,
+        lineWidth: Int,
+        depth: Boolean,
     ) {
         GlStateManager.disableCull()