From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Spottedleaf <Spottedleaf@users.noreply.github.com>
Date: Sun, 31 Jan 2021 02:29:24 -0800
Subject: [PATCH] Optimise general POI access

There are a couple of problems with mojang's POI code.
Firstly, it's all streams. Unsurprisingly, stacking
streams on top of each other is horrible for performance
and ultimately took up half of a villager's tick!

Secondly, sometime's the search radius is large and there are
a significant number of poi entries per chunk section. Even
removing streams at this point doesn't help much. The only solution
is to start at the search point and iterate outwards. This
type of approach shows massive gains for portals, simply because
we can avoid sync loading a large area of chunks. I also tested
a massive farm I found in JellySquid's discord, which showed
to benefit significantly simply because the farm had so many
portal blocks that searching through them all was very slow.

Great care has been taken so that behavior remains identical to
vanilla, however I cannot account for oddball Stream API
implementations, if they even exist (streams can technically
be loose with iteration order in a sorted stream given its
source stream is not tagged with ordered, and mojang does not
tag the source stream as ordered). However in my testing on openjdk
there showed no difference, as expected.

This patch also specifically optimises other areas of code to
use PoiAccess. For example, some villager AI and portaling code
had to be specifically modified.

diff --git a/src/main/java/com/tuinity/tuinity/util/PoiAccess.java b/src/main/java/com/tuinity/tuinity/util/PoiAccess.java
new file mode 100644
index 0000000000000000000000000000000000000000..5018a3378680d98605e0e4a79bf112e032c98925
--- /dev/null
+++ b/src/main/java/com/tuinity/tuinity/util/PoiAccess.java
@@ -0,0 +1,812 @@
+package com.tuinity.tuinity.util;
+
+import it.unimi.dsi.fastutil.doubles.Double2ObjectMap;
+import it.unimi.dsi.fastutil.doubles.Double2ObjectRBTreeMap;
+import it.unimi.dsi.fastutil.longs.Long2ObjectOpenHashMap;
+import net.minecraft.core.BlockPosition;
+import net.minecraft.util.MathHelper;
+import net.minecraft.core.SectionPosition;
+import net.minecraft.world.entity.ai.village.poi.VillagePlace;
+import net.minecraft.world.entity.ai.village.poi.VillagePlaceRecord;
+import net.minecraft.world.entity.ai.village.poi.VillagePlaceSection;
+import net.minecraft.world.entity.ai.village.poi.VillagePlaceType;
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.Iterator;
+import java.util.List;
+import java.util.Map;
+import java.util.Optional;
+import java.util.Set;
+import java.util.function.Predicate;
+
+/**
+ * Provides optimised access to POI data. All returned values will be identical to vanilla.
+ */
+public final class PoiAccess {
+
+    public static final class SortedChunkSectionPos {
+
+        public final int relativeX;
+        public final int absoluteY;
+        public final int relativeZ;
+
+        public final double smallestDistanceToSection;
+
+        public SortedChunkSectionPos(int relativeX, int absoluteY, int relativeZ, double smallestDistanceToSection) {
+            this.relativeX = relativeX;
+            this.absoluteY = absoluteY;
+            this.relativeZ = relativeZ;
+            this.smallestDistanceToSection = smallestDistanceToSection;
+        }
+
+        protected double smallestDistanceAhead;
+
+    }
+
+    protected static double clamp(final double val, final double min, final double max) {
+        return (val < min ? min : (val > max ? max : val));
+    }
+
+    protected static double getSmallestDistanceSquared(final double boxMinX, final double boxMinY, final double boxMinZ,
+                                                       final double boxMaxX, final double boxMaxY, final double boxMaxZ,
+
+                                                       final double circleX, final double circleY, final double circleZ) {
+        // is the circle center inside the box?
+        if (circleX >= boxMinX && circleX <= boxMaxX && circleY >= boxMinY && circleY <= boxMaxY && circleZ >= boxMinZ && circleZ <= boxMaxZ) {
+            return 0.0;
+        }
+
+        final double boxWidthX = (boxMaxX - boxMinX) / 2.0;
+        final double boxWidthY = (boxMaxY - boxMinY) / 2.0;
+        final double boxWidthZ = (boxMaxZ - boxMinZ) / 2.0;
+
+        final double boxCenterX = (boxMinX + boxMaxX) / 2.0;
+        final double boxCenterY = (boxMinY + boxMaxY) / 2.0;
+        final double boxCenterZ = (boxMinZ + boxMaxZ) / 2.0;
+
+        double centerDiffX = circleX - boxCenterX;
+        double centerDiffY = circleY - boxCenterY;
+        double centerDiffZ = circleZ - boxCenterZ;
+
+        centerDiffX = circleX - (clamp(centerDiffX, -boxWidthX, boxWidthX) + boxCenterX);
+        centerDiffY = circleY - (clamp(centerDiffY, -boxWidthY, boxWidthY) + boxCenterY);
+        centerDiffZ = circleZ - (clamp(centerDiffZ, -boxWidthZ, boxWidthZ) + boxCenterZ);
+
+        return (centerDiffX * centerDiffX) + (centerDiffY * centerDiffY) + (centerDiffZ * centerDiffZ);
+    }
+
+
+    // key is:
+    //  upper 32 bits:
+    //   upper 16 bits: max y section
+    //   lower 16 bits: min y section
+    //  lower 32 bits:
+    //   upper 16 bits: section
+    //   lower 16 bits: radius
+    protected static long getKey(final int minSection, final int maxSection, final int section, final int radius) {
+        return (
+                (maxSection & 0xFFFFL) << (64 - 16)
+                | (minSection & 0xFFFFL) << (64 - 32)
+                | (section & 0xFFFFL) << (64 - 48)
+                | (radius & 0xFFFFL) << (64 - 64)
+                );
+    }
+
+    protected static final Long2ObjectOpenHashMap<List<SortedChunkSectionPos>> cachedSortedPositionsAddHalf =
+            new Long2ObjectOpenHashMap<>();
+    protected static final Long2ObjectOpenHashMap<List<SortedChunkSectionPos>> cachedSortedPositionsNoHalf =
+            new Long2ObjectOpenHashMap<>();
+
+    public static List<SortedChunkSectionPos> getSortedPositions(final BlockPosition center, final int radius, final boolean addHalfToSection) {
+        final Long2ObjectOpenHashMap<List<SortedChunkSectionPos>> map = addHalfToSection ? cachedSortedPositionsAddHalf : cachedSortedPositionsNoHalf;
+
+        final int section = center.getY() >> 4;
+        if (section < -16) {
+            return calculateSortedPositions(0, 15, section, radius, addHalfToSection);
+        } else if (section > (16 + 16)) {
+            return calculateSortedPositions(0, 15, section, radius, addHalfToSection);
+        }
+
+        final long key = getKey(0, 15, center.getY() >> 4, radius);
+        synchronized (map) {
+            List<SortedChunkSectionPos> existing = map.get(key);
+            if (existing != null) {
+                return existing;
+            }
+        }
+
+        final List<SortedChunkSectionPos> calculated = calculateSortedPositions(0, 15, section, radius, addHalfToSection);
+
+        synchronized (map) {
+            map.putIfAbsent(key, calculated);
+        }
+
+        return calculated;
+    }
+
+    protected static List<SortedChunkSectionPos> calculateSortedPositions(final int minSection, final int maxSection, final int section, final int radius, final boolean addHalfToSection) {
+        final List<SortedChunkSectionPos> ret = new ArrayList<>();
+
+        final double sourceX = 8.0;
+        final double sourceY = section * 16.0 + 8.0;
+        final double sourceZ = 8.0;
+        final double minSectionAdd = addHalfToSection ? 0.5 : 0.0;
+        final double maxSectionAdd = addHalfToSection ? 15.5 : 16.0;
+        for (int y = minSection; y <= maxSection; ++y) {
+            for (int dz = -radius; dz <= radius; ++dz) {
+                for (int dx = -radius; dx <= radius; ++dx) {
+                    double distanceSquared = getSmallestDistanceSquared(
+                            (double)(dx << 4) + minSectionAdd,
+                            (double)(y << 4) + minSectionAdd,
+                            (double)(dz << 4) + minSectionAdd,
+                            (double)(dx << 4) + maxSectionAdd,
+                            (double)(y << 4) + maxSectionAdd,
+                            (double)(dz << 4) + maxSectionAdd,
+
+                            sourceX, sourceY, sourceZ
+                    );
+                    // max additional distance (due to varying center point)
+                    final double a = (Math.sqrt(2*2*2))*(addHalfToSection ? 7.5 : 8.0);
+                    distanceSquared = Math.sqrt(distanceSquared) - a;
+                    distanceSquared *= distanceSquared;
+
+                    ret.add(new SortedChunkSectionPos(dx, y, dz, distanceSquared - 1.0e-3)); // account for floating point errors
+                }
+            }
+        }
+
+        final BlockPosition relativeCenter = new BlockPosition(8, (section << 4) | 8, 8);
+        ret.sort((s1, s2) -> {
+            return Double.compare(relativeCenter.distanceSquared((s1.relativeX << 4) + 8, (s1.absoluteY << 4) + 8, (s1.relativeZ << 4) + 8, false),
+                    relativeCenter.distanceSquared((s2.relativeX << 4) + 8, (s2.absoluteY << 4) + 8, (s2.relativeZ << 4) + 8, false));
+        });
+
+        double smallest = Double.MAX_VALUE;
+        for (int i = ret.size() - 1; i >= 0; --i) {
+            final SortedChunkSectionPos chunkPos = ret.get(i);
+            chunkPos.smallestDistanceAhead = smallest;
+            if (chunkPos.smallestDistanceToSection < smallest) {
+                smallest = chunkPos.smallestDistanceToSection;
+            }
+        }
+
+        // we set smallestDistanceAhead so that callers iterating over the list do not need to search any more sections
+        // than they need to (the intersects check for a sphere and a 3d box is seriously expensive, especially with
+        // how many sections there are).
+
+        return ret;
+    }
+
+    // only includes x/z axis
+    // finds the closest poi data by distance.
+    public static BlockPosition findClosestPoiDataPosition(final VillagePlace poiStorage,
+                                                           final Predicate<VillagePlaceType> villagePlaceType,
+                                                           // position predicate must not modify chunk POI
+                                                           final Predicate<BlockPosition> positionPredicate,
+                                                           final BlockPosition sourcePosition,
+                                                           final int range, // distance on x y z axis
+                                                           final double maxDistance,
+                                                           final VillagePlace.Occupancy occupancy,
+                                                           final boolean load) {
+        final VillagePlaceRecord ret = findClosestPoiDataRecord(
+                poiStorage, villagePlaceType, positionPredicate, sourcePosition, range, maxDistance, occupancy, load
+        );
+
+        return ret == null ? null : ret.getPosition();
+    }
+
+    // only includes x/z axis
+    // finds the closest poi data by distance. if multiple match the same distance, then they all are returned.
+    public static void findClosestPoiDataPositions(final VillagePlace poiStorage,
+                                                   final Predicate<VillagePlaceType> villagePlaceType,
+                                                   // position predicate must not modify chunk POI
+                                                   final Predicate<BlockPosition> positionPredicate,
+                                                   final BlockPosition sourcePosition,
+                                                   final int range, // distance on x y z axis
+                                                   final double maxDistance,
+                                                   final VillagePlace.Occupancy occupancy,
+                                                   final boolean load,
+                                                   final Set<BlockPosition> ret) {
+        final Set<BlockPosition> positions = new HashSet<>();
+        // pos predicate is last thing that runs before adding to ret.
+        final Predicate<BlockPosition> newPredicate = (final BlockPosition pos) -> {
+            if (positionPredicate != null && !positionPredicate.test(pos)) {
+                return false;
+            }
+            return positions.add(pos.immutableCopy());
+        };
+
+        final List<VillagePlaceRecord> toConvert = new ArrayList<>();
+        findClosestPoiDataRecords(
+                poiStorage, villagePlaceType, newPredicate, sourcePosition, range, maxDistance, occupancy, load, toConvert
+        );
+
+        for (final VillagePlaceRecord record : toConvert) {
+            ret.add(record.getPosition());
+        }
+    }
+
+    // only includes x/z axis
+    // finds the closest poi data by distance.
+    public static VillagePlaceRecord findClosestPoiDataRecord(final VillagePlace poiStorage,
+                                                              final Predicate<VillagePlaceType> villagePlaceType,
+                                                              // position predicate must not modify chunk POI
+                                                              final Predicate<BlockPosition> positionPredicate,
+                                                              final BlockPosition sourcePosition,
+                                                              final int range, // distance on x y z axis
+                                                              final double maxDistance,
+                                                              final VillagePlace.Occupancy occupancy,
+                                                              final boolean load) {
+        final List<VillagePlaceRecord> ret = new ArrayList<>();
+        findClosestPoiDataRecords(
+                poiStorage, villagePlaceType, positionPredicate, sourcePosition, range, maxDistance, occupancy, load, ret
+        );
+        return ret.isEmpty() ? null : ret.get(0);
+    }
+
+    // only includes x/z axis
+    // finds the closest poi data by distance. if multiple match the same distance, then they all are returned.
+    public static void findClosestPoiDataRecords(final VillagePlace poiStorage,
+                                                 final Predicate<VillagePlaceType> villagePlaceType,
+                                                 // position predicate must not modify chunk POI
+                                                 final Predicate<BlockPosition> positionPredicate,
+                                                 final BlockPosition sourcePosition,
+                                                 final int range, // distance on x y z axis
+                                                 final double maxDistance,
+                                                 final VillagePlace.Occupancy occupancy,
+                                                 final boolean load,
+                                                 final List<VillagePlaceRecord> ret) {
+        final Predicate<? super VillagePlaceRecord> occupancyFilter = occupancy.getPredicate();
+
+        final List<VillagePlaceRecord> closestRecords = new ArrayList<>();
+        double closestDistanceSquared = maxDistance * maxDistance;
+
+        final int lowerX = MathHelper.floor(sourcePosition.getX() - range) >> 4;
+        final int lowerZ = MathHelper.floor(sourcePosition.getZ() - range) >> 4;
+        final int upperX = MathHelper.floor(sourcePosition.getX() + range) >> 4;
+        final int upperZ = MathHelper.floor(sourcePosition.getZ() + range) >> 4;
+
+        final List<SortedChunkSectionPos> chunks = getSortedPositions(sourcePosition, (range + 15) >> 4, true);
+
+        final int centerX = sourcePosition.getX() >> 4;
+        final int centerY = sourcePosition.getY() >> 4;
+        final int centerZ = sourcePosition.getZ() >> 4;
+
+        for (final SortedChunkSectionPos relativeSection : chunks) {
+            if (relativeSection.smallestDistanceToSection > closestDistanceSquared) {
+                if (relativeSection.smallestDistanceAhead > closestDistanceSquared) {
+                    break;
+                }
+                continue;
+            }
+            final int sectionX = relativeSection.relativeX + centerX;
+            final int sectionY = relativeSection.absoluteY;
+            final int sectionZ = relativeSection.relativeZ + centerZ;
+
+            if (sectionX < lowerX || sectionX > upperX || sectionZ < lowerZ || sectionZ > upperZ) {
+                // out of bound chunk
+                continue;
+            }
+
+            final double sectionDistanceSquared = getSmallestDistanceSquared(
+                    (sectionX << 4) + 0.5,
+                    (sectionY << 4) + 0.5,
+                    (sectionZ << 4) + 0.5,
+                    (sectionX << 4) + 15.5,
+                    (sectionY << 4) + 15.5,
+                    (sectionZ << 4) + 15.5,
+                    (double)sourcePosition.getX(), (double)sourcePosition.getY(), (double)sourcePosition.getZ()
+            );
+            if (sectionDistanceSquared > closestDistanceSquared) {
+                continue;
+            }
+
+            final Optional<VillagePlaceSection> poiSectionOptional = load ? poiStorage.getOrLoad(SectionPosition.asLong(sectionX, sectionY, sectionZ))
+                    : poiStorage.getIfLoaded(SectionPosition.asLong(sectionX, sectionY, sectionZ));
+
+            if (poiSectionOptional == null || !poiSectionOptional.isPresent()) {
+                continue;
+            }
+
+            final VillagePlaceSection poiSection = poiSectionOptional.orElse(null);
+
+            final Map<VillagePlaceType, Set<VillagePlaceRecord>> sectionData = poiSection.getData();
+            if (sectionData.isEmpty()) {
+                continue;
+            }
+
+            // now we search the section data
+            for (final Map.Entry<VillagePlaceType, Set<VillagePlaceRecord>> entry : sectionData.entrySet()) {
+                if (!villagePlaceType.test(entry.getKey())) {
+                    // filter out by poi type
+                    continue;
+                }
+
+                // now we can look at the poi data
+                for (final VillagePlaceRecord poiData : entry.getValue()) {
+                    if (!occupancyFilter.test(poiData)) {
+                        // filter by occupancy
+                        continue;
+                    }
+
+                    final BlockPosition poiPosition = poiData.getPosition();
+
+                    if (Math.abs(poiPosition.getX() - sourcePosition.getX()) > range
+                            || Math.abs(poiPosition.getZ() - sourcePosition.getZ()) > range) {
+                        // out of range for square radius
+                        continue;
+                    }
+
+                    // it's important that it's poiPosition.distanceSquared(source) : the value actually is different IF the values are swapped!
+                    final double dataRange = poiPosition.distanceSquared(sourcePosition);
+
+                    if (dataRange > closestDistanceSquared) {
+                        // out of range for distance check
+                        continue;
+                    }
+
+                    if (positionPredicate != null && !positionPredicate.test(poiPosition)) {
+                        // filter by position
+                        continue;
+                    }
+
+                    if (dataRange < closestDistanceSquared) {
+                        closestRecords.clear();
+                        closestDistanceSquared = dataRange;
+                    }
+                    closestRecords.add(poiData);
+                }
+            }
+        }
+
+        // uh oh! we might have multiple records that match the distance sorting!
+        // we need to re-order our results by the way vanilla would have iterated over them.
+        closestRecords.sort((record1, record2) -> {
+            // vanilla iterates the same way we do for data inside sections, so we know the ordering inside a section
+            // is fine and should be preserved (this sort is stable so we're good there)
+            // but they iterate sections by x then by z (like the following)
+            // for (int x = -dx; x <= dx; ++x)
+            //     for (int z = -dz; z <= dz; ++z)
+            //  ....
+            // so we need to reorder such that records with lower chunk z, then lower chunk x come first
+            final BlockPosition pos1 = record1.getPosition();
+            final BlockPosition pos2 = record2.getPosition();
+
+            final int cx1 = pos1.getX() >> 4;
+            final int cz1 = pos1.getZ() >> 4;
+
+            final int cx2 = pos2.getX() >> 4;
+            final int cz2 = pos2.getZ() >> 4;
+
+            if (cz2 != cz1) {
+                // want smaller z
+                return Integer.compare(cz1, cz2);
+            }
+
+            if (cx2 != cx1) {
+                // want smaller x
+                return Integer.compare(cx1, cx2);
+            }
+
+            // same chunk
+            // once vanilla has the chunk, it will iterate from all of the chunk sections starting from smaller y
+            // so now we just compare section y, wanting smaller y
+
+            return Integer.compare(pos1.getY() >> 4, pos2.getY() >> 4);
+        });
+
+        // now we match perfectly what vanilla would have outputted, without having to search the whole radius (hopefully).
+        ret.addAll(closestRecords);
+    }
+
+    // finds the closest poi entry pos.
+    public static BlockPosition findNearestPoiPosition(final VillagePlace poiStorage,
+                                                       final Predicate<VillagePlaceType> villagePlaceType,
+                                                       // position predicate must not modify chunk POI
+                                                       final Predicate<BlockPosition> positionPredicate,
+                                                       final BlockPosition sourcePosition,
+                                                       final int range, // distance on x y z axis
+                                                       final double maxDistance,
+                                                       final VillagePlace.Occupancy occupancy,
+                                                       final boolean load) {
+        final VillagePlaceRecord ret = findNearestPoiRecord(
+                poiStorage, villagePlaceType, positionPredicate, sourcePosition, range, maxDistance, occupancy, load
+        );
+        return ret == null ? null : ret.getPosition();
+    }
+
+    // finds the closest `max` poi entry positions.
+    public static void findNearestPoiPositions(final VillagePlace poiStorage,
+                                               final Predicate<VillagePlaceType> villagePlaceType,
+                                               // position predicate must not modify chunk POI
+                                               final Predicate<BlockPosition> positionPredicate,
+                                               final BlockPosition sourcePosition,
+                                               final int range, // distance on x y z axis
+                                               final double maxDistance,
+                                               final VillagePlace.Occupancy occupancy,
+                                               final boolean load,
+                                               final int max,
+                                               final List<BlockPosition> ret) {
+        final Set<BlockPosition> positions = new HashSet<>();
+        // pos predicate is last thing that runs before adding to ret.
+        final Predicate<BlockPosition> newPredicate = (final BlockPosition pos) -> {
+            if (positionPredicate != null && !positionPredicate.test(pos)) {
+                return false;
+            }
+            return positions.add(pos.immutableCopy());
+        };
+
+        final List<VillagePlaceRecord> toConvert = new ArrayList<>();
+        findNearestPoiRecords(
+                poiStorage, villagePlaceType, newPredicate, sourcePosition, range, maxDistance, occupancy, load, max, toConvert
+        );
+
+        for (final VillagePlaceRecord record : toConvert) {
+            ret.add(record.getPosition());
+        }
+    }
+
+    // finds the closest poi entry.
+    public static VillagePlaceRecord findNearestPoiRecord(final VillagePlace poiStorage,
+                                                          final Predicate<VillagePlaceType> villagePlaceType,
+                                                          // position predicate must not modify chunk POI
+                                                          final Predicate<BlockPosition> positionPredicate,
+                                                          final BlockPosition sourcePosition,
+                                                          final int range, // distance on x y z axis
+                                                          final double maxDistance,
+                                                          final VillagePlace.Occupancy occupancy,
+                                                          final boolean load) {
+        final List<VillagePlaceRecord> ret = new ArrayList<>();
+        findNearestPoiRecords(
+                poiStorage, villagePlaceType, positionPredicate, sourcePosition, range, maxDistance, occupancy, load,
+                1, ret
+        );
+        return ret.isEmpty() ? null : ret.get(0);
+    }
+
+    // finds the closest `max` poi entries.
+    public static void findNearestPoiRecords(final VillagePlace poiStorage,
+                                             final Predicate<VillagePlaceType> villagePlaceType,
+                                             // position predicate must not modify chunk POI
+                                             final Predicate<BlockPosition> positionPredicate,
+                                             final BlockPosition sourcePosition,
+                                             final int range, // distance on x y z axis
+                                             final double maxDistance,
+                                             final VillagePlace.Occupancy occupancy,
+                                             final boolean load,
+                                             final int max,
+                                             final List<VillagePlaceRecord> ret) {
+        final Predicate<? super VillagePlaceRecord> occupancyFilter = occupancy.getPredicate();
+
+        final double maxDistanceSquared = maxDistance * maxDistance;
+        final Double2ObjectRBTreeMap<List<VillagePlaceRecord>> closestRecords = new Double2ObjectRBTreeMap<>();
+        int totalRecords = 0;
+        double furthestDistanceSquared = maxDistanceSquared;
+
+        final int lowerX = MathHelper.floor(sourcePosition.getX() - range) >> 4;
+        final int lowerZ = MathHelper.floor(sourcePosition.getZ() - range) >> 4;
+        final int upperX = MathHelper.floor(sourcePosition.getX() + range) >> 4;
+        final int upperZ = MathHelper.floor(sourcePosition.getZ() + range) >> 4;
+
+        final List<SortedChunkSectionPos> chunks = getSortedPositions(sourcePosition, (range + 15) >> 4, true);
+
+        final int centerX = sourcePosition.getX() >> 4;
+        final int centerY = sourcePosition.getY() >> 4;
+        final int centerZ = sourcePosition.getZ() >> 4;
+
+        for (final SortedChunkSectionPos relativeSection : chunks) {
+            if (relativeSection.smallestDistanceToSection > (totalRecords >= max ? furthestDistanceSquared : maxDistanceSquared)) {
+                if (relativeSection.smallestDistanceAhead > (totalRecords >= max ? furthestDistanceSquared : maxDistanceSquared)) {
+                    break;
+                }
+                continue;
+            }
+            final int sectionX = relativeSection.relativeX + centerX;
+            final int sectionY = relativeSection.absoluteY;
+            final int sectionZ = relativeSection.relativeZ + centerZ;
+
+            if (sectionX < lowerX || sectionX > upperX || sectionZ < lowerZ || sectionZ > upperZ) {
+                // out of bound chunk
+                continue;
+            }
+
+            final double sectionDistanceSquared = getSmallestDistanceSquared(
+                    (sectionX << 4) + 0.5,
+                    (sectionY << 4) + 0.5,
+                    (sectionZ << 4) + 0.5,
+                    (sectionX << 4) + 15.5,
+                    (sectionY << 4) + 15.5,
+                    (sectionZ << 4) + 15.5,
+                    (double) sourcePosition.getX(), (double) sourcePosition.getY(), (double) sourcePosition.getZ()
+            );
+
+            if (sectionDistanceSquared > (totalRecords >= max ? furthestDistanceSquared : maxDistanceSquared)) {
+                continue;
+            }
+
+            final Optional<VillagePlaceSection> poiSectionOptional = load ? poiStorage.getOrLoad(SectionPosition.asLong(sectionX, sectionY, sectionZ))
+                    : poiStorage.getIfLoaded(SectionPosition.asLong(sectionX, sectionY, sectionZ));
+
+            if (poiSectionOptional == null || !poiSectionOptional.isPresent()) {
+                continue;
+            }
+
+            final VillagePlaceSection poiSection = poiSectionOptional.orElse(null);
+
+            final Map<VillagePlaceType, Set<VillagePlaceRecord>> sectionData = poiSection.getData();
+            if (sectionData.isEmpty()) {
+                continue;
+            }
+
+            // now we search the section data
+            for (final Map.Entry<VillagePlaceType, Set<VillagePlaceRecord>> entry : sectionData.entrySet()) {
+                if (!villagePlaceType.test(entry.getKey())) {
+                    // filter out by poi type
+                    continue;
+                }
+
+                // now we can look at the poi data
+                for (final VillagePlaceRecord poiData : entry.getValue()) {
+                    if (!occupancyFilter.test(poiData)) {
+                        // filter by occupancy
+                        continue;
+                    }
+
+                    final BlockPosition poiPosition = poiData.getPosition();
+
+                    if (Math.abs(poiPosition.getX() - sourcePosition.getX()) > range
+                            || Math.abs(poiPosition.getZ() - sourcePosition.getZ()) > range) {
+                        // out of range for square radius
+                        continue;
+                    }
+
+                    // it's important that it's poiPosition.distanceSquared(source) : the value actually is different IF the values are swapped!
+                    final double dataRange = poiPosition.distanceSquared(sourcePosition);
+
+                    if (dataRange > maxDistanceSquared) {
+                        // out of range for distance check
+                        continue;
+                    }
+
+                    if (dataRange > furthestDistanceSquared && totalRecords >= max) {
+                        // out of range for distance check
+                        continue;
+                    }
+
+                    if (positionPredicate != null && !positionPredicate.test(poiPosition)) {
+                        // filter by position
+                        continue;
+                    }
+
+                    if (dataRange > furthestDistanceSquared) {
+                        // we know totalRecords < max, so this entry is now our furthest
+                        furthestDistanceSquared = dataRange;
+                    }
+
+                    closestRecords.computeIfAbsent(dataRange, (final double key) -> {
+                        return new ArrayList<>();
+                    }).add(poiData);
+
+                    if (++totalRecords >= max) {
+                        if (closestRecords.size() >= 2) {
+                            int entriesInClosest = 0;
+                            final Iterator<Double2ObjectMap.Entry<List<VillagePlaceRecord>>> iterator = closestRecords.double2ObjectEntrySet().iterator();
+                            double nextFurthestDistanceSquared = 0.0;
+
+                            for (int i = 0, len = closestRecords.size() - 1; i < len; ++i) {
+                                final Double2ObjectMap.Entry<List<VillagePlaceRecord>> recordEntry = iterator.next();
+                                entriesInClosest += recordEntry.getValue().size();
+                                nextFurthestDistanceSquared = recordEntry.getDoubleKey();
+                            }
+
+                            if (entriesInClosest >= max) {
+                                // the last set of entries at range wont even be considered for sure... nuke em
+                                final Double2ObjectMap.Entry<List<VillagePlaceRecord>> recordEntry = iterator.next();
+                                totalRecords -= recordEntry.getValue().size();
+                                iterator.remove();
+
+                                furthestDistanceSquared = nextFurthestDistanceSquared;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        final List<VillagePlaceRecord> closestRecordsUnsorted = new ArrayList<>();
+
+        // we're done here, so now just flatten the map and sort it.
+
+        for (final List<VillagePlaceRecord> records : closestRecords.values()) {
+            closestRecordsUnsorted.addAll(records);
+        }
+
+        // uh oh! we might have multiple records that match the distance sorting!
+        // we need to re-order our results by the way vanilla would have iterated over them.
+        closestRecordsUnsorted.sort((record1, record2) -> {
+            // vanilla iterates the same way we do for data inside sections, so we know the ordering inside a section
+            // is fine and should be preserved (this sort is stable so we're good there)
+            // but they iterate sections by x then by z (like the following)
+            // for (int x = -dx; x <= dx; ++x)
+            //     for (int z = -dz; z <= dz; ++z)
+            //  ....
+            // so we need to reorder such that records with lower chunk z, then lower chunk x come first
+            final BlockPosition pos1 = record1.getPosition();
+            final BlockPosition pos2 = record2.getPosition();
+
+            final int cx1 = pos1.getX() >> 4;
+            final int cz1 = pos1.getZ() >> 4;
+
+            final int cx2 = pos2.getX() >> 4;
+            final int cz2 = pos2.getZ() >> 4;
+
+            if (cz2 != cz1) {
+                // want smaller z
+                return Integer.compare(cz1, cz2);
+            }
+
+            if (cx2 != cx1) {
+                // want smaller x
+                return Integer.compare(cx1, cx2);
+            }
+
+            // same chunk
+            // once vanilla has the chunk, it will iterate from all of the chunk sections starting from smaller y
+            // so now we just compare section y, wanting smaller section y
+
+            return Integer.compare(pos1.getY() >> 4, pos2.getY() >> 4);
+        });
+
+        // trim out any entries exceeding our maximum
+        for (int i = closestRecordsUnsorted.size() - 1; i >= max; --i) {
+            closestRecordsUnsorted.remove(i);
+        }
+
+        // now we match perfectly what vanilla would have outputted, without having to search the whole radius (hopefully).
+        ret.addAll(closestRecordsUnsorted);
+    }
+
+    public static BlockPosition findAnyPoiPosition(final VillagePlace poiStorage,
+                                                   final Predicate<VillagePlaceType> villagePlaceType,
+                                                   final Predicate<BlockPosition> positionPredicate,
+                                                   final BlockPosition sourcePosition,
+                                                   final int range, // distance on x y z axis
+                                                   final VillagePlace.Occupancy occupancy,
+                                                   final boolean load) {
+        final VillagePlaceRecord ret = findAnyPoiRecord(
+                poiStorage, villagePlaceType, positionPredicate, sourcePosition, range, occupancy, load
+        );
+
+        return ret == null ? null : ret.getPosition();
+    }
+
+    public static void findAnyPoiPositions(final VillagePlace poiStorage,
+                                           final Predicate<VillagePlaceType> villagePlaceType,
+                                           final Predicate<BlockPosition> positionPredicate,
+                                           final BlockPosition sourcePosition,
+                                           final int range, // distance on x y z axis
+                                           final VillagePlace.Occupancy occupancy,
+                                           final boolean load,
+                                           final int max,
+                                           final List<BlockPosition> ret) {
+        final Set<BlockPosition> positions = new HashSet<>();
+        // pos predicate is last thing that runs before adding to ret.
+        final Predicate<BlockPosition> newPredicate = (final BlockPosition pos) -> {
+            if (positionPredicate != null && !positionPredicate.test(pos)) {
+                return false;
+            }
+            return positions.add(pos.immutableCopy());
+        };
+
+        final List<VillagePlaceRecord> toConvert = new ArrayList<>();
+        findAnyPoiRecords(
+                poiStorage, villagePlaceType, newPredicate, sourcePosition, range, occupancy, load, max, toConvert
+        );
+
+        for (final VillagePlaceRecord record : toConvert) {
+            ret.add(record.getPosition());
+        }
+    }
+
+    public static VillagePlaceRecord findAnyPoiRecord(final VillagePlace poiStorage,
+                                                      final Predicate<VillagePlaceType> villagePlaceType,
+                                                      final Predicate<BlockPosition> positionPredicate,
+                                                      final BlockPosition sourcePosition,
+                                                      final int range, // distance on x y z axis
+                                                      final VillagePlace.Occupancy occupancy,
+                                                      final boolean load) {
+        final List<VillagePlaceRecord> ret = new ArrayList<>();
+        findAnyPoiRecords(poiStorage, villagePlaceType, positionPredicate, sourcePosition, range, occupancy, load, 1, ret);
+        return ret.isEmpty() ? null : ret.get(0);
+    }
+
+    public static void findAnyPoiRecords(final VillagePlace poiStorage,
+                                         final Predicate<VillagePlaceType> villagePlaceType,
+                                         final Predicate<BlockPosition> positionPredicate,
+                                         final BlockPosition sourcePosition,
+                                         final int range, // distance on x y z axis
+                                         final VillagePlace.Occupancy occupancy,
+                                         final boolean load,
+                                         final int max,
+                                         final List<VillagePlaceRecord> ret) {
+        // the biggest issue with the original mojang implementation is that they chain so many streams together
+        // the amount of streams chained just rolls performance, even if nothing is iterated over
+        final Predicate<? super VillagePlaceRecord> occupancyFilter = occupancy.getPredicate();
+        final double rangeSquared = range * range;
+
+        int added = 0;
+
+        // First up, we need to iterate the chunks
+        // all the values here are in chunk sections
+        final int lowerX = MathHelper.floor(sourcePosition.getX() - range) >> 4;
+        final int lowerY = Math.max(0, MathHelper.floor(sourcePosition.getY() - range) >> 4);
+        final int lowerZ = MathHelper.floor(sourcePosition.getZ() - range) >> 4;
+        final int upperX = MathHelper.floor(sourcePosition.getX() + range) >> 4;
+        final int upperY = Math.min(15, MathHelper.floor(sourcePosition.getY() + range) >> 4);
+        final int upperZ = MathHelper.floor(sourcePosition.getZ() + range) >> 4;
+
+        // Vanilla iterates by x until max is reached then increases z
+        // vanilla also searches by increasing Y section value
+        for (int currZ = lowerZ; currZ <= upperZ; ++currZ) {
+            for (int currX = lowerX; currX <= upperX; ++currX) {
+                for (int currY = lowerY; currY <= upperY; ++currY) { // vanilla searches the entire chunk because they're actually stupid. just search the sections we need
+                    final Optional<VillagePlaceSection> poiSectionOptional = load ? poiStorage.getOrLoad(SectionPosition.asLong(currX, currY, currZ)) :
+                            poiStorage.getIfLoaded(SectionPosition.asLong(currX, currY, currZ));
+                    final VillagePlaceSection poiSection = poiSectionOptional == null ? null : poiSectionOptional.orElse(null);
+                    if (poiSection == null) {
+                        continue;
+                    }
+
+                    final Map<VillagePlaceType, Set<VillagePlaceRecord>> sectionData = poiSection.getData();
+                    if (sectionData.isEmpty()) {
+                        continue;
+                    }
+
+                    // now we search the section data
+                    for (final Map.Entry<VillagePlaceType, Set<VillagePlaceRecord>> entry : sectionData.entrySet()) {
+                        if (!villagePlaceType.test(entry.getKey())) {
+                            // filter out by poi type
+                            continue;
+                        }
+
+                        // now we can look at the poi data
+                        for (final VillagePlaceRecord poiData : entry.getValue()) {
+                            if (!occupancyFilter.test(poiData)) {
+                                // filter by occupancy
+                                continue;
+                            }
+
+                            final BlockPosition poiPosition = poiData.getPosition();
+
+                            if (Math.abs(poiPosition.getX() - sourcePosition.getX()) > range
+                                    || Math.abs(poiPosition.getZ() - sourcePosition.getZ()) > range) {
+                                // out of range for square radius
+                                continue;
+                            }
+
+                            if (poiPosition.distanceSquared(sourcePosition) > rangeSquared) {
+                                // out of range for distance check
+                                continue;
+                            }
+
+                            if (positionPredicate != null && !positionPredicate.test(poiPosition)) {
+                                // filter by position
+                                continue;
+                            }
+
+                            // found one!
+                            ret.add(poiData);
+                            if (++added >= max) {
+                                return;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    private PoiAccess() {
+        throw new RuntimeException();
+    }
+}
diff --git a/src/main/java/net/minecraft/world/entity/ai/behavior/BehaviorFindPosition.java b/src/main/java/net/minecraft/world/entity/ai/behavior/BehaviorFindPosition.java
index 5fe71380b04600d6cd0bb068d239ff8fdfe2afbb..bc8786e2aaeab4dbae4e9c7666ad816bc5bfac3f 100644
--- a/src/main/java/net/minecraft/world/entity/ai/behavior/BehaviorFindPosition.java
+++ b/src/main/java/net/minecraft/world/entity/ai/behavior/BehaviorFindPosition.java
@@ -84,7 +84,11 @@ public class BehaviorFindPosition extends Behavior<EntityCreature> {
                 return true;
             }
         };
-        Set<BlockPosition> set = (Set) villageplace.b(this.b.c(), predicate, entitycreature.getChunkCoordinates(), 48, VillagePlace.Occupancy.HAS_SPACE).limit(5L).collect(Collectors.toSet());
+        // Tuinity start - optimise POI access
+        java.util.List<BlockPosition> poiposes = new java.util.ArrayList<>();
+        com.tuinity.tuinity.util.PoiAccess.findNearestPoiPositions(villageplace, this.b.c(), predicate, entitycreature.getChunkCoordinates(), 48, 48*48, VillagePlace.Occupancy.HAS_SPACE, false, 5, poiposes);
+        Set<BlockPosition> set = new java.util.HashSet<>(poiposes);
+        // Tuinity end - optimise POI access
         PathEntity pathentity = entitycreature.getNavigation().a(set, this.b.d());
 
         if (pathentity != null && pathentity.j()) {
diff --git a/src/main/java/net/minecraft/world/entity/ai/sensing/SensorNearestBed.java b/src/main/java/net/minecraft/world/entity/ai/sensing/SensorNearestBed.java
index 5e4c022bcbf4456bc9020f56fb26fee0c627d722..9427072575df9c0f1b9dfb065b6bef43df1f8fc5 100644
--- a/src/main/java/net/minecraft/world/entity/ai/sensing/SensorNearestBed.java
+++ b/src/main/java/net/minecraft/world/entity/ai/sensing/SensorNearestBed.java
@@ -47,8 +47,12 @@ public class SensorNearestBed extends Sensor<EntityInsentient> {
                     return true;
                 }
             };
-            Stream<BlockPosition> stream = villageplace.a(VillagePlaceType.r.c(), predicate, entityinsentient.getChunkCoordinates(), 48, VillagePlace.Occupancy.ANY);
-            PathEntity pathentity = entityinsentient.getNavigation().a(stream, VillagePlaceType.r.d());
+            // Tuinity start - optimise POI access
+            java.util.List<BlockPosition> poiposes = new java.util.ArrayList<>();
+            // don't ask me why it's unbounded. ask mojang.
+            com.tuinity.tuinity.util.PoiAccess.findAnyPoiPositions(villageplace, VillagePlaceType.r.c(), predicate, entityinsentient.getChunkCoordinates(), 48, VillagePlace.Occupancy.ANY, false, Integer.MAX_VALUE, poiposes);
+            PathEntity pathentity = entityinsentient.getNavigation().a(new java.util.HashSet<>(poiposes), VillagePlaceType.r.d());
+            // Tuinity end - optimise POI access
 
             if (pathentity != null && pathentity.j()) {
                 BlockPosition blockposition = pathentity.m();
diff --git a/src/main/java/net/minecraft/world/entity/ai/village/poi/VillagePlace.java b/src/main/java/net/minecraft/world/entity/ai/village/poi/VillagePlace.java
index a5edfaa41156fc03181f38a8813eb5bcc8c08e48..13d94ecd703b3cd0412e138532d2dd74e5bf250d 100644
--- a/src/main/java/net/minecraft/world/entity/ai/village/poi/VillagePlace.java
+++ b/src/main/java/net/minecraft/world/entity/ai/village/poi/VillagePlace.java
@@ -240,31 +240,47 @@ public class VillagePlace extends RegionFileSection<VillagePlaceSection> {
     }
 
     public Optional<BlockPosition> c(Predicate<VillagePlaceType> predicate, Predicate<BlockPosition> predicate1, BlockPosition blockposition, int i, VillagePlace.Occupancy villageplace_occupancy) {
-        return this.a(predicate, predicate1, blockposition, i, villageplace_occupancy).findFirst();
+        // Tuinity start - re-route to faster logic
+        BlockPosition ret = com.tuinity.tuinity.util.PoiAccess.findAnyPoiPosition(this, predicate, predicate1, blockposition, i, villageplace_occupancy, false);
+        return Optional.ofNullable(ret);
+        // Tuinity end - re-route to faster logic
     }
 
     public Optional<BlockPosition> d(Predicate<VillagePlaceType> predicate, BlockPosition blockposition, int i, VillagePlace.Occupancy villageplace_occupancy) {
-        return this.c(predicate, blockposition, i, villageplace_occupancy).map(VillagePlaceRecord::f).min(Comparator.comparingDouble((blockposition1) -> {
-            return blockposition1.j(blockposition);
-        }));
+        // Tuinity start - re-route to faster logic
+        BlockPosition ret = com.tuinity.tuinity.util.PoiAccess.findClosestPoiDataPosition(this, predicate, null, blockposition, i, i*i, villageplace_occupancy, false);
+        return Optional.ofNullable(ret);
+        // Tuinity end - re-route to faster logic
     }
 
     public Optional<BlockPosition> a(Predicate<VillagePlaceType> predicate, Predicate<BlockPosition> predicate1, BlockPosition blockposition, int i) {
-        return this.c(predicate, blockposition, i, VillagePlace.Occupancy.HAS_SPACE).filter((villageplacerecord) -> {
-            return predicate1.test(villageplacerecord.f());
-        }).findFirst().map((villageplacerecord) -> {
-            villageplacerecord.b();
-            return villageplacerecord.f();
-        });
+        // Tuinity start - re-route to faster logic
+        VillagePlaceRecord ret = com.tuinity.tuinity.util.PoiAccess.findAnyPoiRecord(this, predicate, predicate1, blockposition, i,
+                VillagePlace.Occupancy.HAS_SPACE, false);
+        if (ret == null) {
+            return Optional.empty();
+        }
+        // copy from the map() from the old code
+        ret.b();
+        return Optional.ofNullable(ret.f());
+        // Tuinity end - re-route to faster logic
     }
 
     public Optional<BlockPosition> a(Predicate<VillagePlaceType> predicate, Predicate<BlockPosition> predicate1, VillagePlace.Occupancy villageplace_occupancy, BlockPosition blockposition, int i, Random random) {
-        List<VillagePlaceRecord> list = (List) this.c(predicate, blockposition, i, villageplace_occupancy).collect(Collectors.toList());
-
-        Collections.shuffle(list, random);
-        return list.stream().filter((villageplacerecord) -> {
-            return predicate1.test(villageplacerecord.f());
-        }).findFirst().map(VillagePlaceRecord::f);
+        // Tuinity start - re-route to faster logic
+        List<VillagePlaceRecord> list = new java.util.ArrayList<>();
+        com.tuinity.tuinity.util.PoiAccess.findAnyPoiRecords(this, predicate, predicate1, blockposition, i, villageplace_occupancy, false, Integer.MAX_VALUE, list);
+        // Tuinity end - re-route to faster logic
+
+        // Tuinity start - rewrite this to use improved logic
+        // the old method shuffled the list and then tried to find the first element in it that
+        // matched predicate1, however we moved predicate1 into the poi search. This means we can avoid a shuffle
+        // entirely, and just pick a random element from list
+        if (list.isEmpty()) {
+            return Optional.empty();
+        }
+        return Optional.ofNullable(list.get(random.nextInt(list.size())).f());
+        // Tuinity end - rewrite this to use improved logic
     }
 
     public boolean b(BlockPosition blockposition) {
diff --git a/src/main/java/net/minecraft/world/level/chunk/storage/RegionFileSection.java b/src/main/java/net/minecraft/world/level/chunk/storage/RegionFileSection.java
index 3628b62b8021bbb9b03b3236830b6dccef2d8419..f70c14385c95763b5f270a6e2ce372cf047ba7bb 100644
--- a/src/main/java/net/minecraft/world/level/chunk/storage/RegionFileSection.java
+++ b/src/main/java/net/minecraft/world/level/chunk/storage/RegionFileSection.java
@@ -93,11 +93,12 @@ public class RegionFileSection<R> extends RegionFileCache implements AutoCloseab
     }
     // Tuinity end - actually unload POI data
 
-    @Nullable protected Optional<R> getIfLoaded(long value) { return this.c(value); } // Tuinity - OBFHELPER
+    @Nullable public final Optional<R> getIfLoaded(long value) { return this.c(value); } // Tuinity - OBFHELPER // Tuinity - OBFHELPER
     @Nullable protected Optional<R> c(long i) { // Tuinity - OBFHELPER
         return (Optional) this.c.get(i);
     }
 
+    public final Optional<R> getOrLoad(long coordinate) { return this.d(coordinate); } // Tuinity - OBFHELPER
     protected Optional<R> d(long i) {
         SectionPosition sectionposition = SectionPosition.a(i);
 
diff --git a/src/main/java/net/minecraft/world/level/portal/PortalTravelAgent.java b/src/main/java/net/minecraft/world/level/portal/PortalTravelAgent.java
index 77dfa7eaf178baa55041a829c9dec4851efeedfc..0de6225e11b98cc3d22c002d5f27748cde42a5a0 100644
--- a/src/main/java/net/minecraft/world/level/portal/PortalTravelAgent.java
+++ b/src/main/java/net/minecraft/world/level/portal/PortalTravelAgent.java
@@ -39,16 +39,34 @@ public class PortalTravelAgent {
         // int i = flag ? 16 : 128;
         // CraftBukkit end
 
-        villageplace.a(this.world, blockposition, i);
-        Optional<VillagePlaceRecord> optional = villageplace.b((villageplacetype) -> {
-            return villageplacetype == VillagePlaceType.v;
-        }, blockposition, i, VillagePlace.Occupancy.ANY).sorted(Comparator.comparingDouble((VillagePlaceRecord villageplacerecord) -> { // CraftBukkit - decompile error
-            return villageplacerecord.f().j(blockposition);
-        }).thenComparingInt((villageplacerecord) -> {
-            return villageplacerecord.f().getY();
-        })).filter((villageplacerecord) -> {
-            return this.world.getType(villageplacerecord.f()).b(BlockProperties.E);
-        }).findFirst();
+        // Tuinity start - optimise portals
+        //villageplace.a(this.world, blockposition, i);
+        Optional<VillagePlaceRecord> optional;
+        java.util.List<VillagePlaceRecord> records = new java.util.ArrayList<>();
+        com.tuinity.tuinity.util.PoiAccess.findClosestPoiDataRecords(villageplace,
+                (VillagePlaceType villageplacetype) -> {
+                    return villageplacetype == VillagePlaceType.v; // this should break this entire diff on update if it changes, so TODO check that on diff break
+                },
+                (BlockPosition pos) -> {
+                    net.minecraft.world.level.chunk.IChunkAccess lowest = this.world.getChunkAt(pos.getX() >> 4, pos.getZ() >> 4, net.minecraft.world.level.chunk.ChunkStatus.EMPTY);
+                    if (!lowest.getChunkStatus().isAtLeastStatus(net.minecraft.world.level.chunk.ChunkStatus.FULL)) {
+                        // why would we generate the chunk?
+                        return false;
+                    }
+                    return lowest.getType(pos).contains(BlockProperties.E); // this should break this entire diff on update if it changes, so TODO check that on diff break
+                }, blockposition, i, Double.MAX_VALUE, VillagePlace.Occupancy.ANY, true, records);
+        // this gets us most of the way there, but we bias towards lower y values.
+        VillagePlaceRecord lowestYRecord = null;
+        for (VillagePlaceRecord record : records) {
+            if (lowestYRecord == null) {
+                lowestYRecord = record;
+            } else if (lowestYRecord.getPosition().getY() > record.getPosition().getY()) {
+                lowestYRecord = record;
+            }
+        }
+        // now we're done
+        optional = Optional.ofNullable(lowestYRecord);
+        // Tuinity end - optimise portals
 
         return optional.map((villageplacerecord) -> {
             BlockPosition blockposition1 = villageplacerecord.f();
