@SuppressWarnings(value = "SuspiciousNameCombination")
 private void updateRotation(
     CharacterMovementComponent movementComp,
     CharacterStateEvent result,
     CharacterMoveInputEvent input) {
   if (movementComp.faceMovementDirection && result.getVelocity().lengthSquared() > 0.01f) {
     float yaw = (float) Math.atan2(result.getVelocity().x, result.getVelocity().z);
     result.getRotation().set(new Vector3f(0, 1, 0), yaw);
   } else {
     result.getRotation().set(new Quat4f(TeraMath.DEG_TO_RAD * input.getYaw(), 0, 0));
   }
 }
  public void setToInterpolateState(
      EntityRef entity, CharacterStateEvent a, CharacterStateEvent b, long time) {
    float t = (float) (time - a.getTime()) / (b.getTime() - a.getTime());
    Vector3f newPos = BaseVector3f.lerp(a.getPosition(), b.getPosition(), t);
    Quat4f newRot = BaseQuat4f.interpolate(a.getRotation(), b.getRotation(), t);
    LocationComponent location = entity.getComponent(LocationComponent.class);
    location.setWorldPosition(newPos);
    location.setWorldRotation(newRot);
    entity.saveComponent(location);

    CharacterMovementComponent movementComponent =
        entity.getComponent(CharacterMovementComponent.class);
    movementComponent.mode = a.getMode();
    movementComponent.setVelocity(a.getVelocity());
    movementComponent.grounded = a.isGrounded();
    if (b.getFootstepDelta() < a.getFootstepDelta()) {
      movementComponent.footstepDelta =
          t * (1 + b.getFootstepDelta() - a.getFootstepDelta()) + a.getFootstepDelta();
      if (movementComponent.footstepDelta > 1) {
        movementComponent.footstepDelta -= 1;
      }
    } else {
      movementComponent.footstepDelta =
          t * (b.getFootstepDelta() - a.getFootstepDelta()) + a.getFootstepDelta();
    }
    entity.saveComponent(movementComponent);

    extrapolateCharacterComponent(entity, b);
    setPhysicsLocation(entity, newPos);
  }
 private void extrapolateCharacterMovementComponent(EntityRef entity, CharacterStateEvent state) {
   CharacterMovementComponent movementComponent =
       entity.getComponent(CharacterMovementComponent.class);
   movementComponent.mode = state.getMode();
   movementComponent.setVelocity(state.getVelocity());
   movementComponent.grounded = state.isGrounded();
   entity.saveComponent(movementComponent);
 }
 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());
   }
 }
  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);
  }
  public static 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());
    LocationComponent location = entity.getComponent(LocationComponent.class);
    location.setWorldPosition(newPos);
    location.setWorldRotation(state.getRotation());
    entity.saveComponent(location);

    CharacterMovementComponent movementComponent =
        entity.getComponent(CharacterMovementComponent.class);
    movementComponent.mode = state.getMode();
    movementComponent.setVelocity(state.getVelocity());
    movementComponent.grounded = state.isGrounded();
    entity.saveComponent(movementComponent);

    CharacterComponent characterComponent = entity.getComponent(CharacterComponent.class);
    characterComponent.pitch = state.pitch;
    characterComponent.yaw = state.yaw;
    entity.saveComponent(characterComponent);
    setPhysicsLocation(entity, newPos);
  }
 /**
  * Sets the state of the given entity to the state represented by the CharacterStateEvent. The
  * state of the entity is determined by its LocationComponent (location and orientation of physics
  * body), CharacterMovementComponent (velocity and various movement state variables) and
  * CharacterComponent for pitch and yaw (used for the camera).
  *
  * @param entity
  * @param state
  */
 public void setToState(EntityRef entity, CharacterStateEvent state) {
   LocationComponent location = entity.getComponent(LocationComponent.class);
   CharacterMovementComponent movementComp = entity.getComponent(CharacterMovementComponent.class);
   CharacterComponent characterComponent = entity.getComponent(CharacterComponent.class);
   if (location == null || movementComp == null || characterComponent == null) {
     return;
   }
   location.setWorldPosition(state.getPosition());
   location.setWorldRotation(state.getRotation());
   entity.saveComponent(location);
   movementComp.mode = state.getMode();
   movementComp.setVelocity(state.getVelocity());
   movementComp.grounded = state.isGrounded();
   movementComp.footstepDelta = state.getFootstepDelta();
   entity.saveComponent(movementComp);
   characterComponent.pitch = state.getPitch();
   characterComponent.yaw = state.getYaw();
   entity.saveComponent(characterComponent);
   setPhysicsLocation(entity, state.getPosition());
 }
  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;
          }
        }
      }
    }
  }
  /**
   * 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);
    }
  }