NodeTypes and different moves

Author: Avanatiker

common/src/main/kotlin/com/lambda/interaction/construction/simulation/Simulation.kt

@@ -30,6 +30,7 @@ import com.lambda.module.modules.client.TaskFlowModule
 import com.lambda.threading.runSafe
 import com.lambda.util.BlockUtils.blockState
 import com.lambda.util.world.FastVector
+import com.lambda.util.world.WorldUtils.playerBox
 import com.lambda.util.world.WorldUtils.playerFitsIn
 import com.lambda.util.world.WorldUtils.traversable
 import com.lambda.util.world.toBlockPos
@@ -73,8 +74,6 @@ data class Simulation(
     }
 
     companion object {
-        fun Vec3d.playerBox(): Box = Box(x - 0.3, y, z - 0.3, x + 0.3, y + 1.8, z + 0.3).contract(1.0E-6)
-
         fun Blueprint.simulation(
             interact: InteractionConfig = TaskFlowModule.interact,
             rotation: RotationConfig = TaskFlowModule.rotation,

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

@@ -17,17 +17,25 @@
 
 package com.lambda.module.modules.movement
 
+import com.lambda.config.groups.RotationSettings
 import com.lambda.context.SafeContext
 import com.lambda.event.events.RenderEvent
 import com.lambda.event.events.RotationEvent
+import com.lambda.event.events.TickEvent
 import com.lambda.event.listener.SafeListener.Companion.listen
 import com.lambda.graphics.renderer.esp.builders.buildLine
+import com.lambda.interaction.request.rotation.Rotation
 import com.lambda.interaction.request.rotation.Rotation.Companion.rotationTo
+import com.lambda.interaction.request.rotation.RotationManager.onRotate
+import com.lambda.interaction.request.rotation.visibilty.lookAt
 import com.lambda.module.Module
 import com.lambda.module.tag.ModuleTag
 import com.lambda.pathing.Path
 import com.lambda.pathing.Pathing.findPathAStar
+import com.lambda.pathing.PathingSettings
 import com.lambda.pathing.goal.SimpleGoal
+import com.lambda.threading.runConcurrent
+import com.lambda.util.Communication.info
 import com.lambda.util.math.setAlpha
 import com.lambda.util.player.MovementUtils.buildMovementInput
 import com.lambda.util.player.MovementUtils.mergeFrom
@@ -38,75 +46,96 @@ import net.minecraft.util.math.Vec3d
 import java.awt.Color
 import kotlin.math.cos
 import kotlin.math.sin
+import kotlin.system.measureTimeMillis
 
 object Pathfinder : Module(
     name = "Pathfinder",
     description = "Get from A to B",
     defaultTags = setOf(ModuleTag.MOVEMENT)
 ) {
-    // PID Settings
-    private val kP by setting("P Gain", 0.5, 0.0..2.0, 0.01)
-    private val kI by setting("I Gain", 0.0, 0.0..1.0, 0.01)
-    private val kD by setting("D Gain", 0.2, 0.0..1.0, 0.01)
-    private val tolerance by setting("Node Tolerance", 0.1, 0.01..1.0, 0.01)
+    private val pathing = PathingSettings(this)
+    private val rotation = RotationSettings(this)
 
-    var path = Path()
+    var path: Path? = null
     private var currentTarget: Vec3d? = null
     private var integralError = Vec3d.ZERO
     private var lastError = Vec3d.ZERO
+    private var calculating = false
 
     init {
         onEnable {
-            path = findPathAStar(
-                player.blockPos.toFastVec(),
-                SimpleGoal(fastVectorOf(0, 120, 0))
-            )
-//            currentTarget = Vec3d(0.5, 120.0, 0.5)
             integralError = Vec3d.ZERO
             lastError = Vec3d.ZERO
         }
 
-        listen<RotationEvent.StrafeInput> { event ->
-            if (path.nodes.isEmpty()) return@listen
+        listen<TickEvent.Pre> {
+            if (calculating) return@listen
+            calculating = true
 
-            updateTargetNode()
-            currentTarget?.let { target ->
-                event.strafeYaw = player.eyePos.rotationTo(target).yaw
-                val adjustment = calculatePID(target)
-                val yawRad = Math.toRadians(event.strafeYaw)
-                val forward = -sin(yawRad)
-                val strafe = cos(yawRad)
-
-                val forwardComponent = adjustment.x * forward + adjustment.z * strafe
-                val strafeComponent = adjustment.x * strafe - adjustment.z * forward
-
-                val moveInput = buildMovementInput(
-                    forward = forwardComponent,
-                    strafe = strafeComponent,
-                    jump = player.isOnGround && adjustment.y > 0.5
-                )
-                event.input.mergeFrom(moveInput)
+            runConcurrent {
+                val took = measureTimeMillis {
+                    path = findPathAStar(
+                        player.blockPos.toFastVec(),
+                        //                        SimpleGoal(fastVectorOf(0, 78, 0)),
+                        SimpleGoal(fastVectorOf(0, 120, 0)),
+                        pathing.cutoffTimeout
+                    )
+                }
+                info("Found path of length ${path?.moves?.size} in $took ms")
+                println("Path: ${path?.toString()}")
+                calculating = false
             }
         }
 
+//        listen<RotationEvent.StrafeInput> { event ->
+//            updateTargetNode()
+//            currentTarget?.let { target ->
+//                event.strafeYaw = player.eyePos.rotationTo(target).yaw
+//                val adjustment = calculatePID(target)
+//                val yawRad = Math.toRadians(event.strafeYaw)
+//                val forward = -sin(yawRad)
+//                val strafe = cos(yawRad)
+//
+//                val forwardComponent = adjustment.x * forward + adjustment.z * strafe
+//                val strafeComponent = adjustment.x * strafe - adjustment.z * forward
+//
+//                val moveInput = buildMovementInput(
+//                    forward = forwardComponent,
+//                    strafe = strafeComponent,
+//                    jump = player.isOnGround && adjustment.y > 0.5
+//                )
+//                event.input.mergeFrom(moveInput)
+//            }
+//        }
+//
+//        onRotate {
+//            val nextTarget = path?.moves?.getOrNull(2)?.pos?.toBlockPos() ?: return@onRotate
+//            val part = player.eyePos.rotationTo(Vec3d.ofBottomCenter(nextTarget))
+//            val targetRotation = Rotation(part.yaw, player.pitch.toDouble())
+//
+//            lookAt(targetRotation).requestBy(rotation)
+//        }
+
         listen<RenderEvent.StaticESP> { event ->
-            path.nodes.zipWithNext { current, next ->
+            path?.moves?.zipWithNext { current, next ->
                 val currentPos = current.pos.toBlockPos().toCenterPos()
                 val nextPos = next.pos.toBlockPos().toCenterPos()
-                event.renderer.buildLine(currentPos, nextPos, Color.BLUE.setAlpha(0.25))
+                event.renderer.buildLine(currentPos, nextPos, Color.GREEN)
             }
         }
     }
 
     private fun SafeContext.updateTargetNode() {
-        path.nodes.firstOrNull()?.let { firstNode ->
+        path?.moves?.firstOrNull()?.let { firstNode ->
             val nodeVec = Vec3d.ofBottomCenter(firstNode.pos.toBlockPos())
-            if (player.pos.distanceTo(nodeVec) < tolerance) {
-                path.nodes.removeFirst()
+            if (player.pos.distanceTo(nodeVec) < pathing.tolerance) {
+                path?.moves?.removeFirst()
                 integralError = Vec3d.ZERO
             }
-            val next = path.nodes.firstOrNull()?.pos?.toBlockPos() ?: return
+            val next = path?.moves?.firstOrNull()?.pos?.toBlockPos() ?: return
             currentTarget = Vec3d.ofBottomCenter(next)
+        } ?: run {
+            currentTarget = null
         }
     }
 
@@ -116,8 +145,8 @@ object Pathfinder : Module(
         val derivativeError = error.subtract(lastError)
         lastError = error
 
-        return error.multiply(kP)
-            .add(integralError.multiply(kI))
-            .add(derivativeError.multiply(kD))
+        return error.multiply(pathing.kP)
+            .add(integralError.multiply(pathing.kI))
+            .add(derivativeError.multiply(pathing.kD))
     }
 }

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

@@ -17,15 +17,16 @@
 
 package com.lambda.pathing
 
+import com.lambda.pathing.move.Move
 import com.lambda.util.world.toBlockPos
 
 data class Path(
-    val nodes: ArrayDeque<Node> = ArrayDeque(),
+    val moves: ArrayDeque<Move> = ArrayDeque(),
 ) {
-    fun prepend(node: Node) {
-        nodes.addFirst(node)
+    fun prepend(move: Move) {
+        moves.addFirst(move)
     }
 
     override fun toString() =
-        nodes.joinToString(" -> ") { "(${it.pos.toBlockPos().toShortString()})" }
+        moves.joinToString(" -> ") { "(${it.pos.toBlockPos().toShortString()})" }
 }

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

@@ -18,42 +18,55 @@
 package com.lambda.pathing
 
 import com.lambda.context.SafeContext
-import com.lambda.pathing.Node.Companion.toNode
 import com.lambda.pathing.goal.Goal
+import com.lambda.pathing.move.Move
+import com.lambda.pathing.move.MoveFinder
+import com.lambda.pathing.move.MoveFinder.findPathType
+import com.lambda.pathing.move.MoveFinder.getFeetY
 import com.lambda.pathing.move.MoveFinder.moveOptions
+import com.lambda.pathing.move.TraverseMove
+import com.lambda.util.Communication.warn
 import com.lambda.util.world.FastVector
+import com.lambda.util.world.toBlockPos
 import java.util.PriorityQueue
 
 object Pathing {
-    fun SafeContext.findPathAStar(start: FastVector, goal: Goal): Path {
-        val openSet = PriorityQueue<Node>()
-        val closedSet = mutableSetOf<Node>()
-        val startNode = start.toNode(goal)
+    fun SafeContext.findPathAStar(start: FastVector, goal: Goal, timeout: Long = 50L): Path {
+        MoveFinder.clean()
+        val startedAt = System.currentTimeMillis()
+        val openSet = PriorityQueue<Move>()
+        val closedSet = mutableSetOf<FastVector>()
+        val startFeetY = getFeetY(start.toBlockPos())
+        val startNode = TraverseMove(start, goal.heuristic(start), findPathType(start), startFeetY, 0.0)
         startNode.gCost = 0.0
         openSet.add(startNode)
 
-        while (openSet.isNotEmpty()) {
+        println("Starting pathfinding at ${start.toBlockPos().toShortString()} to $goal")
+
+        while (openSet.isNotEmpty() && startedAt + timeout > System.currentTimeMillis()) {
             val current = openSet.remove()
+            println("Considering node: ${current.pos.toBlockPos()}")
             if (goal.inGoal(current.pos)) {
-//                println("Not yet considered nodes: ${openSet.size}")
-//                println("Closed nodes: ${closedSet.size}")
+                println("Not yet considered nodes: ${openSet.size}")
+                println("Closed nodes: ${closedSet.size}")
                 return current.createPathToSource()
             }
 
-            closedSet.add(current)
+            closedSet.add(current.pos)
 
-            moveOptions(current.pos).forEach { move ->
-                val successor = move.node(current.pos, goal)
-                if (closedSet.contains(successor)) return@forEach
-                val tentativeGCost = current.gCost + move.cost()
-                if (tentativeGCost >= successor.gCost) return@forEach
-                successor.predecessor = current
-                successor.gCost = tentativeGCost
-                openSet.add(successor)
+            moveOptions(current, goal).forEach { move ->
+                println("Considering move: $move")
+                if (closedSet.contains(move.pos)) return@forEach
+                val tentativeGCost = current.gCost + move.cost
+                if (tentativeGCost >= move.gCost) return@forEach
+                move.predecessor = current
+                move.gCost = tentativeGCost
+                openSet.add(move)
+                println("Using move: $move")
             }
         }
 
-        println("No path found")
-        return Path()
+        warn("Only partial path found!")
+        return if (openSet.isNotEmpty()) openSet.remove().createPathToSource() else Path()
     }
 }

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

@@ -0,0 +1,28 @@
+/*
+ * 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.pathing
+
+interface PathingConfig {
+    val kP: Double
+    val kI: Double
+    val kD: Double
+    val tolerance: Double
+    val cutoffTimeout: Long
+
+    val assumeJesus: Boolean
+}