feat: More works on entity physics

AllayMC · Aug 5, 2023 · afca77c · afca77c
1 parent aa17e8e
commit afca77c
Show file tree

Hide file tree

Showing 12 changed files with 142 additions and 36 deletions.
diff --git a/Allay-API/src/main/java/cn/allay/api/datastruct/aabbtree/AABBTree.java b/Allay-API/src/main/java/cn/allay/api/datastruct/aabbtree/AABBTree.java
@@ -1,5 +1,6 @@
 package cn.allay.api.datastruct.aabbtree;
 
+import it.unimi.dsi.fastutil.objects.ObjectArrayList;
 import lombok.Getter;
 import org.joml.FrustumIntersection;
 import org.joml.Matrix4fc;
@@ -39,7 +40,7 @@ public AABBTree() {
     }
 
     public AABBTree(AABBTreeHeuristicFunction<T> insertionHeuristicFunction, double fatAABBMargin) {
-        nodes = new ArrayList<>();
+        nodes = new ObjectArrayList<>();
         root = AABBTreeNode.INVALID_NODE_INDEX;
         this.insertionHeuristicFunction = insertionHeuristicFunction;
         if (this.insertionHeuristicFunction == null) {
@@ -358,12 +359,13 @@ public void remove(T object) {
         syncUpHierarchy(nodeGrandparent);
     }
 
-    public void detectOverlaps(AABBd overlapWith, List<T> result) {
+    public void detectOverlaps(AABBdc overlapWith, List<T> result) {
         detectOverlaps(overlapWith, defaultAABBOverlapFilter, result);
     }
 
-    public void detectOverlaps(AABBd overlapWith, AABBOverlapFilter<T> filter, List<T> result) {
-        traverseTree(aabb -> aabb.intersectsAABB(overlapWith), filter, result);
+    public void detectOverlaps(AABBdc overlapWith, AABBOverlapFilter<T> filter, List<T> result) {
+        var copy = new AABBd(overlapWith);
+        traverseTree(aabb -> aabb.intersectsAABB(copy), filter, result);
     }
 
     public void detectCollisionPairs(List<CollisionPair<T>> result) {

diff --git a/Allay-API/src/main/java/cn/allay/api/datastruct/aabbtree/AABBTreeNode.java b/Allay-API/src/main/java/cn/allay/api/datastruct/aabbtree/AABBTreeNode.java
@@ -2,7 +2,6 @@
 
 import lombok.Getter;
 import org.joml.primitives.AABBd;
-import org.joml.primitives.AABBf;
 
 public final class AABBTreeNode<E extends HasAABB> {
     public static final int INVALID_NODE_INDEX = -1;
@@ -55,7 +54,7 @@ void computeAABBWithMargin(double margin) {
         if (data == null) {
             return;
         }
-        AABBd dataAABB = data.copyAABBTo(aabb);
+        AABBd dataAABB = data.copyOffsetAABBTo(aabb);
         aabb.setMin(dataAABB.minX - margin, dataAABB.minY - margin, dataAABB.minZ - margin);
         aabb.setMax(dataAABB.maxX + margin, dataAABB.maxY + margin, dataAABB.maxZ + margin);
     }

diff --git a/Allay-API/src/main/java/cn/allay/api/datastruct/aabbtree/HasAABB.java b/Allay-API/src/main/java/cn/allay/api/datastruct/aabbtree/HasAABB.java
@@ -8,5 +8,5 @@
  */
 public interface HasAABB {
 
-    AABBd copyAABBTo(AABBd dest);
+    AABBd copyOffsetAABBTo(AABBd dest);
 }
diff --git a/Allay-API/src/main/java/cn/allay/api/entity/Entity.java b/Allay-API/src/main/java/cn/allay/api/entity/Entity.java
@@ -16,8 +16,8 @@ public interface Entity extends
         HasAABB, HasLongId {
 
     @Override
-    default AABBd copyAABBTo(AABBd dest) {
-        var aabb = getAABB();
+    default AABBd copyOffsetAABBTo(AABBd dest) {
+        var aabb = getOffsetAABB();
         dest.setMin(aabb.minX(), aabb.minY(), aabb.minZ());
         dest.setMax(aabb.maxX(), aabb.maxY(), aabb.maxZ());
         return dest;

diff --git a/Allay-API/src/main/java/cn/allay/api/entity/component/impl/base/EntityBaseComponent.java b/Allay-API/src/main/java/cn/allay/api/entity/component/impl/base/EntityBaseComponent.java
@@ -7,12 +7,14 @@
 import cn.allay.api.math.Location3dc;
 import cn.allay.api.client.Client;
 import org.cloudburstmc.protocol.bedrock.packet.BedrockPacket;
+import org.cloudburstmc.protocol.bedrock.packet.MoveEntityDeltaPacket;
 import org.jetbrains.annotations.UnmodifiableView;
 import org.joml.Vector3dc;
 import org.joml.primitives.AABBd;
 import org.joml.primitives.AABBdc;
 
 import java.util.Map;
+import java.util.Set;
 
 /**
  * Allay Project 2023/5/26
@@ -82,7 +84,17 @@ public interface EntityBaseComponent {
     void sendPacketToViewersImmediately(BedrockPacket packet);
 
     @Inject
+    void broadcastMoveToViewers(Set<MoveEntityDeltaPacket.Flag> moveFlags, Location3dc newLoc);
+
+    default boolean enableHeadYaw() {
+        return false;
+    }
+
     default double getBaseOffset() {
         return 0;
     }
+
+    default AABBdc getOffsetAABB() {
+        return new AABBd(getAABB()).translate(getLocation());
+    }
 }
diff --git a/Allay-API/src/main/java/cn/allay/api/entity/component/impl/base/EntityBaseComponentImpl.java b/Allay-API/src/main/java/cn/allay/api/entity/component/impl/base/EntityBaseComponentImpl.java
@@ -20,10 +20,7 @@
 import org.cloudburstmc.protocol.bedrock.data.entity.EntityDataType;
 import org.cloudburstmc.protocol.bedrock.data.entity.EntityDataTypes;
 import org.cloudburstmc.protocol.bedrock.data.entity.EntityFlag;
-import org.cloudburstmc.protocol.bedrock.packet.AddEntityPacket;
-import org.cloudburstmc.protocol.bedrock.packet.BedrockPacket;
-import org.cloudburstmc.protocol.bedrock.packet.RemoveEntityPacket;
-import org.cloudburstmc.protocol.bedrock.packet.SetEntityDataPacket;
+import org.cloudburstmc.protocol.bedrock.packet.*;
 import org.jetbrains.annotations.UnmodifiableView;
 import org.joml.Vector3d;
 import org.joml.Vector3dc;
@@ -32,6 +29,7 @@
 
 import java.util.Collections;
 import java.util.Map;
+import java.util.Set;
 import java.util.concurrent.atomic.AtomicLong;
 import java.util.function.Function;
 
@@ -271,4 +269,19 @@ public void sendPacketToViewers(BedrockPacket packet) {
     public void sendPacketToViewersImmediately(BedrockPacket packet) {
         viewers.values().forEach(client -> client.sendPacketImmediately(packet));
     }
+
+    @Override
+    @Impl
+    public void broadcastMoveToViewers(Set<MoveEntityDeltaPacket.Flag> moveFlags, Location3dc newLoc) {
+        var pk = new MoveEntityDeltaPacket();
+        pk.setRuntimeEntityId(getUniqueId());
+        pk.getFlags().addAll(moveFlags);
+        pk.setX((float) newLoc.x());
+        pk.setY((float) newLoc.y());
+        pk.setZ((float) newLoc.z());
+        pk.setPitch((float) newLoc.pitch());
+        pk.setYaw((float) newLoc.yaw());
+        pk.setHeadYaw((float) newLoc.headYaw());
+        sendPacketToViewers(pk);
+    }
 }
diff --git a/...-API/src/main/java/cn/allay/api/entity/component/impl/base/EntityPlayerBaseComponent.java b/...-API/src/main/java/cn/allay/api/entity/component/impl/base/EntityPlayerBaseComponent.java
@@ -1,6 +1,7 @@
 package cn.allay.api.entity.component.impl.base;
 
 import cn.allay.api.client.Client;
+import cn.allay.api.component.annotation.Impl;
 import cn.allay.api.component.annotation.Inject;
 import cn.allay.api.entity.component.impl.base.EntityBaseComponent;
 
@@ -37,4 +38,14 @@ public interface EntityPlayerBaseComponent extends EntityBaseComponent {
 
     @Inject
     boolean isCrawling();
+
+    @Override
+    default double getBaseOffset() {
+        return 1.62;
+    }
+
+    @Override
+    default boolean enableHeadYaw() {
+        return true;
+    }
 }
diff --git a/Allay-API/src/main/java/cn/allay/api/entity/impl/EntityPlayer.java b/Allay-API/src/main/java/cn/allay/api/entity/impl/EntityPlayer.java
@@ -201,12 +201,6 @@ public void setCrawling(boolean crawling) {
         public boolean isCrawling() {
             return crawling;
         }
-
-        @Override
-        @Impl
-        public double getBaseOffset() {
-            return 1.62;
-        }
     }
 }
 
diff --git a/Allay-API/src/main/java/cn/allay/api/world/entity/EntityPhysicsService.java b/Allay-API/src/main/java/cn/allay/api/world/entity/EntityPhysicsService.java
@@ -26,11 +26,13 @@ public interface EntityPhysicsService {
 
     void offerScheduledMove(Entity entity, Location3dc newLoc);
 
-    default List<Entity> getCollidingEntities(Entity entity) {
+    default List<Entity> computeCollidingEntities(Entity entity) {
         if (entity.hasCollision())
-            return getCollidingEntities(entity.getAABB());
+            return computeCollidingEntities(entity.getOffsetAABB());
         else return Collections.emptyList();
     }
 
-    List<Entity> getCollidingEntities(AABBdc aabb);
+    List<Entity> computeCollidingEntities(AABBdc aabb);
+
+    List<Entity> getCachedEntityCollidingResult(Entity entity);
 }
diff --git a/Allay-Server/src/jmh/java/cn/allay/server/AABBTreeJMHTest.java b/Allay-Server/src/jmh/java/cn/allay/server/AABBTreeJMHTest.java
@@ -3,7 +3,6 @@
 import cn.allay.api.datastruct.aabbtree.AABBTree;
 import cn.allay.api.datastruct.aabbtree.TestEntity;
 import org.joml.primitives.AABBd;
-import org.joml.primitives.AABBf;
 import org.openjdk.jmh.annotations.*;
 
 import java.util.ArrayList;
@@ -41,7 +40,7 @@ public void init() {
         }
         testEntityAABBs = new AABBd[TEST_ENTITY_COUNT];
         for (int i = 0; i < TEST_ENTITY_COUNT; i++) {
-            testEntityAABBs[i] = testEntities[i].copyAABBTo(null);
+            testEntityAABBs[i] = testEntities[i].copyOffsetAABBTo(null);
         }
         testAABBs = new AABBd[TEST_ENTITY_COUNT];
         for (int i = 0; i < TEST_ENTITY_COUNT; i++) {

diff --git a/Allay-Server/src/main/java/cn/allay/server/world/entity/AllayEntityPhysicsService.java b/Allay-Server/src/main/java/cn/allay/server/world/entity/AllayEntityPhysicsService.java
@@ -5,6 +5,8 @@
 import cn.allay.api.entity.Entity;
 import cn.allay.api.math.Location3dc;
 import cn.allay.api.world.entity.EntityPhysicsService;
+import it.unimi.dsi.fastutil.objects.ObjectArrayList;
+import org.cloudburstmc.protocol.bedrock.packet.MoveEntityDeltaPacket;
 import org.joml.primitives.AABBdc;
 
 import java.util.*;
@@ -16,37 +18,93 @@
  * @author daoge_cmd
  */
 public class AllayEntityPhysicsService implements EntityPhysicsService {
+
+    public static final double DIFF_POSITION_THRESHOLD = 0.0001;
+    public static final double DIFF_ROTATION_THRESHOLD = 0.1;
+
     protected Map<Long, Entity> entities = new Long2ObjectNonBlockingMap<>();
     protected Map<Long, Queue<ScheduledMove>> scheduledMoveQueue = new Long2ObjectNonBlockingMap<>();
-    protected Map<Long, Set<Long>> entityCollisionMap = new Long2ObjectNonBlockingMap<>();
+    protected Map<Long, List<Entity>> entityCollisionCache = new Long2ObjectNonBlockingMap<>();
     protected AABBTree<Entity> entityAABBTree = new AABBTree<>();
+    protected Queue<EntityUpdateOperation> entityUpdateOperationQueue = new ConcurrentLinkedQueue<>();
 
     @Override
     public void tick() {
+        handleEntityUpdateQueue();
+        handleScheduledMoveQueue();
+        checkEntityCollision();
+    }
+
+    protected void checkEntityCollision() {
+        entities.values().parallelStream().forEach(entity -> {
+            var result = new ObjectArrayList<Entity>();
+            entityAABBTree.detectOverlaps(entity.getOffsetAABB(), result);
+            entityCollisionCache.put(entity.getUniqueId(), result);
+        });
+    }
+
+    protected void handleScheduledMoveQueue() {
+        for (var entry : scheduledMoveQueue.entrySet()) {
+            var queue = entry.getValue();
+            while (!queue.isEmpty()) {
+                var scheduledMove = queue.poll();
+                var entity = scheduledMove.entity;
+                var newLoc = scheduledMove.newLoc;
+                entity.broadcastMoveToViewers(computeMoveFlags(entity, entity.getLocation(), newLoc), newLoc);
+                entity.setLocation(newLoc);
+            }
+        }
+    }
+
+    protected void handleEntityUpdateQueue() {
+        while (!entityUpdateOperationQueue.isEmpty()) {
+            var operation = entityUpdateOperationQueue.poll();
+            var entity = operation.entity;
+            switch (operation.type) {
+                case ADD -> {
+                    entities.put(entity.getUniqueId(), entity);
+                    entityAABBTree.add(entity);
+                }
+                case REMOVE -> {
+                    entities.remove(entity.getUniqueId());
+                    entityAABBTree.remove(entity);
+                    entityCollisionCache.remove(entity.getUniqueId());
+                }
+                case UPDATE -> entityAABBTree.update(entity);
+            }
+        }
+    }
 
+    protected Set<MoveEntityDeltaPacket.Flag> computeMoveFlags(Entity entity, Location3dc oldLoc, Location3dc newLoc) {
+        var flags = EnumSet.noneOf(MoveEntityDeltaPacket.Flag.class);
+        if (Math.abs(oldLoc.x() - newLoc.x()) > DIFF_POSITION_THRESHOLD) flags.add(MoveEntityDeltaPacket.Flag.HAS_X);
+        if (Math.abs(oldLoc.y() - newLoc.y()) > DIFF_POSITION_THRESHOLD) flags.add(MoveEntityDeltaPacket.Flag.HAS_Y);
+        if (Math.abs(oldLoc.z() - newLoc.z()) > DIFF_POSITION_THRESHOLD) flags.add(MoveEntityDeltaPacket.Flag.HAS_Z);
+        if (Math.abs(oldLoc.yaw() - newLoc.yaw()) > DIFF_ROTATION_THRESHOLD) flags.add(MoveEntityDeltaPacket.Flag.HAS_YAW);
+        if (Math.abs(oldLoc.pitch() - newLoc.pitch()) > DIFF_ROTATION_THRESHOLD) flags.add(MoveEntityDeltaPacket.Flag.HAS_PITCH);
+        if (entity.enableHeadYaw() && Math.abs(oldLoc.headYaw() - newLoc.headYaw()) > DIFF_ROTATION_THRESHOLD) flags.add(MoveEntityDeltaPacket.Flag.HAS_HEAD_YAW);
+        return flags;
     }
 
     @Override
     public void updateEntity(Entity entity) {
         if (!entities.containsKey(entity.getUniqueId()))
-            throw new IllegalArgumentException("Entity " + entity.getUniqueId() + " is not registered in this service");
-        entityAABBTree.update(entity);
+            throw new IllegalArgumentException("Entity " + entity.getUniqueId() + " is not added!");
+        entityUpdateOperationQueue.offer(new EntityUpdateOperation(entity, EntityUpdateType.UPDATE));
     }
 
     @Override
     public void addEntity(Entity entity) {
         if (entities.containsKey(entity.getUniqueId()))
-            throw new IllegalArgumentException("Entity " + entity.getUniqueId() + " is already registered in this service");
-        entities.put(entity.getUniqueId(), entity);
-        entityAABBTree.add(entity);
+            throw new IllegalArgumentException("Entity " + entity.getUniqueId() + " is already added!");
+        entityUpdateOperationQueue.offer(new EntityUpdateOperation(entity, EntityUpdateType.ADD));
     }
 
     @Override
     public void removeEntity(Entity entity) {
         if (!entities.containsKey(entity.getUniqueId()))
-            throw new IllegalArgumentException("Entity " + entity.getUniqueId() + " is not registered in this service");
-        entities.remove(entity.getUniqueId());
-        entityAABBTree.remove(entity);
+            throw new IllegalArgumentException("Entity " + entity.getUniqueId() + " is not added!");
+        entityUpdateOperationQueue.offer(new EntityUpdateOperation(entity, EntityUpdateType.REMOVE));
     }
 
     @Override
@@ -58,14 +116,30 @@ public boolean containEntity(Entity entity) {
     public void offerScheduledMove(Entity entity, Location3dc newLoc) {
         if (!entities.containsKey(entity.getUniqueId()))
             throw new IllegalArgumentException("Entity " + entity.getUniqueId() + " is not registered in this service");
+        if (entity.getLocation().equals(newLoc))
+            return;
         scheduledMoveQueue.computeIfAbsent(entity.getUniqueId(), k -> new ConcurrentLinkedQueue<>()).offer(new ScheduledMove(entity, newLoc));
     }
 
     @Override
-    public List<Entity> getCollidingEntities(AABBdc aabb) {
+    public List<Entity> computeCollidingEntities(AABBdc aabb) {
         var result = new ArrayList<Entity>();
-        return entityAABBTree.detectOverlaps(aabb, result);
+        entityAABBTree.detectOverlaps(aabb, result);
+        return result;
+    }
+
+    @Override
+    public List<Entity> getCachedEntityCollidingResult(Entity entity) {
+        return entityCollisionCache.getOrDefault(entity.getUniqueId(), Collections.emptyList());
     }
 
     protected record ScheduledMove(Entity entity, Location3dc newLoc) {};
+
+    protected record EntityUpdateOperation(Entity entity, EntityUpdateType type) {}
+
+    protected enum EntityUpdateType {
+        ADD,
+        REMOVE,
+        UPDATE
+    }
 }
diff --git a/Allay-Server/src/test/java/cn/allay/api/datastruct/aabbtree/TestEntity.java b/Allay-Server/src/test/java/cn/allay/api/datastruct/aabbtree/TestEntity.java
@@ -33,7 +33,7 @@ public TestEntity(int id, double x, double y, double z, double width, double hei
    }
 
    @Override
-   public AABBd copyAABBTo(AABBd dest) {
+   public AABBd copyOffsetAABBTo(AABBd dest) {
       if (dest == null) {
          dest = new AABBd();
       }