Improvement: Area Navigation updates (#2537)

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

3 files changed, 144 insertions, 0 deletions

diff --git a/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt b/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt
new file mode 100644
index 000000000..4a245a894
--- /dev/null
+++ b/src/main/java/at/hannibal2/skyhanni/utils/GraphUtils.kt
@@ -0,0 +1,68 @@
+package at.hannibal2.skyhanni.utils
+
+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.Stack
+
+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 entry = distances.lastVisitedNode.takeIf(condition)
+        return entry?.let {
+            distances.findPathToDestination(it)
+        }
+    }
+
+    /**
+     * Find the fastest path from [closestNode] to *all* nodes that matches [condition].
+     */
+    fun findFastestPaths(
+        graph: Graph,
+        closestNode: GraphNode,
+        condition: (GraphNode) -> Boolean = { true },
+    ): Pair<MutableMap<GraphNode, Graph>, MutableMap<GraphNode, Double>> {
+        val paths = mutableMapOf<GraphNode, Graph>()
+
+        val map = mutableMapOf<GraphNode, Double>()
+        val distances = graph.findAllShortestDistances(closestNode)
+        for (graphNode in graph.nodes) {
+            if (!condition(graphNode)) continue
+            val (path, distance) = distances.findPathToDestination(graphNode)
+            paths[graphNode] = path
+            map[graphNode] = distance
+        }
+        return Pair(paths, map)
+    }
+
+    /**
+     * Find all maximal sub graphs of the given graph which are not connected
+     */
+    fun findDisjointClusters(graph: Graph): List<Set<GraphNode>> {
+        val universe = graph.toMutableSet()
+        val allClusters = mutableListOf<Set<GraphNode>>()
+        while (universe.isNotEmpty()) {
+            val cluster = mutableSetOf<GraphNode>()
+            allClusters.add(cluster)
+            val queue = Stack<GraphNode>()
+            queue.add(universe.first())
+            while (queue.isNotEmpty()) {
+                val next = queue.pop()
+                universe.remove(next)
+                cluster.add(next)
+                queue.addAll(next.neighbours.keys)
+                queue.retainAll(universe)
+            }
+        }
+        return allClusters
+    }
+}
diff --git a/src/main/java/at/hannibal2/skyhanni/utils/LorenzVec.kt b/src/main/java/at/hannibal2/skyhanni/utils/LorenzVec.kt
index 51bb8b72c..a573ed78d 100644
--- a/src/main/java/at/hannibal2/skyhanni/utils/LorenzVec.kt
+++ b/src/main/java/at/hannibal2/skyhanni/utils/LorenzVec.kt
@@ -9,6 +9,7 @@ import net.minecraft.util.BlockPos
 import net.minecraft.util.Rotations
 import net.minecraft.util.Vec3
 import kotlin.math.abs
+import kotlin.math.absoluteValue
 import kotlin.math.acos
 import kotlin.math.cos
 import kotlin.math.max
@@ -126,6 +127,8 @@ data class LorenzVec(
     fun lengthSquared(): Double = x * x + y * y + z * z
     fun length(): Double = sqrt(this.lengthSquared())
 
+    fun isNormalized(tolerance: Double = 0.01) = (lengthSquared() - 1.0).absoluteValue < tolerance
+
     fun isZero(): Boolean = x == 0.0 && y == 0.0 && z == 0.0
 
     fun clone(): LorenzVec = LorenzVec(x, y, z)
diff --git a/src/main/java/at/hannibal2/skyhanni/utils/RaycastUtils.kt b/src/main/java/at/hannibal2/skyhanni/utils/RaycastUtils.kt
new file mode 100644
index 000000000..d5aafdcd3
--- /dev/null
+++ b/src/main/java/at/hannibal2/skyhanni/utils/RaycastUtils.kt
@@ -0,0 +1,73 @@
+package at.hannibal2.skyhanni.utils
+
+import net.minecraft.client.Minecraft
+
+object RaycastUtils {
+
+    data class Ray(
+        val origin: LorenzVec,
+        val direction: LorenzVec,
+    ) {
+        init {
+            require(direction.isNormalized())
+        }
+    }
+
+    data class Plane(
+        val origin: LorenzVec,
+        val normal: LorenzVec,
+    ) {
+        init {
+            require(normal.isNormalized())
+        }
+    }
+
+    fun createPlayerLookDirectionRay(): Ray {
+        return Ray(
+            LocationUtils.playerEyeLocation(),
+            Minecraft.getMinecraft().thePlayer.lookVec.toLorenzVec()
+        )
+    }
+
+    /**
+     * Create a plane that contains [point] and is orthogonal to [ray].
+     */
+    fun createOrthogonalPlaneToRayAtPoint(
+        ray: Ray,
+        point: LorenzVec,
+    ): Plane {
+        return Plane(point, ray.direction)
+    }
+
+    /**
+     * Intersect a plane (of any orientation) with a ray. The ray and plane may not be parallel to each other.
+     */
+    fun intersectPlaneWithRay(plane: Plane, ray: Ray): LorenzVec {
+//         require(plane.normal.dotProduct(ray.direction).absoluteValue != 0.0)
+        val intersectionPointDistanceAlongRay =
+            (plane.normal.dotProduct(plane.origin) - plane.normal.dotProduct(ray.origin)) / plane.normal.dotProduct(ray.direction)
+        return ray.origin + ray.direction.scale(intersectionPointDistanceAlongRay)
+    }
+
+    /**
+     * Finds the distance between the given ray and the point. If the point is behind the ray origin (according to the ray's direction),
+     * returns [Double.MAX_VALUE] instead.
+     */
+    fun findDistanceToRay(ray: Ray, point: LorenzVec): Double {
+        val plane = createOrthogonalPlaneToRayAtPoint(ray, point)
+        val intersectionPoint = intersectPlaneWithRay(plane, ray)
+        if ((intersectionPoint - ray.origin).dotProduct(ray.direction) < 0) return Double.MAX_VALUE
+        return intersectionPoint.distance(point)
+    }
+
+    inline fun <T> createDistanceToRayEstimator(ray: Ray, crossinline position: (T) -> LorenzVec): (T) -> Double {
+        return {
+            findDistanceToRay(ray, position(it))
+        }
+    }
+
+    fun <T : Any> List<T>.findClosestPointToRay(ray: Ray, positionExtractor: (T) -> LorenzVec): T? {
+        return minByOrNull(createDistanceToRayEstimator(ray, positionExtractor))
+    }
+
+}