+ * Works by getting all the full blocks that an entity could interact with. + * All bounding boxes inside the full blocks are checked for collisions with the entity. + */ + static PhysicsResult handlePhysics(@NotNull BoundingBox boundingBox, + @NotNull Vec velocity, @NotNull Pos entityPosition, + @NotNull Block.Getter getter, + @Nullable PhysicsResult lastPhysicsResult) { + if (velocity.isZero()) { + // TODO should return a constant + return new PhysicsResult(entityPosition, Vec.ZERO, false, false, false, false, velocity, null, Block.AIR); + } + // Fast-exit using cache + final PhysicsResult cachedResult = cachedPhysics(velocity, entityPosition, getter, lastPhysicsResult); + if (cachedResult != null) { + return cachedResult; + } + // Expensive AABB computation + return stepPhysics(boundingBox, velocity, entityPosition, getter); + } + + static Entity canPlaceBlockAt(Instance instance, Point blockPos, Block b) { + for (Entity entity : instance.getNearbyEntities(blockPos, 3)) { + final EntityType type = entity.getEntityType(); + if (type == EntityType.ITEM || type == EntityType.ARROW) + continue; + // Marker Armor Stands should not prevent block placement + if (entity.getEntityMeta() instanceof ArmorStandMeta armorStandMeta && armorStandMeta.isMarker()) + continue; + + final boolean intersects; + if (entity instanceof Player) { + // Ignore spectators + if (((Player) entity).getGameMode() == GameMode.SPECTATOR) + continue; + // Need to move player slightly away from block we're placing. + // If player is at block 40 we cannot place a block at block 39 with side length 1 because the block will be in [39, 40] + // For this reason we subtract a small amount from the player position + Point playerPos = entity.getPosition().add(entity.getPosition().sub(blockPos).mul(0.0000001)); + intersects = b.registry().collisionShape().intersectBox(playerPos.sub(blockPos), entity.getBoundingBox()); + } else { + intersects = b.registry().collisionShape().intersectBox(entity.getPosition().sub(blockPos), entity.getBoundingBox()); + } + if (intersects) return entity; + } + return null; + } + + private static PhysicsResult cachedPhysics(Vec velocity, Pos entityPosition, + Block.Getter getter, PhysicsResult lastPhysicsResult) { + if (lastPhysicsResult != null) { + if (lastPhysicsResult.collisionY() + && velocity.y() == lastPhysicsResult.originalDelta().y() + && lastPhysicsResult.collidedBlockY() != null + && getter.getBlock(lastPhysicsResult.collidedBlockY(), Block.Getter.Condition.TYPE) == lastPhysicsResult.blockTypeY() + && velocity.x() == 0 && velocity.z() == 0 + && entityPosition.samePoint(lastPhysicsResult.newPosition()) + && lastPhysicsResult.blockTypeY() != Block.AIR) { + return lastPhysicsResult; + } + } + return null; + } + + + private static PhysicsResult stepPhysics(@NotNull BoundingBox boundingBox, + @NotNull Vec velocity, @NotNull Pos entityPosition, + @NotNull Block.Getter getter) { + // Allocate once and update values + SweepResult finalResult = new SweepResult(1 - Vec.EPSILON, 0, 0, 0, null); + + boolean foundCollisionX = false, foundCollisionY = false, foundCollisionZ = false; + Point collisionYBlock = null; + Block blockYType = Block.AIR; + + // Query faces to get the points needed for collision + final Vec[] allFaces = calculateFaces(velocity, boundingBox); + PhysicsResult result = computePhysics(boundingBox, velocity, entityPosition, getter, allFaces, finalResult); + // Loop until no collisions are found. + // When collisions are found, the collision axis is set to 0 + // Looping until there are no collisions will allow the entity to move in axis other than the collision axis after a collision. + while (result.collisionX() || result.collisionY() || result.collisionZ()) { + // Reset final result + finalResult.res = 1 - Vec.EPSILON; + finalResult.normalX = 0; + finalResult.normalY = 0; + finalResult.normalZ = 0; + + if (result.collisionX()) foundCollisionX = true; + if (result.collisionZ()) foundCollisionZ = true; + if (result.collisionY()) { + foundCollisionY = true; + // If we are only moving in the y-axis + if (!result.collisionX() && !result.collisionZ() && velocity.x() == 0 && velocity.z() == 0) { + collisionYBlock = result.collidedBlockY(); + blockYType = result.blockTypeY(); + } + } + // If all axis have had collisions, break + if (foundCollisionX && foundCollisionY && foundCollisionZ) break; + // If the entity isn't moving, break + if (result.newVelocity().isZero()) break; + + result = computePhysics(boundingBox, result.newVelocity(), result.newPosition(), getter, allFaces, finalResult); + } + + final double newDeltaX = foundCollisionX ? 0 : velocity.x(); + final double newDeltaY = foundCollisionY ? 0 : velocity.y(); + final double newDeltaZ = foundCollisionZ ? 0 : velocity.z(); + + return new PhysicsResult(result.newPosition(), new Vec(newDeltaX, newDeltaY, newDeltaZ), + newDeltaY == 0 && velocity.y() < 0, + foundCollisionX, foundCollisionY, foundCollisionZ, velocity, collisionYBlock, blockYType); + } + + private static PhysicsResult computePhysics(@NotNull BoundingBox boundingBox, + @NotNull Vec velocity, Pos entityPosition, + @NotNull Block.Getter getter, + @NotNull Vec[] allFaces, + @NotNull SweepResult finalResult) { + // If the movement is small we don't need to run the expensive ray casting. + // Positions of move less than one can have hardcoded blocks to check for every direction + if (velocity.length() < 1) { + fastPhysics(boundingBox, velocity, entityPosition, getter, allFaces, finalResult); + } else { + slowPhysics(boundingBox, velocity, entityPosition, getter, allFaces, finalResult); + } + + final boolean collisionX = finalResult.normalX != 0; + final boolean collisionY = finalResult.normalY != 0; + final boolean collisionZ = finalResult.normalZ != 0; + + double deltaX = finalResult.res * velocity.x(); + double deltaY = finalResult.res * velocity.y(); + double deltaZ = finalResult.res * velocity.z(); + + if (Math.abs(deltaX) < Vec.EPSILON) deltaX = 0; + if (Math.abs(deltaY) < Vec.EPSILON) deltaY = 0; + if (Math.abs(deltaZ) < Vec.EPSILON) deltaZ = 0; + + final Pos finalPos = entityPosition.add(deltaX, deltaY, deltaZ); + + final double remainingX = collisionX ? 0 : velocity.x() - deltaX; + final double remainingY = collisionY ? 0 : velocity.y() - deltaY; + final double remainingZ = collisionZ ? 0 : velocity.z() - deltaZ; + + return new PhysicsResult(finalPos, new Vec(remainingX, remainingY, remainingZ), + collisionY, collisionX, collisionY, collisionZ, + Vec.ZERO, finalResult.collidedShapePosition, finalResult.blockType); + } + + private static void slowPhysics(@NotNull BoundingBox boundingBox, + @NotNull Vec velocity, Pos entityPosition, + @NotNull Block.Getter getter, + @NotNull Vec[] allFaces, + @NotNull SweepResult finalResult) { + // When large moves are done we need to ray-cast to find all blocks that could intersect with the movement + for (Vec point : allFaces) { + BlockIterator iterator = new BlockIterator(Vec.fromPoint(point.add(entityPosition)), velocity, 0, (int) Math.ceil(velocity.length())); + while (iterator.hasNext()) { + Point p = iterator.next(); + // sqrt 3 (1.733) is the maximum error + if (Vec.fromPoint(p.sub(entityPosition)).length() > (finalResult.res * velocity.length() + 1.733)) + break; + if (checkBoundingBox(p.blockX(), p.blockY(), p.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult)) + break; + } + } + } + + private static void fastPhysics(@NotNull BoundingBox boundingBox, + @NotNull Vec velocity, Pos entityPosition, + @NotNull Block.Getter getter, + @NotNull Vec[] allFaces, + @NotNull SweepResult finalResult) { + for (Vec point : allFaces) { + final Vec pointBefore = point.add(entityPosition); + final Vec pointAfter = point.add(entityPosition).add(velocity); + // Entity can pass through up to 4 blocks. Starting block, Two intermediate blocks, and a final block. + // This means we must check every combination of block movements when an entity moves over an axis. + // 000, 001, 010, 011, etc. + // There are 8 of these combinations + // Checks can be limited by checking if we moved across an axis line + + // Pass through (0, 0, 0) + checkBoundingBox(pointBefore.blockX(), pointBefore.blockY(), pointBefore.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); + + if (pointBefore.blockX() != pointAfter.blockX()) { + // Pass through (+1, 0, 0) + checkBoundingBox(pointAfter.blockX(), pointBefore.blockY(), pointBefore.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); + + // Checks for moving through 4 blocks + if (pointBefore.blockY() != pointAfter.blockY()) + // Pass through (+1, +1, 0) + checkBoundingBox(pointAfter.blockX(), pointAfter.blockY(), pointBefore.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); + + if (pointBefore.blockZ() != pointAfter.blockZ()) + // Pass through (+1, 0, +1) + checkBoundingBox(pointAfter.blockX(), pointBefore.blockY(), pointAfter.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); + } + + if (pointBefore.blockY() != pointAfter.blockY()) { + // Pass through (0, +1, 0) + checkBoundingBox(pointBefore.blockX(), pointAfter.blockY(), pointBefore.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); + + // Checks for moving through 4 blocks + if (pointBefore.blockZ() != pointAfter.blockZ()) + // Pass through (0, +1, +1) + checkBoundingBox(pointBefore.blockX(), pointAfter.blockY(), pointAfter.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); + } + + if (pointBefore.blockZ() != pointAfter.blockZ()) { + // Pass through (0, 0, +1) + checkBoundingBox(pointBefore.blockX(), pointBefore.blockY(), pointAfter.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); + } + + // Pass through (+1, +1, +1) + if (pointBefore.blockX() != pointAfter.blockX() + && pointBefore.blockY() != pointAfter.blockY() + && pointBefore.blockZ() != pointAfter.blockZ()) + checkBoundingBox(pointAfter.blockX(), pointAfter.blockY(), pointAfter.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); + } + } + + /** + * Check if a moving entity will collide with a block. Updates finalResult + * + * @param blockX block x position + * @param blockY block y position + * @param blockZ block z position + * @param entityVelocity entity movement vector + * @param entityPosition entity position + * @param boundingBox entity bounding box + * @param getter block getter + * @param finalResult place to store final result of collision + * @return true if entity finds collision, other false + */ + static boolean checkBoundingBox(int blockX, int blockY, int blockZ, + Vec entityVelocity, Pos entityPosition, BoundingBox boundingBox, + Block.Getter getter, SweepResult finalResult) { + // Don't step if chunk isn't loaded yet + final Block currentBlock = getter.getBlock(blockX, blockY, blockZ, Block.Getter.Condition.TYPE); + final Shape currentShape = currentBlock.registry().collisionShape(); + + final boolean currentCollidable = !currentShape.relativeEnd().isZero(); + final boolean currentShort = currentShape.relativeEnd().y() < 0.5; + + // only consider the block below if our current shape is sufficiently short + if (currentShort && shouldCheckLower(entityVelocity, entityPosition, blockX, blockY, blockZ)) { + // we need to check below for a tall block (fence, wall, ...) + final Vec belowPos = new Vec(blockX, blockY - 1, blockZ); + final Block belowBlock = getter.getBlock(belowPos, Block.Getter.Condition.TYPE); + final Shape belowShape = belowBlock.registry().collisionShape(); + + final Vec currentPos = new Vec(blockX, blockY, blockZ); + // don't fall out of if statement, we could end up redundantly grabbing a block, and we only need to + // collision check against the current shape since the below shape isn't tall + if (belowShape.relativeEnd().y() > 1) { + // we should always check both shapes, so no short-circuit here, to handle cases where the bounding box + // hits the current solid but misses the tall solid + return belowShape.intersectBoxSwept(entityPosition, entityVelocity, belowPos, boundingBox, finalResult) | + (currentCollidable && currentShape.intersectBoxSwept(entityPosition, entityVelocity, currentPos, boundingBox, finalResult)); + } else { + return currentCollidable && currentShape.intersectBoxSwept(entityPosition, entityVelocity, currentPos, boundingBox, finalResult); + } + } + + if (currentCollidable && currentShape.intersectBoxSwept(entityPosition, entityVelocity, + new Vec(blockX, blockY, blockZ), boundingBox, finalResult)) { + // if the current collision is sufficiently short, we might need to collide against the block below too + if (currentShort) { + final Vec belowPos = new Vec(blockX, blockY - 1, blockZ); + final Block belowBlock = getter.getBlock(belowPos, Block.Getter.Condition.TYPE); + final Shape belowShape = belowBlock.registry().collisionShape(); + // only do sweep if the below block is big enough to possibly hit + if (belowShape.relativeEnd().y() > 1) + belowShape.intersectBoxSwept(entityPosition, entityVelocity, belowPos, boundingBox, finalResult); + } + return true; + } + return false; + } + + private static boolean shouldCheckLower(Vec entityVelocity, Pos entityPosition, int blockX, int blockY, int blockZ) { + final double yVelocity = entityVelocity.y(); + // if moving horizontally, just check if the floor of the entity's position is the same as the blockY + if (yVelocity == 0) return Math.floor(entityPosition.y()) == blockY; + final double xVelocity = entityVelocity.x(); + final double zVelocity = entityVelocity.z(); + // if moving straight up, don't bother checking for tall solids beneath anything + // if moving straight down, only check for a tall solid underneath the last block + if (xVelocity == 0 && zVelocity == 0) + return yVelocity < 0 && blockY == Math.floor(entityPosition.y() + yVelocity); + // default to true: if no x velocity, only consider YZ line, and vice-versa + final boolean underYX = xVelocity != 0 && computeHeight(yVelocity, xVelocity, entityPosition.y(), entityPosition.x(), blockX) >= blockY; + final boolean underYZ = zVelocity != 0 && computeHeight(yVelocity, zVelocity, entityPosition.y(), entityPosition.z(), blockZ) >= blockY; + // true if the block is at or below the same height as a line drawn from the entity's position to its final + // destination + return underYX && underYZ; + } + + /* + computes the height of the entity at the given block position along a projection of the line it's travelling along + (YX or YZ). the returned value will be greater than or equal to the block height if the block is along the lower + layer of intersections with this line. + */ + private static double computeHeight(double yVelocity, double velocity, double entityY, double pos, int blockPos) { + final double m = yVelocity / velocity; + /* + offsetting by 1 is necessary with a positive slope, because we can clip the bottom-right corner of blocks + without clipping the "bottom-left" (the smallest corner of the block on the YZ or YX plane). without the offset + these would not be considered to be on the lowest layer, since our block position represents the bottom-left + corner + */ + return m * (blockPos - pos + (m > 0 ? 1 : 0)) + entityY; + } + private static Vec[] calculateFaces(Vec queryVec, BoundingBox boundingBox) { final int queryX = (int) Math.signum(queryVec.x()); final int queryY = (int) Math.signum(queryVec.y()); @@ -121,300 +440,4 @@ final class BlockCollision { return facePoints; } - - /** - * Moves an entity with physics applied (ie checking against blocks) - *
- * Works by getting all the full blocks that an entity could interact with. - * All bounding boxes inside the full blocks are checked for collisions with the entity. - */ - static PhysicsResult handlePhysics(@NotNull BoundingBox boundingBox, - @NotNull Vec velocity, @NotNull Pos entityPosition, - @NotNull Block.Getter getter, - @Nullable PhysicsResult lastPhysicsResult) { - // Allocate once and update values - SweepResult finalResult = new SweepResult(1 - Vec.EPSILON, 0, 0, 0, null); - - boolean foundCollisionX = false, foundCollisionY = false, foundCollisionZ = false; - Point collisionYBlock = null; - Block blockYType = Block.AIR; - - // Check cache to see if the entity is standing on a block without moving. - // If the entity isn't moving and the block below hasn't changed, return - if (lastPhysicsResult != null) { - if (lastPhysicsResult.collisionY() - && Math.signum(velocity.y()) == Math.signum(lastPhysicsResult.originalDelta().y()) - && lastPhysicsResult.collidedBlockY() != null - && getter.getBlock(lastPhysicsResult.collidedBlockY(), Block.Getter.Condition.TYPE) == lastPhysicsResult.blockTypeY() - && velocity.x() == 0 && velocity.z() == 0 - && entityPosition.samePoint(lastPhysicsResult.newPosition()) - && lastPhysicsResult.blockTypeY() != Block.AIR) { - foundCollisionY = true; - collisionYBlock = lastPhysicsResult.collidedBlockY(); - blockYType = lastPhysicsResult.blockTypeY(); - } - } - if (velocity.isZero()) { - if (lastPhysicsResult != null) { - return new PhysicsResult(entityPosition, Vec.ZERO, lastPhysicsResult.isOnGround(), - lastPhysicsResult.collisionX(), lastPhysicsResult.collisionY(), lastPhysicsResult.collisionZ(), - velocity, lastPhysicsResult.collidedBlockY(), lastPhysicsResult.blockTypeY()); - } else { - return new PhysicsResult(entityPosition, Vec.ZERO, false, false, false, false, velocity, null, Block.AIR); - } - } - // Query faces to get the points needed for collision - final Vec[] allFaces = calculateFaces(velocity, boundingBox); - PhysicsResult res = handlePhysics(boundingBox, velocity, entityPosition, getter, allFaces, finalResult); - // Loop until no collisions are found. - // When collisions are found, the collision axis is set to 0 - // Looping until there are no collisions will allow the entity to move in axis other than the collision axis after a collision. - while (res.collisionX() || res.collisionY() || res.collisionZ()) { - // Reset final result - finalResult.res = 1 - Vec.EPSILON; - finalResult.normalX = 0; - finalResult.normalY = 0; - finalResult.normalZ = 0; - - if (res.collisionX()) foundCollisionX = true; - if (res.collisionZ()) foundCollisionZ = true; - if (res.collisionY()) { - foundCollisionY = true; - // If we are only moving in the y-axis - if (!res.collisionX() && !res.collisionZ() && velocity.x() == 0 && velocity.z() == 0) { - collisionYBlock = res.collidedBlockY(); - blockYType = res.blockTypeY(); - } - } - // If all axis have had collisions, break - if (foundCollisionX && foundCollisionY && foundCollisionZ) break; - // If the entity isn't moving, break - if (res.newVelocity().isZero()) break; - - res = handlePhysics(boundingBox, res.newVelocity(), res.newPosition(), getter, allFaces, finalResult); - } - - final double newDeltaX = foundCollisionX ? 0 : velocity.x(); - final double newDeltaY = foundCollisionY ? 0 : velocity.y(); - final double newDeltaZ = foundCollisionZ ? 0 : velocity.z(); - - return new PhysicsResult(res.newPosition(), new Vec(newDeltaX, newDeltaY, newDeltaZ), - newDeltaY == 0 && velocity.y() < 0, - foundCollisionX, foundCollisionY, foundCollisionZ, velocity, collisionYBlock, blockYType); - } - - private static PhysicsResult handlePhysics(@NotNull BoundingBox boundingBox, - @NotNull Vec velocity, Pos entityPosition, - @NotNull Block.Getter getter, - @NotNull Vec[] allFaces, - @NotNull SweepResult finalResult) { - // If the movement is small we don't need to run the expensive ray casting. - // Positions of move less than one can have hardcoded blocks to check for every direction - if (velocity.length() < 1) { - for (Vec point : allFaces) { - final Vec pointBefore = point.add(entityPosition); - final Vec pointAfter = point.add(entityPosition).add(velocity); - // Entity can pass through up to 4 blocks. Starting block, Two intermediate blocks, and a final block. - // This means we must check every combination of block movements when an entity moves over an axis. - // 000, 001, 010, 011, etc. - // There are 8 of these combinations - // Checks can be limited by checking if we moved across an axis line - - // Pass through (0, 0, 0) - checkBoundingBox(pointBefore.blockX(), pointBefore.blockY(), pointBefore.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); - - if (pointBefore.blockX() != pointAfter.blockX()) { - // Pass through (+1, 0, 0) - checkBoundingBox(pointAfter.blockX(), pointBefore.blockY(), pointBefore.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); - - // Checks for moving through 4 blocks - if (pointBefore.blockY() != pointAfter.blockY()) - // Pass through (+1, +1, 0) - checkBoundingBox(pointAfter.blockX(), pointAfter.blockY(), pointBefore.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); - - if (pointBefore.blockZ() != pointAfter.blockZ()) - // Pass through (+1, 0, +1) - checkBoundingBox(pointAfter.blockX(), pointBefore.blockY(), pointAfter.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); - } - - if (pointBefore.blockY() != pointAfter.blockY()) { - // Pass through (0, +1, 0) - checkBoundingBox(pointBefore.blockX(), pointAfter.blockY(), pointBefore.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); - - // Checks for moving through 4 blocks - if (pointBefore.blockZ() != pointAfter.blockZ()) - // Pass through (0, +1, +1) - checkBoundingBox(pointBefore.blockX(), pointAfter.blockY(), pointAfter.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); - } - - if (pointBefore.blockZ() != pointAfter.blockZ()) { - // Pass through (0, 0, +1) - checkBoundingBox(pointBefore.blockX(), pointBefore.blockY(), pointAfter.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); - } - - // Pass through (+1, +1, +1) - if (pointBefore.blockX() != pointAfter.blockX() - && pointBefore.blockY() != pointAfter.blockY() - && pointBefore.blockZ() != pointAfter.blockZ()) - checkBoundingBox(pointAfter.blockX(), pointAfter.blockY(), pointAfter.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult); - } - } else { - // When large moves are done we need to ray-cast to find all blocks that could intersect with the movement - for (Vec point : allFaces) { - BlockIterator iterator = new BlockIterator(Vec.fromPoint(point.add(entityPosition)), velocity, 0, (int) Math.ceil(velocity.length())); - while (iterator.hasNext()) { - Point p = iterator.next(); - - // sqrt 3 (1.733) is the maximum error - if (Vec.fromPoint(p.sub(entityPosition)).length() > (finalResult.res * velocity.length() + 1.733)) - break; - - if (checkBoundingBox(p.blockX(), p.blockY(), p.blockZ(), velocity, entityPosition, boundingBox, getter, finalResult)) - break; - } - } - } - - boolean collisionX = finalResult.normalX != 0; - boolean collisionY = finalResult.normalY != 0; - boolean collisionZ = finalResult.normalZ != 0; - - double deltaX = finalResult.res * velocity.x(); - double deltaY = finalResult.res * velocity.y(); - double deltaZ = finalResult.res * velocity.z(); - - if (Math.abs(deltaX) < Vec.EPSILON) deltaX = 0; - if (Math.abs(deltaY) < Vec.EPSILON) deltaY = 0; - if (Math.abs(deltaZ) < Vec.EPSILON) deltaZ = 0; - - final Pos finalPos = entityPosition.add(deltaX, deltaY, deltaZ); - - final double remainingX = collisionX ? 0 : velocity.x() - deltaX; - final double remainingY = collisionY ? 0 : velocity.y() - deltaY; - final double remainingZ = collisionZ ? 0 : velocity.z() - deltaZ; - - return new PhysicsResult(finalPos, new Vec(remainingX, remainingY, remainingZ), - collisionY, collisionX, collisionY, collisionZ, - Vec.ZERO, finalResult.collidedShapePosition, finalResult.blockType); - } - - static Entity canPlaceBlockAt(Instance instance, Point blockPos, Block b) { - for (Entity entity : instance.getNearbyEntities(blockPos, 3)) { - final EntityType type = entity.getEntityType(); - if (type == EntityType.ITEM || type == EntityType.ARROW) - continue; - // Marker Armor Stands should not prevent block placement - if (entity.getEntityMeta() instanceof ArmorStandMeta armorStandMeta && armorStandMeta.isMarker()) - continue; - - final boolean intersects; - if (entity instanceof Player) { - // Ignore spectators - if (((Player) entity).getGameMode() == GameMode.SPECTATOR) - continue; - // Need to move player slightly away from block we're placing. - // If player is at block 40 we cannot place a block at block 39 with side length 1 because the block will be in [39, 40] - // For this reason we subtract a small amount from the player position - Point playerPos = entity.getPosition().add(entity.getPosition().sub(blockPos).mul(0.0000001)); - intersects = b.registry().collisionShape().intersectBox(playerPos.sub(blockPos), entity.getBoundingBox()); - } else { - intersects = b.registry().collisionShape().intersectBox(entity.getPosition().sub(blockPos), entity.getBoundingBox()); - } - if (intersects) return entity; - } - return null; - } - - /** - * Check if a moving entity will collide with a block. Updates finalResult - * - * @param blockX block x position - * @param blockY block y position - * @param blockZ block z position - * @param entityVelocity entity movement vector - * @param entityPosition entity position - * @param boundingBox entity bounding box - * @param getter block getter - * @param finalResult place to store final result of collision - * @return true if entity finds collision, other false - */ - static boolean checkBoundingBox(int blockX, int blockY, int blockZ, - Vec entityVelocity, Pos entityPosition, BoundingBox boundingBox, - Block.Getter getter, SweepResult finalResult) { - // Don't step if chunk isn't loaded yet - final Block currentBlock = getter.getBlock(blockX, blockY, blockZ, Block.Getter.Condition.TYPE); - final Shape currentShape = currentBlock.registry().collisionShape(); - - final boolean currentCollidable = !currentShape.relativeEnd().isZero(); - final boolean currentShort = currentShape.relativeEnd().y() < 0.5; - - // only consider the block below if our current shape is sufficiently short - if (currentShort && shouldCheckLower(entityVelocity, entityPosition, blockX, blockY, blockZ)) { - // we need to check below for a tall block (fence, wall, ...) - final Vec belowPos = new Vec(blockX, blockY - 1, blockZ); - final Block belowBlock = getter.getBlock(belowPos, Block.Getter.Condition.TYPE); - final Shape belowShape = belowBlock.registry().collisionShape(); - - final Vec currentPos = new Vec(blockX, blockY, blockZ); - // don't fall out of if statement, we could end up redundantly grabbing a block, and we only need to - // collision check against the current shape since the below shape isn't tall - if (belowShape.relativeEnd().y() > 1) { - // we should always check both shapes, so no short-circuit here, to handle cases where the bounding box - // hits the current solid but misses the tall solid - return belowShape.intersectBoxSwept(entityPosition, entityVelocity, belowPos, boundingBox, finalResult) | - (currentCollidable && currentShape.intersectBoxSwept(entityPosition, entityVelocity, currentPos, boundingBox, finalResult)); - } else { - return currentCollidable && currentShape.intersectBoxSwept(entityPosition, entityVelocity, currentPos, boundingBox, finalResult); - } - } - - if (currentCollidable && currentShape.intersectBoxSwept(entityPosition, entityVelocity, - new Vec(blockX, blockY, blockZ), boundingBox, finalResult)) { - // if the current collision is sufficiently short, we might need to collide against the block below too - if (currentShort) { - final Vec belowPos = new Vec(blockX, blockY - 1, blockZ); - final Block belowBlock = getter.getBlock(belowPos, Block.Getter.Condition.TYPE); - final Shape belowShape = belowBlock.registry().collisionShape(); - // only do sweep if the below block is big enough to possibly hit - if (belowShape.relativeEnd().y() > 1) - belowShape.intersectBoxSwept(entityPosition, entityVelocity, belowPos, boundingBox, finalResult); - } - return true; - } - return false; - } - - private static boolean shouldCheckLower(Vec entityVelocity, Pos entityPosition, int blockX, int blockY, int blockZ) { - final double yVelocity = entityVelocity.y(); - // if moving horizontally, just check if the floor of the entity's position is the same as the blockY - if (yVelocity == 0) return Math.floor(entityPosition.y()) == blockY; - final double xVelocity = entityVelocity.x(); - final double zVelocity = entityVelocity.z(); - // if moving straight up, don't bother checking for tall solids beneath anything - // if moving straight down, only check for a tall solid underneath the last block - if (xVelocity == 0 && zVelocity == 0) - return yVelocity < 0 && blockY == Math.floor(entityPosition.y() + yVelocity); - // default to true: if no x velocity, only consider YZ line, and vice-versa - final boolean underYX = xVelocity != 0 && computeHeight(yVelocity, xVelocity, entityPosition.y(), entityPosition.x(), blockX) >= blockY; - final boolean underYZ = zVelocity != 0 && computeHeight(yVelocity, zVelocity, entityPosition.y(), entityPosition.z(), blockZ) >= blockY; - // true if the block is at or below the same height as a line drawn from the entity's position to its final - // destination - return underYX && underYZ; - } - - /* - computes the height of the entity at the given block position along a projection of the line it's travelling along - (YX or YZ). the returned value will be greater than or equal to the block height if the block is along the lower - layer of intersections with this line. - */ - private static double computeHeight(double yVelocity, double velocity, double entityY, double pos, int blockPos) { - final double m = yVelocity / velocity; - /* - offsetting by 1 is necessary with a positive slope, because we can clip the bottom-right corner of blocks - without clipping the "bottom-left" (the smallest corner of the block on the YZ or YX plane). without the offset - these would not be considered to be on the lowest layer, since our block position represents the bottom-left - corner - */ - return m * (blockPos - pos + (m > 0 ? 1 : 0)) + entityY; - } } diff --git a/src/main/java/net/minestom/server/collision/BoundingBox.java b/src/main/java/net/minestom/server/collision/BoundingBox.java index 64925fb1b..dc9bec53d 100644 --- a/src/main/java/net/minestom/server/collision/BoundingBox.java +++ b/src/main/java/net/minestom/server/collision/BoundingBox.java @@ -44,13 +44,7 @@ public final class BoundingBox implements Shape { @Override @ApiStatus.Experimental public boolean intersectBoxSwept(@NotNull Point rayStart, @NotNull Point rayDirection, @NotNull Point shapePos, @NotNull BoundingBox moving, @NotNull SweepResult finalResult) { - final boolean isHit = RayUtils.BoundingBoxIntersectionCheck( - moving, rayStart, rayDirection, - this, - shapePos - ); - if (!isHit) return false; - if (RayUtils.SweptAABB(moving, rayStart, rayDirection, this, shapePos, finalResult)) { + if (RayUtils.BoundingBoxIntersectionCheck(moving, rayStart, rayDirection, this, shapePos, finalResult) ) { finalResult.collidedShapePosition = shapePos; finalResult.collidedShape = this; finalResult.blockType = null; diff --git a/src/main/java/net/minestom/server/collision/RayUtils.java b/src/main/java/net/minestom/server/collision/RayUtils.java index 478f76d40..3388d13a2 100644 --- a/src/main/java/net/minestom/server/collision/RayUtils.java +++ b/src/main/java/net/minestom/server/collision/RayUtils.java @@ -11,9 +11,10 @@ final class RayUtils { * @param rayStart Ray start position * @param rayDirection Ray to check * @param collidableStatic Bounding box + * @param finalResult * @return true if an intersection between the ray and the bounding box was found */ - public static boolean BoundingBoxIntersectionCheck(BoundingBox moving, Point rayStart, Point rayDirection, BoundingBox collidableStatic, Point staticCollidableOffset) { + public static boolean BoundingBoxIntersectionCheck(BoundingBox moving, Point rayStart, Point rayDirection, BoundingBox collidableStatic, Point staticCollidableOffset, SweepResult finalResult) { Point bbCentre = new Vec(moving.minX() + moving.width() / 2, moving.minY() + moving.height() / 2 + Vec.EPSILON, moving.minZ() + moving.depth() / 2); Point rayCentre = rayStart.add(bbCentre); @@ -28,11 +29,15 @@ final class RayUtils { double signumRayY = Math.signum(rayDirection.y()); double signumRayZ = Math.signum(rayDirection.z()); + boolean isHit = false; + double percentage = Double.MAX_VALUE; + int collisionFace = -1; + // Intersect X - if (rayDirection.x() != 0) { - // Left side of bounding box - if (rayDirection.x() > 0) { - double xFac = bbOffMin.x() / rayDirection.x(); + // Left side of bounding box + if (rayDirection.x() > 0) { + double xFac = bbOffMin.x() / rayDirection.x(); + if (xFac < percentage) { double yix = rayDirection.y() * xFac + rayCentre.y(); double zix = rayDirection.z() * xFac + rayCentre.z(); @@ -43,12 +48,16 @@ final class RayUtils { && yix <= collidableStatic.maxY() + staticCollidableOffset.y() + moving.height() / 2 && zix >= collidableStatic.minZ() + staticCollidableOffset.z() - moving.depth() / 2 && zix <= collidableStatic.maxZ() + staticCollidableOffset.z() + moving.depth() / 2) { - return true; + isHit = true; + percentage = xFac; + collisionFace = 0; } } - // Right side of bounding box - if (rayDirection.x() < 0) { - double xFac = bbOffMax.x() / rayDirection.x(); + } + // Right side of bounding box + if (rayDirection.x() < 0) { + double xFac = bbOffMax.x() / rayDirection.x(); + if (xFac < percentage) { double yix = rayDirection.y() * xFac + rayCentre.y(); double zix = rayDirection.z() * xFac + rayCentre.z(); @@ -58,15 +67,17 @@ final class RayUtils { && yix <= collidableStatic.maxY() + staticCollidableOffset.y() + moving.height() / 2 && zix >= collidableStatic.minZ() + staticCollidableOffset.z() - moving.depth() / 2 && zix <= collidableStatic.maxZ() + staticCollidableOffset.z() + moving.depth() / 2) { - return true; + isHit = true; + percentage = xFac; + collisionFace = 0; } } } // Intersect Z - if (rayDirection.z() != 0) { - if (rayDirection.z() > 0) { - double zFac = bbOffMin.z() / rayDirection.z(); + if (rayDirection.z() > 0) { + double zFac = bbOffMin.z() / rayDirection.z(); + if (zFac < percentage) { double xiz = rayDirection.x() * zFac + rayCentre.x(); double yiz = rayDirection.y() * zFac + rayCentre.y(); @@ -76,11 +87,15 @@ final class RayUtils { && xiz <= collidableStatic.maxX() + staticCollidableOffset.x() + moving.width() / 2 && yiz >= collidableStatic.minY() + staticCollidableOffset.y() - moving.height() / 2 && yiz <= collidableStatic.maxY() + staticCollidableOffset.y() + moving.height() / 2) { - return true; + isHit = true; + percentage = zFac; + collisionFace = 1; } } - if (rayDirection.z() < 0) { - double zFac = bbOffMax.z() / rayDirection.z(); + } + if (rayDirection.z() < 0) { + double zFac = bbOffMax.z() / rayDirection.z(); + if (zFac < percentage) { double xiz = rayDirection.x() * zFac + rayCentre.x(); double yiz = rayDirection.y() * zFac + rayCentre.y(); @@ -90,15 +105,17 @@ final class RayUtils { && xiz <= collidableStatic.maxX() + staticCollidableOffset.x() + moving.width() / 2 && yiz >= collidableStatic.minY() + staticCollidableOffset.y() - moving.height() / 2 && yiz <= collidableStatic.maxY() + staticCollidableOffset.y() + moving.height() / 2) { - return true; + isHit = true; + percentage = zFac; + collisionFace = 1; } } } // Intersect Y - if (rayDirection.y() != 0) { - if (rayDirection.y() > 0) { - double yFac = bbOffMin.y() / rayDirection.y(); + if (rayDirection.y() > 0) { + double yFac = bbOffMin.y() / rayDirection.y(); + if (yFac < percentage) { double xiy = rayDirection.x() * yFac + rayCentre.x(); double ziy = rayDirection.z() * yFac + rayCentre.z(); @@ -108,11 +125,16 @@ final class RayUtils { && xiy <= collidableStatic.maxX() + staticCollidableOffset.x() + moving.width() / 2 && ziy >= collidableStatic.minZ() + staticCollidableOffset.z() - moving.depth() / 2 && ziy <= collidableStatic.maxZ() + staticCollidableOffset.z() + moving.depth() / 2) { - return true; + isHit = true; + percentage = yFac; + collisionFace = 2; } } - if (rayDirection.y() < 0) { - double yFac = bbOffMax.y() / rayDirection.y(); + } + + if (rayDirection.y() < 0) { + double yFac = bbOffMax.y() / rayDirection.y(); + if (yFac < percentage) { double xiy = rayDirection.x() * yFac + rayCentre.x(); double ziy = rayDirection.z() * yFac + rayCentre.z(); @@ -122,127 +144,30 @@ final class RayUtils { && xiy <= collidableStatic.maxX() + staticCollidableOffset.x() + moving.width() / 2 && ziy >= collidableStatic.minZ() + staticCollidableOffset.z() - moving.depth() / 2 && ziy <= collidableStatic.maxZ() + staticCollidableOffset.z() + moving.depth() / 2) { - return true; + isHit = true; + percentage = yFac; + collisionFace = 2; } } } - return false; - } + percentage *= 0.99999; - // Extended from 2d implementation found here https://www.gamedev.net/tutorials/programming/general-and-gameplay-programming/swept-aabb-collision-detection-and-response-r3084/ - public static boolean SweptAABB(BoundingBox collidableMoving, Point rayStart, Point rayDirection, BoundingBox collidableStatic, Point staticCollidableOffset, SweepResult finalResult) { - double normalx, normaly, normalz; + if (percentage >= 0 && percentage <= finalResult.res) { + finalResult.res = percentage; + finalResult.normalX = 0; + finalResult.normalY = 0; + finalResult.normalZ = 0; - double xInvEntry, yInvEntry, zInvEntry; - double xInvExit, yInvExit, zInvExit; - - // find the distance between the objects on the near and far sides for x, y, z - if (rayDirection.x() > 0.0f) { - xInvEntry = (staticCollidableOffset.x() + collidableStatic.minX()) - (rayStart.x() + collidableMoving.maxX()); - xInvExit = (staticCollidableOffset.x() + collidableStatic.maxX()) - (rayStart.x() + collidableMoving.minX()); - } else { - xInvEntry = (staticCollidableOffset.x() + collidableStatic.maxX()) - (rayStart.x() + collidableMoving.minX()); - xInvExit = (staticCollidableOffset.x() + collidableStatic.minX()) - (rayStart.x() + collidableMoving.maxX()); + if (collisionFace == 0) finalResult.normalX = 1; + if (collisionFace == 1) finalResult.normalZ = 1; + if (collisionFace == 2) finalResult.normalY = 1; } - if (rayDirection.y() > 0.0f) { - yInvEntry = (staticCollidableOffset.y() + collidableStatic.minY()) - (rayStart.y() + collidableMoving.maxY()); - yInvExit = (staticCollidableOffset.y() + collidableStatic.maxY()) - (rayStart.y() + collidableMoving.minY()); - } else { - yInvEntry = (staticCollidableOffset.y() + collidableStatic.maxY()) - (rayStart.y() + collidableMoving.minY()); - yInvExit = (staticCollidableOffset.y() + collidableStatic.minY()) - (rayStart.y() + collidableMoving.maxY()); - } - - if (rayDirection.z() > 0.0f) { - zInvEntry = (staticCollidableOffset.z() + collidableStatic.minZ()) - (rayStart.z() + collidableMoving.maxZ()); - zInvExit = (staticCollidableOffset.z() + collidableStatic.maxZ()) - (rayStart.z() + collidableMoving.minZ()); - } else { - zInvEntry = (staticCollidableOffset.z() + collidableStatic.maxZ()) - (rayStart.z() + collidableMoving.minZ()); - zInvExit = (staticCollidableOffset.z() + collidableStatic.minZ()) - (rayStart.z() + collidableMoving.maxZ()); - } - - // find time of collision and time of leaving for each axis (if statement is to prevent divide by zero) - double xEntry, yEntry, zEntry; - double xExit, yExit, zExit; - - if (rayDirection.x() == 0.0f) { - xEntry = -Double.MAX_VALUE; - xExit = Double.MAX_VALUE; - } else { - xEntry = xInvEntry / rayDirection.x(); - xExit = xInvExit / rayDirection.x(); - } - - if (rayDirection.y() == 0.0f) { - yEntry = -Double.MAX_VALUE; - yExit = Double.MAX_VALUE; - } else { - yEntry = yInvEntry / rayDirection.y(); - yExit = yInvExit / rayDirection.y(); - } - - if (rayDirection.z() == 0.0f) { - zEntry = -Double.MAX_VALUE; - zExit = Double.MAX_VALUE; - } else { - zEntry = zInvEntry / rayDirection.z(); - zExit = zInvExit / rayDirection.z(); - } - - // find the earliest/latest times of collision - double entryTime = Math.max(Math.max(xEntry, yEntry), zEntry); - double exitTime = Math.min(Math.max(xExit, yExit), zExit); - double moveAmount = entryTime * 0.99999; - - if (entryTime > exitTime - || xEntry > 1.0f || yEntry > 1.0f || zEntry > 1.0f - || (xEntry < 0.0f && yEntry < 0.0f && zEntry < 0.0f) - || moveAmount > finalResult.res) { - return false; - } - - // calculate normal of collided surface - if (xEntry > yEntry && xEntry > zEntry) { - if (xInvEntry < 0.0f) { - normalx = 1.0f; - normaly = 0.0f; - normalz = 0.0f; - } else { - normalx = -1.0f; - normaly = 0.0f; - normalz = 0.0f; - } - } else if (yEntry > zEntry) { - if (yInvEntry < 0.0f) { - normalx = 0.0f; - normaly = 1.0f; - normalz = 0.0f; - } else { - normalx = 0.0f; - normaly = -1.0f; - normalz = 0.0f; - } - } else { - if (zInvEntry < 0.0f) { - normalx = 0.0f; - normaly = 0.0f; - normalz = 1.0f; - } else { - normalx = 0.0f; - normaly = 0.0f; - normalz = -1.0f; - } - } - - finalResult.res = moveAmount; - finalResult.normalX = normalx; - finalResult.normalY = normaly; - finalResult.normalZ = normalz; - return true; + return isHit; } public static boolean BoundingBoxRayIntersectionCheck(Vec start, Vec direction, BoundingBox boundingBox, Pos position) { - return BoundingBoxIntersectionCheck(BoundingBox.ZERO, start, direction, boundingBox, position); + return BoundingBoxIntersectionCheck(BoundingBox.ZERO, start, direction, boundingBox, position, new SweepResult(Double.MAX_VALUE, 0, 0, 0, null)); } } diff --git a/src/main/java/net/minestom/server/collision/ShapeImpl.java b/src/main/java/net/minestom/server/collision/ShapeImpl.java index fc4bee640..5779d20fa 100644 --- a/src/main/java/net/minestom/server/collision/ShapeImpl.java +++ b/src/main/java/net/minestom/server/collision/ShapeImpl.java @@ -92,12 +92,8 @@ final class ShapeImpl implements Shape { @NotNull Point shapePos, @NotNull BoundingBox moving, @NotNull SweepResult finalResult) { boolean hitBlock = false; for (BoundingBox blockSection : blockSections) { - // Fast check to see if a collision happens - // Uses minkowski sum - if (!RayUtils.BoundingBoxIntersectionCheck(moving, rayStart, rayDirection, blockSection, shapePos)) - continue; // Update final result if the temp result collision is sooner than the current final result - if (RayUtils.SweptAABB(moving, rayStart, rayDirection, blockSection, shapePos, finalResult)) { + if (RayUtils.BoundingBoxIntersectionCheck(moving, rayStart, rayDirection, blockSection, shapePos, finalResult)) { finalResult.collidedShapePosition = shapePos; finalResult.collidedShape = this; finalResult.blockType = block(); diff --git a/src/test/java/net/minestom/server/collision/EntityBlockPhysicsIntegrationTest.java b/src/test/java/net/minestom/server/collision/EntityBlockPhysicsIntegrationTest.java index e4b1989ad..a136a8d92 100644 --- a/src/test/java/net/minestom/server/collision/EntityBlockPhysicsIntegrationTest.java +++ b/src/test/java/net/minestom/server/collision/EntityBlockPhysicsIntegrationTest.java @@ -890,4 +890,117 @@ public class EntityBlockPhysicsIntegrationTest { assertEqualsPoint(new Pos(0.7, 42, 0.5), res.newPosition()); } + + @Test + public void entityPhysicsCheckNoMoveCache(Env env) { + var instance = env.createFlatInstance(); + var entity = new Entity(EntityType.ZOMBIE); + + entity.setInstance(instance, new Pos(5, 42, 5)).join(); + assertEquals(instance, entity.getInstance()); + + PhysicsResult res = CollisionUtils.handlePhysics(entity, Vec.ZERO); + entity.teleport(res.newPosition()); + res = CollisionUtils.handlePhysics(entity, Vec.ZERO, res); + assertEqualsPoint(new Pos(5, 42, 5), res.newPosition()); + } + + @Test + public void entityPhysicsCheckNoMoveLargeVelocityHit(Env env) { + var instance = env.createFlatInstance(); + var entity = new Entity(EntityType.ZOMBIE); + + final int distance = 20; + for (int x = 0; x < distance; ++x) instance.loadChunk(x, 0).join(); + + instance.setBlock(distance * 8, 43, 5, Block.STONE); + + entity.setInstance(instance, new Pos(5, 42, 5)).join(); + assertEquals(instance, entity.getInstance()); + + PhysicsResult res = CollisionUtils.handlePhysics(entity, Vec.ZERO); + entity.teleport(res.newPosition()); + res = CollisionUtils.handlePhysics(entity, new Vec((distance - 1) * 16, 0, 0), res); + assertEqualsPoint(new Pos(distance * 8 - 0.3, 42, 5), res.newPosition()); + } + + @Test + public void entityPhysicsCheckLargeVelocityHitNoMove(Env env) { + var instance = env.createFlatInstance(); + var entity = new Entity(EntityType.ZOMBIE); + + final int distance = 20; + for (int x = 0; x < distance; ++x) instance.loadChunk(x, 0).join(); + + instance.setBlock(distance * 8, 43, 5, Block.STONE); + + entity.setInstance(instance, new Pos(5, 42, 5)).join(); + assertEquals(instance, entity.getInstance()); + + PhysicsResult res = CollisionUtils.handlePhysics(entity, new Vec((distance - 1) * 16, 0, 0)); + entity.teleport(res.newPosition()); + res = CollisionUtils.handlePhysics(entity, Vec.ZERO, res); + assertEqualsPoint(new Pos(distance * 8 - 0.3, 42, 5), res.newPosition()); + } + + @Test + public void entityPhysicsCheckDoorSubBlockSouthRepeat(Env env) { + var instance = env.createFlatInstance(); + Block b = Block.ACACIA_TRAPDOOR.withProperties(Map.of("facing", "south", "open", "true")); + + instance.setBlock(0, 42, 0, b); + + var entity = new Entity(EntityType.ZOMBIE); + entity.setInstance(instance, new Pos(0.5, 42.5, 0.5)).join(); + assertEquals(instance, entity.getInstance()); + + PhysicsResult res = CollisionUtils.handlePhysics(entity, new Vec(0, 0, -0.4)); + entity.teleport(res.newPosition()); + res = CollisionUtils.handlePhysics(entity, new Vec(0, 0, -0.4), res); + + assertEqualsPoint(new Pos(0.5, 42.5, 0.487), res.newPosition()); + } + + @Test + public void entityPhysicsCheckCollisionDownCache(Env env) { + var instance = env.createFlatInstance(); + instance.setBlock(0, 43, 1, Block.STONE); + + var entity = new Entity(EntityType.ZOMBIE); + entity.setInstance(instance, new Pos(0, 42, 0)).join(); + assertEquals(instance, entity.getInstance()); + + PhysicsResult res = CollisionUtils.handlePhysics(entity, new Vec(0, 0, 10)); + entity.teleport(res.newPosition()); + res = CollisionUtils.handlePhysics(entity, new Vec(0, -10, 0), res); + + assertEqualsPoint(new Pos(0, 40, 0.7), res.newPosition()); + } + + @Test + public void entityPhysicsCheckGravityCached(Env env) { + var instance = env.createFlatInstance(); + instance.setBlock(0, 43, 1, Block.STONE); + + var entity = new Entity(EntityType.ZOMBIE); + entity.setInstance(instance, new Pos(0, 42, 0)).join(); + assertEquals(instance, entity.getInstance()); + + PhysicsResult res = CollisionUtils.handlePhysics(entity, new Vec(0, 0, 10)); + entity.teleport(res.newPosition()); + res = CollisionUtils.handlePhysics(entity, new Vec(0, -10, 0), res); + entity.teleport(res.newPosition()); + + PhysicsResult lastPhysicsResult; + + for (int x = 0; x < 50; ++x) { + lastPhysicsResult = res; + res = CollisionUtils.handlePhysics(entity, new Vec(0, -1.7, 0), res); + entity.teleport(res.newPosition()); + + if (x > 10) assertSame(lastPhysicsResult, res, "Physics result not cached"); + } + + assertEqualsPoint(new Pos(0, 40, 0.7), res.newPosition()); + } } \ No newline at end of file