jelito/Assets/jelycho/Code/Player/PlayerFPPLocomotion.cs

using System.Runtime.CompilerServices;
using RebootKit.Engine.Extensions;
using RebootKit.Engine.Foundation;
using RebootReality.jelycho.KinematicCharacterController.Core;
using Unity.Mathematics;
using UnityEngine;
using Logger = RebootKit.Engine.Foundation.Logger;

namespace RebootReality.jelycho.Player {
    public class PlayerFPPLocomotion : MonoBehaviour, ICharacterController {
        static readonly Logger s_Logger = new Logger(nameof(PlayerFPPLocomotion));

        [ConfigVar("fpp.show_debug", 1)]
        static ConfigVar s_showDebug;
        
        [SerializeField] KinematicCharacterMotor m_Motor;

        public float runSpeed = 10.0f;
        public float sprintSpeed = 20.0f;

        public float acceleration = 8.0f;
        public float groundFriction = 10.0f;
        
        public float jumpHeight = 2.0f;
        public float jumpInputDuration = 0.2f;
        public float coyoteDuration = 0.2f;

        [Range(0.0f, 1.0f)] public float airControl = 0.5f;
        public float gravity = 9.8f;
        
        public float YawRotation { get; set; }

        Vector3 m_WishDir;

        public bool IsSprinting {
            [MethodImpl(MethodImplOptions.AggressiveInlining)] get;
            [MethodImpl(MethodImplOptions.AggressiveInlining)] private set;
        }

        bool m_IsJumpRequested;
        float m_JumpRequestedTime;
        float m_LastGroundedTime;
        
        Vector3 m_LastVelocity;

        public bool IsGrounded {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get {
                return m_Motor.GroundingStatus.IsStableOnGround;
            }
        }

        public float SpeedXZ {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get {
                return m_Motor.Velocity.With(y: 0.0f).magnitude;
            }
        }
        
        public Vector3 LocalVelocity {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get {
                return transform.InverseTransformDirection(m_LastVelocity);
            }
        }
        
        public Vector3 Velocity {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get {
                return m_LastVelocity;
            }
        }

        void Awake() {
            m_Motor.CharacterController = this;
        }

        void OnDrawGizmos() {
            Gizmos.color = Color.blue;
            Gizmos.DrawLine(transform.position,
                            transform.position + m_WishDir * 2.0f);
        }
        
        static GUIStyle s_debugLabelStyle;

        void OnGUI() {
            if (s_showDebug.IndexValue == 0) {
                return;
            }

            if (s_debugLabelStyle == null) {
                s_debugLabelStyle = new GUIStyle(GUI.skin.label) {
                    fontSize = 20,
                    normal = { textColor = Color.white },
                    alignment = TextAnchor.LowerLeft
                };
            }

            GUI.Label(new Rect(0, 0, Screen.width, Screen.height),
                      $"Is Grounded: {m_Motor.GroundingStatus.IsStableOnGround.ToString()}\n" +
                      $"Motor Velocity: {m_Motor.Velocity}, magnitude: {m_Motor.Velocity.magnitude}\n" +
                      $"XZ Motor Velocity: {m_Motor.Velocity.With(y: 0.0f).magnitude}",
                      s_debugLabelStyle);
        }
        
        public void WarpTo(float3 position) {
            m_Motor.SetPosition(position);
            m_LastGroundedTime = 0.0f;
            m_IsJumpRequested = false;
            m_JumpRequestedTime = 0.0f;
            m_WishDir = Vector3.zero;
            m_LastVelocity = Vector3.zero;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void SetWishDirection(Vector3 dir) {
            m_WishDir = dir.normalized;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Jump() {
            m_IsJumpRequested = true;
            m_JumpRequestedTime = Time.time;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void SetSprint(bool isSprinting) {
            IsSprinting = isSprinting;
        }
        
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static float CalculateJumpVelocity(float jumpHeight, float gravity) {
            return math.sqrt(2.0f * gravity * jumpHeight);
        }
        
        public void UpdateRotation(ref Quaternion currentRotation, float deltaTime) {
            currentRotation = Quaternion.AngleAxis(YawRotation, Vector3.up);
        }
        
        void Accelerate(ref Vector3 currentVelocity,
                        float wishSpeed,
                        float control,
                        float deltaTime) {
            float currentSpeed = Vector3.Dot(currentVelocity, m_WishDir);
            
            float addSpeed = wishSpeed - currentSpeed;
            if (addSpeed <= 0.0f) {
                return;
            }
            
            float accelSpeed = acceleration * deltaTime * wishSpeed;
            if (accelSpeed > addSpeed) {
                accelSpeed = addSpeed;
            }
            
            currentVelocity += accelSpeed * control * m_WishDir;
        }

        void AirAccelerate(ref Vector3 currentVelocity,
                           float deltaTime) {
            float wishSpeed = currentVelocity.magnitude;
            if (wishSpeed >= runSpeed) {
                wishSpeed = runSpeed;
            }

            float currentSpeed = Vector3.Dot(currentVelocity, m_WishDir);
            float addSpeed = wishSpeed - currentSpeed;
            if (addSpeed <= 0.0f) {
                return;
            }
            
            float accelSpeed = deltaTime * wishSpeed;
            if (accelSpeed > addSpeed) {
                accelSpeed = addSpeed;
            }

            currentVelocity += accelSpeed * m_WishDir;
        }
        
        void ApplyFriction(ref Vector3 currentVelocity, float friction, float deltaTime) {
            float speed = currentVelocity.With(y: 0.0f).magnitude;
            if (speed <= 0.0f) {
                return;
            }
            
            float stopSpeed = 12.0f;
            float control = speed < stopSpeed ? stopSpeed : speed;
            float newSpeed = speed - friction * control * deltaTime;
            
            if (newSpeed <= 0.0f) {
                newSpeed = 0.0f;
            }
            newSpeed /= speed;

            currentVelocity.x *= newSpeed;
            currentVelocity.z *= newSpeed;
        }

        void GroundMovement(ref Vector3 currentVelocity, float deltaTime) {
            float wishSpeed = IsSprinting ? sprintSpeed : runSpeed;

            ApplyFriction(ref currentVelocity, groundFriction, deltaTime);
            Accelerate(ref currentVelocity, wishSpeed, 1.0f, deltaTime);
        }
        
        void ApplyGravity(ref Vector3 currentVelocity, float deltaTime) {
            currentVelocity.y -= gravity * deltaTime;
        }
        
        public void UpdateVelocity(ref Vector3 currentVelocity, float deltaTime) {
            if (Time.time > m_JumpRequestedTime + jumpInputDuration) {
                m_IsJumpRequested = false;
            }
            
            if (m_Motor.GroundingStatus.IsStableOnGround) {
                m_LastGroundedTime = Time.time;
            }

            bool canJump = m_Motor.GroundingStatus.IsStableOnGround ||
                           Time.time < m_LastGroundedTime + coyoteDuration;

            if (m_IsJumpRequested && canJump) {
                currentVelocity.y = CalculateJumpVelocity(jumpHeight, gravity);
                m_Motor.ForceUnground();

                m_IsJumpRequested = false;
                m_LastGroundedTime = 0.0f;
            }
            
            if (m_Motor.GroundingStatus.IsStableOnGround) {
                GroundMovement(ref currentVelocity, deltaTime);
            } else {
                AirAccelerate(ref currentVelocity, deltaTime);
                ApplyGravity(ref currentVelocity, deltaTime);
            }

            m_LastVelocity = currentVelocity;
        }
        
        public void BeforeCharacterUpdate(float deltaTime) {
        }
        
        public void PostGroundingUpdate(float deltaTime) {
        }
        
        public void AfterCharacterUpdate(float deltaTime) {
        }
        
        public bool IsColliderValidForCollisions(Collider coll) {
            return true;
        }
        
        public void OnGroundHit(Collider hitCollider, Vector3 hitNormal, Vector3 hitPoint, ref HitStabilityReport hitStabilityReport) {
        }
        
        public void OnMovementHit(Collider hitCollider, Vector3 hitNormal, Vector3 hitPoint, ref HitStabilityReport hitStabilityReport) {
        }
        
        public void ProcessHitStabilityReport(Collider hitCollider, Vector3 hitNormal, Vector3 hitPoint, Vector3 atCharacterPosition, Quaternion atCharacterRotation, ref HitStabilityReport hitStabilityReport) {
        }
        
        public void OnDiscreteCollisionDetected(Collider hitCollider) {
        }
    }
}