Rewrote physics once again. This time going through walls should be more difficult

src/main/java/net/minestom/server/collision/BoundingBox.java

@@ -3,6 +3,7 @@ package net.minestom.server.collision;
 import net.minestom.server.entity.Entity;
 import net.minestom.server.utils.BlockPosition;
 import net.minestom.server.utils.Position;
+import net.minestom.server.utils.Vector;
 
 /**
  * See https://wiki.vg/Entity_metadata#Mobs_2
@@ -105,6 +106,60 @@ public class BoundingBox {
         return entity.getPosition().getZ() + (z / 2);
     }
 
+    public Vector[] getBottomFace() {
+        return new Vector[] {
+                new Vector(getMinX(), getMinY(), getMinZ()),
+                new Vector(getMaxX(), getMinY(), getMinZ()),
+                new Vector(getMaxX(), getMinY(), getMaxZ()),
+                new Vector(getMinX(), getMinY(), getMaxZ()),
+        };
+    }
+
+    public Vector[] getTopFace() {
+        return new Vector[] {
+                new Vector(getMinX(), getMaxY(), getMinZ()),
+                new Vector(getMaxX(), getMaxY(), getMinZ()),
+                new Vector(getMaxX(), getMaxY(), getMaxZ()),
+                new Vector(getMinX(), getMaxY(), getMaxZ()),
+        };
+    }
+
+    public Vector[] getLeftFace() {
+        return new Vector[] {
+                new Vector(getMinX(), getMinY(), getMinZ()),
+                new Vector(getMinX(), getMaxY(), getMinZ()),
+                new Vector(getMinX(), getMaxY(), getMaxZ()),
+                new Vector(getMinX(), getMinY(), getMaxZ()),
+        };
+    }
+
+    public Vector[] getRightFace() {
+        return new Vector[] {
+                new Vector(getMaxX(), getMinY(), getMinZ()),
+                new Vector(getMaxX(), getMaxY(), getMinZ()),
+                new Vector(getMaxX(), getMaxY(), getMaxZ()),
+                new Vector(getMaxX(), getMinY(), getMaxZ()),
+        };
+    }
+
+    public Vector[] getFrontFace() {
+        return new Vector[] {
+                new Vector(getMinX(), getMinY(), getMinZ()),
+                new Vector(getMaxX(), getMinY(), getMinZ()),
+                new Vector(getMaxX(), getMaxY(), getMinZ()),
+                new Vector(getMinX(), getMaxY(), getMinZ()),
+        };
+    }
+
+    public Vector[] getBackFace() {
+        return new Vector[] {
+                new Vector(getMinX(), getMinY(), getMaxZ()),
+                new Vector(getMaxX(), getMinY(), getMaxZ()),
+                new Vector(getMaxX(), getMaxY(), getMaxZ()),
+                new Vector(getMinX(), getMaxY(), getMaxZ()),
+        };
+    }
+
     @Override
     public String toString() {
         String result = "BoudingBox";

src/main/java/net/minestom/server/collision/CollisionUtils.java

@@ -9,6 +9,10 @@ import net.minestom.server.utils.Vector;
 
 public class CollisionUtils {
 
+    private static final Vector Y_AXIS = new Vector(0, 1, 0);
+    private static final Vector X_AXIS = new Vector(1, 0, 0);
+    private static final Vector Z_AXIS = new Vector(0, 0, 1);
+
     /**
      * Moves an entity with physics applied (ie checking against blocks)
      * @param entity the entity to move
@@ -21,78 +25,133 @@ public class CollisionUtils {
         Position currentPosition = entity.getPosition();
         BoundingBox boundingBox = entity.getBoundingBox();
 
-        float currentX = currentPosition.getX();
-        float currentY = currentPosition.getY();
-        float currentZ = currentPosition.getZ();
+        Vector intermediaryPosition = new Vector();
+        boolean yCollision = stepAxis(instance, currentPosition.toVector(), Y_AXIS, deltaPosition.getY(),
+                intermediaryPosition,
+                deltaPosition.getY() > 0 ? boundingBox.getTopFace() : boundingBox.getBottomFace()
+                );
 
-        // target_WithBB is the target_ with the length in the _ direction of the bounding box added. Used to determinate block intersections
+        boolean xCollision = stepAxis(instance, intermediaryPosition, X_AXIS, deltaPosition.getX(),
+                intermediaryPosition,
+                deltaPosition.getX() < 0 ? boundingBox.getLeftFace() : boundingBox.getRightFace()
+        );
 
-        // step Y
-        float targetY = entity.getPosition().getY() + deltaPosition.getY();
-        float targetYWithBB = targetY;
-        if(deltaPosition.getY() > 0) {
-            targetYWithBB += boundingBox.getHeight();
-        }
-        BlockPosition yBlock = new BlockPosition(currentX, (int) targetYWithBB, currentZ);
-        boolean yAir = !Block.fromId(instance.getBlockId(yBlock)).isSolid();
-        boolean yIntersect = boundingBox.intersect(yBlock);
+        boolean zCollision = stepAxis(instance, intermediaryPosition, Z_AXIS, deltaPosition.getZ(),
+                intermediaryPosition,
+                deltaPosition.getZ() > 0 ? boundingBox.getBackFace() : boundingBox.getFrontFace()
+        );
 
-        boolean yCollision = true;
-        if(yAir || !yIntersect)
-            yCollision = false;
-        float newY = yCollision ? currentY : targetY;
-
-        if(yCollision) {
-            if(deltaPosition.getY() < 0) {
-                newY = (float) (Math.ceil(newY)+0.01f); // TODO: custom block bounding boxes
-            } else if(deltaPosition.getY() > 0) {
-                newY = (float) (Math.floor(newY)-0.01f); // TODO: custom block bounding boxes
-            }
-        }
-
-        // step X/Z
-        float targetX = entity.getPosition().getX() + deltaPosition.getX();
-        float targetXWithBB = targetX+Math.signum(deltaPosition.getX()) * boundingBox.getWidth()/2f;
-
-        float targetZ = entity.getPosition().getZ() + deltaPosition.getZ();
-        float targetZWithBB = targetZ+Math.signum(deltaPosition.getZ()) * boundingBox.getDepth()/2f;
-
-        BlockPosition xBlock = new BlockPosition(targetXWithBB, (int) newY, currentZ);
-        BlockPosition zBlock = new BlockPosition(currentX, (int) newY, targetZWithBB);
-
-        boolean xAir = !Block.fromId(instance.getBlockId(xBlock)).isSolid();
-        boolean zAir = !Block.fromId(instance.getBlockId(zBlock)).isSolid();
-
-        boolean xIntersect = boundingBox.intersect(xBlock);
-        boolean zIntersect = boundingBox.intersect(zBlock);
-
-        boolean xCollision = true;
-        if(xAir || !xIntersect)
-            xCollision = false;
-        boolean zCollision = true;
-        if(zAir || !zIntersect)
-            zCollision = false;
-        float newX = xCollision ? currentX : targetX;
-        float newZ = zCollision ? currentZ : targetZ;
-
-        velocityOut.copy(entity.getVelocity());
-
-        if(xCollision) {
+        positionOut.setX(intermediaryPosition.getX());
+        positionOut.setY(intermediaryPosition.getY());
+        positionOut.setZ(intermediaryPosition.getZ());
+        velocityOut.copy(deltaPosition);
+        if (xCollision) {
             velocityOut.setX(0f);
         }
-        if(yCollision) {
+        if (yCollision) {
             velocityOut.setY(0f);
         }
-        if(zCollision) {
+        if (zCollision) {
             velocityOut.setZ(0f);
         }
 
-        positionOut.setX(newX);
-        positionOut.setY(newY);
-        positionOut.setZ(newZ);
-
-
         return yCollision && deltaPosition.getY() < 0;
     }
 
+    /**
+     * Steps on a single axis. Checks against collisions for each point of 'corners'. This method assumes that startPosition is valid.
+     * Immediately return false if corners is of length 0.
+     * @param instance instance to check blocks from
+     * @param startPosition starting position for stepping, can be intermediary position from last step
+     * @param axis step direction. Works best if unit vector and aligned to an axis
+     * @param stepAmount how much to step in the direction (in blocks)
+     * @param positionOut the vector in which to store the new position
+     * @param corners the corners to check against
+     * @return true iif a collision has been found
+     */
+    private static boolean stepAxis(Instance instance, Vector startPosition, Vector axis, float stepAmount, Vector positionOut, Vector... corners) {
+        positionOut.copy(startPosition);
+        if (corners.length == 0)
+            return false; // avoid degeneracy in following computations
+        // perform copies to allow in place modifications
+        // prevents making a lot of new objects. Well at least it reduces the count
+        BlockPosition[] cornerPositions = new BlockPosition[corners.length];
+        Vector[] cornersCopy = new Vector[corners.length];
+        for (int i = 0; i < corners.length; i++) {
+            cornersCopy[i] = corners[i].clone();
+            cornerPositions[i] = new BlockPosition(corners[i]);
+        }
+
+        float sign = Math.signum(stepAmount);
+        int blockLength = (int)stepAmount;
+        float remainingLength = stepAmount-blockLength;
+        // used to determine if 'remainingLength' should be used
+        boolean collisionFound = false;
+        for (int i = 0; i < Math.abs(blockLength); i++) {
+            if (!stepOnce(instance, axis, sign, cornersCopy, cornerPositions)) {
+                collisionFound = true;
+                break; // return early to avoid adding errors on other corners.
+            }
+        }
+
+        // add remainingLength
+        Vector direction = new Vector();
+        direction.copy(axis);
+        collisionFound |= !stepOnce(instance, direction, remainingLength, cornersCopy, cornerPositions);
+
+        // find the corner which moved the least
+        float smallestDisplacement = Float.POSITIVE_INFINITY;
+        for (int i = 0; i < corners.length; i++) {
+            float displacement = (float) corners[i].distance(cornersCopy[i]);
+            if (displacement < smallestDisplacement) {
+                smallestDisplacement = displacement;
+            }
+        }
+
+        positionOut.copy(startPosition);
+        positionOut.add(smallestDisplacement*axis.getX()*sign, smallestDisplacement*axis.getY()*sign, smallestDisplacement*axis.getZ()*sign);
+        return collisionFound;
+    }
+
+    /**
+     * Steps once (by a length of 1 block) on the given axis. Returns false if this method encountered a collision
+     * @param instance instance to get blocks from
+     * @param axis the axis to move along
+     * @param cornersCopy the corners of the bounding box to consider (mutable)
+     * @param cornerPositions the corners, converted to BlockPosition (mutable)
+     * @return
+     */
+    private static boolean stepOnce(Instance instance, Vector axis, float amount, Vector[] cornersCopy, BlockPosition[] cornerPositions) {
+        float sign = Math.signum(amount);
+        for (int cornerIndex = 0; cornerIndex < cornersCopy.length; cornerIndex++) {
+            Vector corner = cornersCopy[cornerIndex];
+            BlockPosition blockPos = cornerPositions[cornerIndex];
+            corner.add(axis.getX()*amount, axis.getY()*amount, axis.getZ()*amount);
+            blockPos.setX((int) Math.floor(corner.getX()));
+            blockPos.setY((int) Math.floor(corner.getY()));
+            blockPos.setZ((int) Math.floor(corner.getZ()));
+            short blockId = instance.getBlockId(blockPos);
+            Block block = Block.fromId(blockId);
+
+            // TODO: block collision boxes
+            // TODO: for the moment, always consider a full block
+            if (!block.isAir()) {
+                corner.subtract(axis.getX()*amount, axis.getY()*amount, axis.getZ()*amount);
+
+                if (Math.abs(axis.getX()) > 10e-16) {
+                    corner.setX(blockPos.getX() - axis.getX() * sign);
+                }
+                if (Math.abs(axis.getY()) > 10e-16) {
+                    corner.setY(blockPos.getY() - axis.getY() * sign);
+                }
+                if (Math.abs(axis.getZ()) > 10e-16) {
+                    corner.setZ(blockPos.getZ() - axis.getZ() * sign);
+                }
+
+                return false;
+            }
+        }
+        return true;
+    }
+
 }

src/main/java/net/minestom/server/entity/Entity.java

@@ -249,17 +249,17 @@ public abstract class Entity implements Viewable, DataContainer {
                     velocity.setY(velocity.getY() - gravityDragPerTick*tps);
                 }
 
-                Position newPositionOut = new Position();
                 Vector newVelocityOut = new Vector();
                 Vector deltaPos = new Vector(
-                        getVelocity().getX() / tps,
-                        getVelocity().getY() / tps,
-                        getVelocity().getZ() / tps
+                        getVelocity().getX()/tps,
+                        getVelocity().getY()/tps,
+                        getVelocity().getZ()/tps
                 );
                 onGround = CollisionUtils.handlePhysics(this, deltaPos, newPosition, newVelocityOut);
 
                 refreshPosition(newPosition);
                 velocity.copy(newVelocityOut);
+                velocity.multiply(tps);
 
                 float drag;
                 if(onGround) {
@@ -267,7 +267,8 @@ public abstract class Entity implements Viewable, DataContainer {
                 } else {
                     drag = 0.98f; // air drag
                 }
-                velocity.multiply(drag);
+                velocity.setX(velocity.getX() * drag);
+                velocity.setZ(velocity.getZ() * drag);
 
 
                 sendSynchronization();

src/main/java/net/minestom/server/utils/BlockPosition.java

@@ -19,6 +19,10 @@ public class BlockPosition {
         this.z = (int) Math.floor(z);
     }
 
+    public BlockPosition(Vector position) {
+        this(position.getX(), position.getY(), position.getZ());
+    }
+
     public BlockPosition add(int x, int y, int z) {
         this.x += x;
         this.y += y;

src/main/java/net/minestom/server/utils/Position.java

@@ -143,6 +143,10 @@ public class Position {
         return new BlockPosition(x, y, z);
     }
 
+    public Vector toVector() {
+        return new Vector(x, y, z);
+    }
+
     @Override
     public String toString() {
         return "Position[" + x + ":" + y + ":" + z + "] (" + yaw + "/" + pitch + ")";

src/main/java/net/minestom/server/utils/Vector.java

@@ -25,6 +25,13 @@ public class Vector implements Cloneable {
         return this;
     }
 
+    public Vector add(float x, float y, float z) {
+        this.x += x;
+        this.y += y;
+        this.z += z;
+        return this;
+    }
+
     /**
      * Subtracts a vector from this one.
      *
@@ -38,6 +45,13 @@ public class Vector implements Cloneable {
         return this;
     }
 
+    public Vector subtract(float x, float y, float z) {
+        this.x -= x;
+        this.y -= y;
+        this.z -= z;
+        return this;
+    }
+
     /**
      * Multiplies the vector by another.
      *
@@ -206,11 +220,7 @@ public class Vector implements Cloneable {
      */
     @Override
     public Vector clone() {
-        try {
-            return (Vector) super.clone();
-        } catch (CloneNotSupportedException e) {
-            throw new Error(e);
-        }
+        return new Vector(x, y, z);
     }
 
     public float getX() {