From 16f3064742dd1efc12ee32066245f7c95e818d7a Mon Sep 17 00:00:00 2001 From: vince calder <92699085+awrped@users.noreply.github.com> Date: Sun, 18 Jan 2026 19:57:15 -0600 Subject: [PATCH 1/8] part 1 --- .../init/{MixinAutoDiscover.java => CobaltMixinPlugin.java} | 4 ++-- src/main/resources/cobalt.mixins.json | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) rename src/main/java/org/cobalt/init/{MixinAutoDiscover.java => CobaltMixinPlugin.java} (96%) diff --git a/src/main/java/org/cobalt/init/MixinAutoDiscover.java b/src/main/java/org/cobalt/init/CobaltMixinPlugin.java similarity index 96% rename from src/main/java/org/cobalt/init/MixinAutoDiscover.java rename to src/main/java/org/cobalt/init/CobaltMixinPlugin.java index c7bb5d6..02b5c50 100644 --- a/src/main/java/org/cobalt/init/MixinAutoDiscover.java +++ b/src/main/java/org/cobalt/init/CobaltMixinPlugin.java @@ -16,14 +16,14 @@ import org.spongepowered.asm.mixin.extensibility.IMixinConfigPlugin; import org.spongepowered.asm.mixin.extensibility.IMixinInfo; -public class MixinAutoDiscover implements IMixinConfigPlugin { +public class CobaltMixinPlugin implements IMixinConfigPlugin { private final List mixins = new ArrayList<>(); @Override public void onLoad(String mixinPackage) { try { - URL location = MixinAutoDiscover.class.getProtectionDomain().getCodeSource().getLocation(); + URL location = CobaltMixinPlugin.class.getProtectionDomain().getCodeSource().getLocation(); Path path = Paths.get(location.toURI()); if (Files.isDirectory(path)) { diff --git a/src/main/resources/cobalt.mixins.json b/src/main/resources/cobalt.mixins.json index 6ff9d23..8ebe839 100644 --- a/src/main/resources/cobalt.mixins.json +++ b/src/main/resources/cobalt.mixins.json @@ -3,7 +3,7 @@ "minVersion": "0.8", "compatibilityLevel": "JAVA_21", "package": "org.cobalt.mixin", - "plugin": "org.cobalt.init.MixinAutoDiscover", + "plugin": "org.cobalt.init.CobaltMixinPlugin", "injectors": { "defaultRequire": 1 } From 0b4b23ae338cec15405c5ea0a5e20d574a7185b3 Mon Sep 17 00:00:00 2001 From: vince calder <92699085+awrped@users.noreply.github.com> Date: Sun, 18 Jan 2026 20:17:22 -0600 Subject: [PATCH 2/8] Revert "part 1" This reverts commit 16f3064742dd1efc12ee32066245f7c95e818d7a. --- .../init/{CobaltMixinPlugin.java => MixinAutoDiscover.java} | 4 ++-- src/main/resources/cobalt.mixins.json | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) rename src/main/java/org/cobalt/init/{CobaltMixinPlugin.java => MixinAutoDiscover.java} (96%) diff --git a/src/main/java/org/cobalt/init/CobaltMixinPlugin.java b/src/main/java/org/cobalt/init/MixinAutoDiscover.java similarity index 96% rename from src/main/java/org/cobalt/init/CobaltMixinPlugin.java rename to src/main/java/org/cobalt/init/MixinAutoDiscover.java index 02b5c50..c7bb5d6 100644 --- a/src/main/java/org/cobalt/init/CobaltMixinPlugin.java +++ b/src/main/java/org/cobalt/init/MixinAutoDiscover.java @@ -16,14 +16,14 @@ import org.spongepowered.asm.mixin.extensibility.IMixinConfigPlugin; import org.spongepowered.asm.mixin.extensibility.IMixinInfo; -public class CobaltMixinPlugin implements IMixinConfigPlugin { +public class MixinAutoDiscover implements IMixinConfigPlugin { private final List mixins = new ArrayList<>(); @Override public void onLoad(String mixinPackage) { try { - URL location = CobaltMixinPlugin.class.getProtectionDomain().getCodeSource().getLocation(); + URL location = MixinAutoDiscover.class.getProtectionDomain().getCodeSource().getLocation(); Path path = Paths.get(location.toURI()); if (Files.isDirectory(path)) { diff --git a/src/main/resources/cobalt.mixins.json b/src/main/resources/cobalt.mixins.json index 8ebe839..6ff9d23 100644 --- a/src/main/resources/cobalt.mixins.json +++ b/src/main/resources/cobalt.mixins.json @@ -3,7 +3,7 @@ "minVersion": "0.8", "compatibilityLevel": "JAVA_21", "package": "org.cobalt.mixin", - "plugin": "org.cobalt.init.CobaltMixinPlugin", + "plugin": "org.cobalt.init.MixinAutoDiscover", "injectors": { "defaultRequire": 1 } From 36d49cd4d54be4d3c90d722f09272b90c1bac1cd Mon Sep 17 00:00:00 2001 From: vince calder <92699085+awrped@users.noreply.github.com> Date: Mon, 19 Jan 2026 18:29:25 -0600 Subject: [PATCH 3/8] feat: pathfinder of doom MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit this took me literally all day but im happy to contribute :))))) pls dont close my pr and put notes of what youd want to change i would literally kill myself if this was made for no reason and i just wasted my time. i tried to make it as prod ready as posibel 👍 --- .../kotlin/org/cobalt/api/pathfinder/Node.kt | 70 ++++ .../org/cobalt/api/pathfinder/PathExecutor.kt | 163 +++++++++ .../pathfinder/factory/PathfinderFactory.kt | 14 + .../factory/PathfinderInitializer.kt | 8 + .../factory/impl/AStarPathfinderFactory.kt | 17 + .../pathfinder/pathfinder/AStarPathfinder.kt | 243 ++++++++++++++ .../pathfinder/AbstractPathfinder.kt | 309 ++++++++++++++++++ .../pathfinder/heap/PrimitiveMinHeap.kt | 137 ++++++++ .../processing/EvaluationContextImpl.kt | 49 +++ .../processing/SearchContextImpl.kt | 30 ++ .../pathfinder/pathing/INeighborStrategy.kt | 12 + .../pathfinder/pathing/NeighborStrategies.kt | 44 +++ .../api/pathfinder/pathing/Pathfinder.kt | 23 ++ .../pathfinder/pathing/PathfindingProgress.kt | 13 + .../pathing/calc/DistanceCalculator.kt | 7 + .../configuration/PathfinderConfiguration.kt | 182 +++++++++++ .../pathing/context/EnvironmentContext.kt | 3 + .../pathing/heuristic/HeuristicContext.kt | 34 ++ .../pathing/heuristic/HeuristicStrategies.kt | 6 + .../pathing/heuristic/HeuristicWeights.kt | 22 ++ .../pathing/heuristic/IHeuristicStrategy.kt | 8 + .../heuristic/LinearHeuristicStrategy.kt | 99 ++++++ .../heuristic/SquaredHeuristicStrategy.kt | 100 ++++++ .../pathfinder/pathing/hook/PathfinderHook.kt | 5 + .../pathing/hook/PathfindingContext.kt | 12 + .../api/pathfinder/pathing/processing/Cost.kt | 14 + .../pathing/processing/CostProcessor.kt | 7 + .../pathing/processing/Processor.kt | 11 + .../pathing/processing/ValidationProcessor.kt | 7 + .../pathing/processing/Validators.kt | 121 +++++++ .../processing/context/EvaluationContext.kt | 41 +++ .../processing/context/SearchContext.kt | 15 + .../processing/impl/MinecraftPathProcessor.kt | 127 +++++++ .../api/pathfinder/pathing/result/Path.kt | 10 + .../pathfinder/pathing/result/PathState.kt | 10 + .../pathing/result/PathfinderResult.kt | 9 + .../pathfinder/provider/NavigationPoint.kt | 9 + .../provider/NavigationPointProvider.kt | 12 + .../impl/MinecraftNavigationProvider.kt | 146 +++++++++ .../cobalt/api/pathfinder/result/PathImpl.kt | 27 ++ .../cobalt/api/pathfinder/result/PathUtils.kt | 137 ++++++++ .../pathfinder/result/PathfinderResultImpl.kt | 29 ++ .../api/pathfinder/util/GridRegionData.kt | 33 ++ .../pathfinder/util/ParameterizedSupplier.kt | 5 + .../cobalt/api/pathfinder/util/RegionKey.kt | 20 ++ .../cobalt/api/pathfinder/wrapper/Depth.kt | 13 + .../api/pathfinder/wrapper/PathPosition.kt | 98 ++++++ .../api/pathfinder/wrapper/PathVector.kt | 81 +++++ .../cobalt/internal/command/MainCommand.kt | 41 ++- 49 files changed, 2616 insertions(+), 17 deletions(-) create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/Node.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/PathExecutor.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderFactory.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderInitializer.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/factory/impl/AStarPathfinderFactory.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AbstractPathfinder.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/heap/PrimitiveMinHeap.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/processing/EvaluationContextImpl.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/processing/SearchContextImpl.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/INeighborStrategy.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/NeighborStrategies.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/Pathfinder.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/PathfindingProgress.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/calc/DistanceCalculator.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/configuration/PathfinderConfiguration.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/context/EnvironmentContext.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicContext.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicStrategies.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicWeights.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/IHeuristicStrategy.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/hook/PathfinderHook.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/hook/PathfindingContext.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Cost.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/CostProcessor.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Processor.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/ValidationProcessor.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/context/EvaluationContext.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/context/SearchContext.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/impl/MinecraftPathProcessor.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/Path.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/PathState.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/PathfinderResult.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/provider/NavigationPoint.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/provider/NavigationPointProvider.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/provider/impl/MinecraftNavigationProvider.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/result/PathImpl.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/result/PathfinderResultImpl.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/util/GridRegionData.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/util/ParameterizedSupplier.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/util/RegionKey.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/wrapper/Depth.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathPosition.kt create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathVector.kt diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/Node.kt b/src/main/kotlin/org/cobalt/api/pathfinder/Node.kt new file mode 100644 index 0000000..1cdab39 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/Node.kt @@ -0,0 +1,70 @@ +package org.cobalt.api.pathfinder + + +import org.cobalt.api.pathfinder.pathing.heuristic.HeuristicContext +import org.cobalt.api.pathfinder.pathing.heuristic.HeuristicWeights +import org.cobalt.api.pathfinder.pathing.heuristic.IHeuristicStrategy +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class Node( + private val position: PathPosition, + start: PathPosition, + target: PathPosition, + heuristicWeights: HeuristicWeights, + heuristicStrategy: IHeuristicStrategy, + private val depth: Int, +) : Comparable { + + private val hCost: Double = heuristicStrategy.calculate( + HeuristicContext(position, start, target, heuristicWeights) + ) + + private var gCost: Double = 0.0 + private var parent: Node? = null + + fun getPosition(): PathPosition = position + + fun getHeuristic(): Double = hCost + + fun getParent(): Node? = parent + + fun getDepth(): Int = depth + + fun setGCost(gCost: Double) { + this.gCost = gCost + } + + fun setParent(parent: Node?) { + this.parent = parent + } + + fun isTarget(target: PathPosition): Boolean = this.position == target + + fun getFCost(): Double = getGCost() + getHeuristic() + + fun getGCost(): Double { + return if (this.parent == null) 0.0 else this.gCost + } + + override fun equals(other: Any?): Boolean { + if (other == null || this::class != other::class) return false + other as Node + return position == other.position + } + + override fun hashCode(): Int = position.hashCode() + + override fun compareTo(other: Node): Int { + val fCostComparison = this.getFCost().compareTo(other.getFCost()) + if (fCostComparison != 0) { + return fCostComparison + } + + val heuristicComparison = this.getHeuristic().compareTo(other.getHeuristic()) + if (heuristicComparison != 0) { + return heuristicComparison + } + + return this.depth.compareTo(other.depth) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/PathExecutor.kt b/src/main/kotlin/org/cobalt/api/pathfinder/PathExecutor.kt new file mode 100644 index 0000000..5e7465c --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/PathExecutor.kt @@ -0,0 +1,163 @@ +package org.cobalt.api.pathfinder + +import java.awt.Color +import net.minecraft.client.Minecraft +import net.minecraft.world.phys.AABB +import net.minecraft.world.phys.Vec3 +import org.cobalt.api.event.EventBus +import org.cobalt.api.event.annotation.SubscribeEvent +import org.cobalt.api.event.impl.client.TickEvent +import org.cobalt.api.event.impl.render.WorldRenderEvent +import org.cobalt.api.pathfinder.factory.impl.AStarPathfinderFactory +import org.cobalt.api.pathfinder.pathing.NeighborStrategies +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration +import org.cobalt.api.pathfinder.pathing.processing.impl.MinecraftPathProcessor +import org.cobalt.api.pathfinder.pathing.result.Path +import org.cobalt.api.pathfinder.pathing.result.PathState +import org.cobalt.api.pathfinder.provider.impl.MinecraftNavigationProvider +import org.cobalt.api.pathfinder.wrapper.PathPosition +import org.cobalt.api.util.ChatUtils +import org.cobalt.api.util.render.Render3D + +/* + * TODO: im lazy right now, but chunk and world caching would be alot better, + * if someone could help me do this it would be a great help :)) + */ +object PathExecutor { + + private val mc: Minecraft = Minecraft.getInstance() + private var currentPath: Path? = null + private var currentWaypointIndex: Int = 0 + + init { + EventBus.register(this) + } + + fun start(x: Double, y: Double, z: Double) { + val player = mc.player ?: return + val start = PathPosition(player.x, player.y, player.z) + val target = PathPosition(x, y, z) + + val factory = AStarPathfinderFactory() + + val processor = MinecraftPathProcessor() + val config = + PathfinderConfiguration.builder() + .provider(MinecraftNavigationProvider()) + .maxIterations(20000) + .async(true) + .neighborStrategy(NeighborStrategies.HORIZONTAL_DIAGONAL_AND_VERTICAL) + .nodeValidationProcessors(listOf(processor)) + .nodeCostProcessors(listOf(processor)) + .build() + + val pathfinder = factory.createPathfinder(config) + + ChatUtils.sendDebug("Calculating path to $x, $y, $z...") + val startTime = System.currentTimeMillis() + pathfinder.findPath(start, target).thenAccept { result -> + val duration = System.currentTimeMillis() - startTime + if (result.successful()) { + currentPath = result.getPath() + currentWaypointIndex = 0 + + val state = result.getPathState() + val path = result.getPath() + + if (state == PathState.FOUND) { + ChatUtils.sendMessage( + "§aPath found! §7Calculated in §f${duration}ms §8(${path.length()} nodes)" + ) + } else { + /* + * partial paths can happen, i would recommend improving this functionality + * to whoever maintainer wants to maintain my horrible shitcode of a pathfinder. + * i think partial paths should be refactored, i will write all the cases now + * iteration limit -> self explanatory xd (or if u cant use ur brain + * then its just if the algorithm takes too many iterations to find a goal. + * this is set literaaally like 20 lines above you...) + * fallback -> pf searches everywhere and couldnt find a possible way to get there. + * this can happen in the case of unloaded chunks, or an obstruction + */ + ChatUtils.sendMessage( + "§ePartial path found! §7Calculated in §f${duration}ms §8(${path.length()} nodes)" + ) + } + } else { + ChatUtils.sendMessage("§cFailed to find path: ${result.getPathState()}") + } + } + } + + fun stop() { + currentPath = null + currentWaypointIndex = 0 + } + + /* + * as of writing this, there is intentionally no moving, rotations, + * or jumping yet. i ask that you make sure that the algo works good + * before implementing them, aswell as rots + */ + @SubscribeEvent + fun onTick(@Suppress("UNUSED_PARAMETER") event: TickEvent.Start) { + val path = currentPath ?: return + val player = mc.player ?: return + + val waypoints = path.collect().toList() + if (currentWaypointIndex >= waypoints.size) { + ChatUtils.sendMessage("Reached the end!") // this is kinda icky, someone change this lol + stop() + return + } + + val targetPos = waypoints[currentWaypointIndex].mid() + val targetVec = Vec3(targetPos.getX(), targetPos.getY(), targetPos.getZ()) + + val horizontalDistSq = + (player.x - targetVec.x) * (player.x - targetVec.x) + + (player.z - targetVec.z) * (player.z - targetVec.z) + if (horizontalDistSq < 0.25) { + currentWaypointIndex++ + } + } + + @SubscribeEvent + fun onRender(event: WorldRenderEvent.Last) { + val path = currentPath ?: return + val waypoints = path.collect().toList() + + if (waypoints.size < 2) return + + for (i in 0 until waypoints.size - 1) { + val start = waypoints[i].mid() + val end = waypoints[i + 1].mid() + + Render3D.drawLine( + event.context, + Vec3(start.getX(), start.getY(), start.getZ()), + Vec3(end.getX(), end.getY(), end.getZ()), + Color.CYAN, + true, + 2.0f + ) + } + + if (currentWaypointIndex < waypoints.size) { + val currentPos = waypoints[currentWaypointIndex].mid() + Render3D.drawBox( + event.context, + AABB( + currentPos.getX() - 0.25, + currentPos.getY() - 0.25, + currentPos.getZ() - 0.25, + currentPos.getX() + 0.25, + currentPos.getY() + 0.25, + currentPos.getZ() + 0.25 + ), + Color.GREEN, + true + ) + } + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderFactory.kt b/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderFactory.kt new file mode 100644 index 0000000..bf89122 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderFactory.kt @@ -0,0 +1,14 @@ +package org.cobalt.api.pathfinder.factory + +import org.cobalt.api.pathfinder.pathing.Pathfinder +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration + +interface PathfinderFactory { + fun createPathfinder(): Pathfinder + fun createPathfinder(configuration: PathfinderConfiguration): Pathfinder + fun createPathfinder(configuration: PathfinderConfiguration, initializer: PathfinderInitializer): Pathfinder { + val pathfinder = createPathfinder(configuration) + initializer.initialize(pathfinder, configuration) + return pathfinder + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderInitializer.kt b/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderInitializer.kt new file mode 100644 index 0000000..171dfc7 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderInitializer.kt @@ -0,0 +1,8 @@ +package org.cobalt.api.pathfinder.factory + +import org.cobalt.api.pathfinder.pathing.Pathfinder +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration + +interface PathfinderInitializer { + fun initialize(pathfinder: Pathfinder, configuration: PathfinderConfiguration) +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/factory/impl/AStarPathfinderFactory.kt b/src/main/kotlin/org/cobalt/api/pathfinder/factory/impl/AStarPathfinderFactory.kt new file mode 100644 index 0000000..e83047e --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/factory/impl/AStarPathfinderFactory.kt @@ -0,0 +1,17 @@ +package org.cobalt.api.pathfinder.factory.impl + +import org.cobalt.api.pathfinder.factory.PathfinderFactory +import org.cobalt.api.pathfinder.pathfinder.AStarPathfinder +import org.cobalt.api.pathfinder.pathing.Pathfinder +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration + +class AStarPathfinderFactory : PathfinderFactory { + + override fun createPathfinder(): Pathfinder { + return AStarPathfinder(PathfinderConfiguration.builder().build()) + } + + override fun createPathfinder(configuration: PathfinderConfiguration): Pathfinder { + return AStarPathfinder(configuration) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt new file mode 100644 index 0000000..9568de2 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt @@ -0,0 +1,243 @@ +package org.cobalt.api.pathfinder.pathfinder + +import it.unimi.dsi.fastutil.longs.Long2DoubleMap +import it.unimi.dsi.fastutil.longs.Long2DoubleOpenHashMap +import it.unimi.dsi.fastutil.longs.Long2ObjectMap +import it.unimi.dsi.fastutil.longs.Long2ObjectOpenHashMap +import kotlin.math.abs +import kotlin.math.max +import net.minecraft.util.Mth +import org.cobalt.api.pathfinder.Node +import org.cobalt.api.pathfinder.pathfinder.heap.PrimitiveMinHeap +import org.cobalt.api.pathfinder.pathfinder.processing.EvaluationContextImpl +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration +import org.cobalt.api.pathfinder.pathing.processing.Cost +import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext +import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext +import org.cobalt.api.pathfinder.util.GridRegionData +import org.cobalt.api.pathfinder.util.RegionKey +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfinder(configuration) { + + private val currentSession = ThreadLocal() + + override fun insertStartNode(node: Node, fCost: Double, openSet: PrimitiveMinHeap) { + val session = getSessionOrThrow() + val packedPos = RegionKey.pack(node.getPosition()) + openSet.insertOrUpdate(packedPos, fCost) + session.openSetNodes[packedPos] = node + } + + override fun extractBestNode(openSet: PrimitiveMinHeap): Node { + val session = getSessionOrThrow() + val packedPos = openSet.extractMin() + val node = session.openSetNodes[packedPos]!! + session.openSetNodes.remove(packedPos) + return node + } + + override fun initializeSearch() { + currentSession.set(PathfindingSession()) + } + + override fun processSuccessors( + start: PathPosition, + target: PathPosition, + currentNode: Node, + openSet: PrimitiveMinHeap, + searchContext: SearchContext, + ) { + val session = getSessionOrThrow() + val offsets = neighborStrategy.getOffsets(currentNode.getPosition()) + + for (offset in offsets) { + val neighborPos = currentNode.getPosition().add(offset) + val packedPos = RegionKey.pack(neighborPos) + + if (openSet.contains(packedPos)) { + val existing = session.openSetNodes[packedPos]!! + updateExistingNode(existing, packedPos, currentNode, searchContext, openSet) + continue + } + + val regionData = session.getOrCreateRegionData(neighborPos) + if (regionData.getBloomFilter().mightContain(neighborPos) && + regionData.getRegionalExaminedPositions().contains(packedPos) + ) { + + var shouldReopen = false + if (pathfinderConfiguration.shouldReopenClosedNodes()) { + val oldCost = session.closedSetGCosts[packedPos] + + val tempNeighbor = createNeighborNode(neighborPos, start, target, currentNode) + val context = EvaluationContextImpl( + searchContext, + tempNeighbor, + currentNode, + pathfinderConfiguration.heuristicStrategy + ) + val newGCost = calculateGCost(context) + + if (oldCost.isNaN() || newGCost + Math.ulp(newGCost) < oldCost) { + session.closedSetGCosts[packedPos] = newGCost + shouldReopen = true + } + } + + if (!shouldReopen) continue + } + + val neighbor = createNeighborNode(neighborPos, start, target, currentNode) + neighbor.setParent(currentNode) + + val context = EvaluationContextImpl( + searchContext, + neighbor, + currentNode, + pathfinderConfiguration.heuristicStrategy + ) + + if (!isValidByCustomProcessors(context)) { + continue + } + + val gCost = calculateGCost(context) + neighbor.setGCost(gCost) + val fCost = neighbor.getFCost() + val heapKey = calculateHeapKey(neighbor, fCost) + + openSet.insertOrUpdate(packedPos, heapKey) + session.openSetNodes[packedPos] = neighbor + } + } + + private fun updateExistingNode( + existing: Node, + packedPos: Long, + currentNode: Node, + searchContext: SearchContext, + openSet: PrimitiveMinHeap, + ) { + val context = EvaluationContextImpl( + searchContext, + existing, + currentNode, + pathfinderConfiguration.heuristicStrategy + ) + + val newG = calculateGCost(context) + val tol = Math.ulp(max(abs(newG), abs(existing.getGCost()))) + + if (newG + tol >= existing.getGCost()) return + + if (!isValidByCustomProcessors(context)) { + return + } + + existing.setParent(currentNode) + existing.setGCost(newG) + val newF = existing.getFCost() + val newKey = calculateHeapKey(existing, newF) + val oldKey = openSet.getCost(packedPos) + + if (newKey + Math.ulp(newKey) < oldKey) { + openSet.insertOrUpdate(packedPos, newKey) + } else if (abs(newKey - oldKey) <= Math.ulp(newKey)) { + openSet.insertOrUpdate(packedPos, oldKey - Math.ulp(oldKey)) + } + } + + private fun createNeighborNode( + position: PathPosition, + start: PathPosition, + target: PathPosition, + parent: Node, + ): Node { + return Node( + position, + start, + target, + pathfinderConfiguration.heuristicWeights, + pathfinderConfiguration.heuristicStrategy, + parent.getDepth() + 1 + ) + } + + private fun isValidByCustomProcessors(context: EvaluationContext): Boolean { + if (validationProcessors.isNullOrEmpty()) { + return true + } + + for (validator in validationProcessors) { + if (!validator.isValid(context)) { + return false + } + } + return true + } + + private fun calculateGCost(context: EvaluationContext): Double { + val baseCost = context.getBaseTransitionCost() + var additionalCost = 0.0 + + if (!costProcessors.isNullOrEmpty()) { + for (processor in costProcessors) { + val contribution = processor.calculateCostContribution(context) + additionalCost += contribution?.value ?: Cost.ZERO.value + } + } + + var transitionCost = baseCost + additionalCost + if (transitionCost < 0) { + transitionCost = 0.0 + } + + return context.getPathCostToPreviousPosition() + transitionCost + } + + override fun markNodeAsExpanded(node: Node) { + val session = getSessionOrThrow() + val position = node.getPosition() + val packedPos = RegionKey.pack(position) + + session.openSetNodes.remove(packedPos) + + if (pathfinderConfiguration.shouldReopenClosedNodes()) { + session.closedSetGCosts[packedPos] = node.getGCost() + } + + val regionData = session.getOrCreateRegionData(position) + regionData.getBloomFilter().put(position) + regionData.getRegionalExaminedPositions().add(packedPos) + } + + override fun performAlgorithmCleanup() { + currentSession.remove() + } + + private fun getSessionOrThrow(): PathfindingSession { + return currentSession.get() + ?: throw IllegalStateException("Pathfinding session not initialized. Call initializeSearch() first.") + } + + private inner class PathfindingSession { + val visitedRegions: Long2ObjectMap = Long2ObjectOpenHashMap() + val openSetNodes: Long2ObjectMap = Long2ObjectOpenHashMap() + val closedSetGCosts: Long2DoubleMap = Long2DoubleOpenHashMap().apply { + defaultReturnValue(Double.NaN) + } + + fun getOrCreateRegionData(position: PathPosition): GridRegionData { + val cellSize = pathfinderConfiguration.gridCellSize + val rX = Mth.floorDiv(position.getFlooredX(), cellSize) + val rY = Mth.floorDiv(position.getFlooredY(), cellSize) + val rZ = Mth.floorDiv(position.getFlooredZ(), cellSize) + val regionKey = RegionKey.pack(rX, rY, rZ) + + return visitedRegions.computeIfAbsent(regionKey) { + GridRegionData(pathfinderConfiguration) + } + } + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AbstractPathfinder.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AbstractPathfinder.kt new file mode 100644 index 0000000..f64ef04 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AbstractPathfinder.kt @@ -0,0 +1,309 @@ +package org.cobalt.api.pathfinder.pathfinder + +import java.util.* +import java.util.concurrent.* +import kotlin.math.abs +import kotlin.math.max +import org.cobalt.api.pathfinder.Node +import org.cobalt.api.pathfinder.pathfinder.heap.PrimitiveMinHeap +import org.cobalt.api.pathfinder.pathfinder.processing.EvaluationContextImpl +import org.cobalt.api.pathfinder.pathfinder.processing.SearchContextImpl +import org.cobalt.api.pathfinder.pathing.INeighborStrategy +import org.cobalt.api.pathfinder.pathing.Pathfinder +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration +import org.cobalt.api.pathfinder.pathing.context.EnvironmentContext +import org.cobalt.api.pathfinder.pathing.hook.PathfinderHook +import org.cobalt.api.pathfinder.pathing.hook.PathfindingContext +import org.cobalt.api.pathfinder.pathing.processing.CostProcessor +import org.cobalt.api.pathfinder.pathing.processing.Processor +import org.cobalt.api.pathfinder.pathing.processing.ValidationProcessor +import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext +import org.cobalt.api.pathfinder.pathing.result.Path +import org.cobalt.api.pathfinder.pathing.result.PathState +import org.cobalt.api.pathfinder.pathing.result.PathfinderResult +import org.cobalt.api.pathfinder.provider.NavigationPointProvider +import org.cobalt.api.pathfinder.result.PathImpl +import org.cobalt.api.pathfinder.result.PathfinderResultImpl +import org.cobalt.api.pathfinder.wrapper.Depth +import org.cobalt.api.pathfinder.wrapper.PathPosition + +abstract class AbstractPathfinder( + protected val pathfinderConfiguration: PathfinderConfiguration, +) : Pathfinder { + + companion object { + private val EMPTY_PATH_POSITIONS: Set = LinkedHashSet(0) + private const val TIE_BREAKER_WEIGHT = 1e-6 + + private val PATHING_EXECUTOR_SERVICE: ExecutorService = + Executors.newWorkStealingPool(max(1, Runtime.getRuntime().availableProcessors() / 2)) + + init { + Runtime.getRuntime().addShutdownHook(Thread { shutdownExecutor() }) + } + + private fun shutdownExecutor() { + PATHING_EXECUTOR_SERVICE.shutdown() + try { + if (!PATHING_EXECUTOR_SERVICE.awaitTermination(5, TimeUnit.SECONDS)) { + PATHING_EXECUTOR_SERVICE.shutdownNow() + } + } catch (e: InterruptedException) { + PATHING_EXECUTOR_SERVICE.shutdownNow() + Thread.currentThread().interrupt() + } + } + } + + protected val navigationPointProvider: NavigationPointProvider = pathfinderConfiguration.provider + protected val validationProcessors: List? = pathfinderConfiguration.getNodeValidationProcessors() + protected val costProcessors: List? = pathfinderConfiguration.getNodeCostProcessors() + protected val neighborStrategy: INeighborStrategy = pathfinderConfiguration.neighborStrategy + + private val pathfinderHooks: MutableSet = Collections.synchronizedSet(HashSet()) + + @Volatile + private var abortRequested = false + + override fun findPath( + start: PathPosition, + target: PathPosition, + context: EnvironmentContext?, + ): CompletionStage { + this.abortRequested = false + return initiatePathing(start, target, context) + } + + override fun abort() { + this.abortRequested = true + } + + override fun registerPathfindingHook(hook: PathfinderHook) { + this.pathfinderHooks.add(hook) + } + + private fun initiatePathing( + start: PathPosition, + target: PathPosition, + environmentContext: EnvironmentContext?, + ): CompletionStage { + val effectiveStart = start.floor() + val effectiveTarget = target.floor() + + return if (pathfinderConfiguration.async) { + CompletableFuture.supplyAsync( + { executePathingAlgorithm(effectiveStart, effectiveTarget, environmentContext) }, + PATHING_EXECUTOR_SERVICE + ).exceptionally { throwable -> handlePathingException(start, target, throwable) } + } else { + try { + CompletableFuture.completedFuture(executePathingAlgorithm(effectiveStart, effectiveTarget, environmentContext)) + } catch (e: Exception) { + CompletableFuture.completedFuture(handlePathingException(start, target, e)) + } + } + } + + private fun executePathingAlgorithm( + start: PathPosition, + target: PathPosition, + environmentContext: EnvironmentContext?, + ): PathfinderResult { + initializeSearch() + + val searchContext = SearchContextImpl( + start, + target, + this.pathfinderConfiguration, + this.navigationPointProvider, + environmentContext + ) + + val processors = getProcessors() + + try { + processors.forEach { it.initializeSearch(searchContext) } + + val startNode = createStartNode(start, target) + val startNodeContext = EvaluationContextImpl( + searchContext, + startNode, + null, + pathfinderConfiguration.heuristicStrategy + ) + + if (!validationProcessors.isNullOrEmpty()) { + val isStartNodeInvalid = validationProcessors.any { !it.isValid(startNodeContext) } + if (isStartNodeInvalid) { + return PathfinderResultImpl(PathState.FAILED, PathImpl(start, target, EMPTY_PATH_POSITIONS)) + } + } + + val openSet = PrimitiveMinHeap(1024) + val startKey = try { + calculateHeapKey(startNode, startNode.getFCost()) + } catch (t: Throwable) { + startNode.getFCost() + } + + insertStartNode(startNode, startKey, openSet) + + var currentDepth = 0 + var bestFallbackNode = startNode + + while (!openSet.isEmpty() && currentDepth < pathfinderConfiguration.maxIterations) { + currentDepth++ + + if (this.abortRequested) { + return createAbortedResult(start, target, bestFallbackNode) + } + + val currentNode = extractBestNode(openSet) + markNodeAsExpanded(currentNode) + + pathfinderHooks.forEach { hook -> + hook.onPathfindingStep(PathfindingContext(currentNode.getPosition(), Depth.of(currentDepth))) + } + + if (currentNode.getHeuristic() < bestFallbackNode.getHeuristic()) { + bestFallbackNode = currentNode + } + + if (hasReachedPathLengthLimit(currentNode)) { + return PathfinderResultImpl(PathState.LENGTH_LIMITED, reconstructPath(start, target, currentNode)) + } + + if (currentNode.isTarget(target)) { + return PathfinderResultImpl(PathState.FOUND, reconstructPath(start, target, currentNode)) + } + + processSuccessors(start, target, currentNode, openSet, searchContext) + } + + return determinePostLoopResult(currentDepth, start, target, bestFallbackNode) + + } catch (e: Exception) { + return PathfinderResultImpl(PathState.FAILED, PathImpl(start, target, EMPTY_PATH_POSITIONS)) + } finally { + val finalizeErrors = mutableListOf() + processors.forEach { processor -> + try { + processor.finalizeSearch(searchContext) + } catch (e: Exception) { + finalizeErrors.add(e) + } + } + performAlgorithmCleanup() + } + } + + fun calculateHeapKey(neighbor: Node, fCost: Double): Double { + val heuristic = neighbor.getHeuristic() + val tieBreaker = TIE_BREAKER_WEIGHT * (heuristic / (abs(fCost) + 1)) + var heapKey = fCost - tieBreaker + + if (heapKey.isNaN() || heapKey.isInfinite()) { + heapKey = fCost + } + + return heapKey + } + + private fun getProcessors(): List { + val processors = mutableListOf() + + validationProcessors?.let { processors.addAll(it) } + costProcessors?.let { processors.addAll(it) } + + return processors + } + + private fun createAbortedResult(start: PathPosition, target: PathPosition, fallbackNode: Node): PathfinderResult { + this.abortRequested = false + return PathfinderResultImpl(PathState.ABORTED, reconstructPath(start, target, fallbackNode)) + } + + private fun handlePathingException( + originalStart: PathPosition, + originalTarget: PathPosition, + throwable: Throwable, + ): PathfinderResult { + return PathfinderResultImpl(PathState.FAILED, PathImpl(originalStart, originalTarget, EMPTY_PATH_POSITIONS)) + } + + protected fun createStartNode(startPos: PathPosition, targetPos: PathPosition): Node { + return Node( + startPos, + startPos, + targetPos, + pathfinderConfiguration.heuristicWeights, + pathfinderConfiguration.heuristicStrategy, + 0 + ) + } + + private fun hasReachedPathLengthLimit(currentNode: Node): Boolean { + val maxLength = pathfinderConfiguration.maxLength + return maxLength > 0 && currentNode.getDepth() >= maxLength + } + + private fun determinePostLoopResult( + depthReached: Int, + start: PathPosition, + target: PathPosition, + fallbackNode: Node, + ): PathfinderResult { + return when { + depthReached >= pathfinderConfiguration.maxIterations -> { + PathfinderResultImpl(PathState.MAX_ITERATIONS_REACHED, reconstructPath(start, target, fallbackNode)) + } + + pathfinderConfiguration.fallback -> { + PathfinderResultImpl(PathState.FALLBACK, reconstructPath(start, target, fallbackNode)) + } + + else -> { + PathfinderResultImpl(PathState.FAILED, PathImpl(start, target, EMPTY_PATH_POSITIONS)) + } + } + } + + protected fun reconstructPath(start: PathPosition, target: PathPosition, endNode: Node): Path { + if (endNode.getParent() == null && endNode.getDepth() == 0) { + return PathImpl(start, target, listOf(endNode.getPosition())) + } + + val pathPositions = tracePathPositionsFromNode(endNode) + return PathImpl(start, target, pathPositions) + } + + private fun tracePathPositionsFromNode(leafNode: Node): List { + val path = mutableListOf() + var currentNode: Node? = leafNode + + while (currentNode != null) { + path.add(currentNode.getPosition()) + currentNode = currentNode.getParent() + } + + return path.reversed() + } + + protected abstract fun insertStartNode(node: Node, fCost: Double, openSet: PrimitiveMinHeap) + + protected abstract fun extractBestNode(openSet: PrimitiveMinHeap): Node + + protected abstract fun initializeSearch() + + protected abstract fun markNodeAsExpanded(node: Node) + + protected abstract fun performAlgorithmCleanup() + + protected abstract fun processSuccessors( + requestStart: PathPosition, + requestTarget: PathPosition, + currentNode: Node, + openSet: PrimitiveMinHeap, + searchContext: SearchContext, + ) +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/heap/PrimitiveMinHeap.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/heap/PrimitiveMinHeap.kt new file mode 100644 index 0000000..51825c9 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/heap/PrimitiveMinHeap.kt @@ -0,0 +1,137 @@ +package org.cobalt.api.pathfinder.pathfinder.heap + +import it.unimi.dsi.fastutil.longs.Long2IntOpenHashMap + +class PrimitiveMinHeap(initialCapacity: Int) { + + private val nodeToIndexMap: Long2IntOpenHashMap = Long2IntOpenHashMap(initialCapacity).apply { + defaultReturnValue(-1) + } + + private var nodes: LongArray = LongArray(initialCapacity + 1) + private var costs: DoubleArray = DoubleArray(initialCapacity + 1) + private var size = 0 + + fun isEmpty(): Boolean = size == 0 + + fun size(): Int = size + + fun clear() { + size = 0 + nodeToIndexMap.clear() + } + + fun contains(packedNode: Long): Boolean = nodeToIndexMap.containsKey(packedNode) + + fun getCost(packedNode: Long): Double { + val index = nodeToIndexMap.get(packedNode) + return if (index == -1) Double.MAX_VALUE else costs[index] + } + + fun insertOrUpdate(packedNode: Long, cost: Double) { + val existingIndex = nodeToIndexMap.get(packedNode) + + if (existingIndex != -1) { + if (cost < costs[existingIndex]) { + costs[existingIndex] = cost + siftUp(existingIndex) + } + } else { + ensureCapacity() + size++ + nodes[size] = packedNode + costs[size] = cost + nodeToIndexMap.put(packedNode, size) + siftUp(size) + } + } + + fun extractMin(): Long { + if (size == 0) throw NoSuchElementException() + + val minNode = nodes[1] + nodeToIndexMap.remove(minNode) + + val lastNode = nodes[size] + val lastCost = costs[size] + nodes[1] = lastNode + costs[1] = lastCost + size-- + + if (size > 0) { + nodeToIndexMap.put(lastNode, 1) + siftDown(1) + } + + return minNode + } + + private fun ensureCapacity() { + if (size >= nodes.size - 1) { + val newCap = nodes.size * 2 + val newNodes = LongArray(newCap) + val newCosts = DoubleArray(newCap) + + System.arraycopy(nodes, 0, newNodes, 0, nodes.size) + System.arraycopy(costs, 0, newCosts, 0, costs.size) + + this.nodes = newNodes + this.costs = newCosts + } + } + + private fun siftUp(index: Int) { + var current = index + val nodeToMove = nodes[current] + val costToMove = costs[current] + + while (current > 1) { + val parentIndex = current shr 1 + val parentCost = costs[parentIndex] + + if (costToMove < parentCost) { + nodes[current] = nodes[parentIndex] + costs[current] = parentCost + nodeToIndexMap.put(nodes[current], current) + current = parentIndex + } else { + break + } + } + + nodes[current] = nodeToMove + costs[current] = costToMove + nodeToIndexMap.put(nodeToMove, current) + } + + private fun siftDown(index: Int) { + var current = index + val nodeToMove = nodes[current] + val costToMove = costs[current] + val half = size shr 1 + + while (current <= half) { + var childIndex = current shl 1 + var childCost = costs[childIndex] + val rightIndex = childIndex + 1 + + if (rightIndex <= size && costs[rightIndex] < childCost) { + childIndex = rightIndex + childCost = costs[rightIndex] + } + + if (costToMove > childCost) { + nodes[current] = nodes[childIndex] + costs[current] = childCost + nodeToIndexMap.put(nodes[current], current) + current = childIndex + } else { + break + } + } + + nodes[current] = nodeToMove + costs[current] = costToMove + nodeToIndexMap.put(nodeToMove, current) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/processing/EvaluationContextImpl.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/processing/EvaluationContextImpl.kt new file mode 100644 index 0000000..96c0fe6 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/processing/EvaluationContextImpl.kt @@ -0,0 +1,49 @@ +package org.cobalt.api.pathfinder.pathfinder.processing + +import kotlin.math.max +import org.cobalt.api.pathfinder.Node +import org.cobalt.api.pathfinder.pathing.heuristic.IHeuristicStrategy +import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext +import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class EvaluationContextImpl( + private val searchContext: SearchContext, + private val engineNode: Node, + private val parentEngineNode: Node?, + private val heuristicStrategy: IHeuristicStrategy, +) : EvaluationContext { + + override fun getCurrentPathPosition(): PathPosition = engineNode.getPosition() + + override fun getPreviousPathPosition(): PathPosition? = parentEngineNode?.getPosition() + + override fun getCurrentNodeDepth(): Int = engineNode.getDepth() + + override fun getCurrentNodeHeuristicValue(): Double = engineNode.getHeuristic() + + override fun getPathCostToPreviousPosition(): Double { + return parentEngineNode?.getGCost() ?: 0.0 + } + + override fun getBaseTransitionCost(): Double { + if (parentEngineNode == null) return 0.0 + + val from = parentEngineNode.getPosition() + val to = engineNode.getPosition() + val baseCost = heuristicStrategy.calculateTransitionCost(from, to) + + if (baseCost.isNaN() || baseCost.isInfinite()) { + throw IllegalStateException( + "Heuristic transition cost produced an invalid numeric value: $baseCost" + ) + } + + return max(baseCost, 0.0) + } + + override fun getSearchContext(): SearchContext = searchContext + + override fun getGrandparentPathPosition(): PathPosition? = + parentEngineNode?.getParent()?.getPosition() +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/processing/SearchContextImpl.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/processing/SearchContextImpl.kt new file mode 100644 index 0000000..2da018d --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/processing/SearchContextImpl.kt @@ -0,0 +1,30 @@ +package org.cobalt.api.pathfinder.pathfinder.processing + +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration +import org.cobalt.api.pathfinder.pathing.context.EnvironmentContext +import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext +import org.cobalt.api.pathfinder.provider.NavigationPointProvider +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class SearchContextImpl( + private val startPathPosition: PathPosition, + private val targetPathPosition: PathPosition, + private val pathfinderConfiguration: PathfinderConfiguration, + private val navigationPointProvider: NavigationPointProvider, + private val environmentContext: EnvironmentContext?, +) : SearchContext { + + private val sharedData: MutableMap = HashMap() + + override fun getStartPathPosition(): PathPosition = startPathPosition + + override fun getTargetPathPosition(): PathPosition = targetPathPosition + + override fun getPathfinderConfiguration(): PathfinderConfiguration = pathfinderConfiguration + + override fun getNavigationPointProvider(): NavigationPointProvider = navigationPointProvider + + override fun getSharedData(): MutableMap = sharedData + + override fun getEnvironmentContext(): EnvironmentContext? = environmentContext +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/INeighborStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/INeighborStrategy.kt new file mode 100644 index 0000000..870cbac --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/INeighborStrategy.kt @@ -0,0 +1,12 @@ +package org.cobalt.api.pathfinder.pathing + +import org.cobalt.api.pathfinder.wrapper.PathPosition +import org.cobalt.api.pathfinder.wrapper.PathVector + +fun interface INeighborStrategy { + fun getOffsets(): Iterable + + fun getOffsets(currentPosition: PathPosition): Iterable { + return getOffsets() + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/NeighborStrategies.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/NeighborStrategies.kt new file mode 100644 index 0000000..12b49b2 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/NeighborStrategies.kt @@ -0,0 +1,44 @@ +package org.cobalt.api.pathfinder.pathing + +import org.cobalt.api.pathfinder.wrapper.PathVector + +object NeighborStrategies { + val VERTICAL_AND_HORIZONTAL = INeighborStrategy { + listOf( + PathVector(1.0, 0.0, 0.0), + PathVector(-1.0, 0.0, 0.0), + PathVector(0.0, 0.0, 1.0), + PathVector(0.0, 0.0, -1.0), + PathVector(0.0, 1.0, 0.0), + PathVector(0.0, -1.0, 0.0) + ) + } + + val DIAGONAL_3D = INeighborStrategy { + buildList { + for (x in -1..1) { + for (y in -1..1) { + for (z in -1..1) { + if (x == 0 && y == 0 && z == 0) continue + add(PathVector(x.toDouble(), y.toDouble(), z.toDouble())) + } + } + } + } + } + + val HORIZONTAL_DIAGONAL_AND_VERTICAL = INeighborStrategy { + listOf( + PathVector(1.0, 0.0, 0.0), + PathVector(-1.0, 0.0, 0.0), + PathVector(0.0, 0.0, 1.0), + PathVector(0.0, 0.0, -1.0), + PathVector(0.0, 1.0, 0.0), + PathVector(0.0, -1.0, 0.0), + PathVector(1.0, 0.0, 1.0), + PathVector(1.0, 0.0, -1.0), + PathVector(-1.0, 0.0, 1.0), + PathVector(-1.0, 0.0, -1.0) + ) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/Pathfinder.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/Pathfinder.kt new file mode 100644 index 0000000..1615a8c --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/Pathfinder.kt @@ -0,0 +1,23 @@ +package org.cobalt.api.pathfinder.pathing + +import java.util.concurrent.CompletionStage +import org.cobalt.api.pathfinder.pathing.context.EnvironmentContext +import org.cobalt.api.pathfinder.pathing.hook.PathfinderHook +import org.cobalt.api.pathfinder.pathing.result.PathfinderResult +import org.cobalt.api.pathfinder.wrapper.PathPosition + +interface Pathfinder { + fun findPath(start: PathPosition, target: PathPosition): CompletionStage { + return findPath(start, target, null) + } + + fun findPath( + start: PathPosition, + target: PathPosition, + context: EnvironmentContext?, + ): CompletionStage + + fun abort() + + fun registerPathfindingHook(hook: PathfinderHook) +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/PathfindingProgress.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/PathfindingProgress.kt new file mode 100644 index 0000000..2917314 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/PathfindingProgress.kt @@ -0,0 +1,13 @@ +package org.cobalt.api.pathfinder.pathing + +import org.cobalt.api.pathfinder.wrapper.PathPosition + +data class PathfindingProgress( + private val start: PathPosition, + private val current: PathPosition, + private val target: PathPosition, +) { + fun startPosition(): PathPosition = start + fun currentPosition(): PathPosition = current + fun targetPosition(): PathPosition = target +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/calc/DistanceCalculator.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/calc/DistanceCalculator.kt new file mode 100644 index 0000000..9b599e0 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/calc/DistanceCalculator.kt @@ -0,0 +1,7 @@ +package org.cobalt.api.pathfinder.pathing.calc + +import org.cobalt.api.pathfinder.pathing.PathfindingProgress + +fun interface DistanceCalculator { + fun calculate(progress: PathfindingProgress): M? +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/configuration/PathfinderConfiguration.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/configuration/PathfinderConfiguration.kt new file mode 100644 index 0000000..77735da --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/configuration/PathfinderConfiguration.kt @@ -0,0 +1,182 @@ +package org.cobalt.api.pathfinder.pathing.configuration + +import org.cobalt.api.pathfinder.pathing.INeighborStrategy +import org.cobalt.api.pathfinder.pathing.NeighborStrategies +import org.cobalt.api.pathfinder.pathing.context.EnvironmentContext +import org.cobalt.api.pathfinder.pathing.heuristic.HeuristicStrategies +import org.cobalt.api.pathfinder.pathing.heuristic.HeuristicWeights +import org.cobalt.api.pathfinder.pathing.heuristic.IHeuristicStrategy +import org.cobalt.api.pathfinder.pathing.processing.CostProcessor +import org.cobalt.api.pathfinder.pathing.processing.ValidationProcessor +import org.cobalt.api.pathfinder.provider.NavigationPoint +import org.cobalt.api.pathfinder.provider.NavigationPointProvider +import org.cobalt.api.pathfinder.wrapper.PathPosition + +data class PathfinderConfiguration( + val maxIterations: Int, + val maxLength: Int, + val async: Boolean, + val fallback: Boolean, + val provider: NavigationPointProvider, + val heuristicWeights: HeuristicWeights, + val validationProcessors: List, + val costProcessors: List, + val neighborStrategy: INeighborStrategy, + val gridCellSize: Int, + val bloomFilterSize: Int, + val bloomFilterFpp: Double, + val heuristicStrategy: IHeuristicStrategy, + val reopenClosedNodes: Boolean, +) { + companion object { + fun deepCopy(pathfinderConfiguration: PathfinderConfiguration): PathfinderConfiguration { + return builder() + .maxIterations(pathfinderConfiguration.maxIterations) + .maxLength(pathfinderConfiguration.maxLength) + .async(pathfinderConfiguration.async) + .fallback(pathfinderConfiguration.fallback) + .provider(pathfinderConfiguration.provider) + .heuristicWeights(pathfinderConfiguration.heuristicWeights) + .nodeValidationProcessors(pathfinderConfiguration.validationProcessors) + .nodeCostProcessors(pathfinderConfiguration.costProcessors) + .neighborStrategy(pathfinderConfiguration.neighborStrategy) + .gridCellSize(pathfinderConfiguration.gridCellSize) + .bloomFilterSize(pathfinderConfiguration.bloomFilterSize) + .bloomFilterFpp(pathfinderConfiguration.bloomFilterFpp) + .heuristicStrategy(pathfinderConfiguration.heuristicStrategy) + .reopenClosedNodes(pathfinderConfiguration.reopenClosedNodes) + .build() + } + + fun builder(): PathfinderConfigurationBuilder { + return PathfinderConfigurationBuilder() + } + } + + fun getNodeCostProcessors(): List = costProcessors + + fun getNodeValidationProcessors(): List = validationProcessors + + fun shouldReopenClosedNodes(): Boolean = reopenClosedNodes +} + +class PathfinderConfigurationBuilder { + private var maxIterations: Int = 5000 + private var maxLength: Int = 0 + private var async: Boolean = false + private var fallback: Boolean = true + private var provider: NavigationPointProvider = + object : NavigationPointProvider { + override fun getNavigationPoint( + position: PathPosition, + environmentContext: EnvironmentContext?, + ): NavigationPoint { + return object : NavigationPoint { + override fun isTraversable(): Boolean = true + override fun hasFloor(): Boolean = true + override fun isClimbable(): Boolean = false + override fun isLiquid(): Boolean = false + } + } + } + private var heuristicWeights: HeuristicWeights = HeuristicWeights.DEFAULT_WEIGHTS + private var validationProcessors: List = emptyList() + private var costProcessors: List = emptyList() + private var neighborStrategy: INeighborStrategy = NeighborStrategies.VERTICAL_AND_HORIZONTAL + private var gridCellSize: Int = 12 + private var bloomFilterSize: Int = 1000 + private var bloomFilterFpp: Double = 0.01 + private var heuristicStrategy: IHeuristicStrategy = HeuristicStrategies.LINEAR + private var reopenClosedNodes: Boolean = false + + fun maxIterations(maxIterations: Int): PathfinderConfigurationBuilder { + this.maxIterations = maxIterations + return this + } + + fun maxLength(maxLength: Int): PathfinderConfigurationBuilder { + this.maxLength = maxLength + return this + } + + fun async(async: Boolean): PathfinderConfigurationBuilder { + this.async = async + return this + } + + fun fallback(allowingFallback: Boolean): PathfinderConfigurationBuilder { + this.fallback = allowingFallback + return this + } + + fun provider(provider: NavigationPointProvider): PathfinderConfigurationBuilder { + this.provider = provider + return this + } + + fun heuristicWeights(heuristicWeights: HeuristicWeights): PathfinderConfigurationBuilder { + this.heuristicWeights = heuristicWeights + return this + } + + fun nodeValidationProcessors( + validationProcessors: List, + ): PathfinderConfigurationBuilder { + this.validationProcessors = validationProcessors + return this + } + + fun nodeCostProcessors(costProcessors: List): PathfinderConfigurationBuilder { + this.costProcessors = costProcessors + return this + } + + fun neighborStrategy(neighborStrategy: INeighborStrategy): PathfinderConfigurationBuilder { + this.neighborStrategy = neighborStrategy + return this + } + + fun gridCellSize(gridCellSize: Int): PathfinderConfigurationBuilder { + this.gridCellSize = gridCellSize + return this + } + + fun bloomFilterSize(bloomFilterSize: Int): PathfinderConfigurationBuilder { + this.bloomFilterSize = bloomFilterSize + return this + } + + fun bloomFilterFpp(bloomFilterFpp: Double): PathfinderConfigurationBuilder { + this.bloomFilterFpp = bloomFilterFpp + return this + } + + fun heuristicStrategy(heuristicStrategy: IHeuristicStrategy): PathfinderConfigurationBuilder { + this.heuristicStrategy = heuristicStrategy + return this + } + + fun reopenClosedNodes(reopenClosedNodes: Boolean): PathfinderConfigurationBuilder { + this.reopenClosedNodes = reopenClosedNodes + return this + } + + fun build(): PathfinderConfiguration { + return PathfinderConfiguration( + maxIterations = this.maxIterations, + maxLength = this.maxLength, + async = this.async, + fallback = this.fallback, + provider = this.provider, + heuristicWeights = this.heuristicWeights, + validationProcessors = this.validationProcessors.toList(), + costProcessors = this.costProcessors.toList(), + neighborStrategy = this.neighborStrategy, + gridCellSize = this.gridCellSize, + bloomFilterSize = this.bloomFilterSize, + bloomFilterFpp = this.bloomFilterFpp, + heuristicStrategy = this.heuristicStrategy, + reopenClosedNodes = this.reopenClosedNodes + ) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/context/EnvironmentContext.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/context/EnvironmentContext.kt new file mode 100644 index 0000000..84012c6 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/context/EnvironmentContext.kt @@ -0,0 +1,3 @@ +package org.cobalt.api.pathfinder.pathing.context + +interface EnvironmentContext diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicContext.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicContext.kt new file mode 100644 index 0000000..ac28264 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicContext.kt @@ -0,0 +1,34 @@ +package org.cobalt.api.pathfinder.pathing.heuristic + +import org.cobalt.api.pathfinder.pathing.PathfindingProgress +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class HeuristicContext { + private val pathfindingProgress: PathfindingProgress + private val heuristicWeights: HeuristicWeights + + constructor( + position: PathPosition, + startPosition: PathPosition, + targetPosition: PathPosition, + heuristicWeights: HeuristicWeights, + ) { + this.pathfindingProgress = PathfindingProgress(startPosition, position, targetPosition) + this.heuristicWeights = heuristicWeights + } + + constructor(pathfindingProgress: PathfindingProgress, heuristicWeights: HeuristicWeights) { + this.pathfindingProgress = pathfindingProgress + this.heuristicWeights = heuristicWeights + } + + fun getPathfindingProgress(): PathfindingProgress = pathfindingProgress + + fun position(): PathPosition = pathfindingProgress.currentPosition() + + fun startPosition(): PathPosition = pathfindingProgress.startPosition() + + fun targetPosition(): PathPosition = pathfindingProgress.targetPosition() + + fun heuristicWeights(): HeuristicWeights = heuristicWeights +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicStrategies.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicStrategies.kt new file mode 100644 index 0000000..964af18 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicStrategies.kt @@ -0,0 +1,6 @@ +package org.cobalt.api.pathfinder.pathing.heuristic + +object HeuristicStrategies { + val LINEAR: IHeuristicStrategy = LinearHeuristicStrategy() + val SQUARED: IHeuristicStrategy = SquaredHeuristicStrategy() +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicWeights.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicWeights.kt new file mode 100644 index 0000000..7698935 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicWeights.kt @@ -0,0 +1,22 @@ +package org.cobalt.api.pathfinder.pathing.heuristic + +data class HeuristicWeights +private constructor( + val manhattanWeight: Double, + val octileWeight: Double, + val perpendicularWeight: Double, + val heightWeight: Double, +) { + companion object { + val DEFAULT_WEIGHTS = create(0.0, 1.0, 0.0, 0.0) + + fun create( + manhattanWeight: Double, + octileWeight: Double, + perpendicularWeight: Double, + heightWeight: Double, + ): HeuristicWeights { + return HeuristicWeights(manhattanWeight, octileWeight, perpendicularWeight, heightWeight) + } + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/IHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/IHeuristicStrategy.kt new file mode 100644 index 0000000..6a8ee35 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/IHeuristicStrategy.kt @@ -0,0 +1,8 @@ +package org.cobalt.api.pathfinder.pathing.heuristic + +import org.cobalt.api.pathfinder.wrapper.PathPosition + +interface IHeuristicStrategy { + fun calculate(heuristicContext: HeuristicContext): Double + fun calculateTransitionCost(from: PathPosition, to: PathPosition): Double +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt new file mode 100644 index 0000000..d6e1e0d --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt @@ -0,0 +1,99 @@ +package org.cobalt.api.pathfinder.pathing.heuristic + +import kotlin.math.abs +import kotlin.math.sqrt +import org.cobalt.api.pathfinder.pathing.calc.DistanceCalculator +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class LinearHeuristicStrategy : IHeuristicStrategy { + companion object { + private const val EPSILON = 1e-9 + private const val D1 = 1.0 + private val D2 = sqrt(2.0) + private val D3 = sqrt(3.0) + } + + private val perpendicularCalc = DistanceCalculator { progress -> + val s = progress.startPosition() + val c = progress.currentPosition() + val t = progress.targetPosition() + + val sx = s.getCenteredX() + val sy = s.getCenteredY() + val sz = s.getCenteredZ() + val cx = c.getCenteredX() + val cy = c.getCenteredY() + val cz = c.getCenteredZ() + val tx = t.getCenteredX() + val ty = t.getCenteredY() + val tz = t.getCenteredZ() + + val lineX = tx - sx + val lineY = ty - sy + val lineZ = tz - sz + val lineSq = lineX * lineX + lineY * lineY + lineZ * lineZ + + if (lineSq < EPSILON) { + val dx = cx - sx + val dy = cy - sy + val dz = cz - sz + return@DistanceCalculator sqrt(dx * dx + dy * dy + dz * dz) + } + + val toX = cx - sx + val toY = cy - sy + val toZ = cz - sz + val crossX = toY * lineZ - toZ * lineY + val crossY = toZ * lineX - toX * lineZ + val crossZ = toX * lineY - toY * lineX + val crossSq = crossX * crossX + crossY * crossY + crossZ * crossZ + + sqrt(crossSq / lineSq) + } + + private val octileCalc = DistanceCalculator { progress -> + val dx = abs(progress.currentPosition().getFlooredX() - progress.targetPosition().getFlooredX()) + val dy = abs(progress.currentPosition().getFlooredY() - progress.targetPosition().getFlooredY()) + val dz = abs(progress.currentPosition().getFlooredZ() - progress.targetPosition().getFlooredZ()) + + val min = minOf(dx, dy, dz) + val max = maxOf(dx, dy, dz) + val mid = dx + dy + dz - min - max + + (D3 - D2) * min + (D2 - D1) * mid + D1 * max + } + + private val manhattanCalc = DistanceCalculator { progress -> + val position = progress.currentPosition() + val target = progress.targetPosition() + + (abs(position.getFlooredX() - target.getFlooredX()) + + abs(position.getFlooredY() - target.getFlooredY()) + + abs(position.getFlooredZ() - target.getFlooredZ())).toDouble() + } + + private val heightCalc = DistanceCalculator { progress -> + val position = progress.currentPosition() + val target = progress.targetPosition() + + abs(position.getFlooredY() - target.getFlooredY()).toDouble() + } + + override fun calculate(context: HeuristicContext): Double { + val progress = context.getPathfindingProgress() + val weights = context.heuristicWeights() + + return manhattanCalc.calculate(progress)!! * weights.manhattanWeight + + octileCalc.calculate(progress)!! * weights.octileWeight + + perpendicularCalc.calculate(progress)!! * weights.perpendicularWeight + + heightCalc.calculate(progress)!! * weights.heightWeight + } + + override fun calculateTransitionCost(from: PathPosition, to: PathPosition): Double { + val dx = to.getCenteredX() - from.getCenteredX() + val dy = to.getCenteredY() - from.getCenteredY() + val dz = to.getCenteredZ() - from.getCenteredZ() + + return sqrt(dx * dx + dy * dy + dz * dz) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt new file mode 100644 index 0000000..708c447 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt @@ -0,0 +1,100 @@ +package org.cobalt.api.pathfinder.pathing.heuristic + +import kotlin.math.abs +import kotlin.math.sqrt +import org.cobalt.api.pathfinder.pathing.calc.DistanceCalculator +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class SquaredHeuristicStrategy : IHeuristicStrategy { + companion object { + private const val EPSILON = 1e-9 + private const val D1 = 1.0 + private val D2 = sqrt(2.0) + private val D3 = sqrt(3.0) + } + + private val perpendicularCalc = DistanceCalculator { progress -> + val s = progress.startPosition() + val c = progress.currentPosition() + val t = progress.targetPosition() + + val sx = s.getCenteredX() + val sy = s.getCenteredY() + val sz = s.getCenteredZ() + val cx = c.getCenteredX() + val cy = c.getCenteredY() + val cz = c.getCenteredZ() + val tx = t.getCenteredX() + val ty = t.getCenteredY() + val tz = t.getCenteredZ() + + val lineX = tx - sx + val lineY = ty - sy + val lineZ = tz - sz + val lineSq = lineX * lineX + lineY * lineY + lineZ * lineZ + + if (lineSq < EPSILON) { + val dx = cx - sx + val dy = cy - sy + val dz = cz - sz + return@DistanceCalculator dx * dx + dy * dy + dz * dz + } + + val toX = cx - sx + val toY = cy - sy + val toZ = cz - sz + val crossX = toY * lineZ - toZ * lineY + val crossY = toZ * lineX - toX * lineZ + val crossZ = toX * lineY - toY * lineX + val crossSq = crossX * crossX + crossY * crossY + crossZ * crossZ + + crossSq / lineSq + } + + private val octileCalc = DistanceCalculator { progress -> + val dx = abs(progress.currentPosition().getFlooredX() - progress.targetPosition().getFlooredX()) + val dy = abs(progress.currentPosition().getFlooredY() - progress.targetPosition().getFlooredY()) + val dz = abs(progress.currentPosition().getFlooredZ() - progress.targetPosition().getFlooredZ()) + + val min = minOf(dx, dy, dz) + val max = maxOf(dx, dy, dz) + val mid = dx + dy + dz - min - max + + val octile = (D3 - D2) * min + (D2 - D1) * mid + D1 * max + octile * octile + } + + private val manhattanCalc = DistanceCalculator { progress -> + val c = progress.currentPosition() + val t = progress.targetPosition() + + val manhattan = abs(c.getFlooredX() - t.getFlooredX()) + + abs(c.getFlooredY() - t.getFlooredY()) + + abs(c.getFlooredZ() - t.getFlooredZ()) + + (manhattan * manhattan).toDouble() + } + + private val heightCalc = DistanceCalculator { progress -> + val dy = progress.currentPosition().getFlooredY() - progress.targetPosition().getFlooredY() + (dy * dy).toDouble() + } + + override fun calculate(context: HeuristicContext): Double { + val p = context.getPathfindingProgress() + val w = context.heuristicWeights() + + return manhattanCalc.calculate(p)!! * w.manhattanWeight + + octileCalc.calculate(p)!! * w.octileWeight + + perpendicularCalc.calculate(p)!! * w.perpendicularWeight + + heightCalc.calculate(p)!! * w.heightWeight + } + + override fun calculateTransitionCost(from: PathPosition, to: PathPosition): Double { + val dx = to.getCenteredX() - from.getCenteredX() + val dy = to.getCenteredY() - from.getCenteredY() + val dz = to.getCenteredZ() - from.getCenteredZ() + + return dx * dx + dy * dy + dz * dz + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/hook/PathfinderHook.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/hook/PathfinderHook.kt new file mode 100644 index 0000000..368e8b1 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/hook/PathfinderHook.kt @@ -0,0 +1,5 @@ +package org.cobalt.api.pathfinder.pathing.hook + +interface PathfinderHook { + fun onPathfindingStep(pathfindingContext: PathfindingContext) +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/hook/PathfindingContext.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/hook/PathfindingContext.kt new file mode 100644 index 0000000..b0a0e55 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/hook/PathfindingContext.kt @@ -0,0 +1,12 @@ +package org.cobalt.api.pathfinder.pathing.hook + +import org.cobalt.api.pathfinder.wrapper.Depth +import org.cobalt.api.pathfinder.wrapper.PathPosition + +data class PathfindingContext( + private val currentPosition: PathPosition, + private val depth: Depth, +) { + fun currentPosition(): PathPosition = currentPosition + fun getDepth(): Depth = depth +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Cost.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Cost.kt new file mode 100644 index 0000000..5bc03ce --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Cost.kt @@ -0,0 +1,14 @@ +package org.cobalt.api.pathfinder.pathing.processing + +data class Cost private constructor(val value: Double) { + companion object { + val ZERO = Cost(0.0) + + fun of(value: Double): Cost { + if (value.isNaN() || value < 0) { + throw IllegalArgumentException("Cost must be a positive number or 0") + } + return Cost(value) + } + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/CostProcessor.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/CostProcessor.kt new file mode 100644 index 0000000..56cd4e8 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/CostProcessor.kt @@ -0,0 +1,7 @@ +package org.cobalt.api.pathfinder.pathing.processing + +import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext + +interface CostProcessor : Processor { + fun calculateCostContribution(context: EvaluationContext): Cost +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Processor.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Processor.kt new file mode 100644 index 0000000..4ff6951 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Processor.kt @@ -0,0 +1,11 @@ +package org.cobalt.api.pathfinder.pathing.processing + +import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext + +interface Processor { + fun initializeSearch(context: SearchContext) { + } + + fun finalizeSearch(context: SearchContext) { + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/ValidationProcessor.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/ValidationProcessor.kt new file mode 100644 index 0000000..bec934a --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/ValidationProcessor.kt @@ -0,0 +1,7 @@ +package org.cobalt.api.pathfinder.pathing.processing + +import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext + +interface ValidationProcessor : Processor { + fun isValid(context: EvaluationContext): Boolean +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt new file mode 100644 index 0000000..4584532 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt @@ -0,0 +1,121 @@ +package org.cobalt.api.pathfinder.pathing.processing + +import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext +import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext + +object Validators { + + fun allOf(vararg validators: ValidationProcessor): ValidationProcessor { + return AllOfValidator(*validators) + } + + fun allOf(validators: List): ValidationProcessor { + return AllOfValidator(validators) + } + + fun anyOf(vararg validators: ValidationProcessor): ValidationProcessor { + return AnyOfValidator(*validators) + } + + fun anyOf(validators: List): ValidationProcessor { + return AnyOfValidator(validators) + } + + fun noneOf(vararg validators: ValidationProcessor): ValidationProcessor { + return NoneOfValidator(*validators) + } + + fun noneOf(validators: List): ValidationProcessor { + return NoneOfValidator(validators) + } + + fun not(validator: ValidationProcessor): ValidationProcessor { + return NotValidator(validator) + } + + fun alwaysTrue(): ValidationProcessor { + return AlwaysTrueValidator + } + + fun alwaysFalse(): ValidationProcessor { + return AlwaysFalseValidator + } + + private fun copyAndFilterNulls(vararg validators: ValidationProcessor?): List { + return validators.filterNotNull() + } + + private fun copyAndFilterNulls(validators: List?): List { + return validators?.filterNotNull() ?: emptyList() + } + + private abstract class AbstractCompositeValidator : ValidationProcessor { + protected val children: List + + constructor(vararg validators: ValidationProcessor?) { + this.children = copyAndFilterNulls(*validators) + } + + constructor(validators: List?) { + this.children = copyAndFilterNulls(validators) + } + + override fun initializeSearch(context: SearchContext) { + children.forEach { it.initializeSearch(context) } + } + + override fun finalizeSearch(context: SearchContext) { + children.forEach { it.finalizeSearch(context) } + } + } + + private class AllOfValidator : AbstractCompositeValidator { + constructor(vararg validators: ValidationProcessor?) : super(*validators) + constructor(validators: List?) : super(validators) + + override fun isValid(context: EvaluationContext): Boolean { + return children.all { it.isValid(context) } + } + } + + private class AnyOfValidator : AbstractCompositeValidator { + constructor(vararg validators: ValidationProcessor?) : super(*validators) + constructor(validators: List?) : super(validators) + + override fun isValid(context: EvaluationContext): Boolean { + if (children.isEmpty()) return false + return children.any { it.isValid(context) } + } + } + + private class NoneOfValidator : AbstractCompositeValidator { + constructor(vararg validators: ValidationProcessor?) : super(*validators) + constructor(validators: List?) : super(validators) + + override fun isValid(context: EvaluationContext): Boolean { + return children.none { it.isValid(context) } + } + } + + private class NotValidator(private val child: ValidationProcessor) : ValidationProcessor { + override fun initializeSearch(context: SearchContext) { + child.initializeSearch(context) + } + + override fun isValid(context: EvaluationContext): Boolean { + return !child.isValid(context) + } + + override fun finalizeSearch(context: SearchContext) { + child.finalizeSearch(context) + } + } + + private object AlwaysTrueValidator : ValidationProcessor { + override fun isValid(context: EvaluationContext): Boolean = true + } + + private object AlwaysFalseValidator : ValidationProcessor { + override fun isValid(context: EvaluationContext): Boolean = false + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/context/EvaluationContext.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/context/EvaluationContext.kt new file mode 100644 index 0000000..0ed124c --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/context/EvaluationContext.kt @@ -0,0 +1,41 @@ +package org.cobalt.api.pathfinder.pathing.processing.context + +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration +import org.cobalt.api.pathfinder.pathing.context.EnvironmentContext +import org.cobalt.api.pathfinder.provider.NavigationPointProvider +import org.cobalt.api.pathfinder.wrapper.PathPosition + +interface EvaluationContext { + fun getCurrentPathPosition(): PathPosition + fun getPreviousPathPosition(): PathPosition? + fun getCurrentNodeDepth(): Int + fun getCurrentNodeHeuristicValue(): Double + fun getPathCostToPreviousPosition(): Double + fun getBaseTransitionCost(): Double + fun getSearchContext(): SearchContext + fun getGrandparentPathPosition(): PathPosition? + + fun getPathfinderConfiguration(): PathfinderConfiguration { + return getSearchContext().getPathfinderConfiguration() + } + + fun getNavigationPointProvider(): NavigationPointProvider { + return getSearchContext().getNavigationPointProvider() + } + + fun getSharedData(): MutableMap { + return getSearchContext().getSharedData() + } + + fun getStartPathPosition(): PathPosition { + return getSearchContext().getStartPathPosition() + } + + fun getTargetPathPosition(): PathPosition { + return getSearchContext().getTargetPathPosition() + } + + fun getEnvironmentContext(): EnvironmentContext? { + return getSearchContext().getEnvironmentContext() + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/context/SearchContext.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/context/SearchContext.kt new file mode 100644 index 0000000..bbe6fa3 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/context/SearchContext.kt @@ -0,0 +1,15 @@ +package org.cobalt.api.pathfinder.pathing.processing.context + +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration +import org.cobalt.api.pathfinder.pathing.context.EnvironmentContext +import org.cobalt.api.pathfinder.provider.NavigationPointProvider +import org.cobalt.api.pathfinder.wrapper.PathPosition + +interface SearchContext { + fun getStartPathPosition(): PathPosition + fun getTargetPathPosition(): PathPosition + fun getPathfinderConfiguration(): PathfinderConfiguration + fun getNavigationPointProvider(): NavigationPointProvider + fun getSharedData(): MutableMap + fun getEnvironmentContext(): EnvironmentContext? +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/impl/MinecraftPathProcessor.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/impl/MinecraftPathProcessor.kt new file mode 100644 index 0000000..fbcc53e --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/impl/MinecraftPathProcessor.kt @@ -0,0 +1,127 @@ +package org.cobalt.api.pathfinder.pathing.processing.impl + +import kotlin.math.sqrt +import net.minecraft.client.Minecraft +import net.minecraft.core.BlockPos +import org.cobalt.api.pathfinder.pathing.processing.Cost +import org.cobalt.api.pathfinder.pathing.processing.CostProcessor +import org.cobalt.api.pathfinder.pathing.processing.ValidationProcessor +import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext + +/* +* most logic in this file is derived from minecraft code +* or writeups on pathfinding algorithms, if you want to help contribute +* id prefer for you to keep it the same idea or whatever, but if not +* please write a comment explaining WHY you did it that way. i dont like +* magic numbers that i cant understand. + */ +class MinecraftPathProcessor : CostProcessor, ValidationProcessor { + + private val mc: Minecraft = Minecraft.getInstance() + + companion object { + private const val DEFAULT_MOB_JUMP_HEIGHT = 1.125 // WalkNodeEvaluator + } + + override fun isValid(context: EvaluationContext): Boolean { + val provider = context.getSearchContext().getNavigationPointProvider() + val pos = context.getCurrentPathPosition() + val prev = context.getPreviousPathPosition() + val env = context.getSearchContext().getEnvironmentContext() + + val currentPoint = provider.getNavigationPoint(pos, env) + + if (!currentPoint.isTraversable()) return false + if (prev == null) return true + + val prevPoint = provider.getNavigationPoint(prev, env) + val dy = pos.getY() - prev.getY() + val dx = pos.getFlooredX() - prev.getFlooredX() + val dz = pos.getFlooredZ() - prev.getFlooredZ() + + if (dy > DEFAULT_MOB_JUMP_HEIGHT) return false + + if (Math.abs(dx) == 1 && Math.abs(dz) == 1) { + val corner1Pos = prev.add(dx.toDouble(), 0.0, 0.0) + val corner2Pos = prev.add(0.0, 0.0, dz.toDouble()) + val c1Point = provider.getNavigationPoint(corner1Pos, env) + val c2Point = provider.getNavigationPoint(corner2Pos, env) + + // node3.y <= node.y && node2.y <= node.y + if (!c1Point.isTraversable() || !c2Point.isTraversable()) return false + } + + return when { + dy < -0.5 -> true // falling + dy > 0.5 -> + prevPoint.hasFloor() || + currentPoint.isClimbable() || + currentPoint.isLiquid() // jumping/climbing + else -> + currentPoint.hasFloor() || + prevPoint.hasFloor() || + currentPoint.isClimbable() || + prevPoint.isClimbable() || + currentPoint.isLiquid() || + prevPoint.isLiquid() + } + } + + override fun calculateCostContribution(context: EvaluationContext): Cost { + val level = mc.level ?: return Cost.ZERO + val currentPos = context.getCurrentPathPosition() + val prevPos = context.getPreviousPathPosition() ?: return Cost.ZERO + val provider = context.getSearchContext().getNavigationPointProvider() + val env = context.getSearchContext().getEnvironmentContext() + + val currentPoint = provider.getNavigationPoint(currentPos, env) + val prevPoint = provider.getNavigationPoint(prevPos, env) + + val dy = currentPoint.getFloorLevel() - prevPoint.getFloorLevel() + var additionalCost = 0.0 + + if (dy > 0.1) { + additionalCost += 0.5 * dy + } else if (dy < -0.1) { + additionalCost += 0.1 * Math.abs(dy) + } + + val blockPos = + BlockPos(currentPos.getFlooredX(), currentPos.getFlooredY(), currentPos.getFlooredZ()) + + // i dont want it to like tight corners so more cost + var crampedPenalty = 0.0 + for (i in 2..3) { + if (level.getBlockState(blockPos.above(i)).canOcclude()) { + crampedPenalty += 0.1 / i.toDouble() + } + } + if (level.getBlockState(blockPos.west()).canOcclude() || + level.getBlockState(blockPos.east()).canOcclude() || + level.getBlockState(blockPos.north()).canOcclude() || + level.getBlockState(blockPos.south()).canOcclude() + ) { + crampedPenalty += 0.05 + } + additionalCost += crampedPenalty + + + // just make stuff smoother no more zigzags + val gpPos = context.getGrandparentPathPosition() + if (gpPos != null) { + val v1x = prevPos.getX() - gpPos.getX() + val v1z = prevPos.getZ() - gpPos.getZ() + val v2x = currentPos.getX() - prevPos.getX() + val v2z = currentPos.getZ() - prevPos.getZ() + val dot = v1x * v2x + v1z * v2z + val mag1 = sqrt(v1x * v1x + v1z * v1z) + val mag2 = sqrt(v2x * v2x + v2z * v2z) + if (mag1 > 0.1 && mag2 > 0.1) { + val normalizedDot = dot / (mag1 * mag2) + if (normalizedDot < 0.99) additionalCost += 0.05 + } + } + + return Cost.of(additionalCost) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/Path.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/Path.kt new file mode 100644 index 0000000..69ce867 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/Path.kt @@ -0,0 +1,10 @@ +package org.cobalt.api.pathfinder.pathing.result + +import org.cobalt.api.pathfinder.wrapper.PathPosition + +interface Path : Iterable { + fun length(): Int + fun getStart(): PathPosition + fun getEnd(): PathPosition + fun collect(): Collection +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/PathState.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/PathState.kt new file mode 100644 index 0000000..b8a4c18 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/PathState.kt @@ -0,0 +1,10 @@ +package org.cobalt.api.pathfinder.pathing.result + +enum class PathState { + ABORTED, + FOUND, + FAILED, + FALLBACK, + LENGTH_LIMITED, + MAX_ITERATIONS_REACHED +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/PathfinderResult.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/PathfinderResult.kt new file mode 100644 index 0000000..53e5814 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/result/PathfinderResult.kt @@ -0,0 +1,9 @@ +package org.cobalt.api.pathfinder.pathing.result + +interface PathfinderResult { + fun successful(): Boolean + fun hasFailed(): Boolean + fun hasFallenBack(): Boolean + fun getPathState(): PathState + fun getPath(): Path +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/provider/NavigationPoint.kt b/src/main/kotlin/org/cobalt/api/pathfinder/provider/NavigationPoint.kt new file mode 100644 index 0000000..1720983 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/provider/NavigationPoint.kt @@ -0,0 +1,9 @@ +package org.cobalt.api.pathfinder.provider + +interface NavigationPoint { + fun isTraversable(): Boolean + fun hasFloor(): Boolean + fun getFloorLevel(): Double + fun isClimbable(): Boolean + fun isLiquid(): Boolean +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/provider/NavigationPointProvider.kt b/src/main/kotlin/org/cobalt/api/pathfinder/provider/NavigationPointProvider.kt new file mode 100644 index 0000000..ef354d3 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/provider/NavigationPointProvider.kt @@ -0,0 +1,12 @@ +package org.cobalt.api.pathfinder.provider + +import org.cobalt.api.pathfinder.pathing.context.EnvironmentContext +import org.cobalt.api.pathfinder.wrapper.PathPosition + +interface NavigationPointProvider { + fun getNavigationPoint(position: PathPosition): NavigationPoint { + return getNavigationPoint(position, null) + } + + fun getNavigationPoint(position: PathPosition, environmentContext: EnvironmentContext?): NavigationPoint +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/provider/impl/MinecraftNavigationProvider.kt b/src/main/kotlin/org/cobalt/api/pathfinder/provider/impl/MinecraftNavigationProvider.kt new file mode 100644 index 0000000..d3fe776 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/provider/impl/MinecraftNavigationProvider.kt @@ -0,0 +1,146 @@ +package org.cobalt.api.pathfinder.provider.impl + +import net.minecraft.client.Minecraft +import net.minecraft.core.BlockPos +import net.minecraft.core.Direction.Axis +import net.minecraft.tags.BlockTags +import net.minecraft.tags.FluidTags +import net.minecraft.world.level.block.* +import net.minecraft.world.level.block.state.BlockState +import net.minecraft.world.level.pathfinder.PathComputationType +import net.minecraft.world.phys.shapes.CollisionContext +import org.cobalt.api.pathfinder.pathing.context.EnvironmentContext +import org.cobalt.api.pathfinder.provider.NavigationPoint +import org.cobalt.api.pathfinder.provider.NavigationPointProvider +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class MinecraftNavigationProvider : NavigationPointProvider { + + private val mc: Minecraft = Minecraft.getInstance() + + override fun getNavigationPoint( + position: PathPosition, + environmentContext: EnvironmentContext?, + ): NavigationPoint { + val level = + mc.level + ?: return object : NavigationPoint { + override fun isTraversable() = false + override fun hasFloor() = false + override fun getFloorLevel() = 0.0 + override fun isClimbable() = false + override fun isLiquid() = false + } + + val x = position.getFlooredX() + val y = position.getFlooredY() + val z = position.getFlooredZ() + val blockPos = BlockPos(x, y, z) + + val feetState = level.getBlockState(blockPos) + val headState = level.getBlockState(blockPos.above()) + val belowState = level.getBlockState(blockPos.below()) + + fun canWalkThrough( + state: BlockState, + pos: BlockPos, + ): Boolean { + if (state.isAir) return true + + if (state.`is`(BlockTags.TRAPDOORS) || + state.`is`(Blocks.LILY_PAD) || + state.`is`(Blocks.BIG_DRIPLEAF) + ) { + return true + } + + if (state.`is`(Blocks.POWDER_SNOW) || + state.`is`(Blocks.CACTUS) || + state.`is`(Blocks.SWEET_BERRY_BUSH) || + state.`is`(Blocks.HONEY_BLOCK) || + state.`is`(Blocks.COCOA) || + state.`is`(Blocks.WITHER_ROSE) || + state.`is`(Blocks.POINTED_DRIPSTONE) + ) { + return true + } + + val block = state.block + if (block is DoorBlock) { + return if (state.getValue(DoorBlock.OPEN)) true else block.type().canOpenByHand() + } + + if (block is FenceGateBlock) { + return state.getValue(FenceGateBlock.OPEN) + } + + if (block is BaseRailBlock || block is LeavesBlock) { + return true + } + + if (state.`is`(BlockTags.FENCES) || state.`is`(BlockTags.WALLS)) { + return false + } + + return state.isPathfindable(PathComputationType.LAND) || + state.fluidState.`is`(FluidTags.WATER) + } + + fun canWalkOn(state: BlockState, pos: BlockPos): Boolean { + val block = state.block + if (state.isCollisionShapeFullBlock(level, pos) && + block != Blocks.MAGMA_BLOCK && + block != Blocks.BUBBLE_COLUMN && + block != Blocks.HONEY_BLOCK + ) { + return true + } + + return block is AzaleaBlock || + block is LadderBlock || + block is VineBlock || + block == Blocks.FARMLAND || + block == Blocks.DIRT_PATH || + block == Blocks.SOUL_SAND || + block == Blocks.CHEST || + block == Blocks.ENDER_CHEST || + block == Blocks.GLASS || + block is StairBlock || + block is SlabBlock || + block is BaseRailBlock || + !state.fluidState.isEmpty + } + + // based off WalkNodeEvaluator.getFloorLevel + fun calculateFloorLevel(pos: BlockPos): Double { + val state = level.getFluidState(pos) + if (state.`is`(FluidTags.WATER)) { + return pos.y.toDouble() + 0.5 + } + + val belowPos = pos.below() + val belowState = level.getBlockState(belowPos) + val shape = belowState.getCollisionShape(level, belowPos, CollisionContext.empty()) + return if (shape.isEmpty) { + belowPos.y.toDouble() + } else { + belowPos.y.toDouble() + shape.max(Axis.Y) + } + } + + val canPassFeetVal = canWalkThrough(feetState, blockPos) + val canPassHeadVal = canWalkThrough(headState, blockPos.above()) + val hasStableFloorVal = canWalkOn(belowState, blockPos.below()) + val floorLevelVal = calculateFloorLevel(blockPos) + val isClimbingVal = feetState.block is LadderBlock || feetState.block is VineBlock + val isLiquidVal = !feetState.fluidState.isEmpty + + return object : NavigationPoint { + override fun isTraversable(): Boolean = canPassFeetVal && canPassHeadVal + override fun hasFloor(): Boolean = hasStableFloorVal + override fun getFloorLevel(): Double = floorLevelVal + override fun isClimbable(): Boolean = isClimbingVal + override fun isLiquid(): Boolean = isLiquidVal + } + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathImpl.kt b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathImpl.kt new file mode 100644 index 0000000..1c9a534 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathImpl.kt @@ -0,0 +1,27 @@ +package org.cobalt.api.pathfinder.result + +import org.cobalt.api.pathfinder.pathing.result.Path +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class PathImpl( + private val start: PathPosition, + private val end: PathPosition, + private val positions: Iterable, +) : Path { + + private val length: Int = positions.count() + + override fun getStart(): PathPosition = start + + override fun getEnd(): PathPosition = end + + override fun iterator(): Iterator = positions.iterator() + + override fun length(): Int = length + + override fun collect(): Collection { + val collection = ArrayList(length) + positions.forEach { collection.add(it) } + return collection + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt new file mode 100644 index 0000000..1e4677c --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt @@ -0,0 +1,137 @@ +package org.cobalt.api.pathfinder.result + +import kotlin.math.abs +import kotlin.math.ceil +import kotlin.math.roundToInt +import net.minecraft.util.Mth +import org.cobalt.api.pathfinder.pathing.result.Path +import org.cobalt.api.pathfinder.util.ParameterizedSupplier +import org.cobalt.api.pathfinder.wrapper.PathPosition + +object PathUtils { + + fun interpolate(path: Path, resolution: Double): Path { + require(resolution > 0) { "Resolution must be > 0" } + + val result = ArrayDeque() + var previous: PathPosition? = null + + for (current in path) { + previous?.let { prev -> + interpolateSegment(prev, current, resolution, result) + } + result.addLast(current) + previous = current + } + + return buildPath(result) + } + + fun simplify(path: Path, epsilon: Double): Path { + validateEpsilon(epsilon) + + val result = ArrayDeque() + var index = 0 + val stride = maxOf(1, (1.0 / epsilon).roundToInt()) + + for (pos in path) { + if (index % stride == 0) { + result.addLast(pos) + } + index++ + } + + return buildPath(result) + } + + fun join(first: Path, second: Path): Path { + if (first.length() == 0) return second + if (second.length() == 0) return first + + val result = ArrayDeque() + first.forEach { result.addLast(it) } + second.forEach { result.addLast(it) } + + return buildPath(result) + } + + fun trim(path: Path, maxLength: Int): Path { + require(maxLength > 0) { "maxLength must be > 0" } + + if (path.length() <= maxLength) return path + + val result = ArrayDeque() + var count = 0 + + for (p in path) { + result.addLast(p) + if (++count >= maxLength) break + } + + return buildPath(result) + } + + fun mutatePositions(path: Path, mutator: ParameterizedSupplier): Path { + val result = ArrayDeque(path.length()) + + for (pos in path) { + result.addLast(mutator.accept(pos)) + } + + return buildPath(result) + } + + private fun interpolateSegment( + start: PathPosition, + end: PathPosition, + resolution: Double, + result: ArrayDeque, + ) { + val distance = start.distance(end) + val steps = ceil(distance / resolution).toInt() + + for (i in 1 until steps) { + val progress = i.toDouble() / steps + result.addLast(interpolate(start, end, progress)) + } + } + + private fun interpolate(pos1: PathPosition, pos2: PathPosition, progress: Double): PathPosition { + val x = Mth.lerp(progress, pos1.getX(), pos2.getX()) + val y = Mth.lerp(progress, pos1.getY(), pos2.getY()) + val z = Mth.lerp(progress, pos1.getZ(), pos2.getZ()) + return PathPosition(x, y, z) + } + + private fun buildPath(positions: ArrayDeque): Path { + require(positions.isNotEmpty()) { "Cannot build path from empty position list" } + + val path = PathImpl(positions.first(), positions.last(), positions) + return removeDuplicates(path) + } + + private fun removeDuplicates(path: Path): Path { + val EPS = 1e-12 + val result = ArrayDeque() + var last: PathPosition? = null + + for (pos in path) { + if (last == null || !samePoint(last, pos, EPS)) { + result.addLast(pos) + last = pos + } + } + + return PathImpl(result.first(), result.last(), result) + } + + private fun samePoint(a: PathPosition, b: PathPosition, eps: Double): Boolean { + return abs(a.getX() - b.getX()) <= eps && + abs(a.getY() - b.getY()) <= eps && + abs(a.getZ() - b.getZ()) <= eps + } + + private fun validateEpsilon(epsilon: Double) { + require(epsilon > 0.0 && epsilon <= 1.0) { "Epsilon must be in (0.0, 1.0]" } + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathfinderResultImpl.kt b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathfinderResultImpl.kt new file mode 100644 index 0000000..90f2d7f --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathfinderResultImpl.kt @@ -0,0 +1,29 @@ +package org.cobalt.api.pathfinder.result + +import org.cobalt.api.pathfinder.pathing.result.Path +import org.cobalt.api.pathfinder.pathing.result.PathState +import org.cobalt.api.pathfinder.pathing.result.PathfinderResult + +class PathfinderResultImpl( + private val pathState: PathState, + private val path: Path, +) : PathfinderResult { + + override fun successful(): Boolean { + return pathState == PathState.FOUND || + pathState == PathState.FALLBACK || + pathState == PathState.MAX_ITERATIONS_REACHED + } + + override fun hasFailed(): Boolean { + return pathState == PathState.FAILED || + pathState == PathState.ABORTED || + pathState == PathState.LENGTH_LIMITED + } + + override fun hasFallenBack(): Boolean = pathState == PathState.FALLBACK + + override fun getPathState(): PathState = pathState + + override fun getPath(): Path = path +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/util/GridRegionData.kt b/src/main/kotlin/org/cobalt/api/pathfinder/util/GridRegionData.kt new file mode 100644 index 0000000..9fdaa75 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/util/GridRegionData.kt @@ -0,0 +1,33 @@ +package org.cobalt.api.pathfinder.util + +import com.google.common.hash.BloomFilter +import com.google.common.hash.Funnel +import it.unimi.dsi.fastutil.longs.LongOpenHashSet +import it.unimi.dsi.fastutil.longs.LongSet +import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration +import org.cobalt.api.pathfinder.wrapper.PathPosition + +class GridRegionData { + private val bloomFilter: BloomFilter + private val regionalExaminedPositions: LongSet + + constructor(bloomFilterSize: Int, bloomFilterFpp: Double) { + val pathPositionFunnel = Funnel { pathPosition, into -> + into.putInt(pathPosition.getFlooredX()) + .putInt(pathPosition.getFlooredY()) + .putInt(pathPosition.getFlooredZ()) + } + + bloomFilter = BloomFilter.create(pathPositionFunnel, bloomFilterSize, bloomFilterFpp) + this.regionalExaminedPositions = LongOpenHashSet() + } + + constructor(configuration: PathfinderConfiguration) : this( + configuration.bloomFilterSize, + configuration.bloomFilterFpp + ) + + fun getBloomFilter(): BloomFilter = bloomFilter + + fun getRegionalExaminedPositions(): LongSet = regionalExaminedPositions +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/util/ParameterizedSupplier.kt b/src/main/kotlin/org/cobalt/api/pathfinder/util/ParameterizedSupplier.kt new file mode 100644 index 0000000..521a862 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/util/ParameterizedSupplier.kt @@ -0,0 +1,5 @@ +package org.cobalt.api.pathfinder.util + +fun interface ParameterizedSupplier { + fun accept(value: T): T +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/util/RegionKey.kt b/src/main/kotlin/org/cobalt/api/pathfinder/util/RegionKey.kt new file mode 100644 index 0000000..513d859 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/util/RegionKey.kt @@ -0,0 +1,20 @@ +package org.cobalt.api.pathfinder.util + +import org.cobalt.api.pathfinder.wrapper.PathPosition + +object RegionKey { + private const val MASK_Y = 0xFFFL // 12 Bit + private const val MASK_XZ = 0x3FFFFFFL // 26 Bit + private const val SHIFT_Z = 12 + private const val SHIFT_X = 38 // 12 + 26 + + fun pack(pos: PathPosition): Long { + return pack(pos.getFlooredX(), pos.getFlooredY(), pos.getFlooredZ()) + } + + fun pack(x: Int, y: Int, z: Int): Long { + return ((x.toLong() and MASK_XZ) shl SHIFT_X) or + ((z.toLong() and MASK_XZ) shl SHIFT_Z) or + (y.toLong() and MASK_Y) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/Depth.kt b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/Depth.kt new file mode 100644 index 0000000..827ee59 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/Depth.kt @@ -0,0 +1,13 @@ +package org.cobalt.api.pathfinder.wrapper + +data class Depth private constructor(private var value: Int) { + companion object { + fun of(value: Int): Depth = Depth(value) + } + + fun increment() { + value++ + } + + fun getValue(): Int = value +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathPosition.kt b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathPosition.kt new file mode 100644 index 0000000..5036b21 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathPosition.kt @@ -0,0 +1,98 @@ +package org.cobalt.api.pathfinder.wrapper + +import kotlin.math.sqrt +import net.minecraft.util.Mth + +data class PathPosition( + private val x: Double, + private val y: Double, + private val z: Double, +) : Cloneable { + + fun distanceSquared(otherPosition: PathPosition): Double { + return Mth.square(this.x - otherPosition.x) + + Mth.square(this.y - otherPosition.y) + + Mth.square(this.z - otherPosition.z) + } + + fun distance(otherPosition: PathPosition): Double { + return sqrt(this.distanceSquared(otherPosition)) + } + + fun setX(x: Double): PathPosition = PathPosition(x, this.y, this.z) + + fun setY(y: Double): PathPosition = PathPosition(this.x, y, this.z) + + fun setZ(z: Double): PathPosition = PathPosition(this.x, this.y, z) + + fun getCenteredX(): Double = getFlooredX() + 0.5 + + fun getCenteredY(): Double = getFlooredY() + 0.5 + + fun getCenteredZ(): Double = getFlooredZ() + 0.5 + + fun getFlooredX(): Int = Mth.floor(this.x) + + fun getFlooredY(): Int = Mth.floor(this.y) + + fun getFlooredZ(): Int = Mth.floor(this.z) + + fun add(x: Double, y: Double, z: Double): PathPosition { + return PathPosition(this.x + x, this.y + y, this.z + z) + } + + fun add(vector: PathVector): PathPosition { + return add(vector.getX(), vector.getY(), vector.getZ()) + } + + fun subtract(x: Double, y: Double, z: Double): PathPosition { + return PathPosition(this.x - x, this.y - y, this.z - z) + } + + fun subtract(vector: PathVector): PathPosition { + return subtract(vector.getX(), vector.getY(), vector.getZ()) + } + + fun toVector(): PathVector = PathVector(this.x, this.y, this.z) + + fun floor(): PathPosition = PathPosition(getFlooredX().toDouble(), getFlooredY().toDouble(), getFlooredZ().toDouble()) + + fun mid(): PathPosition = PathPosition(getFlooredX() + 0.5, getFlooredY() + 0.5, getFlooredZ() + 0.5) + + fun midPoint(end: PathPosition): PathPosition { + return PathPosition((this.x + end.x) / 2, (this.y + end.y) / 2, (this.z + end.z) / 2) + } + + public override fun clone(): PathPosition { + return PathPosition(this.x, this.y, this.z) + } + + override fun equals(other: Any?): Boolean { + if (this === other) return true + if (other == null || this::class != other::class) return false + other as PathPosition + return getFlooredX() == other.getFlooredX() && + getFlooredY() == other.getFlooredY() && + getFlooredZ() == other.getFlooredZ() + } + + override fun hashCode(): Int { + val x = getFlooredX() + val y = getFlooredY() + val z = getFlooredZ() + var result = x + result = 31 * result + y + result = 31 * result + z + return result + } + + fun getX(): Double = this.x + + fun getY(): Double = this.y + + fun getZ(): Double = this.z + + override fun toString(): String { + return "PathPosition(x=${getX()}, y=${getY()}, z=${getZ()})" + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathVector.kt b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathVector.kt new file mode 100644 index 0000000..62ca0aa --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathVector.kt @@ -0,0 +1,81 @@ +package org.cobalt.api.pathfinder.wrapper + +import kotlin.math.sqrt +import net.minecraft.util.Mth + +data class PathVector( + private val x: Double, + private val y: Double, + private val z: Double, +) : Cloneable { + + companion object { + fun computeDistance(A: PathVector, B: PathVector, C: PathVector): Double { + val d = C.subtract(B).divide(C.distance(B)) + val v = A.subtract(B) + val t = v.dot(d) + val P = B.add(d.multiply(t)) + return P.distance(A) + } + } + + fun dot(otherVector: PathVector): Double { + return this.x * otherVector.x + this.y * otherVector.y + this.z * otherVector.z + } + + fun length(): Double { + return sqrt(Mth.square(this.x) + Mth.square(this.y) + Mth.square(this.z)) + } + + fun distance(otherVector: PathVector): Double { + return sqrt( + Mth.square(this.x - otherVector.x) + + Mth.square(this.y - otherVector.y) + + Mth.square(this.z - otherVector.z) + ) + } + + fun setX(x: Double): PathVector = PathVector(x, this.y, this.z) + + fun setY(y: Double): PathVector = PathVector(this.x, y, this.z) + + fun setZ(z: Double): PathVector = PathVector(this.x, this.y, z) + + fun subtract(otherVector: PathVector): PathVector { + return PathVector(this.x - otherVector.x, this.y - otherVector.y, this.z - otherVector.z) + } + + fun multiply(value: Double): PathVector { + return PathVector(this.x * value, this.y * value, this.z * value) + } + + fun normalize(): PathVector { + val magnitude = this.length() + return PathVector(this.x / magnitude, this.y / magnitude, this.z / magnitude) + } + + fun divide(value: Double): PathVector { + return PathVector(this.x / value, this.y / value, this.z / value) + } + + fun add(otherVector: PathVector): PathVector { + return PathVector(this.x + otherVector.x, this.y + otherVector.y, this.z + otherVector.z) + } + + fun getCrossProduct(o: PathVector): PathVector { + val x = this.y * o.getZ() - o.getY() * this.z + val y = this.z * o.getX() - o.getZ() * this.x + val z = this.x * o.getY() - o.getX() * this.y + return PathVector(x, y, z) + } + + public override fun clone(): PathVector { + return PathVector(this.x, this.y, this.z) + } + + fun getX(): Double = this.x + + fun getY(): Double = this.y + + fun getZ(): Double = this.z +} diff --git a/src/main/kotlin/org/cobalt/internal/command/MainCommand.kt b/src/main/kotlin/org/cobalt/internal/command/MainCommand.kt index 865305a..4d2e50c 100644 --- a/src/main/kotlin/org/cobalt/internal/command/MainCommand.kt +++ b/src/main/kotlin/org/cobalt/internal/command/MainCommand.kt @@ -5,16 +5,14 @@ import org.cobalt.api.command.Command import org.cobalt.api.command.annotation.DefaultHandler import org.cobalt.api.command.annotation.SubCommand import org.cobalt.api.notification.NotificationManager +import org.cobalt.api.pathfinder.PathExecutor import org.cobalt.api.rotation.EasingType import org.cobalt.api.rotation.RotationExecutor import org.cobalt.api.rotation.strategy.TimedEaseStrategy import org.cobalt.api.util.helper.Rotation import org.cobalt.internal.ui.screen.UIConfig -internal object MainCommand : Command( - name = "cobalt", - aliases = arrayOf("cb") -) { +internal object MainCommand : Command(name = "cobalt", aliases = arrayOf("cb")) { @DefaultHandler fun main() { @@ -24,12 +22,12 @@ internal object MainCommand : Command( @SubCommand fun rotate(yaw: Double, pitch: Double, duration: Int) { RotationExecutor.rotateTo( - Rotation(yaw.toFloat(), pitch.toFloat()), - TimedEaseStrategy( - yawEasing = EasingType.EASE_OUT_EXPO, - pitchEasing = EasingType.EASE_OUT_EXPO, - duration = duration.toLong() - ) + Rotation(yaw.toFloat(), pitch.toFloat()), + TimedEaseStrategy( + yawEasing = EasingType.EASE_OUT_EXPO, + pitchEasing = EasingType.EASE_OUT_EXPO, + duration = duration.toLong() + ) ) } @@ -39,18 +37,27 @@ internal object MainCommand : Command( val pitch = Random.nextFloat() * 180f - 90f RotationExecutor.rotateTo( - Rotation(yaw, pitch), - TimedEaseStrategy( - yawEasing = EasingType.EASE_OUT_EXPO, - pitchEasing = EasingType.EASE_OUT_EXPO, - duration = 400L - ) + Rotation(yaw, pitch), + TimedEaseStrategy( + yawEasing = EasingType.EASE_OUT_EXPO, + pitchEasing = EasingType.EASE_OUT_EXPO, + duration = 400L + ) ) } + @SubCommand + fun start(x: Double, y: Double, z: Double) { + PathExecutor.start(x, y, z) + } + + @SubCommand + fun stop() { + PathExecutor.stop() + } + @SubCommand fun notification(title: String, description: String) { NotificationManager.sendNotification(title, description) } - } From bf28d90ee4867268f38f4a0c9d7b163f5b57a220 Mon Sep 17 00:00:00 2001 From: vince calder <92699085+awrped@users.noreply.github.com> Date: Mon, 19 Jan 2026 18:45:00 -0600 Subject: [PATCH 4/8] cleanup pt1 --- .../api/pathfinder/factory/PathfinderFactory.kt | 14 +++++++++++--- .../pathfinder/factory/PathfinderInitializer.kt | 2 +- .../factory/impl/AStarPathfinderFactory.kt | 4 ---- .../configuration/PathfinderConfiguration.kt | 3 +++ 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderFactory.kt b/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderFactory.kt index bf89122..6c45153 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderFactory.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderFactory.kt @@ -4,9 +4,17 @@ import org.cobalt.api.pathfinder.pathing.Pathfinder import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration interface PathfinderFactory { - fun createPathfinder(): Pathfinder - fun createPathfinder(configuration: PathfinderConfiguration): Pathfinder - fun createPathfinder(configuration: PathfinderConfiguration, initializer: PathfinderInitializer): Pathfinder { + fun createPathfinder(): Pathfinder = createPathfinder(PathfinderConfiguration.DEFAULT) + + fun createPathfinder(configuration: PathfinderConfiguration): Pathfinder = + throw UnsupportedOperationException( + "This factory does not support creating pathfinders with a configuration." + ) + + fun createPathfinder( + configuration: PathfinderConfiguration, + initializer: PathfinderInitializer, + ): Pathfinder { val pathfinder = createPathfinder(configuration) initializer.initialize(pathfinder, configuration) return pathfinder diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderInitializer.kt b/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderInitializer.kt index 171dfc7..378ec5e 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderInitializer.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/factory/PathfinderInitializer.kt @@ -3,6 +3,6 @@ package org.cobalt.api.pathfinder.factory import org.cobalt.api.pathfinder.pathing.Pathfinder import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration -interface PathfinderInitializer { +fun interface PathfinderInitializer { fun initialize(pathfinder: Pathfinder, configuration: PathfinderConfiguration) } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/factory/impl/AStarPathfinderFactory.kt b/src/main/kotlin/org/cobalt/api/pathfinder/factory/impl/AStarPathfinderFactory.kt index e83047e..e8be3e0 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/factory/impl/AStarPathfinderFactory.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/factory/impl/AStarPathfinderFactory.kt @@ -7,10 +7,6 @@ import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration class AStarPathfinderFactory : PathfinderFactory { - override fun createPathfinder(): Pathfinder { - return AStarPathfinder(PathfinderConfiguration.builder().build()) - } - override fun createPathfinder(configuration: PathfinderConfiguration): Pathfinder { return AStarPathfinder(configuration) } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/configuration/PathfinderConfiguration.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/configuration/PathfinderConfiguration.kt index 77735da..b14ac13 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/configuration/PathfinderConfiguration.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/configuration/PathfinderConfiguration.kt @@ -29,6 +29,8 @@ data class PathfinderConfiguration( val reopenClosedNodes: Boolean, ) { companion object { + val DEFAULT: PathfinderConfiguration = builder().build() + fun deepCopy(pathfinderConfiguration: PathfinderConfiguration): PathfinderConfiguration { return builder() .maxIterations(pathfinderConfiguration.maxIterations) @@ -74,6 +76,7 @@ class PathfinderConfigurationBuilder { return object : NavigationPoint { override fun isTraversable(): Boolean = true override fun hasFloor(): Boolean = true + override fun getFloorLevel(): Double = 0.0 override fun isClimbable(): Boolean = false override fun isLiquid(): Boolean = false } From 715c3dc940a3976e5380c902706dd7f83b59157a Mon Sep 17 00:00:00 2001 From: vince calder <92699085+awrped@users.noreply.github.com> Date: Mon, 19 Jan 2026 18:48:49 -0600 Subject: [PATCH 5/8] pt2 --- .../pathfinder/pathfinder/AStarPathfinder.kt | 81 ++++++------- .../pathfinder/AbstractPathfinder.kt | 114 +++++++++++------- .../pathfinder/heap/PrimitiveMinHeap.kt | 25 ++-- 3 files changed, 120 insertions(+), 100 deletions(-) diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt index 9568de2..bf58109 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt @@ -11,7 +11,6 @@ import org.cobalt.api.pathfinder.Node import org.cobalt.api.pathfinder.pathfinder.heap.PrimitiveMinHeap import org.cobalt.api.pathfinder.pathfinder.processing.EvaluationContextImpl import org.cobalt.api.pathfinder.pathing.configuration.PathfinderConfiguration -import org.cobalt.api.pathfinder.pathing.processing.Cost import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext import org.cobalt.api.pathfinder.util.GridRegionData @@ -42,8 +41,8 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind } override fun processSuccessors( - start: PathPosition, - target: PathPosition, + requestStart: PathPosition, + requestTarget: PathPosition, currentNode: Node, openSet: PrimitiveMinHeap, searchContext: SearchContext, @@ -70,13 +69,15 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind if (pathfinderConfiguration.shouldReopenClosedNodes()) { val oldCost = session.closedSetGCosts[packedPos] - val tempNeighbor = createNeighborNode(neighborPos, start, target, currentNode) - val context = EvaluationContextImpl( - searchContext, - tempNeighbor, - currentNode, - pathfinderConfiguration.heuristicStrategy - ) + val tempNeighbor = + createNeighborNode(neighborPos, requestStart, requestTarget, currentNode) + val context = + EvaluationContextImpl( + searchContext, + tempNeighbor, + currentNode, + pathfinderConfiguration.heuristicStrategy + ) val newGCost = calculateGCost(context) if (oldCost.isNaN() || newGCost + Math.ulp(newGCost) < oldCost) { @@ -88,15 +89,16 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind if (!shouldReopen) continue } - val neighbor = createNeighborNode(neighborPos, start, target, currentNode) + val neighbor = createNeighborNode(neighborPos, requestStart, requestTarget, currentNode) neighbor.setParent(currentNode) - val context = EvaluationContextImpl( - searchContext, - neighbor, - currentNode, - pathfinderConfiguration.heuristicStrategy - ) + val context = + EvaluationContextImpl( + searchContext, + neighbor, + currentNode, + pathfinderConfiguration.heuristicStrategy + ) if (!isValidByCustomProcessors(context)) { continue @@ -119,12 +121,13 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind searchContext: SearchContext, openSet: PrimitiveMinHeap, ) { - val context = EvaluationContextImpl( - searchContext, - existing, - currentNode, - pathfinderConfiguration.heuristicStrategy - ) + val context = + EvaluationContextImpl( + searchContext, + existing, + currentNode, + pathfinderConfiguration.heuristicStrategy + ) val newG = calculateGCost(context) val tol = Math.ulp(max(abs(newG), abs(existing.getGCost()))) @@ -179,20 +182,10 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind private fun calculateGCost(context: EvaluationContext): Double { val baseCost = context.getBaseTransitionCost() - var additionalCost = 0.0 - - if (!costProcessors.isNullOrEmpty()) { - for (processor in costProcessors) { - val contribution = processor.calculateCostContribution(context) - additionalCost += contribution?.value ?: Cost.ZERO.value - } - } - - var transitionCost = baseCost + additionalCost - if (transitionCost < 0) { - transitionCost = 0.0 - } + val additionalCost = + costProcessors?.sumOf { it.calculateCostContribution(context).value } ?: 0.0 + val transitionCost = max(0.0, baseCost + additionalCost) return context.getPathCostToPreviousPosition() + transitionCost } @@ -218,15 +211,16 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind private fun getSessionOrThrow(): PathfindingSession { return currentSession.get() - ?: throw IllegalStateException("Pathfinding session not initialized. Call initializeSearch() first.") + ?: throw IllegalStateException( + "Pathfinding session not initialized. Call initializeSearch() first." + ) } private inner class PathfindingSession { val visitedRegions: Long2ObjectMap = Long2ObjectOpenHashMap() val openSetNodes: Long2ObjectMap = Long2ObjectOpenHashMap() - val closedSetGCosts: Long2DoubleMap = Long2DoubleOpenHashMap().apply { - defaultReturnValue(Double.NaN) - } + val closedSetGCosts: Long2DoubleMap = + Long2DoubleOpenHashMap().apply { defaultReturnValue(Double.NaN) } fun getOrCreateRegionData(position: PathPosition): GridRegionData { val cellSize = pathfinderConfiguration.gridCellSize @@ -235,9 +229,10 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind val rZ = Mth.floorDiv(position.getFlooredZ(), cellSize) val regionKey = RegionKey.pack(rX, rY, rZ) - return visitedRegions.computeIfAbsent(regionKey) { - GridRegionData(pathfinderConfiguration) - } + return visitedRegions.computeIfAbsent( + regionKey, + java.util.function.LongFunction { GridRegionData(pathfinderConfiguration) } + ) } } } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AbstractPathfinder.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AbstractPathfinder.kt index f64ef04..0d288e8 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AbstractPathfinder.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AbstractPathfinder.kt @@ -2,6 +2,7 @@ package org.cobalt.api.pathfinder.pathfinder import java.util.* import java.util.concurrent.* +import java.util.concurrent.atomic.AtomicBoolean import kotlin.math.abs import kotlin.math.max import org.cobalt.api.pathfinder.Node @@ -56,26 +57,27 @@ abstract class AbstractPathfinder( } protected val navigationPointProvider: NavigationPointProvider = pathfinderConfiguration.provider - protected val validationProcessors: List? = pathfinderConfiguration.getNodeValidationProcessors() - protected val costProcessors: List? = pathfinderConfiguration.getNodeCostProcessors() + protected val validationProcessors: List? = + pathfinderConfiguration.getNodeValidationProcessors() + protected val costProcessors: List? = + pathfinderConfiguration.getNodeCostProcessors() protected val neighborStrategy: INeighborStrategy = pathfinderConfiguration.neighborStrategy private val pathfinderHooks: MutableSet = Collections.synchronizedSet(HashSet()) - @Volatile - private var abortRequested = false + private val abortRequested = AtomicBoolean(false) override fun findPath( start: PathPosition, target: PathPosition, context: EnvironmentContext?, ): CompletionStage { - this.abortRequested = false + this.abortRequested.set(false) return initiatePathing(start, target, context) } override fun abort() { - this.abortRequested = true + this.abortRequested.set(true) } override fun registerPathfindingHook(hook: PathfinderHook) { @@ -92,12 +94,17 @@ abstract class AbstractPathfinder( return if (pathfinderConfiguration.async) { CompletableFuture.supplyAsync( - { executePathingAlgorithm(effectiveStart, effectiveTarget, environmentContext) }, + { + executePathingAlgorithm(effectiveStart, effectiveTarget, environmentContext) + }, PATHING_EXECUTOR_SERVICE - ).exceptionally { throwable -> handlePathingException(start, target, throwable) } + ) + .exceptionally { throwable -> handlePathingException(start, target, throwable) } } else { try { - CompletableFuture.completedFuture(executePathingAlgorithm(effectiveStart, effectiveTarget, environmentContext)) + CompletableFuture.completedFuture( + executePathingAlgorithm(effectiveStart, effectiveTarget, environmentContext) + ) } catch (e: Exception) { CompletableFuture.completedFuture(handlePathingException(start, target, e)) } @@ -111,13 +118,14 @@ abstract class AbstractPathfinder( ): PathfinderResult { initializeSearch() - val searchContext = SearchContextImpl( - start, - target, - this.pathfinderConfiguration, - this.navigationPointProvider, - environmentContext - ) + val searchContext = + SearchContextImpl( + start, + target, + this.pathfinderConfiguration, + this.navigationPointProvider, + environmentContext + ) val processors = getProcessors() @@ -125,26 +133,31 @@ abstract class AbstractPathfinder( processors.forEach { it.initializeSearch(searchContext) } val startNode = createStartNode(start, target) - val startNodeContext = EvaluationContextImpl( - searchContext, - startNode, - null, - pathfinderConfiguration.heuristicStrategy - ) + val startNodeContext = + EvaluationContextImpl( + searchContext, + startNode, + null, + pathfinderConfiguration.heuristicStrategy + ) if (!validationProcessors.isNullOrEmpty()) { val isStartNodeInvalid = validationProcessors.any { !it.isValid(startNodeContext) } if (isStartNodeInvalid) { - return PathfinderResultImpl(PathState.FAILED, PathImpl(start, target, EMPTY_PATH_POSITIONS)) + return PathfinderResultImpl( + PathState.FAILED, + PathImpl(start, target, EMPTY_PATH_POSITIONS) + ) } } val openSet = PrimitiveMinHeap(1024) - val startKey = try { - calculateHeapKey(startNode, startNode.getFCost()) - } catch (t: Throwable) { - startNode.getFCost() - } + val startKey = + try { + calculateHeapKey(startNode, startNode.getFCost()) + } catch (t: Throwable) { + startNode.getFCost() + } insertStartNode(startNode, startKey, openSet) @@ -154,7 +167,7 @@ abstract class AbstractPathfinder( while (!openSet.isEmpty() && currentDepth < pathfinderConfiguration.maxIterations) { currentDepth++ - if (this.abortRequested) { + if (this.abortRequested.get()) { return createAbortedResult(start, target, bestFallbackNode) } @@ -162,7 +175,9 @@ abstract class AbstractPathfinder( markNodeAsExpanded(currentNode) pathfinderHooks.forEach { hook -> - hook.onPathfindingStep(PathfindingContext(currentNode.getPosition(), Depth.of(currentDepth))) + hook.onPathfindingStep( + PathfindingContext(currentNode.getPosition(), Depth.of(currentDepth)) + ) } if (currentNode.getHeuristic() < bestFallbackNode.getHeuristic()) { @@ -170,7 +185,10 @@ abstract class AbstractPathfinder( } if (hasReachedPathLengthLimit(currentNode)) { - return PathfinderResultImpl(PathState.LENGTH_LIMITED, reconstructPath(start, target, currentNode)) + return PathfinderResultImpl( + PathState.LENGTH_LIMITED, + reconstructPath(start, target, currentNode) + ) } if (currentNode.isTarget(target)) { @@ -181,7 +199,6 @@ abstract class AbstractPathfinder( } return determinePostLoopResult(currentDepth, start, target, bestFallbackNode) - } catch (e: Exception) { return PathfinderResultImpl(PathState.FAILED, PathImpl(start, target, EMPTY_PATH_POSITIONS)) } finally { @@ -218,17 +235,24 @@ abstract class AbstractPathfinder( return processors } - private fun createAbortedResult(start: PathPosition, target: PathPosition, fallbackNode: Node): PathfinderResult { - this.abortRequested = false + private fun createAbortedResult( + start: PathPosition, + target: PathPosition, + fallbackNode: Node, + ): PathfinderResult { + this.abortRequested.set(false) return PathfinderResultImpl(PathState.ABORTED, reconstructPath(start, target, fallbackNode)) } private fun handlePathingException( originalStart: PathPosition, originalTarget: PathPosition, - throwable: Throwable, + @Suppress("UNUSED_PARAMETER") throwable: Throwable, ): PathfinderResult { - return PathfinderResultImpl(PathState.FAILED, PathImpl(originalStart, originalTarget, EMPTY_PATH_POSITIONS)) + return PathfinderResultImpl( + PathState.FAILED, + PathImpl(originalStart, originalTarget, EMPTY_PATH_POSITIONS) + ) } protected fun createStartNode(startPos: PathPosition, targetPos: PathPosition): Node { @@ -255,7 +279,10 @@ abstract class AbstractPathfinder( ): PathfinderResult { return when { depthReached >= pathfinderConfiguration.maxIterations -> { - PathfinderResultImpl(PathState.MAX_ITERATIONS_REACHED, reconstructPath(start, target, fallbackNode)) + PathfinderResultImpl( + PathState.MAX_ITERATIONS_REACHED, + reconstructPath(start, target, fallbackNode) + ) } pathfinderConfiguration.fallback -> { @@ -278,15 +305,10 @@ abstract class AbstractPathfinder( } private fun tracePathPositionsFromNode(leafNode: Node): List { - val path = mutableListOf() - var currentNode: Node? = leafNode - - while (currentNode != null) { - path.add(currentNode.getPosition()) - currentNode = currentNode.getParent() - } - - return path.reversed() + return generateSequence(leafNode) { it.getParent() } + .map { it.getPosition() } + .toList() + .reversed() } protected abstract fun insertStartNode(node: Node, fCost: Double, openSet: PrimitiveMinHeap) diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/heap/PrimitiveMinHeap.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/heap/PrimitiveMinHeap.kt index 51825c9..c997ef5 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/heap/PrimitiveMinHeap.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/heap/PrimitiveMinHeap.kt @@ -4,9 +4,8 @@ import it.unimi.dsi.fastutil.longs.Long2IntOpenHashMap class PrimitiveMinHeap(initialCapacity: Int) { - private val nodeToIndexMap: Long2IntOpenHashMap = Long2IntOpenHashMap(initialCapacity).apply { - defaultReturnValue(-1) - } + private val nodeToIndexMap: Long2IntOpenHashMap = + Long2IntOpenHashMap(initialCapacity).apply { defaultReturnValue(-1) } private var nodes: LongArray = LongArray(initialCapacity + 1) private var costs: DoubleArray = DoubleArray(initialCapacity + 1) @@ -21,6 +20,16 @@ class PrimitiveMinHeap(initialCapacity: Int) { nodeToIndexMap.clear() } + fun peekMin(): Long { + if (size == 0) throw NoSuchElementException() + return nodes[1] + } + + fun peekMinCost(): Double { + if (size == 0) throw NoSuchElementException() + return costs[1] + } + fun contains(packedNode: Long): Boolean = nodeToIndexMap.containsKey(packedNode) fun getCost(packedNode: Long): Double { @@ -69,14 +78,8 @@ class PrimitiveMinHeap(initialCapacity: Int) { private fun ensureCapacity() { if (size >= nodes.size - 1) { val newCap = nodes.size * 2 - val newNodes = LongArray(newCap) - val newCosts = DoubleArray(newCap) - - System.arraycopy(nodes, 0, newNodes, 0, nodes.size) - System.arraycopy(costs, 0, newCosts, 0, costs.size) - - this.nodes = newNodes - this.costs = newCosts + nodes = nodes.copyOf(newCap) + costs = costs.copyOf(newCap) } } From 3a0f460596273fbd928424d781257ed29873b019 Mon Sep 17 00:00:00 2001 From: vince calder <92699085+awrped@users.noreply.github.com> Date: Mon, 19 Jan 2026 18:54:58 -0600 Subject: [PATCH 6/8] pt3 i think --- .../pathfinder/pathing/NeighborStrategies.kt | 26 ++-- .../pathing/heuristic/HeuristicWeights.kt | 14 +- .../heuristic/InternalHeuristicUtils.kt | 50 +++++++ .../heuristic/LinearHeuristicStrategy.kt | 101 ++++++------- .../heuristic/SquaredHeuristicStrategy.kt | 105 ++++++-------- .../pathing/processing/Validators.kt | 136 +++++++----------- 6 files changed, 209 insertions(+), 223 deletions(-) create mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/InternalHeuristicUtils.kt diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/NeighborStrategies.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/NeighborStrategies.kt index 12b49b2..0c63d0a 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/NeighborStrategies.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/NeighborStrategies.kt @@ -3,7 +3,7 @@ package org.cobalt.api.pathfinder.pathing import org.cobalt.api.pathfinder.wrapper.PathVector object NeighborStrategies { - val VERTICAL_AND_HORIZONTAL = INeighborStrategy { + private val VERTICAL_AND_HORIZONTAL_OFFSETS = listOf( PathVector(1.0, 0.0, 0.0), PathVector(-1.0, 0.0, 0.0), @@ -12,22 +12,19 @@ object NeighborStrategies { PathVector(0.0, 1.0, 0.0), PathVector(0.0, -1.0, 0.0) ) - } - val DIAGONAL_3D = INeighborStrategy { - buildList { - for (x in -1..1) { - for (y in -1..1) { - for (z in -1..1) { - if (x == 0 && y == 0 && z == 0) continue - add(PathVector(x.toDouble(), y.toDouble(), z.toDouble())) - } + private val DIAGONAL_3D_OFFSETS = buildList { + for (x in -1..1) { + for (y in -1..1) { + for (z in -1..1) { + if (x == 0 && y == 0 && z == 0) continue + add(PathVector(x.toDouble(), y.toDouble(), z.toDouble())) } } } } - val HORIZONTAL_DIAGONAL_AND_VERTICAL = INeighborStrategy { + private val HORIZONTAL_DIAGONAL_AND_VERTICAL_OFFSETS = listOf( PathVector(1.0, 0.0, 0.0), PathVector(-1.0, 0.0, 0.0), @@ -40,5 +37,12 @@ object NeighborStrategies { PathVector(-1.0, 0.0, 1.0), PathVector(-1.0, 0.0, -1.0) ) + + val VERTICAL_AND_HORIZONTAL = INeighborStrategy { VERTICAL_AND_HORIZONTAL_OFFSETS } + + val DIAGONAL_3D = INeighborStrategy { DIAGONAL_3D_OFFSETS } + + val HORIZONTAL_DIAGONAL_AND_VERTICAL = INeighborStrategy { + HORIZONTAL_DIAGONAL_AND_VERTICAL_OFFSETS } } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicWeights.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicWeights.kt index 7698935..b4c76bc 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicWeights.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/HeuristicWeights.kt @@ -1,22 +1,12 @@ package org.cobalt.api.pathfinder.pathing.heuristic -data class HeuristicWeights -private constructor( +data class HeuristicWeights( val manhattanWeight: Double, val octileWeight: Double, val perpendicularWeight: Double, val heightWeight: Double, ) { companion object { - val DEFAULT_WEIGHTS = create(0.0, 1.0, 0.0, 0.0) - - fun create( - manhattanWeight: Double, - octileWeight: Double, - perpendicularWeight: Double, - heightWeight: Double, - ): HeuristicWeights { - return HeuristicWeights(manhattanWeight, octileWeight, perpendicularWeight, heightWeight) - } + val DEFAULT_WEIGHTS = HeuristicWeights(0.0, 1.0, 0.0, 0.0) } } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/InternalHeuristicUtils.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/InternalHeuristicUtils.kt new file mode 100644 index 0000000..5339e71 --- /dev/null +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/InternalHeuristicUtils.kt @@ -0,0 +1,50 @@ +package org.cobalt.api.pathfinder.pathing.heuristic + +import kotlin.math.sqrt +import org.cobalt.api.pathfinder.pathing.PathfindingProgress + +internal object InternalHeuristicUtils { + private const val EPSILON = 1e-9 + + fun calculatePerpendicularDistanceSq(progress: PathfindingProgress): Double { + val s = progress.startPosition() + val c = progress.currentPosition() + val t = progress.targetPosition() + + val sx = s.getCenteredX() + val sy = s.getCenteredY() + val sz = s.getCenteredZ() + val cx = c.getCenteredX() + val cy = c.getCenteredY() + val cz = c.getCenteredZ() + val tx = t.getCenteredX() + val ty = t.getCenteredY() + val tz = t.getCenteredZ() + + val lineX = tx - sx + val lineY = ty - sy + val lineZ = tz - sz + val lineSq = lineX * lineX + lineY * lineY + lineZ * lineZ + + if (lineSq < EPSILON) { + val dx = cx - sx + val dy = cy - sy + val dz = cz - sz + return dx * dx + dy * dy + dz * dz + } + + val toX = cx - sx + val toY = cy - sy + val toZ = cz - sz + val crossX = toY * lineZ - toZ * lineY + val crossY = toZ * lineX - toX * lineZ + val crossZ = toX * lineY - toY * lineX + val crossSq = crossX * crossX + crossY * crossY + crossZ * crossZ + + return crossSq / lineSq + } + + fun calculatePerpendicularDistance(progress: PathfindingProgress): Double { + return sqrt(calculatePerpendicularDistanceSq(progress)) + } +} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt index d6e1e0d..28c5c43 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt @@ -13,71 +13,54 @@ class LinearHeuristicStrategy : IHeuristicStrategy { private val D3 = sqrt(3.0) } - private val perpendicularCalc = DistanceCalculator { progress -> - val s = progress.startPosition() - val c = progress.currentPosition() - val t = progress.targetPosition() - - val sx = s.getCenteredX() - val sy = s.getCenteredY() - val sz = s.getCenteredZ() - val cx = c.getCenteredX() - val cy = c.getCenteredY() - val cz = c.getCenteredZ() - val tx = t.getCenteredX() - val ty = t.getCenteredY() - val tz = t.getCenteredZ() - - val lineX = tx - sx - val lineY = ty - sy - val lineZ = tz - sz - val lineSq = lineX * lineX + lineY * lineY + lineZ * lineZ - - if (lineSq < EPSILON) { - val dx = cx - sx - val dy = cy - sy - val dz = cz - sz - return@DistanceCalculator sqrt(dx * dx + dy * dy + dz * dz) + private val perpendicularCalc = + DistanceCalculator { progress -> + InternalHeuristicUtils.calculatePerpendicularDistance(progress) } - val toX = cx - sx - val toY = cy - sy - val toZ = cz - sz - val crossX = toY * lineZ - toZ * lineY - val crossY = toZ * lineX - toX * lineZ - val crossZ = toX * lineY - toY * lineX - val crossSq = crossX * crossX + crossY * crossY + crossZ * crossZ - - sqrt(crossSq / lineSq) - } - - private val octileCalc = DistanceCalculator { progress -> - val dx = abs(progress.currentPosition().getFlooredX() - progress.targetPosition().getFlooredX()) - val dy = abs(progress.currentPosition().getFlooredY() - progress.targetPosition().getFlooredY()) - val dz = abs(progress.currentPosition().getFlooredZ() - progress.targetPosition().getFlooredZ()) - - val min = minOf(dx, dy, dz) - val max = maxOf(dx, dy, dz) - val mid = dx + dy + dz - min - max - - (D3 - D2) * min + (D2 - D1) * mid + D1 * max - } + private val octileCalc = + DistanceCalculator { progress -> + val dx = + abs( + progress.currentPosition().getFlooredX() - + progress.targetPosition().getFlooredX() + ) + val dy = + abs( + progress.currentPosition().getFlooredY() - + progress.targetPosition().getFlooredY() + ) + val dz = + abs( + progress.currentPosition().getFlooredZ() - + progress.targetPosition().getFlooredZ() + ) + + val min = minOf(dx, dy, dz) + val max = maxOf(dx, dy, dz) + val mid = dx + dy + dz - min - max + + (D3 - D2) * min + (D2 - D1) * mid + D1 * max + } - private val manhattanCalc = DistanceCalculator { progress -> - val position = progress.currentPosition() - val target = progress.targetPosition() + private val manhattanCalc = + DistanceCalculator { progress -> + val position = progress.currentPosition() + val target = progress.targetPosition() - (abs(position.getFlooredX() - target.getFlooredX()) + - abs(position.getFlooredY() - target.getFlooredY()) + - abs(position.getFlooredZ() - target.getFlooredZ())).toDouble() - } + (abs(position.getFlooredX() - target.getFlooredX()) + + abs(position.getFlooredY() - target.getFlooredY()) + + abs(position.getFlooredZ() - target.getFlooredZ())) + .toDouble() + } - private val heightCalc = DistanceCalculator { progress -> - val position = progress.currentPosition() - val target = progress.targetPosition() + private val heightCalc = + DistanceCalculator { progress -> + val position = progress.currentPosition() + val target = progress.targetPosition() - abs(position.getFlooredY() - target.getFlooredY()).toDouble() - } + abs(position.getFlooredY() - target.getFlooredY()).toDouble() + } override fun calculate(context: HeuristicContext): Double { val progress = context.getPathfindingProgress() diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt index 708c447..53bb7f9 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt @@ -13,72 +13,57 @@ class SquaredHeuristicStrategy : IHeuristicStrategy { private val D3 = sqrt(3.0) } - private val perpendicularCalc = DistanceCalculator { progress -> - val s = progress.startPosition() - val c = progress.currentPosition() - val t = progress.targetPosition() - - val sx = s.getCenteredX() - val sy = s.getCenteredY() - val sz = s.getCenteredZ() - val cx = c.getCenteredX() - val cy = c.getCenteredY() - val cz = c.getCenteredZ() - val tx = t.getCenteredX() - val ty = t.getCenteredY() - val tz = t.getCenteredZ() - - val lineX = tx - sx - val lineY = ty - sy - val lineZ = tz - sz - val lineSq = lineX * lineX + lineY * lineY + lineZ * lineZ - - if (lineSq < EPSILON) { - val dx = cx - sx - val dy = cy - sy - val dz = cz - sz - return@DistanceCalculator dx * dx + dy * dy + dz * dz + private val perpendicularCalc = + DistanceCalculator { progress -> + InternalHeuristicUtils.calculatePerpendicularDistanceSq(progress) } - val toX = cx - sx - val toY = cy - sy - val toZ = cz - sz - val crossX = toY * lineZ - toZ * lineY - val crossY = toZ * lineX - toX * lineZ - val crossZ = toX * lineY - toY * lineX - val crossSq = crossX * crossX + crossY * crossY + crossZ * crossZ - - crossSq / lineSq - } - - private val octileCalc = DistanceCalculator { progress -> - val dx = abs(progress.currentPosition().getFlooredX() - progress.targetPosition().getFlooredX()) - val dy = abs(progress.currentPosition().getFlooredY() - progress.targetPosition().getFlooredY()) - val dz = abs(progress.currentPosition().getFlooredZ() - progress.targetPosition().getFlooredZ()) - - val min = minOf(dx, dy, dz) - val max = maxOf(dx, dy, dz) - val mid = dx + dy + dz - min - max - - val octile = (D3 - D2) * min + (D2 - D1) * mid + D1 * max - octile * octile - } + private val octileCalc = + DistanceCalculator { progress -> + val dx = + abs( + progress.currentPosition().getFlooredX() - + progress.targetPosition().getFlooredX() + ) + val dy = + abs( + progress.currentPosition().getFlooredY() - + progress.targetPosition().getFlooredY() + ) + val dz = + abs( + progress.currentPosition().getFlooredZ() - + progress.targetPosition().getFlooredZ() + ) + + val min = minOf(dx, dy, dz) + val max = maxOf(dx, dy, dz) + val mid = dx + dy + dz - min - max + + val octile = (D3 - D2) * min + (D2 - D1) * mid + D1 * max + octile * octile + } - private val manhattanCalc = DistanceCalculator { progress -> - val c = progress.currentPosition() - val t = progress.targetPosition() + private val manhattanCalc = + DistanceCalculator { progress -> + val c = progress.currentPosition() + val t = progress.targetPosition() - val manhattan = abs(c.getFlooredX() - t.getFlooredX()) + - abs(c.getFlooredY() - t.getFlooredY()) + - abs(c.getFlooredZ() - t.getFlooredZ()) + val manhattan = + abs(c.getFlooredX() - t.getFlooredX()) + + abs(c.getFlooredY() - t.getFlooredY()) + + abs(c.getFlooredZ() - t.getFlooredZ()) - (manhattan * manhattan).toDouble() - } + (manhattan * manhattan).toDouble() + } - private val heightCalc = DistanceCalculator { progress -> - val dy = progress.currentPosition().getFlooredY() - progress.targetPosition().getFlooredY() - (dy * dy).toDouble() - } + private val heightCalc = + DistanceCalculator { progress -> + val dy = + progress.currentPosition().getFlooredY() - + progress.targetPosition().getFlooredY() + (dy * dy).toDouble() + } override fun calculate(context: HeuristicContext): Double { val p = context.getPathfindingProgress() diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt index 4584532..a67bc32 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt @@ -5,117 +5,91 @@ import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext object Validators { - fun allOf(vararg validators: ValidationProcessor): ValidationProcessor { - return AllOfValidator(*validators) - } + object Validators { - fun allOf(validators: List): ValidationProcessor { - return AllOfValidator(validators) - } + fun allOf(vararg validators: ValidationProcessor): ValidationProcessor = + AllOfValidator(*validators) - fun anyOf(vararg validators: ValidationProcessor): ValidationProcessor { - return AnyOfValidator(*validators) - } + fun allOf(validators: List): ValidationProcessor = + AllOfValidator(validators) - fun anyOf(validators: List): ValidationProcessor { - return AnyOfValidator(validators) - } + fun anyOf(vararg validators: ValidationProcessor): ValidationProcessor = + AnyOfValidator(*validators) - fun noneOf(vararg validators: ValidationProcessor): ValidationProcessor { - return NoneOfValidator(*validators) - } + fun anyOf(validators: List): ValidationProcessor = + AnyOfValidator(validators) - fun noneOf(validators: List): ValidationProcessor { - return NoneOfValidator(validators) - } + fun noneOf(vararg validators: ValidationProcessor): ValidationProcessor = + NoneOfValidator(*validators) - fun not(validator: ValidationProcessor): ValidationProcessor { - return NotValidator(validator) - } + fun noneOf(validators: List): ValidationProcessor = + NoneOfValidator(validators) - fun alwaysTrue(): ValidationProcessor { - return AlwaysTrueValidator - } + fun not(validator: ValidationProcessor): ValidationProcessor = NotValidator(validator) - fun alwaysFalse(): ValidationProcessor { - return AlwaysFalseValidator - } + fun alwaysTrue(): ValidationProcessor = AlwaysTrueValidator - private fun copyAndFilterNulls(vararg validators: ValidationProcessor?): List { - return validators.filterNotNull() - } + fun alwaysFalse(): ValidationProcessor = AlwaysFalseValidator - private fun copyAndFilterNulls(validators: List?): List { - return validators?.filterNotNull() ?: emptyList() - } + private fun copyAndFilterNulls( + vararg validators: ValidationProcessor?, + ): List = validators.filterNotNull() - private abstract class AbstractCompositeValidator : ValidationProcessor { - protected val children: List + private fun copyAndFilterNulls( + validators: List?, + ): List = validators?.filterNotNull() ?: emptyList() - constructor(vararg validators: ValidationProcessor?) { - this.children = copyAndFilterNulls(*validators) - } + private abstract class AbstractCompositeValidator(validators: List) : + ValidationProcessor { + protected val children: List = validators.filterNotNull() - constructor(validators: List?) { - this.children = copyAndFilterNulls(validators) - } + override fun initializeSearch(context: SearchContext) = + children.forEach { it.initializeSearch(context) } - override fun initializeSearch(context: SearchContext) { - children.forEach { it.initializeSearch(context) } + override fun finalizeSearch(context: SearchContext) = + children.forEach { it.finalizeSearch(context) } } - override fun finalizeSearch(context: SearchContext) { - children.forEach { it.finalizeSearch(context) } + private class AllOfValidator : AbstractCompositeValidator { + constructor(vararg validators: ValidationProcessor?) : super(validators.toList()) + constructor(validators: List?) : super(validators ?: emptyList()) + + override fun isValid(context: EvaluationContext): Boolean = + children.all { it.isValid(context) } } - } - private class AllOfValidator : AbstractCompositeValidator { - constructor(vararg validators: ValidationProcessor?) : super(*validators) - constructor(validators: List?) : super(validators) + private class AnyOfValidator : AbstractCompositeValidator { + constructor(vararg validators: ValidationProcessor?) : super(validators.toList()) + constructor(validators: List?) : super(validators ?: emptyList()) - override fun isValid(context: EvaluationContext): Boolean { - return children.all { it.isValid(context) } + override fun isValid(context: EvaluationContext): Boolean { + if (children.isEmpty()) return false + return children.any { it.isValid(context) } + } } - } - private class AnyOfValidator : AbstractCompositeValidator { - constructor(vararg validators: ValidationProcessor?) : super(*validators) - constructor(validators: List?) : super(validators) + private class NoneOfValidator : AbstractCompositeValidator { + constructor(vararg validators: ValidationProcessor?) : super(validators.toList()) + constructor(validators: List?) : super(validators ?: emptyList()) - override fun isValid(context: EvaluationContext): Boolean { - if (children.isEmpty()) return false - return children.any { it.isValid(context) } + override fun isValid(context: EvaluationContext): Boolean = + children.none { it.isValid(context) } } - } - private class NoneOfValidator : AbstractCompositeValidator { - constructor(vararg validators: ValidationProcessor?) : super(*validators) - constructor(validators: List?) : super(validators) + private class NotValidator(private val child: ValidationProcessor) : ValidationProcessor { + override fun initializeSearch(context: SearchContext) = child.initializeSearch(context) - override fun isValid(context: EvaluationContext): Boolean { - return children.none { it.isValid(context) } - } - } + override fun isValid(context: EvaluationContext): Boolean = !child.isValid(context) - private class NotValidator(private val child: ValidationProcessor) : ValidationProcessor { - override fun initializeSearch(context: SearchContext) { - child.initializeSearch(context) + override fun finalizeSearch(context: SearchContext) = child.finalizeSearch(context) } - override fun isValid(context: EvaluationContext): Boolean { - return !child.isValid(context) + private object AlwaysTrueValidator : ValidationProcessor { + override fun isValid(context: EvaluationContext): Boolean = true } - override fun finalizeSearch(context: SearchContext) { - child.finalizeSearch(context) + private object AlwaysFalseValidator : ValidationProcessor { + override fun isValid(context: EvaluationContext): Boolean = false } } - - private object AlwaysTrueValidator : ValidationProcessor { - override fun isValid(context: EvaluationContext): Boolean = true - } - - private object AlwaysFalseValidator : ValidationProcessor { - override fun isValid(context: EvaluationContext): Boolean = false - } } From e69634d2264a5104a747a68c669bdcfaeff3220a Mon Sep 17 00:00:00 2001 From: vince calder <92699085+awrped@users.noreply.github.com> Date: Mon, 19 Jan 2026 19:03:55 -0600 Subject: [PATCH 7/8] pt ??? --- .../org/cobalt/api/pathfinder/PathExecutor.kt | 42 ++-- .../pathfinder/pathfinder/AStarPathfinder.kt | 6 +- .../heuristic/InternalHeuristicUtils.kt | 18 +- .../heuristic/LinearHeuristicStrategy.kt | 32 +-- .../heuristic/SquaredHeuristicStrategy.kt | 34 +--- .../processing/impl/MinecraftPathProcessor.kt | 28 ++- .../impl/MinecraftNavigationProvider.kt | 184 +++++++++--------- .../cobalt/api/pathfinder/result/PathImpl.kt | 12 +- .../cobalt/api/pathfinder/result/PathUtils.kt | 14 +- .../api/pathfinder/util/GridRegionData.kt | 18 +- .../cobalt/api/pathfinder/util/RegionKey.kt | 4 +- .../api/pathfinder/wrapper/PathPosition.kt | 109 ++++------- .../api/pathfinder/wrapper/PathVector.kt | 69 ++----- 13 files changed, 231 insertions(+), 339 deletions(-) diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/PathExecutor.kt b/src/main/kotlin/org/cobalt/api/pathfinder/PathExecutor.kt index 5e7465c..aeabe4a 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/PathExecutor.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/PathExecutor.kt @@ -70,14 +70,14 @@ object PathExecutor { ) } else { /* - * partial paths can happen, i would recommend improving this functionality - * to whoever maintainer wants to maintain my horrible shitcode of a pathfinder. - * i think partial paths should be refactored, i will write all the cases now - * iteration limit -> self explanatory xd (or if u cant use ur brain - * then its just if the algorithm takes too many iterations to find a goal. - * this is set literaaally like 20 lines above you...) - * fallback -> pf searches everywhere and couldnt find a possible way to get there. - * this can happen in the case of unloaded chunks, or an obstruction + * partial paths can happen, i would recommend improving this functionality + * to whoever maintainer wants to maintain my horrible shitcode of a pathfinder. + * i think partial paths should be refactored, i will write all the cases now + * iteration limit -> self explanatory xd (or if u cant use ur brain + * then its just if the algorithm takes too many iterations to find a goal. + * this is set literaaally like 20 lines above you...) + * fallback -> pf searches everywhere and couldnt find a possible way to get there. + * this can happen in the case of unloaded chunks, or an obstruction */ ChatUtils.sendMessage( "§ePartial path found! §7Calculated in §f${duration}ms §8(${path.length()} nodes)" @@ -95,10 +95,10 @@ object PathExecutor { } /* - * as of writing this, there is intentionally no moving, rotations, - * or jumping yet. i ask that you make sure that the algo works good - * before implementing them, aswell as rots - */ + * as of writing this, there is intentionally no moving, rotations, + * or jumping yet. i ask that you make sure that the algo works good + * before implementing them, aswell as rots + */ @SubscribeEvent fun onTick(@Suppress("UNUSED_PARAMETER") event: TickEvent.Start) { val path = currentPath ?: return @@ -112,7 +112,7 @@ object PathExecutor { } val targetPos = waypoints[currentWaypointIndex].mid() - val targetVec = Vec3(targetPos.getX(), targetPos.getY(), targetPos.getZ()) + val targetVec = Vec3(targetPos.x, targetPos.y, targetPos.z) val horizontalDistSq = (player.x - targetVec.x) * (player.x - targetVec.x) + @@ -135,8 +135,8 @@ object PathExecutor { Render3D.drawLine( event.context, - Vec3(start.getX(), start.getY(), start.getZ()), - Vec3(end.getX(), end.getY(), end.getZ()), + Vec3(start.x, start.y, start.z), + Vec3(end.x, end.y, end.z), Color.CYAN, true, 2.0f @@ -148,12 +148,12 @@ object PathExecutor { Render3D.drawBox( event.context, AABB( - currentPos.getX() - 0.25, - currentPos.getY() - 0.25, - currentPos.getZ() - 0.25, - currentPos.getX() + 0.25, - currentPos.getY() + 0.25, - currentPos.getZ() + 0.25 + currentPos.x - 0.25, + currentPos.y - 0.25, + currentPos.z - 0.25, + currentPos.x + 0.25, + currentPos.y + 0.25, + currentPos.z + 0.25 ), Color.GREEN, true diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt index bf58109..ec39f3c 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt @@ -224,9 +224,9 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind fun getOrCreateRegionData(position: PathPosition): GridRegionData { val cellSize = pathfinderConfiguration.gridCellSize - val rX = Mth.floorDiv(position.getFlooredX(), cellSize) - val rY = Mth.floorDiv(position.getFlooredY(), cellSize) - val rZ = Mth.floorDiv(position.getFlooredZ(), cellSize) + val rX = Mth.floorDiv(position.flooredX, cellSize) + val rY = Mth.floorDiv(position.flooredY, cellSize) + val rZ = Mth.floorDiv(position.flooredZ, cellSize) val regionKey = RegionKey.pack(rX, rY, rZ) return visitedRegions.computeIfAbsent( diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/InternalHeuristicUtils.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/InternalHeuristicUtils.kt index 5339e71..f99a106 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/InternalHeuristicUtils.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/InternalHeuristicUtils.kt @@ -11,15 +11,15 @@ internal object InternalHeuristicUtils { val c = progress.currentPosition() val t = progress.targetPosition() - val sx = s.getCenteredX() - val sy = s.getCenteredY() - val sz = s.getCenteredZ() - val cx = c.getCenteredX() - val cy = c.getCenteredY() - val cz = c.getCenteredZ() - val tx = t.getCenteredX() - val ty = t.getCenteredY() - val tz = t.getCenteredZ() + val sx = s.centeredX + val sy = s.centeredY + val sz = s.centeredZ + val cx = c.centeredX + val cy = c.centeredY + val cz = c.centeredZ + val tx = t.centeredX + val ty = t.centeredY + val tz = t.centeredZ val lineX = tx - sx val lineY = ty - sy diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt index 28c5c43..fa8f4c3 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt @@ -20,21 +20,9 @@ class LinearHeuristicStrategy : IHeuristicStrategy { private val octileCalc = DistanceCalculator { progress -> - val dx = - abs( - progress.currentPosition().getFlooredX() - - progress.targetPosition().getFlooredX() - ) - val dy = - abs( - progress.currentPosition().getFlooredY() - - progress.targetPosition().getFlooredY() - ) - val dz = - abs( - progress.currentPosition().getFlooredZ() - - progress.targetPosition().getFlooredZ() - ) + val dx = abs(progress.currentPosition().flooredX - progress.targetPosition().flooredX) + val dy = abs(progress.currentPosition().flooredY - progress.targetPosition().flooredY) + val dz = abs(progress.currentPosition().flooredZ - progress.targetPosition().flooredZ) val min = minOf(dx, dy, dz) val max = maxOf(dx, dy, dz) @@ -48,9 +36,9 @@ class LinearHeuristicStrategy : IHeuristicStrategy { val position = progress.currentPosition() val target = progress.targetPosition() - (abs(position.getFlooredX() - target.getFlooredX()) + - abs(position.getFlooredY() - target.getFlooredY()) + - abs(position.getFlooredZ() - target.getFlooredZ())) + (abs(position.flooredX - target.flooredX) + + abs(position.flooredY - target.flooredY) + + abs(position.flooredZ - target.flooredZ)) .toDouble() } @@ -59,7 +47,7 @@ class LinearHeuristicStrategy : IHeuristicStrategy { val position = progress.currentPosition() val target = progress.targetPosition() - abs(position.getFlooredY() - target.getFlooredY()).toDouble() + abs(position.flooredY - target.flooredY).toDouble() } override fun calculate(context: HeuristicContext): Double { @@ -73,9 +61,9 @@ class LinearHeuristicStrategy : IHeuristicStrategy { } override fun calculateTransitionCost(from: PathPosition, to: PathPosition): Double { - val dx = to.getCenteredX() - from.getCenteredX() - val dy = to.getCenteredY() - from.getCenteredY() - val dz = to.getCenteredZ() - from.getCenteredZ() + val dx = to.centeredX - from.centeredX + val dy = to.centeredY - from.centeredY + val dz = to.centeredZ - from.centeredZ return sqrt(dx * dx + dy * dy + dz * dz) } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt index 53bb7f9..f0f13e3 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt @@ -20,21 +20,9 @@ class SquaredHeuristicStrategy : IHeuristicStrategy { private val octileCalc = DistanceCalculator { progress -> - val dx = - abs( - progress.currentPosition().getFlooredX() - - progress.targetPosition().getFlooredX() - ) - val dy = - abs( - progress.currentPosition().getFlooredY() - - progress.targetPosition().getFlooredY() - ) - val dz = - abs( - progress.currentPosition().getFlooredZ() - - progress.targetPosition().getFlooredZ() - ) + val dx = abs(progress.currentPosition().flooredX - progress.targetPosition().flooredX) + val dy = abs(progress.currentPosition().flooredY - progress.targetPosition().flooredY) + val dz = abs(progress.currentPosition().flooredZ - progress.targetPosition().flooredZ) val min = minOf(dx, dy, dz) val max = maxOf(dx, dy, dz) @@ -50,18 +38,16 @@ class SquaredHeuristicStrategy : IHeuristicStrategy { val t = progress.targetPosition() val manhattan = - abs(c.getFlooredX() - t.getFlooredX()) + - abs(c.getFlooredY() - t.getFlooredY()) + - abs(c.getFlooredZ() - t.getFlooredZ()) + abs(c.flooredX - t.flooredX) + + abs(c.flooredY - t.flooredY) + + abs(c.flooredZ - t.flooredZ) (manhattan * manhattan).toDouble() } private val heightCalc = DistanceCalculator { progress -> - val dy = - progress.currentPosition().getFlooredY() - - progress.targetPosition().getFlooredY() + val dy = progress.currentPosition().flooredY - progress.targetPosition().flooredY (dy * dy).toDouble() } @@ -76,9 +62,9 @@ class SquaredHeuristicStrategy : IHeuristicStrategy { } override fun calculateTransitionCost(from: PathPosition, to: PathPosition): Double { - val dx = to.getCenteredX() - from.getCenteredX() - val dy = to.getCenteredY() - from.getCenteredY() - val dz = to.getCenteredZ() - from.getCenteredZ() + val dx = to.centeredX - from.centeredX + val dy = to.centeredY - from.centeredY + val dz = to.centeredZ - from.centeredZ return dx * dx + dy * dy + dz * dz } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/impl/MinecraftPathProcessor.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/impl/MinecraftPathProcessor.kt index fbcc53e..edfcb9c 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/impl/MinecraftPathProcessor.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/impl/MinecraftPathProcessor.kt @@ -9,11 +9,11 @@ import org.cobalt.api.pathfinder.pathing.processing.ValidationProcessor import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext /* -* most logic in this file is derived from minecraft code -* or writeups on pathfinding algorithms, if you want to help contribute -* id prefer for you to keep it the same idea or whatever, but if not -* please write a comment explaining WHY you did it that way. i dont like -* magic numbers that i cant understand. + * most logic in this file is derived from minecraft code + * or writeups on pathfinding algorithms, if you want to help contribute + * id prefer for you to keep it the same idea or whatever, but if not + * please write a comment explaining WHY you did it that way. i dont like + * magic numbers that i cant understand. */ class MinecraftPathProcessor : CostProcessor, ValidationProcessor { @@ -35,9 +35,9 @@ class MinecraftPathProcessor : CostProcessor, ValidationProcessor { if (prev == null) return true val prevPoint = provider.getNavigationPoint(prev, env) - val dy = pos.getY() - prev.getY() - val dx = pos.getFlooredX() - prev.getFlooredX() - val dz = pos.getFlooredZ() - prev.getFlooredZ() + val dy = pos.y - prev.y + val dx = pos.flooredX - prev.flooredX + val dz = pos.flooredZ - prev.flooredZ if (dy > DEFAULT_MOB_JUMP_HEIGHT) return false @@ -86,8 +86,7 @@ class MinecraftPathProcessor : CostProcessor, ValidationProcessor { additionalCost += 0.1 * Math.abs(dy) } - val blockPos = - BlockPos(currentPos.getFlooredX(), currentPos.getFlooredY(), currentPos.getFlooredZ()) + val blockPos = BlockPos(currentPos.flooredX, currentPos.flooredY, currentPos.flooredZ) // i dont want it to like tight corners so more cost var crampedPenalty = 0.0 @@ -105,14 +104,13 @@ class MinecraftPathProcessor : CostProcessor, ValidationProcessor { } additionalCost += crampedPenalty - // just make stuff smoother no more zigzags val gpPos = context.getGrandparentPathPosition() if (gpPos != null) { - val v1x = prevPos.getX() - gpPos.getX() - val v1z = prevPos.getZ() - gpPos.getZ() - val v2x = currentPos.getX() - prevPos.getX() - val v2z = currentPos.getZ() - prevPos.getZ() + val v1x = prevPos.x - gpPos.x + val v1z = prevPos.z - gpPos.z + val v2x = currentPos.x - prevPos.x + val v2z = currentPos.z - prevPos.z val dot = v1x * v2x + v1z * v2z val mag1 = sqrt(v1x * v1x + v1z * v1z) val mag2 = sqrt(v2x * v2x + v2z * v2z) diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/provider/impl/MinecraftNavigationProvider.kt b/src/main/kotlin/org/cobalt/api/pathfinder/provider/impl/MinecraftNavigationProvider.kt index d3fe776..354bd6a 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/provider/impl/MinecraftNavigationProvider.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/provider/impl/MinecraftNavigationProvider.kt @@ -5,6 +5,7 @@ import net.minecraft.core.BlockPos import net.minecraft.core.Direction.Axis import net.minecraft.tags.BlockTags import net.minecraft.tags.FluidTags +import net.minecraft.world.level.Level import net.minecraft.world.level.block.* import net.minecraft.world.level.block.state.BlockState import net.minecraft.world.level.pathfinder.PathComputationType @@ -32,106 +33,19 @@ class MinecraftNavigationProvider : NavigationPointProvider { override fun isLiquid() = false } - val x = position.getFlooredX() - val y = position.getFlooredY() - val z = position.getFlooredZ() + val x = position.flooredX + val y = position.flooredY + val z = position.flooredZ val blockPos = BlockPos(x, y, z) val feetState = level.getBlockState(blockPos) val headState = level.getBlockState(blockPos.above()) val belowState = level.getBlockState(blockPos.below()) - fun canWalkThrough( - state: BlockState, - pos: BlockPos, - ): Boolean { - if (state.isAir) return true - - if (state.`is`(BlockTags.TRAPDOORS) || - state.`is`(Blocks.LILY_PAD) || - state.`is`(Blocks.BIG_DRIPLEAF) - ) { - return true - } - - if (state.`is`(Blocks.POWDER_SNOW) || - state.`is`(Blocks.CACTUS) || - state.`is`(Blocks.SWEET_BERRY_BUSH) || - state.`is`(Blocks.HONEY_BLOCK) || - state.`is`(Blocks.COCOA) || - state.`is`(Blocks.WITHER_ROSE) || - state.`is`(Blocks.POINTED_DRIPSTONE) - ) { - return true - } - - val block = state.block - if (block is DoorBlock) { - return if (state.getValue(DoorBlock.OPEN)) true else block.type().canOpenByHand() - } - - if (block is FenceGateBlock) { - return state.getValue(FenceGateBlock.OPEN) - } - - if (block is BaseRailBlock || block is LeavesBlock) { - return true - } - - if (state.`is`(BlockTags.FENCES) || state.`is`(BlockTags.WALLS)) { - return false - } - - return state.isPathfindable(PathComputationType.LAND) || - state.fluidState.`is`(FluidTags.WATER) - } - - fun canWalkOn(state: BlockState, pos: BlockPos): Boolean { - val block = state.block - if (state.isCollisionShapeFullBlock(level, pos) && - block != Blocks.MAGMA_BLOCK && - block != Blocks.BUBBLE_COLUMN && - block != Blocks.HONEY_BLOCK - ) { - return true - } - - return block is AzaleaBlock || - block is LadderBlock || - block is VineBlock || - block == Blocks.FARMLAND || - block == Blocks.DIRT_PATH || - block == Blocks.SOUL_SAND || - block == Blocks.CHEST || - block == Blocks.ENDER_CHEST || - block == Blocks.GLASS || - block is StairBlock || - block is SlabBlock || - block is BaseRailBlock || - !state.fluidState.isEmpty - } - - // based off WalkNodeEvaluator.getFloorLevel - fun calculateFloorLevel(pos: BlockPos): Double { - val state = level.getFluidState(pos) - if (state.`is`(FluidTags.WATER)) { - return pos.y.toDouble() + 0.5 - } - - val belowPos = pos.below() - val belowState = level.getBlockState(belowPos) - val shape = belowState.getCollisionShape(level, belowPos, CollisionContext.empty()) - return if (shape.isEmpty) { - belowPos.y.toDouble() - } else { - belowPos.y.toDouble() + shape.max(Axis.Y) - } - } - - val canPassFeetVal = canWalkThrough(feetState, blockPos) - val canPassHeadVal = canWalkThrough(headState, blockPos.above()) - val hasStableFloorVal = canWalkOn(belowState, blockPos.below()) - val floorLevelVal = calculateFloorLevel(blockPos) + val canPassFeetVal = canWalkThrough(level, feetState, blockPos) + val canPassHeadVal = canWalkThrough(level, headState, blockPos.above()) + val hasStableFloorVal = canWalkOn(level, belowState, blockPos.below()) + val floorLevelVal = calculateFloorLevel(level, blockPos) val isClimbingVal = feetState.block is LadderBlock || feetState.block is VineBlock val isLiquidVal = !feetState.fluidState.isEmpty @@ -143,4 +57,86 @@ class MinecraftNavigationProvider : NavigationPointProvider { override fun isLiquid(): Boolean = isLiquidVal } } + + private fun canWalkThrough(level: Level, state: BlockState, pos: BlockPos): Boolean { + if (state.isAir) return true + + if (state.`is`(BlockTags.TRAPDOORS) || + state.`is`(Blocks.LILY_PAD) || + state.`is`(Blocks.BIG_DRIPLEAF) + ) { + return true + } + + if (state.`is`(Blocks.POWDER_SNOW) || + state.`is`(Blocks.CACTUS) || + state.`is`(Blocks.SWEET_BERRY_BUSH) || + state.`is`(Blocks.HONEY_BLOCK) || + state.`is`(Blocks.COCOA) || + state.`is`(Blocks.WITHER_ROSE) || + state.`is`(Blocks.POINTED_DRIPSTONE) + ) { + return true + } + + val block = state.block + if (block is DoorBlock) { + return if (state.getValue(DoorBlock.OPEN)) true else block.type().canOpenByHand() + } + + if (block is FenceGateBlock) { + return state.getValue(FenceGateBlock.OPEN) + } + + if (block is BaseRailBlock || block is LeavesBlock) { + return true + } + + if (state.`is`(BlockTags.FENCES) || state.`is`(BlockTags.WALLS)) { + return false + } + + return state.isPathfindable(PathComputationType.LAND) || state.fluidState.`is`(FluidTags.WATER) + } + + private fun canWalkOn(level: Level, state: BlockState, pos: BlockPos): Boolean { + val block = state.block + if (state.isCollisionShapeFullBlock(level, pos) && + block != Blocks.MAGMA_BLOCK && + block != Blocks.BUBBLE_COLUMN && + block != Blocks.HONEY_BLOCK + ) { + return true + } + + return block is AzaleaBlock || + block is LadderBlock || + block is VineBlock || + block == Blocks.FARMLAND || + block == Blocks.DIRT_PATH || + block == Blocks.SOUL_SAND || + block == Blocks.CHEST || + block == Blocks.ENDER_CHEST || + block == Blocks.GLASS || + block is StairBlock || + block is SlabBlock || + block is BaseRailBlock || + !state.fluidState.isEmpty + } + + private fun calculateFloorLevel(level: Level, pos: BlockPos): Double { + val state = level.getFluidState(pos) + if (state.`is`(FluidTags.WATER)) { + return pos.y.toDouble() + 0.5 + } + + val belowPos = pos.below() + val belowState = level.getBlockState(belowPos) + val shape = belowState.getCollisionShape(level, belowPos, CollisionContext.empty()) + return if (shape.isEmpty) { + belowPos.y.toDouble() + } else { + belowPos.y.toDouble() + shape.max(Axis.Y) + } + } } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathImpl.kt b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathImpl.kt index 1c9a534..8a24e46 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathImpl.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathImpl.kt @@ -6,22 +6,16 @@ import org.cobalt.api.pathfinder.wrapper.PathPosition class PathImpl( private val start: PathPosition, private val end: PathPosition, - private val positions: Iterable, + private val positions: Collection, ) : Path { - private val length: Int = positions.count() - override fun getStart(): PathPosition = start override fun getEnd(): PathPosition = end override fun iterator(): Iterator = positions.iterator() - override fun length(): Int = length + override fun length(): Int = positions.size - override fun collect(): Collection { - val collection = ArrayList(length) - positions.forEach { collection.add(it) } - return collection - } + override fun collect(): Collection = positions.toList() } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt index 1e4677c..a907e3d 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt @@ -17,9 +17,7 @@ object PathUtils { var previous: PathPosition? = null for (current in path) { - previous?.let { prev -> - interpolateSegment(prev, current, resolution, result) - } + previous?.let { prev -> interpolateSegment(prev, current, resolution, result) } result.addLast(current) previous = current } @@ -97,9 +95,9 @@ object PathUtils { } private fun interpolate(pos1: PathPosition, pos2: PathPosition, progress: Double): PathPosition { - val x = Mth.lerp(progress, pos1.getX(), pos2.getX()) - val y = Mth.lerp(progress, pos1.getY(), pos2.getY()) - val z = Mth.lerp(progress, pos1.getZ(), pos2.getZ()) + val x = Mth.lerp(progress, pos1.x, pos2.x) + val y = Mth.lerp(progress, pos1.y, pos2.y) + val z = Mth.lerp(progress, pos1.z, pos2.z) return PathPosition(x, y, z) } @@ -126,9 +124,7 @@ object PathUtils { } private fun samePoint(a: PathPosition, b: PathPosition, eps: Double): Boolean { - return abs(a.getX() - b.getX()) <= eps && - abs(a.getY() - b.getY()) <= eps && - abs(a.getZ() - b.getZ()) <= eps + return abs(a.x - b.x) <= eps && abs(a.y - b.y) <= eps && abs(a.z - b.z) <= eps } private fun validateEpsilon(epsilon: Double) { diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/util/GridRegionData.kt b/src/main/kotlin/org/cobalt/api/pathfinder/util/GridRegionData.kt index 9fdaa75..78f3697 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/util/GridRegionData.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/util/GridRegionData.kt @@ -12,20 +12,20 @@ class GridRegionData { private val regionalExaminedPositions: LongSet constructor(bloomFilterSize: Int, bloomFilterFpp: Double) { - val pathPositionFunnel = Funnel { pathPosition, into -> - into.putInt(pathPosition.getFlooredX()) - .putInt(pathPosition.getFlooredY()) - .putInt(pathPosition.getFlooredZ()) - } + val pathPositionFunnel = + Funnel { pathPosition, into -> + into.putInt(pathPosition.flooredX) + .putInt(pathPosition.flooredY) + .putInt(pathPosition.flooredZ) + } bloomFilter = BloomFilter.create(pathPositionFunnel, bloomFilterSize, bloomFilterFpp) this.regionalExaminedPositions = LongOpenHashSet() } - constructor(configuration: PathfinderConfiguration) : this( - configuration.bloomFilterSize, - configuration.bloomFilterFpp - ) + constructor( + configuration: PathfinderConfiguration, + ) : this(configuration.bloomFilterSize, configuration.bloomFilterFpp) fun getBloomFilter(): BloomFilter = bloomFilter diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/util/RegionKey.kt b/src/main/kotlin/org/cobalt/api/pathfinder/util/RegionKey.kt index 513d859..49cf952 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/util/RegionKey.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/util/RegionKey.kt @@ -8,9 +8,7 @@ object RegionKey { private const val SHIFT_Z = 12 private const val SHIFT_X = 38 // 12 + 26 - fun pack(pos: PathPosition): Long { - return pack(pos.getFlooredX(), pos.getFlooredY(), pos.getFlooredZ()) - } + fun pack(pos: PathPosition): Long = pack(pos.flooredX, pos.flooredY, pos.flooredZ) fun pack(x: Int, y: Int, z: Int): Long { return ((x.toLong() and MASK_XZ) shl SHIFT_X) or diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathPosition.kt b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathPosition.kt index 5036b21..44d567a 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathPosition.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathPosition.kt @@ -3,96 +3,65 @@ package org.cobalt.api.pathfinder.wrapper import kotlin.math.sqrt import net.minecraft.util.Mth -data class PathPosition( - private val x: Double, - private val y: Double, - private val z: Double, -) : Cloneable { - - fun distanceSquared(otherPosition: PathPosition): Double { - return Mth.square(this.x - otherPosition.x) + - Mth.square(this.y - otherPosition.y) + - Mth.square(this.z - otherPosition.z) +data class PathPosition(val x: Double, val y: Double, val z: Double) { + + val flooredX: Int + get() = Mth.floor(x) + val flooredY: Int + get() = Mth.floor(y) + val flooredZ: Int + get() = Mth.floor(z) + + val centeredX: Double + get() = flooredX + 0.5 + val centeredY: Double + get() = flooredY + 0.5 + val centeredZ: Double + get() = flooredZ + 0.5 + + fun distanceSquared(other: PathPosition): Double { + return Mth.square(x - other.x) + Mth.square(y - other.y) + Mth.square(z - other.z) } - fun distance(otherPosition: PathPosition): Double { - return sqrt(this.distanceSquared(otherPosition)) - } - - fun setX(x: Double): PathPosition = PathPosition(x, this.y, this.z) - - fun setY(y: Double): PathPosition = PathPosition(this.x, y, this.z) - - fun setZ(z: Double): PathPosition = PathPosition(this.x, this.y, z) - - fun getCenteredX(): Double = getFlooredX() + 0.5 + fun distance(other: PathPosition): Double = sqrt(distanceSquared(other)) - fun getCenteredY(): Double = getFlooredY() + 0.5 + fun setX(x: Double): PathPosition = copy(x = x) + fun setY(y: Double): PathPosition = copy(y = y) + fun setZ(z: Double): PathPosition = copy(z = z) - fun getCenteredZ(): Double = getFlooredZ() + 0.5 + fun add(x: Double, y: Double, z: Double): PathPosition = + PathPosition(this.x + x, this.y + y, this.z + z) - fun getFlooredX(): Int = Mth.floor(this.x) + fun add(vector: PathVector): PathPosition = add(vector.x, vector.y, vector.z) - fun getFlooredY(): Int = Mth.floor(this.y) + fun subtract(x: Double, y: Double, z: Double): PathPosition = + PathPosition(this.x - x, this.y - y, this.z - z) - fun getFlooredZ(): Int = Mth.floor(this.z) + fun subtract(vector: PathVector): PathPosition = subtract(vector.x, vector.y, vector.z) - fun add(x: Double, y: Double, z: Double): PathPosition { - return PathPosition(this.x + x, this.y + y, this.z + z) - } + fun toVector(): PathVector = PathVector(x, y, z) - fun add(vector: PathVector): PathPosition { - return add(vector.getX(), vector.getY(), vector.getZ()) - } + fun floor(): PathPosition = + PathPosition(flooredX.toDouble(), flooredY.toDouble(), flooredZ.toDouble()) - fun subtract(x: Double, y: Double, z: Double): PathPosition { - return PathPosition(this.x - x, this.y - y, this.z - z) - } - - fun subtract(vector: PathVector): PathPosition { - return subtract(vector.getX(), vector.getY(), vector.getZ()) - } - - fun toVector(): PathVector = PathVector(this.x, this.y, this.z) - - fun floor(): PathPosition = PathPosition(getFlooredX().toDouble(), getFlooredY().toDouble(), getFlooredZ().toDouble()) - - fun mid(): PathPosition = PathPosition(getFlooredX() + 0.5, getFlooredY() + 0.5, getFlooredZ() + 0.5) + fun mid(): PathPosition = PathPosition(flooredX + 0.5, flooredY + 0.5, flooredZ + 0.5) fun midPoint(end: PathPosition): PathPosition { - return PathPosition((this.x + end.x) / 2, (this.y + end.y) / 2, (this.z + end.z) / 2) - } - - public override fun clone(): PathPosition { - return PathPosition(this.x, this.y, this.z) + return PathPosition((x + end.x) / 2, (y + end.y) / 2, (z + end.z) / 2) } override fun equals(other: Any?): Boolean { if (this === other) return true - if (other == null || this::class != other::class) return false - other as PathPosition - return getFlooredX() == other.getFlooredX() && - getFlooredY() == other.getFlooredY() && - getFlooredZ() == other.getFlooredZ() + if (other !is PathPosition) return false + return flooredX == other.flooredX && flooredY == other.flooredY && flooredZ == other.flooredZ } override fun hashCode(): Int { - val x = getFlooredX() - val y = getFlooredY() - val z = getFlooredZ() - var result = x - result = 31 * result + y - result = 31 * result + z + var result = flooredX + result = 31 * result + flooredY + result = 31 * result + flooredZ return result } - fun getX(): Double = this.x - - fun getY(): Double = this.y - - fun getZ(): Double = this.z - - override fun toString(): String { - return "PathPosition(x=${getX()}, y=${getY()}, z=${getZ()})" - } + override fun toString(): String = "PathPosition(x=$x, y=$y, z=$z)" } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathVector.kt b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathVector.kt index 62ca0aa..faa99c7 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathVector.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/PathVector.kt @@ -3,11 +3,7 @@ package org.cobalt.api.pathfinder.wrapper import kotlin.math.sqrt import net.minecraft.util.Mth -data class PathVector( - private val x: Double, - private val y: Double, - private val z: Double, -) : Cloneable { +data class PathVector(val x: Double, val y: Double, val z: Double) { companion object { fun computeDistance(A: PathVector, B: PathVector, C: PathVector): Double { @@ -19,63 +15,34 @@ data class PathVector( } } - fun dot(otherVector: PathVector): Double { - return this.x * otherVector.x + this.y * otherVector.y + this.z * otherVector.z - } - - fun length(): Double { - return sqrt(Mth.square(this.x) + Mth.square(this.y) + Mth.square(this.z)) - } + fun dot(other: PathVector): Double = x * other.x + y * other.y + z * other.z - fun distance(otherVector: PathVector): Double { - return sqrt( - Mth.square(this.x - otherVector.x) + - Mth.square(this.y - otherVector.y) + - Mth.square(this.z - otherVector.z) - ) - } + fun length(): Double = sqrt(Mth.square(x) + Mth.square(y) + Mth.square(z)) - fun setX(x: Double): PathVector = PathVector(x, this.y, this.z) + fun distance(other: PathVector): Double = + sqrt(Mth.square(x - other.x) + Mth.square(y - other.y) + Mth.square(z - other.z)) - fun setY(y: Double): PathVector = PathVector(this.x, y, this.z) + fun setX(x: Double): PathVector = copy(x = x) + fun setY(y: Double): PathVector = copy(y = y) + fun setZ(z: Double): PathVector = copy(z = z) - fun setZ(z: Double): PathVector = PathVector(this.x, this.y, z) + fun subtract(other: PathVector): PathVector = PathVector(x - other.x, y - other.y, z - other.z) - fun subtract(otherVector: PathVector): PathVector { - return PathVector(this.x - otherVector.x, this.y - otherVector.y, this.z - otherVector.z) - } - - fun multiply(value: Double): PathVector { - return PathVector(this.x * value, this.y * value, this.z * value) - } + fun multiply(value: Double): PathVector = PathVector(x * value, y * value, z * value) fun normalize(): PathVector { - val magnitude = this.length() - return PathVector(this.x / magnitude, this.y / magnitude, this.z / magnitude) + val magnitude = length() + return PathVector(x / magnitude, y / magnitude, z / magnitude) } - fun divide(value: Double): PathVector { - return PathVector(this.x / value, this.y / value, this.z / value) - } + fun divide(value: Double): PathVector = PathVector(x / value, y / value, z / value) - fun add(otherVector: PathVector): PathVector { - return PathVector(this.x + otherVector.x, this.y + otherVector.y, this.z + otherVector.z) - } + fun add(other: PathVector): PathVector = PathVector(x + other.x, y + other.y, z + other.z) fun getCrossProduct(o: PathVector): PathVector { - val x = this.y * o.getZ() - o.getY() * this.z - val y = this.z * o.getX() - o.getZ() * this.x - val z = this.x * o.getY() - o.getX() * this.y - return PathVector(x, y, z) - } - - public override fun clone(): PathVector { - return PathVector(this.x, this.y, this.z) + val crossX = y * o.z - o.y * z + val crossY = z * o.x - o.z * x + val crossZ = x * o.y - o.x * y + return PathVector(crossX, crossY, crossZ) } - - fun getX(): Double = this.x - - fun getY(): Double = this.y - - fun getZ(): Double = this.z } From dcb635790e61207014d77b2a60329226e4a27f67 Mon Sep 17 00:00:00 2001 From: vince calder <92699085+awrped@users.noreply.github.com> Date: Mon, 19 Jan 2026 19:10:42 -0600 Subject: [PATCH 8/8] cleanup should be done --- .../pathfinder/pathfinder/AStarPathfinder.kt | 3 +- .../heuristic/LinearHeuristicStrategy.kt | 1 - .../heuristic/SquaredHeuristicStrategy.kt | 9 +- .../pathing/processing/Validators.kt | 95 ------------- .../cobalt/api/pathfinder/result/PathUtils.kt | 133 ------------------ .../pathfinder/util/ParameterizedSupplier.kt | 5 - .../cobalt/api/pathfinder/wrapper/Depth.kt | 1 + 7 files changed, 7 insertions(+), 240 deletions(-) delete mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt delete mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt delete mode 100644 src/main/kotlin/org/cobalt/api/pathfinder/util/ParameterizedSupplier.kt diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt index ec39f3c..bcab41f 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathfinder/AStarPathfinder.kt @@ -4,6 +4,7 @@ import it.unimi.dsi.fastutil.longs.Long2DoubleMap import it.unimi.dsi.fastutil.longs.Long2DoubleOpenHashMap import it.unimi.dsi.fastutil.longs.Long2ObjectMap import it.unimi.dsi.fastutil.longs.Long2ObjectOpenHashMap +import java.util.function.LongFunction import kotlin.math.abs import kotlin.math.max import net.minecraft.util.Mth @@ -231,7 +232,7 @@ class AStarPathfinder(configuration: PathfinderConfiguration) : AbstractPathfind return visitedRegions.computeIfAbsent( regionKey, - java.util.function.LongFunction { GridRegionData(pathfinderConfiguration) } + LongFunction { GridRegionData(pathfinderConfiguration) } ) } } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt index fa8f4c3..4e22dcc 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/LinearHeuristicStrategy.kt @@ -7,7 +7,6 @@ import org.cobalt.api.pathfinder.wrapper.PathPosition class LinearHeuristicStrategy : IHeuristicStrategy { companion object { - private const val EPSILON = 1e-9 private const val D1 = 1.0 private val D2 = sqrt(2.0) private val D3 = sqrt(3.0) diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt index f0f13e3..a741a09 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/heuristic/SquaredHeuristicStrategy.kt @@ -7,19 +7,18 @@ import org.cobalt.api.pathfinder.wrapper.PathPosition class SquaredHeuristicStrategy : IHeuristicStrategy { companion object { - private const val EPSILON = 1e-9 private const val D1 = 1.0 private val D2 = sqrt(2.0) private val D3 = sqrt(3.0) } private val perpendicularCalc = - DistanceCalculator { progress -> + DistanceCalculator { progress -> InternalHeuristicUtils.calculatePerpendicularDistanceSq(progress) } private val octileCalc = - DistanceCalculator { progress -> + DistanceCalculator { progress -> val dx = abs(progress.currentPosition().flooredX - progress.targetPosition().flooredX) val dy = abs(progress.currentPosition().flooredY - progress.targetPosition().flooredY) val dz = abs(progress.currentPosition().flooredZ - progress.targetPosition().flooredZ) @@ -33,7 +32,7 @@ class SquaredHeuristicStrategy : IHeuristicStrategy { } private val manhattanCalc = - DistanceCalculator { progress -> + DistanceCalculator { progress -> val c = progress.currentPosition() val t = progress.targetPosition() @@ -46,7 +45,7 @@ class SquaredHeuristicStrategy : IHeuristicStrategy { } private val heightCalc = - DistanceCalculator { progress -> + DistanceCalculator { progress -> val dy = progress.currentPosition().flooredY - progress.targetPosition().flooredY (dy * dy).toDouble() } diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt b/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt deleted file mode 100644 index a67bc32..0000000 --- a/src/main/kotlin/org/cobalt/api/pathfinder/pathing/processing/Validators.kt +++ /dev/null @@ -1,95 +0,0 @@ -package org.cobalt.api.pathfinder.pathing.processing - -import org.cobalt.api.pathfinder.pathing.processing.context.EvaluationContext -import org.cobalt.api.pathfinder.pathing.processing.context.SearchContext - -object Validators { - - object Validators { - - fun allOf(vararg validators: ValidationProcessor): ValidationProcessor = - AllOfValidator(*validators) - - fun allOf(validators: List): ValidationProcessor = - AllOfValidator(validators) - - fun anyOf(vararg validators: ValidationProcessor): ValidationProcessor = - AnyOfValidator(*validators) - - fun anyOf(validators: List): ValidationProcessor = - AnyOfValidator(validators) - - fun noneOf(vararg validators: ValidationProcessor): ValidationProcessor = - NoneOfValidator(*validators) - - fun noneOf(validators: List): ValidationProcessor = - NoneOfValidator(validators) - - fun not(validator: ValidationProcessor): ValidationProcessor = NotValidator(validator) - - fun alwaysTrue(): ValidationProcessor = AlwaysTrueValidator - - fun alwaysFalse(): ValidationProcessor = AlwaysFalseValidator - - private fun copyAndFilterNulls( - vararg validators: ValidationProcessor?, - ): List = validators.filterNotNull() - - private fun copyAndFilterNulls( - validators: List?, - ): List = validators?.filterNotNull() ?: emptyList() - - private abstract class AbstractCompositeValidator(validators: List) : - ValidationProcessor { - protected val children: List = validators.filterNotNull() - - override fun initializeSearch(context: SearchContext) = - children.forEach { it.initializeSearch(context) } - - override fun finalizeSearch(context: SearchContext) = - children.forEach { it.finalizeSearch(context) } - } - - private class AllOfValidator : AbstractCompositeValidator { - constructor(vararg validators: ValidationProcessor?) : super(validators.toList()) - constructor(validators: List?) : super(validators ?: emptyList()) - - override fun isValid(context: EvaluationContext): Boolean = - children.all { it.isValid(context) } - } - - private class AnyOfValidator : AbstractCompositeValidator { - constructor(vararg validators: ValidationProcessor?) : super(validators.toList()) - constructor(validators: List?) : super(validators ?: emptyList()) - - override fun isValid(context: EvaluationContext): Boolean { - if (children.isEmpty()) return false - return children.any { it.isValid(context) } - } - } - - private class NoneOfValidator : AbstractCompositeValidator { - constructor(vararg validators: ValidationProcessor?) : super(validators.toList()) - constructor(validators: List?) : super(validators ?: emptyList()) - - override fun isValid(context: EvaluationContext): Boolean = - children.none { it.isValid(context) } - } - - private class NotValidator(private val child: ValidationProcessor) : ValidationProcessor { - override fun initializeSearch(context: SearchContext) = child.initializeSearch(context) - - override fun isValid(context: EvaluationContext): Boolean = !child.isValid(context) - - override fun finalizeSearch(context: SearchContext) = child.finalizeSearch(context) - } - - private object AlwaysTrueValidator : ValidationProcessor { - override fun isValid(context: EvaluationContext): Boolean = true - } - - private object AlwaysFalseValidator : ValidationProcessor { - override fun isValid(context: EvaluationContext): Boolean = false - } - } -} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt b/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt deleted file mode 100644 index a907e3d..0000000 --- a/src/main/kotlin/org/cobalt/api/pathfinder/result/PathUtils.kt +++ /dev/null @@ -1,133 +0,0 @@ -package org.cobalt.api.pathfinder.result - -import kotlin.math.abs -import kotlin.math.ceil -import kotlin.math.roundToInt -import net.minecraft.util.Mth -import org.cobalt.api.pathfinder.pathing.result.Path -import org.cobalt.api.pathfinder.util.ParameterizedSupplier -import org.cobalt.api.pathfinder.wrapper.PathPosition - -object PathUtils { - - fun interpolate(path: Path, resolution: Double): Path { - require(resolution > 0) { "Resolution must be > 0" } - - val result = ArrayDeque() - var previous: PathPosition? = null - - for (current in path) { - previous?.let { prev -> interpolateSegment(prev, current, resolution, result) } - result.addLast(current) - previous = current - } - - return buildPath(result) - } - - fun simplify(path: Path, epsilon: Double): Path { - validateEpsilon(epsilon) - - val result = ArrayDeque() - var index = 0 - val stride = maxOf(1, (1.0 / epsilon).roundToInt()) - - for (pos in path) { - if (index % stride == 0) { - result.addLast(pos) - } - index++ - } - - return buildPath(result) - } - - fun join(first: Path, second: Path): Path { - if (first.length() == 0) return second - if (second.length() == 0) return first - - val result = ArrayDeque() - first.forEach { result.addLast(it) } - second.forEach { result.addLast(it) } - - return buildPath(result) - } - - fun trim(path: Path, maxLength: Int): Path { - require(maxLength > 0) { "maxLength must be > 0" } - - if (path.length() <= maxLength) return path - - val result = ArrayDeque() - var count = 0 - - for (p in path) { - result.addLast(p) - if (++count >= maxLength) break - } - - return buildPath(result) - } - - fun mutatePositions(path: Path, mutator: ParameterizedSupplier): Path { - val result = ArrayDeque(path.length()) - - for (pos in path) { - result.addLast(mutator.accept(pos)) - } - - return buildPath(result) - } - - private fun interpolateSegment( - start: PathPosition, - end: PathPosition, - resolution: Double, - result: ArrayDeque, - ) { - val distance = start.distance(end) - val steps = ceil(distance / resolution).toInt() - - for (i in 1 until steps) { - val progress = i.toDouble() / steps - result.addLast(interpolate(start, end, progress)) - } - } - - private fun interpolate(pos1: PathPosition, pos2: PathPosition, progress: Double): PathPosition { - val x = Mth.lerp(progress, pos1.x, pos2.x) - val y = Mth.lerp(progress, pos1.y, pos2.y) - val z = Mth.lerp(progress, pos1.z, pos2.z) - return PathPosition(x, y, z) - } - - private fun buildPath(positions: ArrayDeque): Path { - require(positions.isNotEmpty()) { "Cannot build path from empty position list" } - - val path = PathImpl(positions.first(), positions.last(), positions) - return removeDuplicates(path) - } - - private fun removeDuplicates(path: Path): Path { - val EPS = 1e-12 - val result = ArrayDeque() - var last: PathPosition? = null - - for (pos in path) { - if (last == null || !samePoint(last, pos, EPS)) { - result.addLast(pos) - last = pos - } - } - - return PathImpl(result.first(), result.last(), result) - } - - private fun samePoint(a: PathPosition, b: PathPosition, eps: Double): Boolean { - return abs(a.x - b.x) <= eps && abs(a.y - b.y) <= eps && abs(a.z - b.z) <= eps - } - - private fun validateEpsilon(epsilon: Double) { - require(epsilon > 0.0 && epsilon <= 1.0) { "Epsilon must be in (0.0, 1.0]" } - } -} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/util/ParameterizedSupplier.kt b/src/main/kotlin/org/cobalt/api/pathfinder/util/ParameterizedSupplier.kt deleted file mode 100644 index 521a862..0000000 --- a/src/main/kotlin/org/cobalt/api/pathfinder/util/ParameterizedSupplier.kt +++ /dev/null @@ -1,5 +0,0 @@ -package org.cobalt.api.pathfinder.util - -fun interface ParameterizedSupplier { - fun accept(value: T): T -} diff --git a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/Depth.kt b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/Depth.kt index 827ee59..5aac33f 100644 --- a/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/Depth.kt +++ b/src/main/kotlin/org/cobalt/api/pathfinder/wrapper/Depth.kt @@ -1,5 +1,6 @@ package org.cobalt.api.pathfinder.wrapper +@ConsistentCopyVisibility data class Depth private constructor(private var value: Int) { companion object { fun of(value: Int): Depth = Depth(value)