--- a/net/minecraft/world/level/pathfinder/PathFinder.java
+++ b/net/minecraft/world/level/pathfinder/PathFinder.java
@@ -38,9 +38,12 @@
         if (node == null) {
             return null;
         } else {
-            Map<Target, BlockPos> map = positions.stream().collect(Collectors.toMap((pos) -> {
-                return this.nodeEvaluator.getGoal((double)pos.getX(), (double)pos.getY(), (double)pos.getZ());
-            }, Function.identity()));
+            // Paper start - remove streams - and optimize collection
+            List<Map.Entry<Target, BlockPos>> map = Lists.newArrayList();
+            for (BlockPos pos : positions) {
+                map.add(new java.util.AbstractMap.SimpleEntry<>(this.nodeEvaluator.getGoal(pos.getX(), pos.getY(), pos.getZ()), pos));
+            }
+            // Paper end
             Path path = this.findPath(world.getProfiler(), node, map, followRange, distance, rangeMultiplier);
             this.nodeEvaluator.done();
             return path;
@@ -48,18 +51,19 @@
     }
 
     @Nullable
-    private Path findPath(ProfilerFiller profiler, Node startNode, Map<Target, BlockPos> positions, float followRange, int distance, float rangeMultiplier) {
+    // Paper start - optimize collection
+    private Path findPath(ProfilerFiller profiler, Node startNode, List<Map.Entry<Target, BlockPos>> positions, float followRange, int distance, float rangeMultiplier) {
         profiler.push("find_path");
         profiler.markForCharting(MetricCategory.PATH_FINDING);
-        Set<Target> set = positions.keySet();
+        // Set<Target> set = positions.keySet();
         startNode.g = 0.0F;
-        startNode.h = this.getBestH(startNode, set);
+        startNode.h = this.getBestH(startNode, positions); // Paper - optimize collection
         startNode.f = startNode.h;
         this.openSet.clear();
         this.openSet.insert(startNode);
-        Set<Node> set2 = ImmutableSet.of();
+        // Set<Node> set2 = ImmutableSet.of(); // Paper - unused - diff on change
         int i = 0;
-        Set<Target> set3 = Sets.newHashSetWithExpectedSize(set.size());
+        List<Map.Entry<Target, BlockPos>> entryList = Lists.newArrayListWithExpectedSize(positions.size()); // Paper - optimize collection
         int j = (int)((float)this.maxVisitedNodes * rangeMultiplier);
 
         while(!this.openSet.isEmpty()) {
@@ -71,14 +75,18 @@
             Node node = this.openSet.pop();
             node.closed = true;
 
-            for(Target target : set) {
+            // Paper start - optimize collection
+            for(int i1 = 0; i1 < positions.size(); i1++) {
+                final Map.Entry<Target, BlockPos> entry = positions.get(i1);
+                Target target = entry.getKey();
                 if (node.distanceManhattan(target) <= (float)distance) {
                     target.setReached();
-                    set3.add(target);
+                    entryList.add(entry);
+                    // Paper end
                 }
             }
 
-            if (!set3.isEmpty()) {
+            if (!entryList.isEmpty()) { // Paper - rename variable
                 break;
             }
 
@@ -93,7 +101,7 @@
                     if (node2.walkedDistance < followRange && (!node2.inOpenSet() || g < node2.g)) {
                         node2.cameFrom = node;
                         node2.g = g;
-                        node2.h = this.getBestH(node2, set) * 1.5F;
+                        node2.h = this.getBestH(node2, positions) * 1.5F; // Paper - list instead of set
                         if (node2.inOpenSet()) {
                             this.openSet.changeCost(node2, node2.g + node2.h);
                         } else {
@@ -105,23 +113,31 @@
             }
         }
 
-        Optional<Path> optional = !set3.isEmpty() ? set3.stream().map((node) -> {
-            return this.reconstructPath(node.getBestNode(), positions.get(node), true);
-        }).min(Comparator.comparingInt(Path::getNodeCount)) : set.stream().map((target) -> {
-            return this.reconstructPath(target.getBestNode(), positions.get(target), false);
-        }).min(Comparator.comparingDouble(Path::getDistToTarget).thenComparingInt(Path::getNodeCount));
-        profiler.pop();
-        return !optional.isPresent() ? null : optional.get();
+        // Paper start - remove streams - and optimize collection
+        Path best = null;
+        boolean entryListIsEmpty = entryList.isEmpty();
+        Comparator<Path> comparator = entryListIsEmpty ? Comparator.comparingInt(Path::getNodeCount)
+            : Comparator.comparingDouble(Path::getDistToTarget).thenComparingInt(Path::getNodeCount);
+        for (Map.Entry<Target, BlockPos> entry : entryListIsEmpty ? positions : entryList) {
+            Path path = this.reconstructPath(entry.getKey().getBestNode(), entry.getValue(), !entryListIsEmpty);
+            if (best == null || comparator.compare(path, best) < 0)
+                best = path;
+        }
+        return best;
+        // Paper end
     }
 
     protected float distance(Node a, Node b) {
         return a.distanceTo(b);
     }
 
-    private float getBestH(Node node, Set<Target> targets) {
+    private float getBestH(Node node, List<Map.Entry<Target, BlockPos>> targets) { // Paper - optimize collection - Set<Target> -> List<Map.Entry<Target, BlockPos>>
         float f = Float.MAX_VALUE;
 
-        for(Target target : targets) {
+        // Paper start - optimize collection
+        for (int i = 0, targetsSize = targets.size(); i < targetsSize; i++) {
+            final Target target = targets.get(i).getKey();
+            // Paper end
             float g = node.distanceTo(target);
             target.updateBest(g, node);
             f = Math.min(g, f);
