HoHCI-BikeSim/Assets/Scripts/Controller/Bicycle/RbBicycleController.cs

using System;
using UnityEngine;
using Wheels;

namespace Controller.Bicycle
{
    public class RbBicycleController : BicycleControllerBaseBehaviour, IBicycleController
    {
        #region Variables

        private const float THRESHOLD_GRADIENT_ZERO = .05f;

        private Transform rbTransform;
        private float currentSteerAngle;
        private float currentLeaningAngle;
        private float currentSpeedSensed;
        private float currentSpeedAdjusted;
        
        //private float previousSpeedAdjusted;
        //private float maxDifToPreviousSpeedDueToGradientPerSecond = 2f;

        [Header("Slope Impact")] public bool adjustSpeedToSlope = true;

        public Vector3 Forward => rbTransform.forward;

        public Vector3 Right => -rbTransform.right;

        public Vector3 Up => rbTransform.up;

        public BicycleControllerMode ControllerMode
        {
            get => controllerMode;
            set => controllerMode = value;
        }

        public float CurrentSpeed
        {
            get => adjustSpeedToSlope ? currentSpeedAdjusted : currentSpeedSensed;
            set => currentSpeedSensed = Mathf.Clamp(value, 0, maxSpeed);
        }

        public float CurrentSpeedKph => CurrentSpeed * 3.6f;

        public float CurrentSteerAngle
        {
            get => currentSteerAngle;
            set => currentSteerAngle = Mathf.Clamp(value, -maxSteeringAngle, maxSteeringAngle);
        }

        public float CurrentLeaningAngle
        {
            get => currentLeaningAngle;
            set
            {
                //don't lean while standing / walking to bike
                if (rigidBody.velocity.magnitude < .5f) return;
                currentLeaningAngle = Mathf.Clamp(value, -maxLeaningAngle, maxLeaningAngle);
            }
        }

        public Vector3 RigidBodyVelocity => rigidBody.velocity;

        #endregion

        private void Awake()
        {
            rbTransform = rigidBody.transform;
            rigidBody.freezeRotation = true;
            rigidBody.centerOfMass = centerOfMass.position;
        }

        private void FixedUpdate()
        {
            //rigidBody.isKinematic = currentSpeed <= THRESHOLD_STANDING;

            ApplyVelocity();
            ApplySteerAngleAndRotation();
        }

        private void ApplyVelocity()
        {
            var targetVelocity = CalculateTargetVelocity();

            var velocityChange = targetVelocity - rigidBody.velocity;
            velocityChange.y = 0;
            rigidBody.AddForce(velocityChange, ForceMode.VelocityChange);
        }

        private Vector3 CalculateTargetVelocity()
        {
            AdjustSpeedToGradientIfNeeded();
            var tv = new Vector3(0, 0, currentSpeedAdjusted);
            tv = rbTransform.TransformDirection(tv);
            return tv;
        }

        private void AdjustSpeedToGradientIfNeeded()
        {
            //previousSpeedAdjusted = currentSpeedAdjusted;
            var bikeAngle = rbTransform.localRotation.eulerAngles.x;
            if (!adjustSpeedToSlope || Mathf.Abs(bikeAngle) <= THRESHOLD_GRADIENT_ZERO)
            {
                currentSpeedAdjusted = currentSpeedSensed;
                return;
            }

            if (bikeAngle > 180)
            {
                bikeAngle -= 360f;
            }
            else if (bikeAngle < -180)
            {
                bikeAngle = 360f - bikeAngle;
            }

            var gradientDeg = -bikeAngle;
            var gradient = Mathf.Tan(gradientDeg * Mathf.Deg2Rad);

            //var maxDif = maxDifToPreviousSpeedDueToGradientPerSecond * Time.deltaTime;
            //var dif = Math.Abs(currentSpeedSensed)
            
            currentSpeedAdjusted = gradient < 0
                ? currentSpeedSensed * 1.5f
                : BicyclePhysics.SpeedAtGradientForSpeedAtFlat(currentSpeedSensed, rigidBody.mass,
                    gradient); //TODO make work for downhill - and lerp between the speeds!
        }

        private void ApplySteerAngleAndRotation()
        {
            //don't lean and rotate when veeeeeery sloooow/standing. Otherwise bike will rotate already
            if (CurrentSpeed < 0.3f) //ca 1 km/h
            {
                CurrentSteerAngle = 0;
                CurrentLeaningAngle = 0;
            }

            var sumMode = controllerMode.weightLeaning + controllerMode.weightSteering;
            var calculatedSteerAngle = (controllerMode.weightSteering * CurrentSteerAngle +
                                        controllerMode.weightLeaning + currentLeaningAngle) /
                                       sumMode; //TODO: maybe define what leaning angle means as steering angle;
            
            var r = rbTransform.localRotation.eulerAngles;

            float rectifiedZ;
            if (r.z > 180f)
            {
                rectifiedZ = -360 + r.z;
            }else if (r.z < -180f)
            {
                rectifiedZ = 360 + r.z;
            }
            else
            {
                rectifiedZ = r.z;
            }
            
            var leanDif = -CurrentLeaningAngle - rectifiedZ;
            rbTransform.localRotation =
                Quaternion.Euler(r + new Vector3(0, calculatedSteerAngle, leanDif) * Time.fixedDeltaTime);
        }
    }
}