From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Spottedleaf <spottedleaf@spottedleaf.dev>
Date: Thu, 9 Jul 2020 13:34:59 -0700
Subject: [PATCH] Optimise WorldServer#notify

Iterating over all of the navigators in the world is pretty expensive.
Instead, only iterate over navigators in the current region that are
eligible for repathing.

diff --git a/src/main/java/net/minecraft/server/level/ChunkMap.java b/src/main/java/net/minecraft/server/level/ChunkMap.java
index bc6a4bfe7df804ee22791fb767f059a541a3900f..a578ff8a88ef944516150303e96f8b49bc797f64 100644
--- a/src/main/java/net/minecraft/server/level/ChunkMap.java
+++ b/src/main/java/net/minecraft/server/level/ChunkMap.java
@@ -301,15 +301,81 @@ public class ChunkMap extends ChunkStorage implements ChunkHolder.PlayerProvider
     public final io.papermc.paper.chunk.SingleThreadChunkRegionManager dataRegionManager;
 
     public static final class DataRegionData implements io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionData {
+        // Paper start - optimise notify()
+        private io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet<Mob> navigators;
+
+        public io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet<Mob> getNavigators() {
+            return this.navigators;
+        }
+
+        public boolean addToNavigators(final Mob navigator) {
+            if (this.navigators == null) {
+                this.navigators = new io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet<>();
+            }
+            return this.navigators.add(navigator);
+        }
+
+        public boolean removeFromNavigators(final Mob navigator) {
+            if (this.navigators == null) {
+                return false;
+            }
+            return this.navigators.remove(navigator);
+        }
+        // Paper end - optimise notify()
     }
 
     public static final class DataRegionSectionData implements io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionSectionData {
 
+        // Paper start - optimise notify()
+        private io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet<Mob> navigators;
+
+        public io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet<Mob> getNavigators() {
+            return this.navigators;
+        }
+
+        public boolean addToNavigators(final io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionSection section, final Mob navigator) {
+            if (this.navigators == null) {
+                this.navigators = new io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet<>();
+            }
+            final boolean ret = this.navigators.add(navigator);
+            if (ret) {
+                final DataRegionData data = (DataRegionData)section.getRegion().regionData;
+                if (!data.addToNavigators(navigator)) {
+                    throw new IllegalStateException();
+                }
+            }
+            return ret;
+        }
+
+        public boolean removeFromNavigators(final io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionSection section, final Mob navigator) {
+            if (this.navigators == null) {
+                return false;
+            }
+            final boolean ret = this.navigators.remove(navigator);
+            if (ret) {
+                final DataRegionData data = (DataRegionData)section.getRegion().regionData;
+                if (!data.removeFromNavigators(navigator)) {
+                    throw new IllegalStateException();
+                }
+            }
+            return ret;
+        }
+        // Paper end - optimise notify()
+
         @Override
         public void removeFromRegion(final io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionSection section,
                                      final io.papermc.paper.chunk.SingleThreadChunkRegionManager.Region from) {
             final DataRegionSectionData sectionData = (DataRegionSectionData)section.sectionData;
             final DataRegionData fromData = (DataRegionData)from.regionData;
+            // Paper start - optimise notify()
+            if (sectionData.navigators != null) {
+                for (final Iterator<Mob> iterator = sectionData.navigators.unsafeIterator(io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet.ITERATOR_FLAG_SEE_ADDITIONS); iterator.hasNext();) {
+                    if (!fromData.removeFromNavigators(iterator.next())) {
+                        throw new IllegalStateException();
+                    }
+                }
+            }
+            // Paper end - optimise notify()
         }
 
         @Override
@@ -319,6 +385,15 @@ public class ChunkMap extends ChunkStorage implements ChunkHolder.PlayerProvider
             final DataRegionSectionData sectionData = (DataRegionSectionData)section.sectionData;
             final DataRegionData oldRegionData = oldRegion == null ? null : (DataRegionData)oldRegion.regionData;
             final DataRegionData newRegionData = (DataRegionData)newRegion.regionData;
+            // Paper start - optimise notify()
+            if (sectionData.navigators != null) {
+                for (final Iterator<Mob> iterator = sectionData.navigators.unsafeIterator(io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet.ITERATOR_FLAG_SEE_ADDITIONS); iterator.hasNext();) {
+                    if (!newRegionData.addToNavigators(iterator.next())) {
+                        throw new IllegalStateException();
+                    }
+                }
+            }
+            // Paper end - optimise notify()
         }
     }
 
diff --git a/src/main/java/net/minecraft/server/level/ServerLevel.java b/src/main/java/net/minecraft/server/level/ServerLevel.java
index 6053b7797cc4f6ed1ffbfc35048bab1a96586254..7922ecc1bfeb2d00a7a1bce0431efec4f8c57eec 100644
--- a/src/main/java/net/minecraft/server/level/ServerLevel.java
+++ b/src/main/java/net/minecraft/server/level/ServerLevel.java
@@ -1096,6 +1096,7 @@ public class ServerLevel extends Level implements WorldGenLevel {
     public void tickNonPassenger(Entity entity) {
         // Paper start - log detailed entity tick information
         io.papermc.paper.util.TickThread.ensureTickThread("Cannot tick an entity off-main");
+        if (!entity.isRemoved()) this.entityManager.updateNavigatorsInRegion(entity); // Paper - optimise notify
         try {
             if (currentlyTickingEntity.get() == null) {
                 currentlyTickingEntity.lazySet(entity);
@@ -1551,9 +1552,18 @@ public class ServerLevel extends Level implements WorldGenLevel {
 
         if (Shapes.joinIsNotEmpty(voxelshape, voxelshape1, BooleanOp.NOT_SAME)) {
             List<PathNavigation> list = new ObjectArrayList();
-            Iterator iterator = this.navigatingMobs.iterator();
+            // Paper start - optimise notify()
+            io.papermc.paper.chunk.SingleThreadChunkRegionManager.Region region = this.getChunkSource().chunkMap.dataRegionManager.getRegion(pos.getX() >> 4, pos.getZ() >> 4);
+            if (region == null) {
+                return;
+            }
+            io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet<Mob> navigatorsFromRegion = ((ChunkMap.DataRegionData)region.regionData).getNavigators();
+            if (navigatorsFromRegion == null) {
+                return;
+            }
+            io.papermc.paper.util.maplist.IteratorSafeOrderedReferenceSet.Iterator<Mob> iterator = navigatorsFromRegion.iterator();
 
-            while (iterator.hasNext()) {
+            try { while (iterator.hasNext()) { // Paper end - optimise notify()
                 // CraftBukkit start - fix SPIGOT-6362
                 Mob entityinsentient;
                 try {
@@ -1575,16 +1585,23 @@ public class ServerLevel extends Level implements WorldGenLevel {
 
             try {
                 this.isUpdatingNavigations = true;
-                iterator = list.iterator();
+                // Paper start - optimise notify()
+                Iterator<PathNavigation> navigationIterator = list.iterator();
 
-                while (iterator.hasNext()) {
-                    PathNavigation navigationabstract1 = (PathNavigation) iterator.next();
+                while (navigationIterator.hasNext()) {
+                    PathNavigation navigationabstract1 = navigationIterator.next();
+                    // Paper end - optimise notify()
 
                     navigationabstract1.recomputePath();
                 }
             } finally {
                 this.isUpdatingNavigations = false;
             }
+            // Paper start - optimise notify()
+            } finally {
+                iterator.finishedIterating();
+            }
+            // Paper end - optimise notify()
 
         }
         } // Paper
@@ -2384,10 +2401,12 @@ public class ServerLevel extends Level implements WorldGenLevel {
 
         public void onTickingStart(Entity entity) {
             ServerLevel.this.entityTickList.add(entity);
+            ServerLevel.this.entityManager.addNavigatorsIfPathingToRegion(entity); // Paper - optimise notify
         }
 
         public void onTickingEnd(Entity entity) {
             ServerLevel.this.entityTickList.remove(entity);
+            ServerLevel.this.entityManager.removeNavigatorsFromData(entity); // Paper - optimise notify
             // Paper start - Reset pearls when they stop being ticked
             if (paperConfig.disableEnderpearlExploit && entity instanceof net.minecraft.world.entity.projectile.ThrownEnderpearl pearl) {
                 pearl.cachedOwner = null;
diff --git a/src/main/java/net/minecraft/world/entity/ai/navigation/PathNavigation.java b/src/main/java/net/minecraft/world/entity/ai/navigation/PathNavigation.java
index b47cd6d8ed02875bd9af54d27b7c1cda340e7f9f..d35032a8d2612d555c3dad1fe496d7ae1c5a285b 100644
--- a/src/main/java/net/minecraft/world/entity/ai/navigation/PathNavigation.java
+++ b/src/main/java/net/minecraft/world/entity/ai/navigation/PathNavigation.java
@@ -27,7 +27,7 @@ import net.minecraft.world.phys.Vec3;
 
 public abstract class PathNavigation {
     private static final int MAX_TIME_RECOMPUTE = 20;
-    protected final Mob mob;
+    protected final Mob mob; public final Mob getEntity() { return this.mob; } // Paper - public accessor
     protected final Level level;
     @Nullable
     protected Path path;
@@ -40,7 +40,7 @@ public abstract class PathNavigation {
     protected long lastTimeoutCheck;
     protected double timeoutLimit;
     protected float maxDistanceToWaypoint = 0.5F;
-    protected boolean hasDelayedRecomputation;
+    protected boolean hasDelayedRecomputation; protected final boolean needsPathRecalculation() { return this.hasDelayedRecomputation; } // Paper - public accessor
     protected long timeLastRecompute;
     protected NodeEvaluator nodeEvaluator;
     @Nullable
@@ -50,6 +50,13 @@ public abstract class PathNavigation {
     public final PathFinder pathFinder;
     private boolean isStuck;
 
+    // Paper start
+    public boolean isViableForPathRecalculationChecking() {
+        return !this.needsPathRecalculation() &&
+                (this.path != null && !this.path.isDone() && this.path.getNodeCount() != 0);
+    }
+    // Paper end
+
     public PathNavigation(Mob mob, Level world) {
         this.mob = mob;
         this.level = world;
@@ -413,7 +420,7 @@ public abstract class PathNavigation {
     public boolean shouldRecomputePath(BlockPos pos) {
         if (this.hasDelayedRecomputation) {
             return false;
-        } else if (this.path != null && !this.path.isDone() && this.path.getNodeCount() != 0) {
+        } else if (this.path != null && !this.path.isDone() && this.path.getNodeCount() != 0) { // Paper - diff on change - needed for isViableForPathRecalculationChecking()
             Node node = this.path.getEndNode();
             Vec3 vec3 = new Vec3(((double)node.x + this.mob.getX()) / 2.0D, ((double)node.y + this.mob.getY()) / 2.0D, ((double)node.z + this.mob.getZ()) / 2.0D);
             return pos.closerToCenterThan(vec3, (double)(this.path.getNodeCount() - this.path.getNextNodeIndex()));
diff --git a/src/main/java/net/minecraft/world/level/entity/PersistentEntitySectionManager.java b/src/main/java/net/minecraft/world/level/entity/PersistentEntitySectionManager.java
index 5029ab81e3d7943a001b6367083eb511ce7d3572..db2ef605bccbb9024f787cd58f3cb93df03d5532 100644
--- a/src/main/java/net/minecraft/world/level/entity/PersistentEntitySectionManager.java
+++ b/src/main/java/net/minecraft/world/level/entity/PersistentEntitySectionManager.java
@@ -71,6 +71,65 @@ public class PersistentEntitySectionManager<T extends EntityAccess> implements A
     }
     // CraftBukkit end
 
+    // Paper start - optimise notify()
+    public final void removeNavigatorsFromData(Entity entity, final int chunkX, final int chunkZ) {
+        if (!(entity instanceof net.minecraft.world.entity.Mob)) {
+            return;
+        }
+        io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionSection section =
+            this.entitySliceManager.world.getChunkSource().chunkMap.dataRegionManager.getRegionSection(chunkX, chunkZ);
+        if (section != null) {
+            net.minecraft.server.level.ChunkMap.DataRegionSectionData sectionData = (net.minecraft.server.level.ChunkMap.DataRegionSectionData)section.sectionData;
+            sectionData.removeFromNavigators(section, ((net.minecraft.world.entity.Mob)entity));
+        }
+    }
+
+    public final void removeNavigatorsFromData(Entity entity) {
+        if (!(entity instanceof net.minecraft.world.entity.Mob)) {
+            return;
+        }
+        BlockPos entityPos = entity.blockPosition();
+        io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionSection section =
+            this.entitySliceManager.world.getChunkSource().chunkMap.dataRegionManager.getRegionSection(entityPos.getX() >> 4, entityPos.getZ() >> 4);
+        if (section != null) {
+            net.minecraft.server.level.ChunkMap.DataRegionSectionData sectionData = (net.minecraft.server.level.ChunkMap.DataRegionSectionData)section.sectionData;
+            sectionData.removeFromNavigators(section, ((net.minecraft.world.entity.Mob)entity));
+        }
+    }
+
+    public final void addNavigatorsIfPathingToRegion(Entity entity) {
+        if (!(entity instanceof net.minecraft.world.entity.Mob)) {
+            return;
+        }
+        BlockPos entityPos = entity.blockPosition();
+        io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionSection section =
+            this.entitySliceManager.world.getChunkSource().chunkMap.dataRegionManager.getRegionSection(entityPos.getX() >> 4, entityPos.getZ() >> 4);
+        if (section != null) {
+            net.minecraft.server.level.ChunkMap.DataRegionSectionData sectionData = (net.minecraft.server.level.ChunkMap.DataRegionSectionData)section.sectionData;
+            if (((net.minecraft.world.entity.Mob)entity).getNavigation().isViableForPathRecalculationChecking()) {
+                sectionData.addToNavigators(section, ((net.minecraft.world.entity.Mob)entity));
+            }
+        }
+    }
+
+    public final void updateNavigatorsInRegion(Entity entity) {
+        if (!(entity instanceof net.minecraft.world.entity.Mob)) {
+            return;
+        }
+        BlockPos entityPos = entity.blockPosition();
+        io.papermc.paper.chunk.SingleThreadChunkRegionManager.RegionSection section =
+            this.entitySliceManager.world.getChunkSource().chunkMap.dataRegionManager.getRegionSection(entityPos.getX() >> 4, entityPos.getZ() >> 4);
+        if (section != null) {
+            net.minecraft.server.level.ChunkMap.DataRegionSectionData sectionData = (net.minecraft.server.level.ChunkMap.DataRegionSectionData)section.sectionData;
+            if (((net.minecraft.world.entity.Mob)entity).getNavigation().isViableForPathRecalculationChecking()) {
+                sectionData.addToNavigators(section, ((net.minecraft.world.entity.Mob)entity));
+            } else {
+                sectionData.removeFromNavigators(section, ((net.minecraft.world.entity.Mob)entity));
+            }
+        }
+    }
+    // Paper end - optimise notify()
+
     void removeSectionIfEmpty(long sectionPos, EntitySection<T> section) {
         if (section.isEmpty()) {
             this.sectionStorage.remove(sectionPos);
@@ -456,11 +515,25 @@ public class PersistentEntitySectionManager<T extends EntityAccess> implements A
         @Override
         public void onMove() {
             BlockPos blockposition = this.entity.blockPosition();
-            long i = SectionPos.asLong(blockposition);
+            long i = SectionPos.asLong(blockposition); final long newSectionPos = i; // Paper - diff on change, new position section
 
             if (i != this.currentSectionKey) {
                 PersistentEntitySectionManager.this.entitySliceManager.moveEntity((Entity)this.entity); // Paper
-                Visibility visibility = this.currentSection.getStatus();
+                Visibility visibility = this.currentSection.getStatus(); final Visibility oldVisibility = visibility; // Paper - diff on change - this should be OLD section visibility
+                // Paper start
+                int shift = PersistentEntitySectionManager.this.entitySliceManager.world.getChunkSource().chunkMap.dataRegionManager.regionChunkShift;
+                int oldChunkX = io.papermc.paper.util.CoordinateUtils.getChunkSectionX(this.currentSectionKey);
+                int oldChunkZ = io.papermc.paper.util.CoordinateUtils.getChunkSectionZ(this.currentSectionKey);
+                int oldRegionX = oldChunkX >> shift;
+                int oldRegionZ = oldChunkZ >> shift;
+
+                int newRegionX = io.papermc.paper.util.CoordinateUtils.getChunkSectionX(newSectionPos) >> shift;
+                int newRegionZ = io.papermc.paper.util.CoordinateUtils.getChunkSectionZ(newSectionPos) >> shift;
+
+                if (oldRegionX != newRegionX || oldRegionZ != newRegionZ) {
+                    PersistentEntitySectionManager.this.removeNavigatorsFromData((Entity)this.entity, oldChunkX, oldChunkZ);
+                }
+                // Paper end
 
                 if (!this.currentSection.remove(this.entity)) {
                     PersistentEntitySectionManager.LOGGER.warn("Entity {} wasn't found in section {} (moving to {})", new Object[]{this.entity, SectionPos.of(this.currentSectionKey), i});
@@ -472,6 +545,11 @@ public class PersistentEntitySectionManager<T extends EntityAccess> implements A
                 entitysection.add(this.entity);
                 this.currentSection = entitysection;
                 this.currentSectionKey = i;
+                // Paper start
+                if ((oldRegionX != newRegionX || oldRegionZ != newRegionZ) && oldVisibility.isTicking() && entitysection.getStatus().isTicking()) {
+                    PersistentEntitySectionManager.this.addNavigatorsIfPathingToRegion((Entity)this.entity);
+                }
+                // Paper end
                 this.updateStatus(visibility, entitysection.getStatus());
             }
 
