@Override
  public void generate(
      AxionElementGenerationCallback callback,
      Vector3f position,
      Matrix4f rotation,
      String axionParameter) {
    Vector3f workVector = new Vector3f();

    callback.setMainBlock(position, baseBlock);
    int rangeInt = (int) range;
    for (int x = -rangeInt; x <= rangeInt; x++) {
      for (int y = -rangeInt; y <= rangeInt; y++) {
        for (int z = -rangeInt; z <= Math.min(rangeInt, maxZ); z++) {
          double distanceSquare = x * x + y * y + z * z;
          if (distanceSquare < innerRangeSquare) {
            workVector.set(x, y, z);
            rotation.transformVector(workVector);
            workVector.add(position);
            callback.setAdditionalBlock(workVector, baseBlock);
          } else if (distanceSquare < rangeSquare) {
            workVector.set(x, y, z);
            rotation.transformVector(workVector);
            workVector.add(position);
            callback.setAdditionalBlock(workVector, surroundBlock);
          }
        }
      }
    }

    callback.advance(advance);
  }
  private MoveResult move(
      final Vector3f startPosition,
      final Vector3f moveDelta,
      final float stepHeight,
      final float slopeFactor,
      final CharacterCollider collider) {
    steppedUpDist = 0;
    stepped = false;
    Vector3f position = new Vector3f(startPosition);
    boolean hitTop = false;
    boolean hitBottom = false;
    boolean hitSide;

    // Actual upwards movement
    if (moveDelta.y > 0) {
      hitTop = moveDelta.y - moveUp(moveDelta.y, collider, position) > physics.getEpsilon();
    }
    hitSide =
        moveHorizontal(
            new Vector3f(moveDelta.x, 0, moveDelta.z), collider, position, slopeFactor, stepHeight);
    if (moveDelta.y < 0 || steppedUpDist > 0) {
      float dist = (moveDelta.y < 0) ? moveDelta.y : 0;
      dist -= steppedUpDist;
      hitBottom = moveDown(dist, slopeFactor, collider, position);
    }
    if (!hitBottom && stepHeight > 0) {
      Vector3f tempPos = new Vector3f(position);
      hitBottom = moveDown(-stepHeight, slopeFactor, collider, tempPos);
      // Don't apply step down if nothing to step onto
      if (hitBottom) {
        position.set(tempPos);
      }
    }
    return new MoveResult(position, hitSide, hitBottom, hitTop);
  }
 private void followToParent(final CharacterStateEvent state, EntityRef entity) {
   LocationComponent locationComponent = entity.getComponent(LocationComponent.class);
   if (!locationComponent.getParent().equals(EntityRef.NULL)) {
     Vector3f velocity = new Vector3f(locationComponent.getWorldPosition());
     velocity.sub(state.getPosition());
     state.getVelocity().set(velocity);
     state.getPosition().set(locationComponent.getWorldPosition());
   }
 }
  private void climb(
      final CharacterStateEvent state, CharacterMoveInputEvent input, Vector3f desiredVelocity) {
    if (state.getClimbDirection() == null) {
      return;
    }
    Vector3f tmp;

    Vector3i climbDir3i = state.getClimbDirection();
    Vector3f climbDir3f = climbDir3i.toVector3f();

    Quat4f rotation = new Quat4f(TeraMath.DEG_TO_RAD * state.getYaw(), 0, 0);
    tmp = new Vector3f(0.0f, 0.0f, -1.0f);
    QuaternionUtil.quatRotate(rotation, tmp, tmp);
    float angleToClimbDirection = tmp.angle(climbDir3f);

    boolean clearMovementToDirection = !state.isGrounded();

    // facing the ladder or looking down or up
    if (angleToClimbDirection < Math.PI / 4.0 || Math.abs(input.getPitch()) > 60f) {
      float pitchAmount = state.isGrounded() ? 45f : 90f;
      float pitch = input.getPitch() > 30f ? pitchAmount : -pitchAmount;
      rotation = new Quat4f(TeraMath.DEG_TO_RAD * state.getYaw(), TeraMath.DEG_TO_RAD * pitch, 0);
      QuaternionUtil.quatRotate(rotation, desiredVelocity, desiredVelocity);

      // looking sidewards from ladder
    } else if (angleToClimbDirection < Math.PI * 3.0 / 4.0) {
      float rollAmount = state.isGrounded() ? 45f : 90f;
      tmp = new Vector3f();
      QuaternionUtil.quatRotate(rotation, climbDir3f, tmp);
      float leftOrRight = tmp.x;
      float plusOrMinus = (leftOrRight < 0f ? -1.0f : 1.0f) * (climbDir3i.x != 0 ? -1.0f : 1.0f);
      rotation =
          new Quat4f(
              TeraMath.DEG_TO_RAD * input.getYaw(),
              0f,
              TeraMath.DEG_TO_RAD * rollAmount * plusOrMinus);
      QuaternionUtil.quatRotate(rotation, desiredVelocity, desiredVelocity);

      // facing away from ladder
    } else {
      rotation = new Quat4f(TeraMath.DEG_TO_RAD * state.getYaw(), 0, 0);
      QuaternionUtil.quatRotate(rotation, desiredVelocity, desiredVelocity);
      clearMovementToDirection = false;
    }

    // clear out movement towards or away from the ladder
    if (clearMovementToDirection) {
      if (climbDir3i.x != 0) {
        desiredVelocity.x = 0f;
      }
      if (climbDir3i.z != 0) {
        desiredVelocity.z = 0f;
      }
    }
  }
  public void setToExtrapolateState(EntityRef entity, CharacterStateEvent state, long time) {
    float t = (time - state.getTime()) * 0.0001f;
    Vector3f newPos = new Vector3f(state.getVelocity());
    newPos.scale(t);
    newPos.add(state.getPosition());
    extrapolateLocationComponent(entity, state, newPos);

    extrapolateCharacterMovementComponent(entity, state);

    extrapolateCharacterComponent(entity, state);
    setPhysicsLocation(entity, newPos);
  }
 private void updateFocalDistance(HitResult hitInfo, float delta) {
   float focusRate = 4.0f; // how fast the focus distance is updated
   // if the hit result from a trace has a recorded a hit
   if (hitInfo.isHit()) {
     Vector3f playerToTargetRay = new Vector3f();
     // calculate the distance from the player to the hit point
     playerToTargetRay.sub(hitInfo.getHitPoint(), localPlayer.getPosition());
     // gradually adjust focalDistance from it's current value to the hit point distance
     focalDistance = TeraMath.lerp(focalDistance, playerToTargetRay.length(), delta * focusRate);
     // if nothing was hit, gradually adjust the focusDistance to the maximum length of the update
     // function trace
   } else {
     focalDistance = TeraMath.lerp(focalDistance, targetDistance, delta * focusRate);
   }
 }
 private float moveUp(float riseAmount, CharacterCollider collider, Vector3f position) {
   Vector3f to =
       new Vector3f(position.x, position.y + riseAmount + VERTICAL_PENETRATION_LEEWAY, position.z);
   if (collider != null) {
     SweepCallback callback = collider.sweep(position, to, VERTICAL_PENETRATION_LEEWAY, -1f);
     if (callback.hasHit()) {
       float actualDist =
           Math.max(
               0,
               ((riseAmount + VERTICAL_PENETRATION_LEEWAY) * callback.getClosestHitFraction())
                   - VERTICAL_PENETRATION_LEEWAY);
       position.y += actualDist;
       return actualDist;
     }
   }
   position.y += riseAmount;
   return riseAmount;
 }
  @ReceiveEvent(components = {HealthComponent.class})
  public void onCrash(HorizontalCollisionEvent event, EntityRef entity) {
    HealthComponent health = entity.getComponent(HealthComponent.class);

    Vector3f vel = new Vector3f(event.getVelocity());
    vel.y = 0;
    float speed = vel.length();

    if (speed > health.horizontalDamageSpeedThreshold) {
      int damage =
          (int)
              ((speed - health.horizontalDamageSpeedThreshold)
                  * health.excessSpeedDamageMultiplier);
      if (damage > 0) {
        checkDamage(
            entity, damage, EngineDamageTypes.PHYSICAL.get(), EntityRef.NULL, EntityRef.NULL);
      }
    }
  }
  @ReceiveEvent
  public void onCrash(
      HorizontalCollisionEvent event,
      EntityRef entity,
      CharacterSoundComponent characterSounds,
      HealthComponent healthComponent) {
    Vector3f horizVelocity = new Vector3f(event.getVelocity());
    horizVelocity.y = 0;
    float velocity = horizVelocity.length();

    if (velocity > healthComponent.horizontalDamageSpeedThreshold) {
      if (characterSounds.lastSoundTime + CharacterSoundSystem.MIN_TIME < time.getGameTimeInMs()) {
        StaticSound sound = random.nextItem(characterSounds.landingSounds);
        if (sound != null) {
          entity.send(new PlaySoundEvent(sound, characterSounds.landingVolume));
          characterSounds.lastSoundTime = time.getGameTimeInMs();
          entity.saveComponent(characterSounds);
        }
      }
    }
  }
 private Vector3f extractResidualMovement(
     Vector3f hitNormal, Vector3f direction, float normalMag) {
   float movementLength = direction.length();
   if (movementLength > physics.getEpsilon()) {
     direction.normalize();
     Vector3f reflectDir = Vector3fUtil.reflect(direction, hitNormal, new Vector3f());
     reflectDir.normalize();
     Vector3f perpendicularDir =
         Vector3fUtil.getPerpendicularComponent(reflectDir, hitNormal, new Vector3f());
     if (normalMag != 0.0f) {
       Vector3f perpComponent = new Vector3f(perpendicularDir);
       perpComponent.scale(normalMag * movementLength);
       direction.set(perpComponent);
     }
   }
   return direction;
 }
  @ReceiveEvent
  public void onActivate(
      ActivateEvent event, EntityRef entity, TunnelActionComponent tunnelActionComponent) {

    Vector3f dir = new Vector3f(event.getDirection());
    dir.scale(4.0f);
    Vector3f origin = new Vector3f(event.getOrigin());
    origin.add(dir);
    Vector3i blockPos = new Vector3i();

    int particleEffects = 0;
    int blockCounter = tunnelActionComponent.maxDestroyedBlocks;
    for (int s = 0; s <= tunnelActionComponent.maxTunnelDepth; s++) {
      origin.add(dir);
      if (!worldProvider.isBlockRelevant(origin)) {
        break;
      }

      for (int i = 0; i < tunnelActionComponent.maxRaysCast; i++) {
        Vector3f direction = random.nextVector3f();
        Vector3f impulse = new Vector3f(direction);
        impulse.scale(tunnelActionComponent.explosiveForce);

        for (int j = 0; j < 3; j++) {
          Vector3f target = new Vector3f(origin);

          target.x += direction.x * j;
          target.y += direction.y * j;
          target.z += direction.z * j;

          blockPos.set((int) target.x, (int) target.y, (int) target.z);

          Block currentBlock = worldProvider.getBlock(blockPos);

          if (currentBlock.isDestructible()) {
            if (particleEffects < tunnelActionComponent.maxParticalEffects) {
              EntityBuilder builder = entityManager.newBuilder("engine:smokeExplosion");
              builder.getComponent(LocationComponent.class).setWorldPosition(target);
              builder.build();
              particleEffects++;
            }
            if (random.nextFloat() < tunnelActionComponent.thoroughness) {
              EntityRef blockEntity = blockEntityRegistry.getEntityAt(blockPos);
              blockEntity.send(
                  new DoDamageEvent(
                      tunnelActionComponent.damageAmount, tunnelActionComponent.damageType));
            }

            blockCounter--;
          }

          if (blockCounter <= 0) {
            return;
          }
        }
      }
    }
    // No blocks were destroyed, so cancel the event
    if (blockCounter == tunnelActionComponent.maxDestroyedBlocks) {
      event.consume();
    }
  }
 @Override
 public String toString(Vector3f value) {
   return value != null ? value.toString() : "";
 }
  private void walk(
      final CharacterMovementComponent movementComp,
      final CharacterStateEvent state,
      CharacterMoveInputEvent input,
      EntityRef entity) {
    Vector3f desiredVelocity = new Vector3f(input.getMovementDirection());

    float lengthSquared = desiredVelocity.lengthSquared();

    // If the length of desired movement is > 1, normalise it to prevent movement being faster than
    // allowed.
    // (Desired velocity < 1 is allowed, as the character may wish to walk/crawl/otherwise move
    // slowly)
    if (lengthSquared > 1) {
      desiredVelocity.normalize();
    }
    desiredVelocity.scale(movementComp.speedMultiplier);

    float maxSpeed = getMaxSpeed(entity, movementComp);
    if (input.isRunning()) {
      maxSpeed *= movementComp.runFactor;
    }

    // As we can't use it, remove the y component of desired movement while maintaining speed.
    if (movementComp.grounded && desiredVelocity.y != 0) {
      float speed = desiredVelocity.length();
      desiredVelocity.y = 0;
      if (desiredVelocity.x != 0 || desiredVelocity.z != 0) {
        desiredVelocity.normalize();
        desiredVelocity.scale(speed);
      }
    }
    desiredVelocity.scale(maxSpeed);

    if (movementComp.mode == MovementMode.CLIMBING) {
      climb(state, input, desiredVelocity);
    }

    // Modify velocity towards desired, up to the maximum rate determined by friction
    Vector3f velocityDiff = new Vector3f(desiredVelocity);
    velocityDiff.sub(state.getVelocity());
    velocityDiff.scale(Math.min(movementComp.mode.scaleInertia * input.getDelta(), 1.0f));
    Vector3f endVelocity = new Vector3f(state.getVelocity());
    endVelocity.x += velocityDiff.x;
    endVelocity.z += velocityDiff.z;
    if (movementComp.mode.scaleGravity == 0) {
      // apply the velocity without gravity
      endVelocity.y += velocityDiff.y;
    } else if (movementComp.mode.applyInertiaToVertical) {
      endVelocity.y +=
          Math.max(
              -TERMINAL_VELOCITY,
              velocityDiff.y - (GRAVITY * movementComp.mode.scaleGravity) * input.getDelta());
    } else {
      endVelocity.y =
          Math.max(
              -TERMINAL_VELOCITY,
              state.getVelocity().y
                  - (GRAVITY * movementComp.mode.scaleGravity) * input.getDelta());
    }
    Vector3f moveDelta = new Vector3f(endVelocity);
    moveDelta.scale(input.getDelta());
    CharacterCollider collider =
        movementComp.mode.useCollision ? physics.getCharacterCollider(entity) : null;
    MoveResult moveResult =
        move(
            state.getPosition(),
            moveDelta,
            (state.getMode() != MovementMode.CLIMBING
                    && state.isGrounded()
                    && movementComp.mode.canBeGrounded)
                ? movementComp.stepHeight
                : 0,
            movementComp.slopeFactor,
            collider);
    Vector3f distanceMoved = new Vector3f(moveResult.getFinalPosition());
    distanceMoved.sub(state.getPosition());
    state.getPosition().set(moveResult.getFinalPosition());
    if (input.isFirstRun() && distanceMoved.length() > 0) {
      entity.send(new MovedEvent(distanceMoved, state.getPosition()));
    }

    if (moveResult.isBottomHit()) {
      if (!state.isGrounded() && movementComp.mode.canBeGrounded) {
        if (input.isFirstRun()) {
          Vector3f landVelocity = new Vector3f(state.getVelocity());
          landVelocity.y +=
              (distanceMoved.y / moveDelta.y) * (endVelocity.y - state.getVelocity().y);
          logger.debug("Landed at " + landVelocity);
          entity.send(new VerticalCollisionEvent(state.getPosition(), landVelocity));
        }
        state.setGrounded(true);
      }
      endVelocity.y = 0;

      // Jumping is only possible, if the entity is standing on ground
      if (input.isJumpRequested()) {
        state.setGrounded(false);
        endVelocity.y += movementComp.jumpSpeed;
        if (input.isFirstRun()) {
          entity.send(new JumpEvent());
        }
      }
    } else {
      if (moveResult.isTopHit() && endVelocity.y > 0) {
        endVelocity.y = -0.5f * endVelocity.y;
      }
      state.setGrounded(false);
    }
    state.getVelocity().set(endVelocity);
    if (input.isFirstRun() && moveResult.isHorizontalHit()) {
      entity.send(new HorizontalCollisionEvent(state.getPosition(), state.getVelocity()));
    }
    if (state.isGrounded()
        || movementComp.mode == MovementMode.SWIMMING
        || movementComp.mode == MovementMode.DIVING) {
      state.setFootstepDelta(
          state.getFootstepDelta()
              + distanceMoved.length() / movementComp.distanceBetweenFootsteps);
      if (state.getFootstepDelta() > 1) {
        state.setFootstepDelta(state.getFootstepDelta() - 1);
        if (input.isFirstRun()) {
          switch (movementComp.mode) {
            case WALKING:
              entity.send(new FootstepEvent());
              break;
            case DIVING:
            case SWIMMING:
              entity.send(new SwimStrokeEvent(worldProvider.getBlock(state.getPosition())));
              break;
          }
        }
      }
    }
  }
  private boolean moveHorizontal(
      Vector3f horizMove,
      CharacterCollider collider,
      Vector3f position,
      float slopeFactor,
      float stepHeight) {
    float remainingFraction = 1.0f;
    float dist = horizMove.length();
    if (dist < physics.getEpsilon()) {
      return false;
    }
    boolean horizontalHit = false;
    Vector3f normalizedDir = Vector3fUtil.safeNormalize(horizMove, new Vector3f());

    if (collider == null) {
      // ignore collision
      normalizedDir.scale(dist);
      position.add(normalizedDir);
      return false;
    }

    Vector3f targetPos = new Vector3f(normalizedDir);
    targetPos.scale(dist + HORIZONTAL_PENETRATION_LEEWAY);
    targetPos.add(position);
    int iteration = 0;
    Vector3f lastHitNormal = new Vector3f(0, 1, 0);
    while (remainingFraction >= 0.01f && iteration++ < 10) {
      SweepCallback callback =
          collider.sweep(position, targetPos, HORIZONTAL_PENETRATION, slopeFactor);

      /* Note: this isn't quite correct (after the first iteration the closestHitFraction is only for part of the moment)
      but probably close enough */
      float actualDist =
          Math.max(
              0,
              (dist + HORIZONTAL_PENETRATION_LEEWAY) * callback.getClosestHitFraction()
                  - HORIZONTAL_PENETRATION_LEEWAY);
      if (actualDist != 0) {
        remainingFraction -= actualDist / dist;
      }
      if (callback.hasHit()) {
        if (actualDist > physics.getEpsilon()) {
          Vector3f actualMove = new Vector3f(normalizedDir);
          actualMove.scale(actualDist);
          position.add(actualMove);
        }
        dist -= actualDist;
        Vector3f newDir = new Vector3f(normalizedDir);
        newDir.scale(dist);
        float slope = callback.getHitNormalWorld().dot(new Vector3f(0, 1, 0));

        // We step up if we're hitting a big slope, or if we're grazing
        // the ground, otherwise we move up a shallow slope.
        if (slope < slopeFactor || 1 - slope < physics.getEpsilon()) {
          boolean stepping =
              checkStep(collider, position, newDir, callback, slopeFactor, stepHeight);
          if (!stepping) {
            horizontalHit = true;
            Vector3f newHorizDir = new Vector3f(newDir.x, 0, newDir.z);
            Vector3f horizNormal =
                new Vector3f(callback.getHitNormalWorld().x, 0, callback.getHitNormalWorld().z);
            if (horizNormal.lengthSquared() > physics.getEpsilon()) {
              horizNormal.normalize();
              if (lastHitNormal.dot(horizNormal) > physics.getEpsilon()) {
                break;
              }
              lastHitNormal.set(horizNormal);
              extractResidualMovement(horizNormal, newHorizDir);
            }
            newDir.set(newHorizDir);
          }
        } else {
          // Hitting a shallow slope, move up it
          Vector3f newHorizDir = new Vector3f(newDir.x, 0, newDir.z);
          extractResidualMovement(callback.getHitNormalWorld(), newDir);
          Vector3f modHorizDir = new Vector3f(newDir);
          modHorizDir.y = 0;
          newDir.scale(newHorizDir.length() / modHorizDir.length());
        }
        float sqrDist = newDir.lengthSquared();
        if (sqrDist > physics.getEpsilon()) {
          newDir.normalize();
          if (newDir.dot(normalizedDir) <= 0.0f) {
            break;
          }
        } else {
          break;
        }
        dist = (float) Math.sqrt(sqrDist);
        normalizedDir.set(newDir);
        targetPos.set(normalizedDir);
        targetPos.scale(dist + HORIZONTAL_PENETRATION_LEEWAY);
        targetPos.add(position);
      } else {
        normalizedDir.scale(dist);
        position.add(normalizedDir);
        break;
      }
    }
    return horizontalHit;
  }
  private boolean moveDown(
      float dist, float slopeFactor, CharacterCollider collider, Vector3f position) {
    if (collider == null) {
      position.y += dist;
      return false;
    }

    float remainingDist = -dist;
    Vector3f targetPos = new Vector3f(position);
    targetPos.y -= remainingDist + VERTICAL_PENETRATION_LEEWAY;
    Vector3f normalizedDir = new Vector3f(0, -1, 0);
    boolean hit = false;
    int iteration = 0;
    while (remainingDist > physics.getEpsilon() && iteration++ < 10) {
      SweepCallback callback = collider.sweep(position, targetPos, VERTICAL_PENETRATION, -1.0f);
      float actualDist =
          Math.max(
              0,
              (remainingDist + VERTICAL_PENETRATION_LEEWAY) * callback.getClosestHitFraction()
                  - VERTICAL_PENETRATION_LEEWAY);
      Vector3f expectedMove = new Vector3f(targetPos);
      expectedMove.sub(position);
      if (expectedMove.lengthSquared() > physics.getEpsilon()) {
        expectedMove.normalize();
        expectedMove.scale(actualDist);
        position.add(expectedMove);
      }
      remainingDist -= actualDist;
      if (remainingDist < physics.getEpsilon()) {
        break;
      }
      if (callback.hasHit()) {
        float originalSlope = callback.getHitNormalWorld().dot(new Vector3f(0, 1, 0));
        if (originalSlope < slopeFactor) {
          float slope = callback.calculateAverageSlope(originalSlope, CHECK_FORWARD_DIST);
          if (slope < slopeFactor) {
            remainingDist -= actualDist;
            expectedMove.set(targetPos);
            expectedMove.sub(position);
            extractResidualMovement(callback.getHitNormalWorld(), expectedMove);
            float sqrDist = expectedMove.lengthSquared();
            if (sqrDist > physics.getEpsilon()) {
              expectedMove.normalize();
              if (expectedMove.dot(normalizedDir) <= 0.0f) {
                hit = true;
                break;
              }
            } else {
              hit = true;
              break;
            }
            if (expectedMove.y > -physics.getEpsilon()) {
              hit = true;
              break;
            }
            normalizedDir.set(expectedMove);
            expectedMove.scale(-remainingDist / expectedMove.y + HORIZONTAL_PENETRATION_LEEWAY);
            targetPos.set(position);
            targetPos.add(expectedMove);
          } else {
            hit = true;
            break;
          }
        } else {
          hit = true;
          break;
        }
      } else {
        break;
      }
    }
    if (iteration >= 10) {
      hit = true;
    }
    return hit;
  }
  /**
   * Checks whether a character should change movement mode (from being underwater or in a ladder).
   * A higher and lower point of the character is tested for being in water, only if both points are
   * in water does the character count as swimming. <br>
   * <br>
   * Sends the OnEnterLiquidEvent and OnLeaveLiquidEvent events.
   *
   * @param movementComp The movement component of the character.
   * @param state The current state of the character.
   */
  private void checkMode(
      final CharacterMovementComponent movementComp,
      final CharacterStateEvent state,
      final CharacterStateEvent oldState,
      EntityRef entity,
      boolean firstRun) {
    // If we are ghosting or we can't move, the mode cannot be changed.
    if (!state.getMode().respondToEnvironment) {
      return;
    }

    Vector3f worldPos = state.getPosition();
    Vector3f top = new Vector3f(worldPos);
    Vector3f bottom = new Vector3f(worldPos);
    top.y += 0.5f * movementComp.height;
    bottom.y -= 0.5f * movementComp.height;

    final boolean topUnderwater = worldProvider.getBlock(top).isLiquid();
    final boolean bottomUnderwater = worldProvider.getBlock(bottom).isLiquid();

    final boolean newSwimming = !topUnderwater && bottomUnderwater;
    final boolean newDiving = topUnderwater && bottomUnderwater;
    boolean newClimbing = false;

    // TODO: refactor this knot of if-else statements into something easy to read. Some sub-methods
    // and switch statements would be nice.
    if (!newSwimming && !newDiving) { // TODO: generalize to isClimbingAllowed() or similar
      Vector3f[] sides = {
        new Vector3f(worldPos),
        new Vector3f(worldPos),
        new Vector3f(worldPos),
        new Vector3f(worldPos),
        new Vector3f(worldPos)
      };
      float factor = 1.0f;
      sides[0].x += factor * movementComp.radius;
      sides[1].x -= factor * movementComp.radius;
      sides[2].z += factor * movementComp.radius;
      sides[3].z -= factor * movementComp.radius;
      sides[4].y -= movementComp.height;

      float distance = 100f;

      for (Vector3f side : sides) {
        Block block = worldProvider.getBlock(side);
        if (block.isClimbable()) {
          // If any of our sides are near a climbable block, check if we are near to the side
          Vector3i myPos = new Vector3i(worldPos, 0.5f);
          Vector3i climbBlockPos = new Vector3i(side, 0.5f);
          Vector3i dir = new Vector3i(block.getDirection().getVector3i());
          float currentDistance = 10f;

          if (dir.x != 0
              && Math.abs(worldPos.x - (float) climbBlockPos.x + (float) dir.x * .5f)
                  < movementComp.radius + 0.1f) {
            newClimbing = true;
            if (myPos.x < climbBlockPos.x) {
              dir.x = -dir.x;
            }
            currentDistance = Math.abs(climbBlockPos.z - worldPos.z);

          } else if (dir.z != 0
              && Math.abs(worldPos.z - (float) climbBlockPos.z + (float) dir.z * .5f)
                  < movementComp.radius + 0.1f) {
            newClimbing = true;
            if (myPos.z < climbBlockPos.z) {
              dir.z = -dir.z;
            }
            currentDistance = Math.abs(climbBlockPos.z - worldPos.z);
          }

          // if there are multiple climb blocks, choose the nearest one. This can happen when there
          // are two
          // adjacent ledges around a corner.
          if (currentDistance < distance) {
            distance = currentDistance;
            state.setClimbDirection(dir);
          }
        }
      }
    }

    if (newDiving) {
      if (state.getMode() != MovementMode.DIVING) {
        state.setMode(MovementMode.DIVING);
      }
    } else if (newSwimming) {
      if (state.getMode() != MovementMode.SWIMMING) {
        state.setMode(MovementMode.SWIMMING);
      }
      state.getVelocity().y += 0.02;
    } else if (state.getMode() == MovementMode.SWIMMING) {
      if (newClimbing) {
        state.setMode(MovementMode.CLIMBING);
        state.getVelocity().y = 0;
      } else {
        if (state.getVelocity().y > 0) {
          state.getVelocity().y += 4;
        }
        state.setMode(MovementMode.WALKING);
      }
    } else if (newClimbing != (state.getMode() == MovementMode.CLIMBING)) {
      // We need to toggle the climbing mode
      state.getVelocity().y = 0;
      state.setMode((newClimbing) ? MovementMode.CLIMBING : MovementMode.WALKING);
    }
  }