TheVVaS-Assets/RPGCore.Movement/Runtime/ObjectModules/UnitMovement/UnitMovementModule.cs

using System;
using RPGCore.Movement.ObjectModules.UnitMovement.Events;
using RPGCore.Movement.ObjectModules.UnitMovement.Actions;
using RPGCore.Core;
using RPGCore.Core.Objects;
using RPGCoreCommon.Helpers;
using RPGCoreCommon.Settings;
using UnityEngine;
using UnityEngine.AI;

namespace RPGCore.Movement.ObjectModules.UnitMovement
{
    [Serializable]
    [RequireComponent(typeof(UnitObject))]
    [RequireComponent(typeof(CapsuleCollider))]
    public class UnitMovementModule : ObjectModule<UnitObject>
    {
        // SERIALIZED
        [Header("General")]
        [SerializeField] public float skin = 0.03f;
        [SerializeField] public float groundDistance = 0.1f;
        [SerializeField] public float groundFriction = 5f;
        [Header("On Ground")]
        [SerializeField] public float moveMaxSpeed = 3f;
        [SerializeField] public float moveMaxSlope = 45f;
        [SerializeField] public float groundAcceleration = 15f;
        [SerializeField] public float sprintSpeedMultiplier = 2f;
        [SerializeField] public float sprintMaxDeviation = 67.5f;
        [SerializeField] public float jumpPower = 5f;
        [Header("In Air")]
        [SerializeField] public float airMaxSpeed = 3f;
        [SerializeField] public float airAcceleration = 1.5f;
        [SerializeField] public float gravity = 9.81f;
        [SerializeField] public float fallMaxSpeed = 20f;
        [Header("(optional) Rotation")]
        [SerializeField] public float rotationSpeed = 180f;
        [SerializeField] public UnitMovementRotateType rotateType;
        
        // RUNTIME
        private RotateAction _rotateAction;

        public RaycastHit groundHit { get; private set; }
        public float groundSteepness { get; private set; }
        public float groundAngle { get; private set; }
        public bool isOnGround { get; private set; }
        public float rotateYaw { get; set; }
        public Vector3 accelerationVector { get; private set; }
        public Vector3 moveInput { get; set; }
        public bool isMoving { get; private set; }
        public bool isSprinting { get; set; }

        private void Awake()
        {
            var agent = parent.GetComponent<NavMeshAgent>();
            agent.enabled = false;
            agent.speed = moveMaxSpeed;
            agent.updateRotation = false;
            
            parent.events.RegisterAfter<MoveStartEvent>(OnMoveStart);
            parent.events.Register<MoveEndEvent>(OnMoveEnd);
        }
        
        private void FixedUpdate()
        {
            CheckGround();
            HandleGroundFriction();
            HandleMovement();
            HandleGravity();

            HandleRotation();
        }

        private void HandleMovement()
        {
            if (moveInput == Vector3.zero)
            {
                isSprinting = false;
                return;
            }
            
            // TODO: ruch rampą w górę i zaczyna się pozioma podłoga - jeśli velocity.y podobne do accelerationVector.y to wtedy snapping?
            
            CheckForwardSprint();
            accelerationVector = moveInput * (GetSpeed() * Time.fixedDeltaTime);
            MovementOnGroundNormal();
            MovementOnObstacle();
            
            AddMoveVectorToVelocity();
        }
        
        private void CheckForwardSprint()
        {
            var unitYaw = parent.rigidbody.rotation.eulerAngles.y;
            var moveYaw = Mathf.Atan2(moveInput.x, moveInput.z) * Mathf.Rad2Deg;
        
            if (Mathf.Abs(Mathf.DeltaAngle(unitYaw, moveYaw)) < sprintMaxDeviation) return;
            
            isSprinting = false;
        }

        private float GetSpeed()
        {
            var speed = isOnGround ? groundAcceleration : airAcceleration;
            if (isSprinting) speed *= sprintSpeedMultiplier;
            return speed;
        }

        private void MovementOnGroundNormal()
        {
            if (groundAngle > moveMaxSlope) return;
            accelerationVector = Quaternion.FromToRotation(Vector3.up, groundHit.normal) * accelerationVector;
        }

        private void MovementOnObstacle()
        {
            const int limit = 3;

            var currentPosition = parent.rigidbody.position;
            var tempPosition = currentPosition;
            var tempDirection = accelerationVector.normalized;
            var tempDistance = accelerationVector.magnitude;
            
            for (var i = 1; i <= limit; i++)
            {
                var isHit = Physics.CapsuleCast(
                    tempPosition.AddY(parent.unitCollider.radius),
                    tempPosition.AddY(parent.unitCollider.height - parent.unitCollider.radius),
                    parent.unitCollider.radius,
                    tempDirection,
                    out var hit,
                    tempDistance,
                    1 << SettingsManager.Get<CoreSettings>().staticLayer
                );

                if (!isHit)
                {
                    // No obstacle found
                    accelerationVector = tempPosition + tempDirection * tempDistance - currentPosition;
                    return;
                }
                
                tempPosition += tempDirection * (hit.distance - skin);
                tempDistance -= hit.distance - skin;

                if (i == limit)
                {
                    // Checks reached limit - dont check further
                    accelerationVector = tempPosition - currentPosition;
                    return;
                }

                var planeNormal = hit.normal;
                if (Vector3.Angle(hit.normal, Vector3.up) > moveMaxSlope) planeNormal = planeNormal.SetY(0).normalized;
                var projectedVector = Vector3.ProjectOnPlane(tempDirection * tempDistance, planeNormal);
                
                tempDistance = projectedVector.magnitude;
                tempDirection = projectedVector.normalized;
            }

            throw new Exception($"{nameof(MovementOnObstacle)} - this should never be reached!");
        }

        private void AddMoveVectorToVelocity()
        {
            var speed = Vector3.Dot(parent.rigidbody.linearVelocity, accelerationVector.normalized);
            var maxSpeed = isOnGround ? moveMaxSpeed : airMaxSpeed;
            if (isSprinting) maxSpeed *= sprintSpeedMultiplier;
            var possibleSpeed = maxSpeed - speed;
            
            if (possibleSpeed <= 0) return;
            
            var moveVectorMultiplier = Mathf.Min(possibleSpeed / accelerationVector.magnitude, 1f);
            parent.rigidbody.linearVelocity += accelerationVector * moveVectorMultiplier;
        }
        
        private void CheckGround()
        {
            var groundFound = Physics.SphereCast(
                parent.transform.position.AddY(parent.unitCollider.radius),
                parent.unitCollider.radius - skin,
                Vector3.down,
                out var hit,
                skin + groundDistance,
                1 << SettingsManager.Get<CoreSettings>().staticLayer);
            groundHit = hit;

            groundAngle = 90f;
            groundSteepness = 1f;

            if (groundFound)
            {
                groundAngle = Vector3.Angle(groundHit.normal, Vector3.up);
                groundSteepness = 0f;

                if (groundAngle > moveMaxSlope)
                {
                    groundSteepness = Mathf.InverseLerp(0f, 90f, groundAngle);
                    groundFound = false;
                }
            }

            var wasOnGround = isOnGround;
            isOnGround = groundFound;
            
            if (wasOnGround && !isOnGround)
                parent.events.Invoke(new FallEvent { unit = parent });
            
            if (!wasOnGround && isOnGround)
                parent.events.Invoke(new LandEvent { unit = parent });
        }

        private void HandleGravity()
        {
            if (groundSteepness <= 0f)
            {
                // Ground below - push a little bit towards ground
                parent.rigidbody.linearVelocity -= groundHit.normal * Time.fixedDeltaTime;
            }
            else
            {
                // Airborne - falling
                var newVelocity = parent.rigidbody.linearVelocity.AddY(- gravity * Time.fixedDeltaTime);
                newVelocity.y = Mathf.Max(newVelocity.y, - fallMaxSpeed);
                parent.rigidbody.linearVelocity = newVelocity;
            }
        }

        private void HandleGroundFriction()
        {
            if (!isOnGround) return;
            if (groundSteepness >= 0.99f) return;

            var velocity = parent.rigidbody.linearVelocity;
            velocity -= velocity * (groundFriction * (1 - groundSteepness) * Time.fixedDeltaTime);
            
            if (Mathf.Abs(velocity.x) < 0.03f) velocity.x = 0f;
            if (Mathf.Abs(velocity.y) < 0.03f) velocity.y = 0f;
            if (Mathf.Abs(velocity.z) < 0.03f) velocity.z = 0f;
            
            parent.rigidbody.linearVelocity = velocity;
        }

        private void HandleRotation()
        {
            // Rotation disabled - wasn't rotating before - do nothing
            if (rotateType is UnitMovementRotateType.None && _rotateAction == null) return; 
            
            // Rotation enabled - wasn't rotating before - start rotating
            if (rotateType is not UnitMovementRotateType.None && _rotateAction == null)
            {
                _rotateAction = new RotateAction(rotationSpeed);
                parent.actions.Execute(_rotateAction);
            }
            
            // Rotating disabled - was rotating before - stop rotating
            if (rotateType is UnitMovementRotateType.None && _rotateAction != null)
            {
                _rotateAction.CancelIt();
                _rotateAction = null;
            }
            
            if (_rotateAction == null) return;

            switch (rotateType)
            {
                case UnitMovementRotateType.MoveDirection:
                    // Rotating enabled - was rotating before - update rotating yaw to movement direction
                    _rotateAction.SetYaw(Quaternion.LookRotation(accelerationVector).eulerAngles.y);
                    break;
                case UnitMovementRotateType.CustomDirection:
                    // Rotating enabled - was rotating before - update rotating yaw to custom direction
                    _rotateAction.SetYaw(rotateYaw);
                    break;
                default:
                    throw new ArgumentOutOfRangeException();
            }
        }

        public void OnMoveStart(MoveStartEvent ev)
        {
            isMoving = true;
        }
        
        public void OnMoveEnd(MoveEndEvent ev)
        {
            isMoving = false;
            accelerationVector = Vector3.zero;
            
            if (_rotateAction != null)
            {
                _rotateAction.CancelIt();
                _rotateAction = null;
            }
        }
    }
}