PathFollower

package com.hypixel.hytale.server.npc.navigation;

import com.hypixel.hytale.component.ComponentAccessor;
import com.hypixel.hytale.component.Ref;
import com.hypixel.hytale.math.vector.Vector3d;
import com.hypixel.hytale.server.core.modules.physics.util.PhysicsMath;
import com.hypixel.hytale.server.core.universe.world.storage.EntityStore;
import com.hypixel.hytale.server.npc.NPCPlugin;
import com.hypixel.hytale.server.npc.movement.Steering;
import com.hypixel.hytale.server.npc.movement.controllers.MotionController;
import com.hypixel.hytale.server.npc.movement.controllers.ProbeMoveData;
import com.hypixel.hytale.server.npc.util.NPCPhysicsMath;
import java.util.logging.Level;
import javax.annotation.Nonnull;
import javax.annotation.Nullable;

public class PathFollower {
   @Nullable
   protected IWaypoint currentWaypoint;
   protected double currentWaypointDistanceSquared;
   protected PathFollower.FrozenWaypoint frozenWaypoint;
   protected boolean isWaypointFrozen;
   protected final Vector3d lastWaypointPosition = new Vector3d();
   protected final Vector3d direction = new Vector3d();
   protected final Vector3d tempVector = new Vector3d();
   protected final Vector3d tempPath = new Vector3d();
   protected final Vector3d projection = new Vector3d();
   protected final Vector3d rejection = new Vector3d();
   protected int pathSmoothing;
   protected double blendHeading;
   protected double relativeSpeed;
   protected double relativeSpeedWaypoint;
   protected double waypointRadius;
   protected double rejectionWeight = 3.0;
   protected double waypointRadiusSquared;
   protected boolean debugNodes = false;
   protected boolean shouldSmoothPath = true;

   public PathFollower() {
   }

   public void setPathSmoothing(int pathSmoothing) {
      this.pathSmoothing = pathSmoothing;
   }

   public double getRelativeSpeed() {
      return this.relativeSpeed;
   }

   public void setRelativeSpeed(double relativeSpeed) {
      this.relativeSpeed = relativeSpeed;
   }

   public void setRelativeSpeedWaypoint(double relativeSpeedWaypoint) {
      this.relativeSpeedWaypoint = relativeSpeedWaypoint;
   }

   public void setWaypointRadius(double waypointRadius) {
      this.waypointRadius = waypointRadius;
      this.waypointRadiusSquared = waypointRadius * waypointRadius;
   }

   public void setDebugNodes(boolean debugNodes) {
      this.debugNodes = debugNodes;
   }

   public boolean shouldSmoothPath() {
      return this.shouldSmoothPath;
   }

   public void setRejectionWeight(double rejectionWeight) {
      this.rejectionWeight = rejectionWeight;
   }

   public void setBlendHeading(double blendHeading) {
      if (blendHeading < 0.0 || blendHeading > 1.0) {
         blendHeading = 0.0;
      }

      this.blendHeading = blendHeading;
   }

   @Nullable
   public IWaypoint getCurrentWaypoint() {
      return this.currentWaypoint;
   }

   @Nullable
   public Vector3d getCurrentWaypointPosition() {
      return this.currentWaypoint == null ? null : this.currentWaypoint.getPosition();
   }

   @Nullable
   public IWaypoint getNextWaypoint() {
      return this.currentWaypoint == null ? null : this.currentWaypoint.next();
   }

   @Nullable
   public Vector3d getNextWaypointPosition() {
      IWaypoint waypoint = this.getNextWaypoint();
      return waypoint == null ? null : waypoint.getPosition();
   }

   public void setPath(IWaypoint firstWaypoint, @Nonnull Vector3d startPosition) {
      this.currentWaypoint = firstWaypoint;
      this.lastWaypointPosition.assign(startPosition);
      this.currentWaypointDistanceSquared = 1.7976931348623157E308;
      this.shouldSmoothPath = true;
      this.isWaypointFrozen = false;
   }

   public void clearPath() {
      this.currentWaypoint = null;
      this.isWaypointFrozen = false;
   }

   public boolean pathInFinalStage() {
      if (this.currentWaypoint == null) {
         return true;
      } else if (this.currentWaypoint.next() != null) {
         return false;
      } else {
         this.freezeWaypoint();
         return true;
      }
   }

   public boolean freezeWaypoint() {
      if (this.currentWaypoint == null) {
         return false;
      } else {
         if (this.frozenWaypoint == null) {
            this.frozenWaypoint = new PathFollower.FrozenWaypoint();
         }

         if (this.currentWaypoint == this.frozenWaypoint) {
            return true;
         } else {
            this.frozenWaypoint.position.assign(this.currentWaypoint.getPosition());
            this.currentWaypoint = this.frozenWaypoint;
            this.isWaypointFrozen = true;
            return true;
         }
      }
   }

   public boolean isWaypointFrozen() {
      return this.isWaypointFrozen;
   }

   public void setWaypointFrozen(boolean waypointFrozen) {
      this.isWaypointFrozen = waypointFrozen;
   }

   public void executePath(@Nonnull Vector3d currentPosition, @Nonnull MotionController activeMotionController, @Nonnull Steering desiredSteering) {
      Vector3d target = this.getCurrentWaypointPosition();
      if (target != null) {
         this.tempVector.assign(target).subtract(currentPosition);
         double length = this.tempVector.length();
         desiredSteering.setMaxDistance(length);
         if (length > this.waypointRadius) {
            this.direction.assign(this.tempVector);
            this.computeRejection(currentPosition, target, activeMotionController);
            this.direction.subtract(this.rejection);
            desiredSteering.setTranslation(this.direction.scale(this.relativeSpeed / length));
         } else {
            if (length > 0.1) {
               this.direction.assign(this.tempVector);
            }

            desiredSteering.setTranslation(this.direction.scale(this.relativeSpeedWaypoint / length));
         }
      }
   }

   public void computeRejection(@Nonnull Vector3d currentPosition, @Nonnull Vector3d target, @Nonnull MotionController activeMotionController) {
      this.tempPath.assign(target).subtract(this.lastWaypointPosition).scale(activeMotionController.getComponentSelector());
      this.tempVector.assign(currentPosition).subtract(this.lastWaypointPosition).scale(activeMotionController.getComponentSelector());
      double dotDD = this.tempPath.squaredLength();
      double dotDP = this.tempPath.dot(this.tempVector);
      this.projection.assign(this.tempPath).scale(dotDP / dotDD);
      this.rejection.assign(this.tempVector).subtract(this.projection).scale(this.rejectionWeight);
   }

   public boolean updateCurrentTarget(@Nonnull Vector3d entityPosition, @Nonnull MotionController motionController) {
      if (this.currentWaypoint == null) {
         return false;
      } else {
         Vector3d waypointPosition = this.currentWaypoint.getPosition();
         double distanceSquared = motionController.waypointDistanceSquared(waypointPosition, entityPosition);
         double projectionLength = 0.0;
         boolean reachedWaypoint;
         if (!(distanceSquared <= 1.0000000000000002E-10) && (!(distanceSquared < 0.01) || !(distanceSquared > this.currentWaypointDistanceSquared))) {
            projectionLength = NPCPhysicsMath.dotProduct(waypointPosition, this.lastWaypointPosition, entityPosition, motionController.getComponentSelector());
            reachedWaypoint = projectionLength < 0.0;
         } else {
            reachedWaypoint = true;
         }

         this.currentWaypointDistanceSquared = distanceSquared;
         if (this.debugNodes) {
            NPCPlugin.get()
               .getLogger()
               .at(Level.INFO)
               .log(
                  "=== Target len=%s before=%s targetdist=%s  proj=%s pos=%s tgt=%s",
                  this.currentWaypoint.getLength(),
                  !reachedWaypoint,
                  Math.sqrt(distanceSquared),
                  projectionLength,
                  Vector3d.formatShortString(entityPosition),
                  Vector3d.formatShortString(waypointPosition)
               );
         }

         if (reachedWaypoint) {
            this.lastWaypointPosition.assign(waypointPosition);
            this.currentWaypoint = this.currentWaypoint.next();
            if (this.currentWaypoint == null) {
               this.isWaypointFrozen = false;
               return false;
            }

            this.currentWaypointDistanceSquared = 1.7976931348623157E308;
            this.shouldSmoothPath = true;
            waypointPosition = this.currentWaypoint.getPosition();
            if (this.blendHeading > 0.0) {
               distanceSquared = motionController.waypointDistanceSquared(waypointPosition, entityPosition);
            }
         }

         motionController.requirePreciseMovement(waypointPosition);
         if (this.blendHeading <= 0.0) {
            return true;
         } else {
            motionController.enableHeadingBlending();
            if (distanceSquared > this.waypointRadiusSquared) {
               return true;
            } else {
               IWaypoint nextWaypoint = this.getNextWaypoint();
               if (nextWaypoint == null) {
                  return true;
               } else {
                  this.tempVector.assign(nextWaypoint.getPosition()).subtract(waypointPosition);
                  distanceSquared = NPCPhysicsMath.projectedLengthSquared(this.tempVector, motionController.getComponentSelector());
                  if (distanceSquared < 0.001) {
                     return true;
                  } else {
                     float yaw = PhysicsMath.headingFromDirection(this.tempVector.x, this.tempVector.z);
                     motionController.enableHeadingBlending((double)yaw, waypointPosition, this.blendHeading);
                     return true;
                  }
               }
            }
         }
      }
   }

   public void smoothPath(
      @Nonnull Ref<EntityStore> ref,
      @Nonnull Vector3d position,
      @Nonnull MotionController motionController,
      @Nonnull ProbeMoveData probeMoveData,
      @Nonnull ComponentAccessor<EntityStore> componentAccessor
   ) {
      this.shouldSmoothPath = false;
      if (this.pathSmoothing > 0) {
         IWaypoint node = this.currentWaypoint;
         if (node != null) {
            int startLength = node.getLength();

            IWaypoint startNode;
            int skip;
            do {
               startNode = node;
               int length = node.getLength();
               if (length == 1) {
                  skip = 0;
                  break;
               }

               skip = Math.min(this.pathSmoothing, length - 1);
               node = node.advance(skip);
            } while (this.canMoveTo(ref, motionController, position, node.getPosition(), probeMoveData, componentAccessor));

            while (skip > 1) {
               int middleSkip = skip / 2;
               IWaypoint middleNode = startNode.advance(middleSkip);
               if (this.canMoveTo(ref, motionController, position, middleNode.getPosition(), probeMoveData, componentAccessor)) {
                  startNode = middleNode;
                  skip -= middleSkip;
               } else {
                  skip = middleSkip;
               }
            }

            if (this.debugNodes) {
               int l = startNode.getLength();
               NPCPlugin.get()
                  .getLogger()
                  .at(Level.INFO)
                  .log(
                     "=== New Target len=%s skipped=%s pos=%s tgt=%s dist=%s",
                     l,
                     startLength - l,
                     Vector3d.formatShortString(position),
                     Vector3d.formatShortString(startNode.getPosition()),
                     position.distanceTo(startNode.getPosition())
                  );
            }

            this.currentWaypoint = startNode;
            this.currentWaypointDistanceSquared = 1.7976931348623157E308;
         }
      }
   }

   protected boolean canMoveTo(
      @Nonnull Ref<EntityStore> ref,
      @Nonnull MotionController motionController,
      @Nonnull Vector3d position,
      @Nonnull Vector3d targetPosition,
      @Nonnull ProbeMoveData probeMoveData,
      @Nonnull ComponentAccessor<EntityStore> componentAccessor
   ) {
      probeMoveData.setPosition(position).setTargetPosition(targetPosition);
      return probeMoveData.canMoveTo(ref, motionController, 9.999999994736442E-8, 0.5, componentAccessor);
   }

   private static class FrozenWaypoint implements IWaypoint {
      protected final Vector3d position = new Vector3d();

      private FrozenWaypoint() {
      }

      @Override
      public int getLength() {
         return 1;
      }

      @Nonnull
      @Override
      public Vector3d getPosition() {
         return this.position;
      }

      @Nullable
      @Override
      public IWaypoint advance(int skip) {
         return null;
      }

      @Nullable
      @Override
      public IWaypoint next() {
         return null;
      }
   }
}