Implement player entity collision pushing.

core/src/main/java/org/geysermc/geyser/entity/type/Entity.java

@@ -412,6 +412,16 @@ public void setFlags(ByteEntityMetadata entityMetadata) {
         setInvisible((xd & 0x20) == 0x20);
     }
 
+    /**
+     * Ensure that collision is possible if this entity is a player. Should be overridden for players as their team
+     * might change the status of this.
+     * @param session the Bedrock client session
+     * @return if this entity can collide with the session's player
+     */
+    public boolean isPushable(GeyserSession session) {
+        return false;
+    }
+
     /**
      * Set a boolean - whether the entity is invisible or visible
      *

core/src/main/java/org/geysermc/geyser/entity/type/LivingEntity.java

@@ -46,9 +46,14 @@
 import org.geysermc.geyser.entity.vehicle.HappyGhastVehicleComponent;
 import org.geysermc.geyser.inventory.GeyserItemStack;
 import org.geysermc.geyser.item.Items;
+import org.geysermc.geyser.level.block.Blocks;
+import org.geysermc.geyser.level.block.property.Properties;
+import org.geysermc.geyser.level.block.type.BlockState;
+import org.geysermc.geyser.level.block.type.TrapDoorBlock;
 import org.geysermc.geyser.registry.type.ItemMapping;
 import org.geysermc.geyser.scoreboard.Team;
 import org.geysermc.geyser.session.GeyserSession;
+import org.geysermc.geyser.session.cache.tags.BlockTag;
 import org.geysermc.geyser.translator.item.ItemTranslator;
 import org.geysermc.geyser.util.AttributeUtils;
 import org.geysermc.geyser.util.EntityUtils;
@@ -278,6 +283,29 @@ public void setParticles(ObjectEntityMetadata<List<Particle>> entityMetadata) {
         }
     }
 
+    public boolean isOnClimbableBlock() {
+        Vector3i blockPos = this.position.toInt();
+        BlockState state = session.getGeyser().getWorldManager().blockAt(session, blockPos);
+        if (session.getTagCache().is(BlockTag.CLIMBABLE, state.block())) {
+            return true;
+        }
+
+        if (state.block() instanceof TrapDoorBlock) {
+            if (!state.getValue(Properties.OPEN)) {
+                return false;
+            } else {
+                BlockState belowState = session.getGeyser().getWorldManager().blockAt(session, blockPos.down());
+                return belowState.is(Blocks.LADDER) && belowState.getValue(Properties.HORIZONTAL_FACING) == state.getValue(Properties.HORIZONTAL_FACING);
+            }
+        }
+        return false;
+    }
+
+    @Override
+    public boolean isPushable(GeyserSession session) {
+        return this.getFlag(EntityFlag.HAS_GRAVITY) && this.isAlive() && !this.isOnClimbableBlock();
+    }
+
     protected boolean hasShield(boolean offhand) {
         ItemMapping shieldMapping = session.getItemMappings().getStoredItems().shield();
         if (offhand) {

core/src/main/java/org/geysermc/geyser/entity/type/living/animal/horse/AbstractHorseEntity.java

@@ -296,4 +296,9 @@ protected boolean canUseSlot(EquipmentSlot slot) {
             return isAlive() && !isBaby() && getFlag(EntityFlag.TAMED);
         }
     }
+
+    @Override
+    public boolean isPushable(GeyserSession session) {
+        return this.getPassengers().isEmpty();
+    }
 }

core/src/main/java/org/geysermc/geyser/entity/type/living/monster/WardenEntity.java

@@ -71,6 +71,11 @@ public void setAngerLevel(IntEntityMetadata entityMetadata) {
         dirtyMetadata.put(EntityDataTypes.HEARTBEAT_INTERVAL_TICKS, heartBeatDelay);
     }
 
+    @Override
+    public boolean isPushable(GeyserSession session) {
+        return super.isPushable(session) && !getFlag(EntityFlag.DIGGING) && !getFlag(EntityFlag.EMERGING);
+    }
+
     @Override
     public void tick() {
         if (++tickCount % heartBeatDelay == 0 && !silent) {

core/src/main/java/org/geysermc/geyser/entity/type/player/PlayerEntity.java

@@ -52,6 +52,7 @@
 import org.geysermc.geyser.entity.type.LivingEntity;
 import org.geysermc.geyser.entity.type.living.animal.tameable.ParrotEntity;
 import org.geysermc.geyser.level.block.Blocks;
+import org.geysermc.geyser.scoreboard.Team;
 import org.geysermc.geyser.session.GeyserSession;
 import org.geysermc.geyser.util.ChunkUtils;
 import org.geysermc.mcprotocollib.protocol.data.game.entity.metadata.EntityMetadata;
@@ -295,6 +296,23 @@ public void setPosition(Vector3f position) {
         return bedPosition;
     }
 
+    @Override
+    public boolean isPushable(GeyserSession session) {
+        boolean canCollide = super.isPushable(session);
+        if (this instanceof SessionPlayerEntity) { // This is the session player, don't check for team collision rule.
+            return canCollide;
+        }
+
+        Team team = session.getWorldCache().getScoreboard().getTeamFor(username);
+        if (team == null) return true;
+        return switch (team.collisionRule()) {
+            case NEVER -> false;
+            case PUSH_OWN_TEAM -> canCollide && team.hasEntity(session.getPlayerEntity().getUsername());
+            case PUSH_OTHER_TEAMS -> canCollide && !team.hasEntity(session.getPlayerEntity().getUsername());
+            default -> canCollide;
+        };
+    }
+
     public void setAbsorptionHearts(FloatEntityMetadata entityMetadata) {
         // Extra hearts - is not metadata but an attribute on Bedrock
         UpdateAttributesPacket attributesPacket = new UpdateAttributesPacket();

core/src/main/java/org/geysermc/geyser/entity/type/player/SessionPlayerEntity.java

@@ -476,11 +476,12 @@ public float getJumpVelocity() {
         return velocity + 0.1F * session.getEffectCache().getJumpPower();
     }
 
+    @Override
     public boolean isOnClimbableBlock() {
         if (session.getGameMode() == GameMode.SPECTATOR) {
             return false;
         }
-        Vector3i pos = getPosition().down(EntityDefinitions.PLAYER.offset()).toInt();
+        Vector3i pos = this.position().toInt();
         BlockState state = session.getGeyser().getWorldManager().blockAt(session, pos);
         if (session.getTagCache().is(BlockTag.CLIMBABLE, state.block())) {
             return true;

core/src/main/java/org/geysermc/geyser/scoreboard/Scoreboard.java

@@ -41,6 +41,7 @@
 import org.geysermc.geyser.scoreboard.display.slot.PlayerlistDisplaySlot;
 import org.geysermc.geyser.scoreboard.display.slot.SidebarDisplaySlot;
 import org.geysermc.geyser.session.GeyserSession;
+import org.geysermc.mcprotocollib.protocol.data.game.scoreboard.CollisionRule;
 import org.geysermc.mcprotocollib.protocol.data.game.scoreboard.NameTagVisibility;
 import org.geysermc.mcprotocollib.protocol.data.game.scoreboard.ScoreboardPosition;
 import org.geysermc.mcprotocollib.protocol.data.game.scoreboard.TeamColor;
@@ -173,7 +174,8 @@ public void registerNewTeam(
         Component prefix,
         Component suffix,
         NameTagVisibility visibility,
-        TeamColor color
+        TeamColor color,
+        CollisionRule collisionRule
     ) {
         Team team = teams.get(teamName);
         if (team != null) {
@@ -183,7 +185,7 @@ public void registerNewTeam(
             return;
         }
 
-        team = new Team(this, teamName, players, name, prefix, suffix, visibility, color);
+        team = new Team(this, teamName, players, name, prefix, suffix, visibility, color, collisionRule);
         teams.put(teamName, team);
 
         // Update command parameters - is safe to send even if the command enum doesn't exist on the client (as of 1.19.51)

core/src/main/java/org/geysermc/geyser/scoreboard/Team.java

@@ -34,6 +34,7 @@
 import org.geysermc.geyser.session.GeyserSession;
 import org.geysermc.geyser.text.ChatColor;
 import org.geysermc.geyser.translator.text.MessageTranslator;
+import org.geysermc.mcprotocollib.protocol.data.game.scoreboard.CollisionRule;
 import org.geysermc.mcprotocollib.protocol.data.game.scoreboard.NameTagVisibility;
 import org.geysermc.mcprotocollib.protocol.data.game.scoreboard.TeamColor;
 
@@ -48,6 +49,7 @@ public final class Team {
     private final Set<Entity> managedEntities;
     @NonNull private NameTagVisibility nameTagVisibility = NameTagVisibility.ALWAYS;
     private TeamColor color;
+    private CollisionRule collisionRule;
 
     private String name;
     private String prefix;
@@ -62,14 +64,17 @@ public Team(
         Component prefix,
         Component suffix,
         NameTagVisibility visibility,
-        TeamColor color
+        TeamColor color,
+        CollisionRule collisionRule
     ) {
         this.scoreboard = scoreboard;
         this.id = id;
         this.entities = new ObjectOpenHashSet<>();
         this.managedEntities = new ObjectOpenHashSet<>();
         this.lastUpdate = LAST_UPDATE_DEFAULT;
 
+        this.collisionRule = collisionRule;
+
         // doesn't call entity update
         updateProperties(name, prefix, suffix, visibility, color);
         // calls entity update
@@ -316,6 +321,10 @@ public TeamColor color() {
         return color;
     }
 
+    public CollisionRule collisionRule() {
+        return collisionRule;
+    }
+
     public long lastUpdate() {
         return lastUpdate;
     }

core/src/main/java/org/geysermc/geyser/translator/protocol/bedrock/entity/player/input/BedrockMovePlayer.java

@@ -25,15 +25,22 @@
 
 package org.geysermc.geyser.translator.protocol.bedrock.entity.player.input;
 
+import org.cloudburstmc.math.GenericMath;
 import org.cloudburstmc.math.vector.Vector3d;
 import org.cloudburstmc.math.vector.Vector3f;
 import org.cloudburstmc.protocol.bedrock.data.PlayerAuthInputData;
+import org.cloudburstmc.protocol.bedrock.data.entity.EntityFlag;
 import org.cloudburstmc.protocol.bedrock.packet.PlayerAuthInputPacket;
+import org.cloudburstmc.protocol.bedrock.packet.SetEntityMotionPacket;
 import org.geysermc.geyser.entity.EntityDefinitions;
+import org.geysermc.geyser.entity.type.Entity;
+import org.geysermc.geyser.entity.type.player.PlayerEntity;
 import org.geysermc.geyser.entity.type.player.SessionPlayerEntity;
+import org.geysermc.geyser.level.physics.BoundingBox;
 import org.geysermc.geyser.level.physics.CollisionResult;
 import org.geysermc.geyser.session.GeyserSession;
 import org.geysermc.geyser.text.ChatColor;
+import org.geysermc.geyser.util.MathUtils;
 import org.geysermc.mcprotocollib.network.packet.Packet;
 import org.geysermc.mcprotocollib.protocol.packet.ingame.serverbound.player.ServerboundMovePlayerPosPacket;
 import org.geysermc.mcprotocollib.protocol.packet.ingame.serverbound.player.ServerboundMovePlayerPosRotPacket;
@@ -219,6 +226,42 @@ static void translate(GeyserSession session, PlayerAuthInputPacket packet) {
         if (entity.getRightParrot() != null) {
             entity.getRightParrot().moveAbsolute(entity.getPosition(), entity.getYaw(), entity.getPitch(), entity.getHeadYaw(), true, false);
         }
+
+        // If player is inside a vehicle or player or the player itself CAN'T be push then don't do any kind of pushing.
+        if (entity.getVehicle() == null && entity.isPushable(session)) {
+            // TODO: Is looping through every entities a good idea?
+            for (Entity entity1 : session.getEntityCache().getEntities().values()) {
+                final BoundingBox entityBoundingBox = new BoundingBox(0, 0, 0, entity1.getBoundingBoxWidth(), entity1.getBoundingBoxHeight(), entity1.getBoundingBoxWidth());
+                entityBoundingBox.translate(entity1 instanceof PlayerEntity playerEntity ? playerEntity.position().toDouble() : entity1.getPosition().toDouble());
+
+                // If this entity can't collide with the player or isn't near the player or is the player itself, continue.
+                if (entity1 == entity || !entityBoundingBox.checkIntersection(session.getCollisionManager().getPlayerBoundingBox()) || !entity1.isPushable(session)) {
+                    continue;
+                }
+
+                float d = entity.getPosition().getX() - entity1.getPosition().getX();
+                float e = entity.getPosition().getZ() - entity1.getPosition().getZ();
+                float f = MathUtils.absMax(d, e);
+
+                Vector3f vector3f = Vector3f.from(d, 0, e);
+                if (f >= 0.01f) {
+                    f = (float) GenericMath.sqrt(f);
+                    float g = Math.min(1 / f, 1);
+
+                    vector3f = vector3f.div(f);
+                    vector3f = vector3f.mul(g);
+                    vector3f = vector3f.mul(0.05F);
+
+                    // The push motion should be relative to the current motion, we don't want player to fly by sending y vel 0.
+                    entity.setMotion(entity.getMotion().add(vector3f));
+
+                    SetEntityMotionPacket motionPacket = new SetEntityMotionPacket();
+                    motionPacket.setRuntimeEntityId(entity.getGeyserId());
+                    motionPacket.setMotion(entity.getMotion());
+                    session.sendUpstreamPacket(motionPacket);
+                }
+            }
+        }
     }
 
     private static boolean isInvalidNumber(float val) {

core/src/main/java/org/geysermc/geyser/translator/protocol/java/scoreboard/JavaSetPlayerTeamTranslator.java

@@ -63,7 +63,8 @@ public void translate(GeyserSession session, ClientboundSetPlayerTeamPacket pack
                 packet.getPrefix(),
                 packet.getSuffix(),
                 packet.getNameTagVisibility(),
-                packet.getColor()
+                packet.getColor(),
+                packet.getCollisionRule()
             );
         } else {
             Team team = scoreboard.getTeam(packet.getTeamName());

core/src/main/java/org/geysermc/geyser/util/MathUtils.java

@@ -28,6 +28,23 @@
 public class MathUtils {
     public static final double SQRT_OF_TWO = Math.sqrt(2);
 
+    /**
+     * Find the absolute value of 2 number and find what the larger number is.
+     *
+     * @param d The first value
+     * @param e The second value
+     * @return The absolute value of the number that is larger when it's absolute.
+     */
+    public static float absMax(float d, float e) {
+        if (d < 0.0) {
+            d = -d;
+        }
+        if (e < 0.0) {
+            e = -e;
+        }
+        return Math.max(d, e);
+    }
+
     /**
      * Wrap the given float degrees to be between -180.0 and 180.0.
      *