Fix refining of path start

Author: Avanatiker

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

@@ -51,6 +51,7 @@ import com.lambda.util.player.MovementUtils.buildMovementInput
 import com.lambda.util.player.MovementUtils.mergeFrom
 import com.lambda.util.world.FastVector
 import com.lambda.util.world.WorldUtils.hasSupport
+import com.lambda.util.world.WorldUtils.isPathClear
 import com.lambda.util.world.fastVectorOf
 import com.lambda.util.world.toBlockPos
 import com.lambda.util.world.toFastVec
@@ -120,7 +121,8 @@ object Pathfinder : Module(
                 currentStart = playerPos.toFastVec()
                 needsUpdate = true
             }
-            updateTargetNode()
+            if (pathing.moveAlongPath) updateTargetNode()
+//            info("${isPathClear(playerPos, targetPos)}")
         }
 
 //        listen<WorldEvent.BlockUpdate.Client> {
@@ -231,7 +233,6 @@ object Pathfinder : Module(
         }
         info("A* (Length: ${long.length().string} Nodes: ${long.size} T: $aStar ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)")
 //        println("Long: $long | Short: $short")
-        short.moves.removeFirstOrNull()
         coarsePath = long
         refinedPath = short
     }
@@ -252,7 +253,6 @@ object Pathfinder : Module(
         }
         info("Lazy D* Lite (Length: ${long.length().string} Nodes: ${long.size} Graph Size: ${graph.size} T: $dStar ms) and \u03b8* (Length: ${short.length().string} Nodes: ${short.size} T: $thetaStar ms)")
 //        println("Long: $long | Short: $short")
-        short.moves.removeFirstOrNull()
         coarsePath = long
         refinedPath = short
     }

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

@@ -73,7 +73,7 @@ object Pathing {
     }
 
     fun SafeContext.thetaStarClearance(path: Path, config: PathingConfig): Path {
-        if (path.moves.isEmpty()) return Path()
+        if (path.moves.isEmpty()) return path
 
         val cleanedPath = Path()
         var currentIndex = 0
@@ -84,22 +84,18 @@ object Pathing {
             cleanedPath.append(startMove)
 
             // Attempt to skip over as many nodes as possible
-            // by checking if they share the same Y and have a clear path
             var nextIndex = currentIndex + 1
             while (nextIndex < path.moves.size) {
                 val candidateMove = path.moves[nextIndex]
+                val startPos = startMove.pos.toBlockPos()
+                val candidatePos = candidateMove.pos.toBlockPos()
 
                 // Only try to skip if both moves are on the same Y level
-                if (startMove.pos.y != candidateMove.pos.y) break
+                if (startPos.y != candidatePos.y) break
 
                 // Verify there's a clear path from the start move to the candidate
-                if (
-                    isPathClear(
-                        startMove.pos.toBlockPos(),
-                        candidateMove.pos.toBlockPos(),
-                        config.clearancePrecision
-                    )
-                ) nextIndex++ else break
+                val isClear = isPathClear(startPos, candidatePos, config.clearancePrecision)
+                if (isClear) nextIndex++ else break
             }
 
             // Move to the last node that was confirmed reachable

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

@@ -112,10 +112,7 @@ class LazyGraph(
             }
         }
         dirtyNodes.take(maxElements).forEach { node ->
-            renderer.buildOutline(
-                Box.of(node.toCenterVec3d(), 0.3, 0.3, 0.3),
-                Color.RED
-            )
+            renderer.buildOutline(Box.of(node.toCenterVec3d(), 0.2, 0.2, 0.2), Color.RED)
         }
     }